【ROS机器人】 --- 2-3. 定位_AMCL

本节目标：加载一张地图，在地图中放置一个机器人实现对机器人的定位。
实现：

1. 打开gazebo和模型文件（car_move.launch）
2. 加载地图
3. 启动机器人（gazebo）
4. 启动rviz，完成rviz配置

前期准备

1. 延用工作空间test2_ws;
2. 功能包demo5_navigation存放代码；
3. amcl和rviz代码在demo5_navigation/launch/nav2_test.launch；
4. amcl代码demo5_navigation/launch/nav2_amcl.launch；
5. rviz配置在demo5_navigation/config/rviz/nav2.rviz；

简单介绍

SLAM中的定位是指全局定位用于地图。这里的定位是指当前定位，用于导航。在ROS的导航功能包集中的AMCL就是一种提供给机器人定位的功能包。被集成到了navigation中。核心节点AMCL。在amcl功能包下，有差分diff和全向移动omni的例子，本节选用差速为例修改：

roscd amcl
cd examples
gedit  amcl_diff.launch 

I、 ACML文件

对照ACML的例子编写launch/nav2_amcl.launch，主要修改三个坐标系：

<param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
<param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
<param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

II、汇总launch文件

launch/nav2_test.launch
实现打开rviz，加载地图，执行acml的过程。

III、执行查看

1. 加载机器人模型(gazebo)

roslaunch demo4_urdf_gazebo car_move.launch 

2. 执行AMCL算法

roslaunch demo5_navigation/launch/nav2_test.launch 

3. 配置rviz并保存，本节重要的主要是pasearray

4. 键入控制机器人运动：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py 

可以看到线束最多的地方就是概率最大的位置。

源码

nav2_amcl.launch

<launch>
        <node pkg="amcl" type="amcl" name="amcl" output="screen">
                <!-- 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="transform_tolerance" value="0.2" />
                <param name="gui_publish_rate" value="10.0"/>
                <param name="laser_max_beams" value="30"/>
                <param name="min_particles" value="500"/>
                <param name="max_particles" value="5000"/>
                <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.8"/>
                <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_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.2"/>
                <param name="update_min_a" value="0.5"/>

                <param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
                <param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
                <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

                <param name="resample_interval" value="1"/>
                <param name="transform_tolerance" value="0.1"/>
                <param name="recovery_alpha_slow" value="0.0"/>
                <param name="recovery_alpha_fast" value="0.0"/>
        </node>
</launch>

nav2_test.launch

这里已经保存了rviz的配置为nav2.rviz并读取。

<launch>
        <!-- 启动 rviz -->
        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        <node pkg="rviz" type="rviz" name="rviz" args="-d $(find demo5_navigation)/config/rviz/nav2.rviz"  />

        <!-- 加载地图服务 -->
        <include file="$(find demo5_navigation)/launch/nav1_pub_map.launch" />

        <!-- AMCL文件 -->
        <include file="$(find demo5_navigation)/launch/nav2_amcl.launch" />

</launch>