Save MoveIt! Octomap

Save MoveIt! Octomap

本文关键字：Octomap MoveIt Save      更新时间：2023-10-16

我正在运行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＆amp;）的引用。即使我包含所有可用于OCTOMAP和OCTOMATH的标题，此错误仍保持不变。我该如何解决？

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

相关文章：