在ROS下面的一個(gè)組件實(shí)現(xiàn)Octomap顯示

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

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

注:使用平臺(tái)是 ubuntu14.04 ROS Indigo 版本

1.安裝octomap

這個(gè)功能需要借助ros,因此我們打開(kāi)一個(gè)終端.(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

安裝上這個(gè)插件以后你可以啟動(dòng) rviz ,這時(shí)候這個(gè)模塊會(huì)多一個(gè)octo打頭的模組.如下圖所示:


? ?

2.發(fā)布點(diǎn)云數(shù)據(jù)

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

/**
*
* 函數(shù)功能:讀取pcl點(diǎn)云文件并發(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ù)類型 最終通過(guò)topic發(fā)布

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

cout<<"path = "<

我們通過(guò)如下代碼單獨(dú)啟動(dòng)點(diǎn)云發(fā)布節(jié)點(diǎn)

rosrun publish_pointcloud publish_pointcloud

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

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



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

rostopic echo /pointcloud/output

然后再打開(kāi)新的終端運(yùn)行RVIZ:

rosrun rviz rviz

點(diǎn)擊add 按鈕添加 "PointCloud2模塊"

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

設(shè)置FixedFram為"camera"

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

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

? ?

3.Octomap 實(shí)時(shí)顯示

接下來(lái)的工作就簡(jiǎn)單了,我們自己寫(xiě)一個(gè)launch文件去啟動(dòng) octomap_server ,創(chuàng)建 octomaptransform.launch 文件,填入下面代碼:

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

接下來(lái)首先啟動(dòng)點(diǎn)云發(fā)布節(jié)點(diǎn)

rosrun publish_pointcloud publish_pointcloud

其次啟動(dòng)了這個(gè)節(jié)點(diǎn)以后，我們?cè)偃?dòng)Octomap服務(wù)節(jié)點(diǎn), 正確啟動(dòng)以后會(huì)有幾個(gè) octomap 相關(guān)的 topic 發(fā)布: (如下圖)

roslaunch publish_pointcloud octomaptransform.launch

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




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




最后我們將所有的啟動(dòng)命令寫(xiě)入到一個(gè)launch文件中，我們?cè)趐ublish_pointcloud 包中的 launch 文件夾下面編輯一個(gè)名為demo.launch的文件,填入下面代碼:

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

roslaunch publish_pointcloud demo.launch

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

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

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

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

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

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

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

審核編輯：劉清