原出處：https://blog.csdn.net/x1825048925/article/details/77187168
作者：今夕何夕
作者博客：https://blog.csdn.net/x1825048925
說明
這份源碼能實現Kobuki自主探索房間並建立房間的三維地圖，地圖表現形式是八叉樹。
源碼下載與配置
步驟：
- 安裝octomap_ros和rviz插件
sudo apt-get install ros-indigo-octomap*- 1
- 源碼安裝
cd turtlebot_ws/src
git clone https://github.com/RobustFieldAutonomyLab/turtlebot_exploration_3d.git
catkin_make- 1
- 2
- 3
- deb包安裝：
sudo apt-get update
sudo apt-get install ros-indigo-turtlebot-exploration-3d- 1
- 2
運行：
roslaunch turtlebot_exploration_3d minimal_explo.launch
roslaunch turtlebot_exploration_3d turtlebot_gmapping.launch
rosrun turtlebot_exploration_3d turtlebot_exploration_3d
roslaunch turtlebot_exploration_3d exploration_rviz.launch- 1
- 2
- 3
- 4
最後一條指令是打開RVIZ查看地圖，不打開機器人已經能夠開始探索啦。
代碼解讀
主程序是在src目錄下的turtlebot_exploration_3d.cpp，主要講解這個文件。
1. 全局變量聲明和配置
位置：exploration.h的21到40行
說明：
- 第25行的octo_reso變量定義了octomap中的最小分辨率，如果要提高octomap的分辨率可以適當減小該變量的值；
- 第28行的num_of_bay定義爲高斯過程的迭代次數;
- 第31至33行是對octomap和octomap消息的變量聲明，特別是cur_tree，作爲後面程序中octomap的指針變量；
- 第37行聲明的三維點變量kinect_orig在程序中表示機器人的實時位置；
第40行octomap_name_3d變量在turlebot_exploration.cpp的第38行中被初始化爲Octomap3D_%m%d_%R_%S.ot的形式，並在exploration.h的第291行中，作爲保存octomap的名字；
2. 全局變量初始化
位置： turtlebot_exploration_3d.cpp的25到72行
說明：第42至47行定義了程序中消息的訂閱和發佈, kinect_sub爲訂閱者接受kinect圖像信息, GoalMarker_pub發佈目標點信息, Candidates_pub發佈候選點信息,
Frontier_points_pub發佈前沿點信息, pub_twist發佈對機器人的控制信息,Octomap_pub發佈octomap的信息;- 第64至68行初始化octomap;
第69行定義下一個探測點next_vp;
3. 更新機器人的初始位置
位置: turtlebot_exploration_3d.cpp的74到108行
說明:第77至88行從kinect發佈的消息中獲取圖像信息, 通過lookupTransform函數獲取位置信息，並且更新機器人的位置kinect_orig;
- 第91行ros::spinOnce()是ros的回調函數, 回調在exploration.h中265行定義的kinectCallbacks函數，通過kinect傳來的圖像消息,更新併發送和保存octomap信息;
第97至103行爲旋轉60度，原因是twist_cmd.angular.z定義旋轉角速度爲0.6，旋轉時間由ros::Duration(2.6)定義爲2.6-1=1.6的旋轉角度爲(0.6×1.6) × 180/π = 55 ≈ 60；
4. 獲取併發布前沿點信息
位置: turtlebot_exploration_3d.cpp的115到157行
說明:第115行中，通過extractFrontierPoints函數從octomap中獲取前沿點，該獲取前沿點的函數定義在exploration.h的111至184行;
第118至157行根據獲取到的前沿點數據frontier_groups，創建前沿點消息Frontier_points_cubelist並將前沿點消息發佈出去;
5. 提取候選點
位置: turtlebot_exploration_3d.cpp的159到169行
說明:在160行中，使用extractCandidateViewPoints函數從前沿點中提取候選點，該函數聲明在exploration.h的188至252行;
在164至169行中，如果候選點的數量不夠，程序終止;
6. 爲每個候選點初始化交互信息(mutual information)
位置: turtlebot_exploration_3d.cpp的175到203行
說明:第177行使用countFreeVolume函數計算octomap中的空閒空間體積大小保存在before中，該函數定義在exploration.h的80至87行;
第185至196行中，使用calc_MI函數初始化交互信息數組MIs, 爲每個元素賦值爲該候選點的空閒空間與當前空閒空間的體積之差，保存在MIs數組中；
7. 高斯過程迴歸
位置: turtlebot_exploration_3d.cpp的207到251行
說明:第212至223行初始化高斯迴歸的參數;
- 第225至250行執行高斯迴歸過程，得到acquisition function,並計算交互信息;
詳細可見論文:Information-Theoretic Exploration with Bayesian Optimization
8. 發佈候選點信息
位置: turtlebot_exploration_3d.cpp的266到297行
說明:第272至294行，把每個候選點的信息加入要發佈的消息CandidatesMarker_array中；
第295行發佈候選點消息；
9. 選取併到達目標點
位置: turtlebot_exploration_3d.cpp的304到384行
說明:第306行，通過候選點和交互信息選取下一個目標點，該交互信息數組已經使用exploration.h中第42至50行的sort_MIs函數進行了排序;
- 第307至333行，向rviz中發佈目標點信息;
- 第336行，使用定義在navigation_utils.cpp中的goToDest函數向MoveBaseClient發送目標點信息，使機器人移動到目標點;
- 第338至373行，如果到達了目標點，則更新octomap並且發送msg_octomap消息;
- 第374至382行，如果沒有到達目標點，且沒有更多的目標點可以選擇，程序結束；
10. 點雲地圖的保存
在程序中使用kinect採集的點雲信息的位置在exploration.h中，第265至296行的kinectCallbacks函數，該函數通過ros::spinOnce被調用. 該函數傳入的參數cloud2_msg爲ros的消息，程序266至277行經過如下轉化
pcl::PCLPointCloud2 cloud2;
pcl_conversions::toPCL(*cloud2_msg, cloud2);
PointCloud* cloud (new PointCloud);
PointCloud* cloud_local (new PointCloud);
pcl::fromPCLPointCloud2(cloud2,*cloud_local);
octomap::Pointcloud hits;
ros::Duration(0.07).sleep();
while(!pcl_ros::transformPointCloud("/map", *cloud_local, *cloud, *tf_listener))
{
ros::Duration(0.01).sleep();
}- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
便把點雲消息轉化爲點雲數據保存在cloud指針指向的點雲中；之後就可以用相應的保存點雲數據函數保存點雲圖像，比如
pcl::io::savePCDFileASCII (cloud_name, cloud); // 保存點雲- 1
Octomap的保存和讀取
環境配置
- 下載octomap源碼
git clone https://github.com/OctoMap/octomap- 1
- 編譯
mkdir build
cd build
cmake ..
make- 1
- 2
- 3
- 4
- 使用
cd octomap/bin
./octovis the .ot file to view- 1
- 2
這就是這份開源代碼的個人解讀與擴展使用
