C++ 省省吧！八达图

我正在运行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);
}

---

请原谅这些名字的混乱和缺乏想象力。在转换为抽象八叉树之前，它一直有效。当我做catkin_时，我有一个错误，很多很多引用是未定义的，例如对octomath:：Quaternion:：Quaternion（octomath:：Quaternion const&）的引用。即使我包含了octomap和octomath可用的所有标题，错误仍然保持不变。我该如何解决这个问题呢？

好吧，我的问题实际上相当愚蠢：我没有包括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
}

记住要包括所有需要的标题，如octomapmsgs/conversions.h等等