Octomap 在ROS環境下實時顯示

常在別人論文的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-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發佈出去.代碼如下:

    #include<ros/ros.h>  
    #include<pcl/point_cloud.h>  
    #include<pcl_conversions/pcl_conversions.h>  
    #include<sensor_msgs/PointCloud2.h>  
    #include<pcl/io/pcd_io.h>

    int main (int argc, char **argv)  
    {  
      ros::init (argc, argv, "orbslam");  
      ros::NodeHandle nh;  
      ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("/orbslam2_with_kinect2/output", 10);  

      pcl::PointCloud<pcl::PointXYZ> cloud;  
      sensor_msgs::PointCloud2 output;  
      pcl::io::loadPCDFile ("/home/crp/catkin_ws/test.pcd", cloud);  
      pcl::toROSMsg(cloud,output);// 轉換成ROS下的數據類型 最終通過topic發佈

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

      ros::Rate loop_rate(1);  
      while (ros::ok())  
      {  
        pcl_pub.publish(output);  
        ros::spinOnce();  
        loop_rate.sleep();  
      }  
      return 0;  
    }  

相關的依賴項配置可以參考這個博客[1] .啓動這個代碼就可以看到發佈的點雲數據的topic.你可以使用rostopic echo 來檢查是否有數據輸出.

如我的命令是: rostopic echo /orbslam2_with_kinect2/output

打開新的終端運行:

rosrun rviz rviz

點擊add 按鈕添加 "PointCloud2模塊"
設置topic爲 "/orbslam2_with_kinect2/output"
設置FixedFram爲"camera_rgb_frame"
設置完成以後你可以看到界面中會顯示出topic 發佈的點雲數據,如下圖一樣:
(一定要確保topic上面有數據,後面需要讀取這個topic 轉換成octomap)

3.Octomap 實時顯示

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

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.1" />

    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <param name="frame_id" type="string" value="/camera_rgb_frame" />

    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="50.0" />
    <param name="latch" value="true" />

    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="1000" />
    <param name="pointcloud_min_z" value="0" />

    <!-- topic from where pointcloud2 messages are subscribed -->
    <remap from="cloud_in" to="/orbslam2_with_kinect2/output" />

  </node>
</launch>

—–原諒我忘記了這個代碼在哪裏參考了.

啓動了這個節點以後,會有幾個相關的 topic 發佈: (如下圖)

roslaunch test.launch

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

到這裏你可以將點雲數據發佈到一個指定的 topic 上,然後調用 Octomap 在ROS下的組件進行實時轉換,併發布到另外一個 topic 上去.最後通過可視化工具 rviz 進行顯示. 後續你可以將ORB的關鍵幀生成點雲,這個代碼高博以及寫過了,可在github找到.最後你把這個地圖發佈到一個topic上.,就可以實現實時的Octomap 啦,在做導航什麼的就方便了.

下面是我自己做的一個DEMO:
https://v.youku.com/v_show/id_XMjkyMTA4NTA1Ng==.html?spm=a2hzp.8244740.0.0

最後，在網上找了很久都沒有發現能脫離ROS使用的API函數. 但是這個也沒有關係,基本上這種在線跑的SLAM,傳感器數據都是依賴ROS讀取進來的,所以使用RVIZ顯示沒啥影響.

歡迎拍磚,討論 [email protected]

如果你覺得本教程對你有所幫助,歡迎對小弟進行小額的贊助.(一臉賤笑中)

參考鏈接

[1] http://www.cnblogs.com/li-yao7758258/p/6651326.html