Сохранить MoveIt! Octomap

Question

Сохранить MoveIt! Octomap

Я использую Ubuntu 14.04 и ROS Indigo. Я хочу получить доступ к OctoMap, сгенерированному с помощью MoveIt! и преобразовать его в стандартный формат OctoMap и сохранить / обработать его. Следующий фрагмент кода должен сделать это:

void cloud_cb(const moveit_msgs::PlanningScenePtr& input)
{
moveit_msgs::PlanningScene::Ptr my_planning_scene(new moveit_msgs::PlanningScene);
*my_planning_scene = *input;
moveit_msgs::PlanningSceneWorld my_world = (*my_planning_scene).world;
moveit_msgs::PlanningSceneWorld::Ptr real_map(&my_world);
octomap_msgs::OctomapWithPose octomap_pose_content = (*real_map).octomap;
octomap_msgs::OctomapWithPose::Ptr octomap_pose(&octomap_pose_content);
octomap_msgs::Octomap octomap_content = (*octomap_pose).octomap;

octomap::AbstractOcTree* my_map = octomap_msgs::binaryMsgToMap(octomap_content);
}

Пожалуйста, прости беспорядок и недостаток воображения для имен. Работает, вплоть до преобразования в AbstractOcTree. Когда я catkin_make, у меня возникает ошибка, что многие, многие ссылки не определены, как, например, ссылка на octomath :: Quaternion :: Quaternion (octomath :: Quaternion const&). Даже если я включу все заголовки, доступные для octomap и octomath, ошибка останется прежней. Как я могу решить это?

0

c++moveit save

Решение

Другие решения

Других решений пока нет …

Источник

Accepted Answer

Хорошо, моя проблема была довольно глупой: я не включил каталоги OctoMap и не связал библиотеки в CMakeList, как указано в http://wiki.ros.org/octomap. Глупый я. В любом случае я публикую здесь свое решение. Я не знаю, есть ли более умный / быстрый способ сделать это, но этот работает:

using namespace octomap;

void cloud_cb(const moveit_msgs::PlanningScenePtr& input)
{
// Extracting the MoveIt! planning scene world published by /move_group
moveit_msgs::PlanningScene::Ptr my_planning_scene(new moveit_msgs::PlanningScene);
*my_planning_scene = *input;
moveit_msgs::PlanningSceneWorld my_world = (*my_planning_scene).world;

// Extracting only the OctoMap
octomap_msgs::OctomapWithPose octomap_pose = my_world.octomap;
octomap_msgs::Octomap octomap = octomap_pose.octomap;

// Conversion from octomap_msgs to octomap
AbstractOcTree* my_abstract_map = octomap_msgs::msgToMap(octomap);

// Obtaining the actual OctoMap tree
OcTree* my_map = (OcTree*)my_abstract_map;
OcTree tree = *my_map;

tree.writeBinary("my_tree.bt"); //if you want to save the OcTree in a file
}

И не забудьте включить все необходимые заголовки, как octomap_msgs / conversions.h и так далее.

0