Octomap 在ROS环境下实时显示

更新日志：

• 2019-10-4：添加launch文件一键启动
• 2019-10-13：打包源文件和测试点云文件到网盘供大家下载

常在别人论文的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<std::string>("path", path, "/home/crp/catkin_ws/test.pcd");
	nh.param<std::string>("frame_id", frame_id, "camera");
	nh.param<std::string>("topic", topic, "/pointcloud/output");
    nh.param<int>("hz", hz, 5);
   
	ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> (topic, 10);  

	pcl::PointCloud<pcl::PointXYZ> 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 = "<<path<<endl;
	cout<<"frame_id = "<<frame_id<<endl;
	cout<<"topic = "<<topic<<endl;
	cout<<"hz = "<<hz<<endl;

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

相关的依赖项配置可以参考这个博客[1] . 我们通过如下代码单独启动点云发布节点

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 文件,填入下面代码:

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

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

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

    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="100.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="/pointcloud/output" />
 
  </node>
</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的文件,填入下面代码:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
 
  <node name="publish_pointcloud" pkg="publish_pointcloud" type="publish_pointcloud">
	<param name="path" value="$(find publish_pointcloud)/data/test.pcd" type="str" />
	<param name="frame_id" value="camera" type="str" />
	<param name="topic" value="/pointcloud/output" type="str" />
	<param name="hz" value="2" type="int" />
  </node>

  <!-- Load ocotmap launch -->
  <include file="$(find publish_pointcloud)/launch/octomaptransform.launch" />

  <!-- RViz -->
  <node pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find publish_pointcloud)/rviz/OctomapShow.rviz"/>

</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的构建， 下面是我自己做的一个DEMO:
https://v.youku.com/v_show/id_XMjkyMTA4NTA1Ng==.html?spm=a2hzp.8244740.0.0

Octomap 实时显示

利用ORB-SLAM来构建点云和八叉树地图的教程可参考：
　　　ORB-SLAM2 在线构建稠密点云（一）
　　　ORB-SLAM2 在线构建稠密点云（二）

最后，在网上找了很久都没有发现能脱离ROS使用的API函数. 但是这个也没有关系,基本上这种在线跑的SLAM,传感器数据都是依赖ROS读取进来的,所以使用RVIZ显示没啥影响.

欢迎拍砖,讨论 [email protected] （代码无法运行情况，请大家在评论留言或者邮件）

参考链接

[1] http://www.cnblogs.com/li-yao7758258/p/6651326.html
[2] 代码链接： https://github.com/RuPingCen/publish_pointcloud