0

我正在运行 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 的标头，错误仍然保持不变。我该如何解决这个问题？

4

1 回答 1

0

好的，我的问题实际上很愚蠢：我没有包含 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 等等。

于 2017-03-08T16:59:46.183 回答