保存MoveIt！ Octomap

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目錄，也沒有鏈接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等。