ros navigation 中的amcl编译和运行

ros 中的navigation可以根据机器人自动实现路径规划，自主定位。其中的amcl实现了一种基于蒙特卡洛的粒子滤波定位方法。
本文测试环境为ubuntu18.04 + ros melodic。

1. ros安装

详见官方网站安装步骤:
http://wiki.ros.org/melodic/Installation/Ubuntu

2. navigation 安装

ros-nav的介绍见:http://wiki.ros.org/navigation
在主目录下新建catkin_ws工程, 在其中的src文件夹下载源码，分支为对应的melodic-devel ：

cd ~
mkdir catkin_ws
cd catkin_ws
mkdir src
cd src
git clone -b melodic-devel https://github.com/ros-planning/navigation.git

初始化catkin工程并编译：

cd ~/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make

3.运行

a. 新开一个终端

roscore

b. 新开第二个终端

cd catkin_ws
source devel/setup.zsh 或者 source devel/setup.zsh （根据自己终端选择）
roslaunch amcl amcl_diff.launch
# 接着会显示:
[ INFO] [1628238691.407115582]: Requesting the map...
[ WARN] [1628238691.408038696]: Request for map failed; trying again...

这说明没有地图，但是基本环境是没有问题的。
c. 下载地图和测试数据
下载地址: http://download.ros.org/data/amcl/

上图中的*.pgm是地图文件，0.05表示分辨率。其他的bag包都是激光雷达的数据文件。
将所有数据下载到: /home/yx/catkin_ws/src/navigation/amcl/test
d. 建立基本定位和全局定位launch文件
文件示例在/amcl/test下，分别为basic_localization_stage.xml和global_localization_stage.xml，将其拷贝到amcl/examples中:

cp test/basic_localization_stage.xml examples/
cp test/global_localization_stage.xml examples/
mv examples/basic_localization_stage.xml  examples/basic_localization_stage.launch
mv examples/global_localization_stage.xml  examples/global_localization_stage.launch

然后在对应的launch文件末尾添加rviz显示的操作:

<node pkg="rviz" name="rviz" type="rviz" args="-d $(find amcl)/rviz/rviz.rviz" />

最后对应的基本定位launch文件为:

<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch><param name="/use_sim_time" value="true"/><node name="rosbag" pkg="rosbag" type="play" args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/> <!--数据包自行修改替换--><node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/><node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen"><remap from="scan" to="base_scan" /><param name="transform_tolerance" value="0.2" /><param name="gui_publish_rate" value="5.0"/><param name="save_pose_rate" value="0.5"/><param name="laser_max_beams" value="100"/><param name="laser_max_range" value="10.0"/><param name="min_particles" value="300"/><param name="max_particles" value="2000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_model_type" value="diff-corrected"/><param name="odom_alpha1" value="0.1"/><param name="odom_alpha2" value="0.1"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.1"/><param name="odom_alpha4" value="0.1"/><param name="odom_alpha5" value="0.0"/><param name="laser_z_hit" value="0.90"/><param name="laser_z_short" value="0.1"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.05"/><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="likelihood_field_prob"/><!-- <param name="laser_model_type" value="beam"/> --><param name="do_beamskip" value="true"/><param name="beam_skip_distance"  value="0.25" /><param name="beam_skip_threshold" value="0.5" />  <!--0.3--><param name="beam_skip_error_threshold_" value="0.8" />  <!--0.9--><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.2"/><param name="update_min_a" value="0.3"/><param name="odom_frame_id" value="odom"/><param name="resample_interval" value="3"/><param name="transform_tolerance" value="2.0"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><param name="initial_pose_x" value="47.443"/><param name="initial_pose_y" value="21.421"/><param name="initial_pose_a" value="-1.003"/><param name="initial_cov_xx" value="0.01"/>   <!--0.01-->   <!--default:0.5 * 0.5--><param name="initial_cov_yy" value="0.01"/>   <!--0.01-->   <!--default:0.5 * 0.5--><param name="initial_cov_aa" value="0.01"/> <!--0.01-->   <!--default:0.0685=(M_PI/12.0) * (M_PI/12.0) --></node><test time-limit="180" test-name="basic_localization_stage" pkg="amcl" type="basic_localization.py" args="0 47.060 21.603 -1.053 0.75 0.75 90.0"/><node pkg="rviz" name="rviz" type="rviz" args="-d $(find amcl)/rviz/rviz.rviz" />
</launch>

全局定位launch文件为:

<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch><param name="/use_sim_time" value="true"/><node name="rosbag" pkg="rosbag" type="play"args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/global_localization_stage_indexed.bag"/><node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/><node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen"><remap from="scan" to="base_scan" /><param name="transform_tolerance" value="0.2" /><param name="gui_publish_rate" value="10.0"/><param name="save_pose_rate" value="0.5"/><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_model_type" value="omni"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><param name="odom_alpha3" value="0.8"/><param name="odom_alpha4" value="0.2"/><param name="odom_alpha5" value="0.1"/><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_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="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><test time-limit="180" test-name="global_localization_stage" pkg="amcl"type="basic_localization.py" args="1 10.00 9.67 6.21 0.25 0.25 28.0" /><node pkg="rviz" name="rviz" type="rviz" args="-d $(find amcl)/rviz/rviz.rviz" />
</launch>

e. 重新编译和运行

cd catkin_ws
catkin_make
source devel/setup.zsh 或者 source devel/setup.bash
roslaunch amcl basic_localization_stage.launch

这时应该会正常出现rviz的画面，点击左下角的Add, 选择By topic, 依次添加Map, LaserScan , Pose, PoseArray(粒子云)。接着，便可以在rviz中看到amcl是怎么实时定位的。

同理也可以运行全局定位。amcl中的basic定位和global定位的区别其实就是一个给了初始位置的先验信息，一个没给初始位置的先验信息, 后者需要全局进行搜索。这些可以在launch文件的参数中体现出来。