在前面的工作中，我们成功使用了gmapping +move_base实现自主导航，并且成功在仿真环境中运行了lio sam，今天打算结合，采用LIO-SAM建图和定位，move_base实现导航。大致内容如下：

仿真环境准备

在之前的内容中，已经介绍了仿真实验环境怎么搭建，具体可以参考链接：
https://www.cnblogs.com/binbin2002/p/18296477
https://www.cnblogs.com/binbin2002/p/18326867

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在前面的文章中已经详细介绍了，可以参考：https://www.cnblogs.com/binbin2002/p/18296477

这里只对关键内容进行进一步说明，采用官方给定的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效果图 右 导航地图)