slam小车实现——ROS篇：基于move_base和amcl实现导航功能

前篇回顾

在之前的一篇文章中(slam建图方法）我们已经实现了slam，并将地图保存在了/home/kiltto/develop/mapSave目录下，本次我们将利用这个地图实现定位和导航规划的功能（小车底层驱动还没实现好，暂时还开不了。。。）

move_base和amcl是干嘛的

1、move_base：负责完成全局路径规划和本地实时规划；

为什么需要两种规划方式？
因为单靠全局路径规划是无法克服小车在行进过程中多变的外部环境对行进路径的干扰的，有的时候因为障碍物的遮挡等原因，小车必须要重新规划路线，这个时候就需要本地实时规划功能。

2、amcl：一种自适应的蒙特卡罗定位方法，使用了粒子滤波器，在rviz调试的过程中可以看到粒子的分布状态，具体实现随着往后理解的深入再补充。

launch文件编写

参考《ROS机器人开发实践》中nav_demo的launch文件，把启动过程分为三个重要的步骤：加载地图、启动move_base节点、启动amcl节点

1，入口launch启动文件

<launch>

    <param name="use_sim_time" value="false" />

    <!-- 设置地图的配置文件 -->
    <arg name="map" default="test.yaml" />

    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="/home/kiltto/develop/mapSave/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mrobot_navigation)/launch/move_base.launch" />

    <!-- 启动AMCL节点 -->
    <include file="$(find mrobot_navigation)/launch/amcl.launch" />

    <!-- 设置一个/odom与/map之间的静态坐标变换 -->
    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/nav.rviz"/>

</launch>

2，调用的move_base.launch

• move_base启动过程需要配置代价地图（constmap）的相关参数：
  （1）通用配置文件：配置全局和本地的代价地图的通用配置；
  （2）全局规划配置文件（local_costmap_params.yaml）：配置全局代价地图参数；
  （3）本地地图配置文件（global_costmap_params.yaml）：配置本地代价地图参数；
• 然后还需要配置本地规划器（base_local_planner_params.yaml）的配置参数，本地规划器是用来根据全局规划的路径计算发布给小车的速度控制指令（/cmd_vel）

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    
    <!-- 通用配置文件（全局） -->
    <rosparam file="$(find mrobot_navigation)/config/mrobot/costmap_common_params.yaml" command="load" ns="global_costmap" />
    
    <!-- 通用配置文件（本地） -->
	<rosparam file="$(find mrobot_navigation)/config/mrobot/costmap_common_params.yaml" command="load" ns="local_costmap" />
    
    <!-- 本地代价地图参数 -->
    <rosparam file="$(find mrobot_navigation)/config/mrobot/local_costmap_params.yaml" command="load" />
    
    <!-- 全局代价地图参数 -->
    <rosparam file="$(find mrobot_navigation)/config/mrobot/global_costmap_params.yaml" command="load" />
    
    <!-- 本地规划器 -->
    <rosparam file="$(find mrobot_navigation)/config/mrobot/base_local_planner_params.yaml" command="load" />
  </node>
  
</launch>

3，调用的amcl.launch

<launch>
    <arg name="use_map_topic" default="false"/>
    <arg name="scan_topic" default="scan"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_max_range" value="12.0"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.2"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.25"/>
        <param name="update_min_a" value="0.2"/>
        <param name="odom_frame_id" value="odom"/>
        <param name="resample_interval" value="1"/>
        <!-- Increase tolerance because the computer can get quite busy -->
        <param name="transform_tolerance" value="1.0"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

开始（模拟：假设小车没在运动）

1、模拟发布odom

rostopic pub -r 10 /odom nav_msgs/Odometry '{pose: {pose: {position: {x: 0, y: 0, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 0}}}, twist: {twist: {linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}}}'

2、模拟发布cmd_vel

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x:, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'

3、发布tf：

1、base_link：机器人位置为原点的坐标系
2、odom：里程计安装位置为原点的坐标系
3、map：地图坐标系

由于我们使用了amcl，因此tf树必须满足：bask_link—>odom—>map，如图所示

4、启动launch脚本

roslaunch mrobot_navigation nav_demo.launch

由图可见已经实现了对当前位置的定位，绿色箭头为以初始位置为中心分布的粒子

5、在rviz上发布导航点


可见，路径已经生成，实现了我们想要的导航功能