4- LIO SAM+ Move_base 实现自主导航
在前面的工作中，我们成功使用了gmapping +move_base实现自主导航，并且成功在仿真环境中运行了lio sam，今天打算结合，采用LIO-SAM建图和定位，move_base实现导航。大致内容如下：
仿真环境准备
在之前的内容中，已经介绍了仿真实验环境怎么搭建，具体可以参考链接：
https://www.cnblogs.com/binbin2002/p/18296477
LIO SAM安装配置
LIO SAM作为一个开源的紧耦合的SLAM算法，网上教程太多了，这里贴几个个人觉得写的比较好的链接：
https://py1995.blog.csdn.net/article/details/137286327?spm=1001.2014.3001.5502
https://blog.csdn.net/weixin_40653140/article/details/137235669?spm=1001.2014.3001.5506
安装Octomap Server
https://blog.csdn.net/qq_41694024/article/details/134703116?spm=1001.2014.3001.5506
Octomap的安装
采用命令的形式安装octomap server功能包
sudo apt install ros-melodic-octomap-msgs ros-melodic-octomap-ros ros-melodic-octomap-rviz-plugins ros-melodic-octomap-server
Octomap节点的使用，构建octomap和occupied map
我们新建一个launch文件，在launch文件中输入以下内容：
<launch>
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<!-- 地图分辨率 -->
<param name="resolution" value="0.50" />
<!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
<!-- 静态全局地图的 frame_id，但在增量式构建地图时，需要提供输入的点云帧和静态全局帧之间的 TF 变换 -->
<param name="frame_id" type="string" value="map" />
<!-- maximum range to integrate (speedup!) -->
<!-- 传感器最大感知范围 -->
<param name="sensor_model/max_range" value="50.0" />
<param name="latch" value="true" />
<!--max/min height for occupancy map, should be in meters-->
<!-- 保留的z轴范围内的点 -->
<param name="pointcloud_max_z" value="5" />
<param name="pointcloud_min_z" value="-1.5" />
<!-- 机器人坐标系 base_link，滤除地面需要该 frame -->
<param name = "base_frame_id" type = "string" value = "base_link" />
<!-- filter ground plane, distance value should be big! -->
<param name = "filter_ground" type = "bool" value = "true" />
<!-- 要检测为平面的 Z 轴阈值 value 值 通常大于分割值 -->
<param name = "ground_filter/plane_distance" type = "double" value = "0.4" />
<!-- 分割地面的 Z 轴阈值 value 值 ，小于该阈值被认为是平面 -->
<param name = "ground_filter/distance" type = "double" value = "0.3" />
<!-- 分割地面的角度阈值 value 值，小于该阈值的平面被认为是地面-->
<param name="ground_filter/angle" value="0.10" />
<!-- data source to integrate (PointCloud2) -->
<remap from="cloud_in" to="/lio_sam/deskew/cloud_deskewed" />
<!-- <remap from="cloud_in" to="voxel_grid/output" /> -->
</node>
</launch>
这里面各个参数的含义解释：
resolution：分辨率或精度，越小精度越高计算需要资源越庞大
frame_id：世界坐标系的话题，即原点坐标系
sensor_model/max_range：扫描的最大距离
cloud_in：点云话题
pointcloud_max_z：最大的Z扫描值
pointcloud_min_z：最小的Z扫描值（激光雷达和垂直平面有一定距离）
当然还有地面剔除相关参数：
filter_ground：是否采用地面去除
ground_filter/plane_distance：要检测为平面的 Z 轴阈值 value 值 通常大于平面阈值
ground_filter/distance：要检测为平面的z轴阈值，低于这个值被认为是平面
ground_filter/angle：分割地面的的角度阈值，低于这个值的平面被认为是地面
RVIZ显示和地图保存
发布以后，使用rostopic list可以看到octomap相关话题，也可以在rviz中选择话题名称显示。
源码编译使用
当然还可以使用源码编译的方法完成上述：相关操作参考我的github链接：https://github.com/binbin2002/octomap_with_slam
安装Pointcloud_to_Laserscan
sudo apt-get install ros-melodic-tf2-sensor-msgs
在launch文件中添加以下内容：
<?xml version="1.0"?>
<launch>
<!-- run pointcloud_to_laserscan node -->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<!-- 重映射订阅的cloud_in和发布的scan话题 -->
<remap from="cloud_in" to="/non_ground_point"/>
<remap from="scan" to="base_scan"/>
<rosparam>
target_frame: base_link # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.01
min_height: -0.2
max_height: 1.0
angle_min: -3.1415926 # -M_PI
angle_max: 3.1415926 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.1
range_min: 1.0
range_max: 50
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</launch>
Move_base配置
这里只对关键内容进行进一步说明，采用官方给定的move_base文档：http://wiki.ros.org/move_base，
Move_base的核心图来讲解
图中，白色部分是move_base必须有的且已经实现的部分，灰色是我们可以提供的，蓝色是必须需要的。
通过查看move_base订阅和发布的topic来看，实践上我们只需要关注以下几个move_base关键的topic就行
- /odom ：Move_base订阅的里程计信息，
- /map ：Move_base订阅的地图服务信息
- /scan：Move_base订阅的激光数据信息
在lio sam中，/odom需要remap为/lio_sam/mapping/odometry, /scan需要remap成pointcloud_to_laserscan发布的/base_scan /map则不需要变，对应的整体参数如下:参数用默认的yaml文件即可，也可根据需要自行调整。
</node>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<remap from="odom" to="/lio_sam/mapping/odometry"/>
<remap from="scan" to="/base_scan"/>
<remap from="cmd_vel" to="robot1/cmd_vel"/>
<rosparam file="$(find gazebo_pioneer3at)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find gazebo_pioneer3at)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find gazebo_pioneer3at)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find gazebo_pioneer3at)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find gazebo_pioneer3at)/param/planner.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="planner_frequency" value="1.0" />
<param name="planner_patience" value="5.0" />
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="15.0" />
<param name="clearing_rotation_allowed" value="true" />
</node>
<!-- 需要用navigation对应的rviz，默认rviz不能发布目标点，ps：不知道问题在哪 -->
<node pkg="rviz" type="rviz" name="octomap_rviz" args="-d $(find gmapping)/navigation.rviz" />
运行效果截图
实验仿真环境
运行效果图(左 lio sam效果图 右 导航地图)
