常在別人論文的demo演示視頻中看到能夠實時顯示Octomap, 在經過幾番查找以后發現這個功能可以通過rviz(ROS下面的一個組件)實現。

實現的思路是將點云數據通過ROS發布到某個topic上面比如"/outputCloud",再啟動 octomap 節點將數據讀入該topic并發布到另一個新的topic 上面去。最后在rviz 里面接收這個新topic 達到實時顯示的目的.

注:使用平臺是 ubuntu14.04 ROS Indigo 版本

1.安裝octomap

這個功能需要借助ros,因此我們打開一個終端.(ctrl+alt+T)輸入下面指令安裝octomap (可以直接使用sudo apt-get install ros-indigo-octomap* ，如果你是Ubuntu16 的把 “indigo” 替換 “kinetic” 即可)

sudo apt-get install ros-indigo-octomap-ros #安裝octomap

sudo apt-get install ros-indigo-octomap-msgs

sudo apt-get install ros-indigo-octomap-server

安裝octomap 在 rviz 中的插件

sudo apt-get install ros-indigo-octomap-rviz-plugins

安裝上這個插件以后你可以啟動 rviz ,這時候這個模塊會多一個octo打頭的模組.如下圖所示:


? ?

2.發布點云數據

這里我先使用一個我自己在實驗室跑ORB生成的稠密點云文件,把這個點云文件加載然后通過一個topic發布出去。 如果你手頭沒有現成的點云文件可以在這個地方下載點云文件作為測試使用（test.pcd），完整的代碼和數據我已經打包放在了github上，源文件代碼如下:

/**
*
* 函數功能:讀取pcl點云文件并發布到topic上去
* maker: crp
* data: 2016-6-8
*/

#include
#include
#include
#include
#include
#include

#include
#include
#include
#include
#include

using namespace std;

int main (int argc, char **argv)
{
std::string topic,path,frame_id;
int hz=5;

ros::init (argc, argv, "publish_pointcloud");
ros::NodeHandle nh;

nh.param("path", path, "/home/crp/catkin_ws/test.pcd");
nh.param("frame_id", frame_id, "camera");
nh.param("topic", topic, "/pointcloud/output");
nh.param("hz", hz, 5);

ros::Publisher pcl_pub = nh.advertise (topic, 10);

pcl::PointCloud cloud;
sensor_msgs::PointCloud2 output;
pcl::io::loadPCDFile (path, cloud);
pcl::toROSMsg(cloud,output);// 轉換成ROS下的數據類型 最終通過topic發布

output.header.stamp=ros::Time::now();
output.header.frame_id =frame_id;

cout<<"path = "<

我們通過如下代碼單獨啟動點云發布節點

rosrun publish_pointcloud publish_pointcloud

注意： 這里你需要把path修改為你電腦上存放test.pcd文件的路徑，同時注意我們使用的坐標系是“camera” （這里需要和后面和octomaptransform.launch 文件中的 frame_id 參數一致，否則你會出現Octomap沒有發布數據的情況）

啟動這個代碼就可以看到發布的點云數據的topic.你可以使用rostopic echo 來檢查是否有數據輸出。我發布的點云數據的topic是“/pointcloud/output”



因此我用的命令為：（如果有數據輸出表示你正確的讀取并發布了點云數據）

rostopic echo /pointcloud/output

然后再打開新的終端運行RVIZ:

rosrun rviz rviz

點擊add 按鈕添加 "PointCloud2模塊"

設置topic為 "/pointcloud/output"

設置FixedFram為"camera"

設置完成以后你可以看到界面中會顯示出topic 發布的點云數據,如下圖一樣:

(一定要確保topic上面有數據,后面需要讀取這個topic 轉換成octomap，原來版本中使用的坐標系為“camera_rgb_frame”,修訂后的坐標系為"camera")

? ?

3.Octomap 實時顯示

接下來的工作就簡單了,我們自己寫一個launch文件去啟動 octomap_server ,創建 octomaptransform.launch 文件,填入下面代碼:

注意，這個文件里面有的frame_id 和 remap topic 的值必須和發布節點中的frame_id以及數據發布的topic一致。

接下來首先啟動點云發布節點

rosrun publish_pointcloud publish_pointcloud

其次啟動了這個節點以后，我們再去啟動Octomap服務節點, 正確啟動以后會有幾個 octomap 相關的 topic 發布: (如下圖)

roslaunch publish_pointcloud octomaptransform.launch

最后在rviz 中添加一個 “OccupancyGrid” 模塊(三維格點). 設置 topic 為"/octomap_full",即可以得到如下結果:




如果你直接下載的我的代碼【3】和數據應該的得到的是如下的效果圖：




最后我們將所有的啟動命令寫入到一個launch文件中，我們在publish_pointcloud 包中的 launch 文件夾下面編輯一個名為demo.launch的文件,填入下面代碼:

就可以通過上面的launch文件一鍵啟動節點以及RVIZ了。啟動命令為：

roslaunch publish_pointcloud demo.launch

到這里你已經可以將點云數據發布到一個指定的 topic 上,然后調用 Octomap 在ROS下的srv組件進行實時轉換,并發布到另外一個 Octomap topic 上去.最后通過可視化工具 rviz 進行顯示Octomap。

如果你在其他節點發布點云的數據，然后使用cotomap服務節點進行轉換是，最重要的是要注意octomap的輸入話題（topic）和數據的坐標系(frame_id)兩個參數的設置，通常octomap 沒有數據輸出都是由于這兩個參數設置錯誤導致的。 注意，對于實現增量式的Octomap構建（也就是像SLAM構建點云一樣，一邊走一邊生成全局的octomap），有兩種方法實現。

第一種方法是你把每次SLAM計算得到的當前時刻位姿和點云數據（當前彩色幀和深度幀）進行處理，利用這個位姿把當前時刻的點云旋轉到世界坐標系下發布給Octomap 節點。

由于Octomap 本身具有維護地圖的功能，它自己會去拼接八叉樹地圖，這可以省去很多事情。

另外一種思路就是你使用點云庫自帶的地圖維護工具，把Octomap只當做一個轉換工具，每次都發布全局的點云地圖給octomap節點（隨著點云數據的增大會出現程序崩潰的現象）。

第二種方法下你可以將ORB的關鍵幀生成點云然后一直發布更新后的點云,這個代碼高博以及寫過了,可在github找到. 你將這個包編譯到ROS上以后，再將這個算法生成的全局點云地圖發布到octomap節點上,也就可以實現實時的Octomap 啦,再做導航什么的就方便了。

以上兩種思路都可以實現環境Octomap的構建，。

審核編輯：劉清