常在別人論文的demo演示視頻中看到能夠?qū)崟r顯示Octomap, 在經(jīng)過幾番查找以后發(fā)現(xiàn)這個功能可以通過rviz(ROS下面的一個組件)實現(xiàn)。

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

注:使用平臺是 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.發(fā)布點云數(shù)據(jù)

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

/**
*
* 函數(shù)功能:讀取pcl點云文件并發(fā)布到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);// 轉(zhuǎn)換成ROS下的數(shù)據(jù)類型 最終通過topic發(fā)布

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

cout<<"path = "<

我們通過如下代碼單獨啟動點云發(fā)布節(jié)點

rosrun publish_pointcloud publish_pointcloud

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

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



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

rostopic echo /pointcloud/output

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

rosrun rviz rviz

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

設(shè)置topic為 "/pointcloud/output"

設(shè)置FixedFram為"camera"

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

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

? ?

3.Octomap 實時顯示

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

注意，這個文件里面有的frame_id 和 remap topic 的值必須和發(fā)布節(jié)點中的frame_id以及數(shù)據(jù)發(fā)布的topic一致。

接下來首先啟動點云發(fā)布節(jié)點

rosrun publish_pointcloud publish_pointcloud

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

roslaunch publish_pointcloud octomaptransform.launch

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




如果你直接下載的我的代碼【3】和數(shù)據(jù)應(yīng)該的得到的是如下的效果圖：




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

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

roslaunch publish_pointcloud demo.launch

到這里你已經(jīng)可以將點云數(shù)據(jù)發(fā)布到一個指定的 topic 上,然后調(diào)用 Octomap 在ROS下的srv組件進(jìn)行實時轉(zhuǎn)換,并發(fā)布到另外一個 Octomap topic 上去.最后通過可視化工具 rviz 進(jìn)行顯示Octomap。

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

第一種方法是你把每次SLAM計算得到的當(dāng)前時刻位姿和點云數(shù)據(jù)（當(dāng)前彩色幀和深度幀）進(jìn)行處理，利用這個位姿把當(dāng)前時刻的點云旋轉(zhuǎn)到世界坐標(biāo)系下發(fā)布給Octomap 節(jié)點。

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

另外一種思路就是你使用點云庫自帶的地圖維護(hù)工具，把Octomap只當(dāng)做一個轉(zhuǎn)換工具，每次都發(fā)布全局的點云地圖給octomap節(jié)點（隨著點云數(shù)據(jù)的增大會出現(xiàn)程序崩潰的現(xiàn)象）。

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

以上兩種思路都可以實現(xiàn)環(huán)境Octomap的構(gòu)建，。

審核編輯：劉清