基于稀疏点云地图实现Turtlebot2真实环境下导航

最近在完成基于稀疏点云地图实现turtlebot导航的工作，大致实现路线为稀疏地图-》稠密点云地图-》离线八叉树地图-》二维栅格地图-》保存地图并基于turtlebot完成真实环境下的路径规划导航。遂将查阅的资料归纳整理。

环境：ubtuntu16

1. 稠密点云地图

（1）源码

在ORBSLAM2构建稀疏点云地图的基础上增加了单独的稠密点云pcl显示线程

https://github.com/xiaobainixi/ORB-SLAM2_RGBD_DENSE_MAP.git

（2）编译运行

此处记得提前将原ORBSLAM2的词袋模型粘贴过来

     chmod +x build.sh
     ./build.sh

    mkdir build
    cd build
    cmake ..
    make -j

    ./run/rgbd_tum Vocabulary/ORBvoc.bin path_to_settings path_to_sequence path_to_association

这里以TUM数据集 rgbd_dataset_freiburg3_long_office_household序列为例

（3）保存pcd文件

在viewer线程中加入以下代码用于本地保存稠密点云地图

pcl：io：savePCDFile(“./pointcloud.pcd”,*globalmap)

1. 八叉树地图

这里主要实现离线转换，后续可考虑换成实时转换，可参考：

https://github.com/IATBOMSW/ORB-SLAM2_DENSE.git

（1）准备工作

octomap工具包安装

sudo apt-get install ros-indigo-octomap-ros 
sudo apt-get install ros-indigo-octomap-msgs
sudo apt-get install ros-indigo-octomap-server
sudo apt-get install ros-indigo-octomap-rviz-plugins

下载源码

GitHub - RuPingCen/publish_pointcloud: this code can be used for transfom the pointcloud into octomap

（2）具体实现

将publish_pointcloud.cpp中pcd的路径改为上一节本地保存的pcd文件路径

nh.param<std::string>("path", path, "./test.pcd");

demo.launch同理

<param name="path" value="./test.pcd" type="str" />

另外，需注意demo.launch与publish_pointcloud.cpp中发布的话题需要对应上

启动命令

roslaunch publish_pointcloud demo.launch

此时已经将稠密点云地图发布到指定的 topic 上,并完成八叉树转换,这里可通过可视化工具 rviz 进行显示

点击rviz中add，添加PointCloud2模块，设置topic为代码中你设置的话题，可通过rostopic echo 来检查是否有相应话题输出，此时可显示pcd文件的点云地图

添加OccupancyGrid 模块，设置 topic 为/octomap_full，即可看到点云地图转换后的八叉树地图

具体实现可参考：Octomap 在ROS环境下实时显示_ros octomap_熊猫飞天的博客-CSDN博客

1. 二维栅格地图

（1）加载话题实现

添加OccupancyMap 模块，设置 topic 为/octomap_full，即可看到八叉树地图转换后的二维栅格地图

这里仅展示滤除冗余信息之后的效果，黑色代表占据状态，即障碍物

（2）可能遇到的问题：

• 地面信息未滤除

解决方法很多，可以在生成点云地图的时候加滤波算法

这里介绍一个简单的方法：

修改demo.launch中点云地图z轴的阈值，将z轴的最小值微调至刚好把地面信息滤除即可。

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

但是有个缺点，针对不同场景的点云地图，可能需要设置不同的阈值，比较麻烦，有时间推荐加滤波算法进去

具体的滤波算法可参考这里的工作：

https://github.com/IATBOMSW/ORB-SLAM2_DENSE

• 点云地图坐标系错误，导致八叉树无法正确2D投影到栅格地图

这里可以通过pcl中的transforms完成空间中的坐标系转换

Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    //transform.scale(1/2);
    transform.translate(Eigen::Vector3f(0,0,1));
    transform.translate(Eigen::Vector3f(0,0,1));
    transform.translate(Eigen::Vector3f(0,0,1));
    transform.translate(Eigen::Vector3f(0,0,1));
    transform.rotate(Eigen::AngleAxisf(-M_PI/2,Eigen::Vector3f(1,0,0)));
    transform.rotate(Eigen::AngleAxisf(-M_PI/8,Eigen::Vector3f(1,0,0)));
    transform.translate(Eigen::Vector3f(0,1,0));
    transform.translate(Eigen::Vector3f(0,1,0));
    pcl::transformPointCloud(cloud,cloud,transform);

translate、rotate分别为坐标系的位移和旋转

1. 基于turtlebot完成真实环境下的路径规划导航

（1）保存地图

将上述二维栅格地图进行本地保存，其中/projected_map是octomap_server中二维栅格地图默认发布的名字

rosrun map_server map_saver map:=/projected_map -f map1

此时会生成pgm文件和yaml文件，其中yaml文件中机器人初始位置的Z轴需要改成0，因为此时机器人处于二维世界

根据上述步骤，在真实环境下亦可通过SLAM系统稀疏建图并最终转换成二维栅格地图

（2）加载地图

此时启动turtlebot2机器人

roslaunch turtlebot_bringup minimal.launch

加载上述步骤中保存的二维栅格地图文件

roslaunch turtlebot_navigation amcl_demo.launch map_file:=path/map1.yaml

rviz实时显示二维栅格导航地图和机器人位置

roslaunch turtlebot_rviz_launchers view_navigation.launch

（3）避障导航

此时开启算法定位（注：此处并没有开启kinect v2实现获取深度速度并转为激光数据）

故这块应该会报红，但是不妨碍机器人可以避障导航

roslaunch kinect2_gmapping.launch

• 实例效果

1. 总结

整个过程都是基于前人的工程化工作，实现起来还是不难的，但是可以看出整个过程还是存在一些问题

（1）八叉树离线

这里实现方式还是以离线为主，导致整体步骤不连贯，后续有时间将实时八叉树建图加进去

（2）滤波算法

在实现稠密点云地图时应考虑加入有效的滤波算法，而不是手动调整阈值，比较麻烦

（3）纯视觉导航

这块还是借用了kinect2_gmapping算法，感觉整体过程还是比较粗糙的，后续考虑纯视觉自动避障导航