常在别人论文的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]
如果你觉得本教程对你有所帮助,欢迎对小弟进行小额的赞助.(一脸贱笑中)
