保存移动它！章鱼图

Question

我是运行 Ubuntu 14.04 和 ROS Indigo。我想访问通过 MoveIt 生成的 OctoMap！并将其转换为标准的 OctoMap 格式并 save/process 它。下面的一段代码应该可以做到：

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 的所有 headers，错误仍然存在。我该如何解决？

Answer 1

好吧，我的问题实际上很愚蠢：我没有包含 OctoMap 目录，也没有 link CMakeList 中的库，如 http://wiki.ros.org/octomap 中所述。愚蠢的我。无论如何，我都会在这里发布我的解决方案。我不知道是否有 smarter/faster 的方法，但这个有效：

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
}

并记得包括所有需要的 headers，如 octomap_msgs/conversions.h 等等。

保存移动它！章鱼图

Save MoveIt! Octomap

c++

save

ros

moveit