拯救MoveIt！八面图

Question

我运行的是Ubuntu 14.04和ROS Indigo。我想访问通过MoveIt生成的OctoMap！并将其转换为标准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的所有头文件，错误仍然是一样的。我该如何解决这个问题呢？

Answer 1

好吧，我的问题实际上很愚蠢:我没有包括OctoMap目录，也没有像http://wiki.ros.org/octomap中所说的那样在CMakeList中链接库。愚蠢的我。无论如何，我都会在这里发布我的解决方案。我不知道是否有更聪明/更快的方法，但这一种方法有效：

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/Conversions.h等等。