1.项目概述
欢迎来到“Where Am I(我在哪里)?”定位项目!在这个项目中，将学习如何利用ROS AMCL包在Gazebo模拟环境的地图中准确地定位移动机器人。
在完成本项目的过程中，涉及机器人软件工程的几个方面，重点是ROS：
创建ROS包，在自定义Gazebo世界中，启动自定义机器人模型
使用ROS AMCL包，远程操作或导航来定位机器人
探索、添加和调优对应于每个包的特定参数，以实现最佳的定位结果
项目任务如下:
环境：本地安装或虚拟机
如果使用本机ROS安装或使用VM，则需要安装以下包中的某些包。按照如下所示安装它们:
$ sudo apt-get install ros-kinetic-navigation
$ sudo apt-get install ros-kinetic-map-server
$ sudo apt-get install ros-kinetic-move-base
$ sudo apt-get install ros-kinetic-amcl2.仿真设置
在项目project1: Go Chase it!中，已经构建了模拟环境和机器人。在本项目中，再次利用该模拟环境和机器人。
设置catkin_ws文件夹以及src文件夹，然后从上一个项目抓取代码（复制上一个项目的模拟环境和机器人）至重新命名的文件包（ 比如：myrobotb）中。即，创建用于启动Gazebo世界和机器人模拟的包。然后运行catkin_make和source devel/setup.bash脚本。启动world.launch以验证是否可以正常运行!
roslaunch myrobotb world.launch3.地图设置
现在已经准备好了模拟环境，但还不能将机器人进行定位，机器人对周围环境一无所知!让我们为它生成一个地图，这样它就知道在这个环境中会发生什么。
一般来说，在机器人项目的开发过程中，工程师们利用测绘工具来测量和绘制机器人将运行的区域。由于我们是在模拟环境中开发，所以问题得到了简化。我们可以直接使用ROS包:pgm_map_creator（GitHub - udacity/pgm_map_creator: Create pgm map from Gazebo world file for ROS localization）从Gazebo世界生成地图。
PGM地图文件
ROS AMCL包使用的地图是一个pgm文件。pgm文件是一种灰度图像文件。有关pgm文件或更通用的pnm文件的更多信息，请参阅Netpbm格式Wiki页面（en.wikipedia.org）。
默认情况下，AMCL包把“深色”像素作为pgm地图文件中的障碍物，而“浅色”像素作为自由空间。阈值可以设置为一个参数，我们将在构建launch启动文件时讨论这个参数。
导航到ROS包文件夹并创建一个maps文件夹，用于存放地图文件。
$ cd /home/workspace/catkin_ws/src/myrobotb
$ mkdir mapsPGM地图创建器
安装依赖关系
我们需要libignion -math2-dev和protobuf-compiler来编译地图创建器:
sudo apt-get install libignition-math2-dev protobuf-compiler克隆存储库
将包pgm_map_creator克隆到src文件夹。
cd /home/workspace/catkin_ws/src/
git clone https://github.com/udacity/pgm_map_creator.git构建包:
cd ..
catkin_make添加和编辑World文件
复制Project1中创建的Gazebo world(比如：Robotroom1)到world文件夹
cp Robotroom1 src/pgm_map_creator/world/Robotroom1插入地图创建器插件到地图文件。打开地图文件（Robotroom1），在文件的末尾添加以下标签，在</world>标签之前:
<plugin filename="libcollision_map_creator.so" name="collision_map_creator"/>创建PGM地图!
打开一个终端，运行gzerver和地图文件:
gzserver src/pgm_map_creator/world/Robotroom1打开另一个终端，启动request_publisher节点
roslaunch pgm_map_creator request_publisher.launch等待插件生成的地图，它将位于pgm_map_creator的地图文件夹!打开并快速查看一下地图。如果地图是裁剪的，则需要调整launch/request_publisher. launch中的参数，即x和y值，它定义了地图的尺寸:
<arg name="xmin" default="-15" />
<arg name="xmax" default="15" />
<arg name="ymin" default="-15" />
<arg name="ymax" default="15" />
<arg name="scan_height" default="5" />
<arg name="resolution" default="0.01" />编辑地图
地图是一个图像，可以使用图像处理软件(如Linux中的gimp)编辑它。如果由于模型的原因地图不准确，可直接编辑pgm文件!
将地图添加到包中
现在有了地图文件（比如：map1227），把它移动到创建的maps文件夹。
cd /home/workspace/catkin_ws/
cp src/pgm_map_creator/maps/map1227 src/Robotroom1/maps/map1227还需要一个yaml文件来提供关于地图的元数据，创建一个yaml文件:
cd src/Robotroom1/src/maps
touch map1227.yaml打开yaml文件并添加以下代码行:
image: map1227
resolution: 0.01
origin: [-15.0, -15.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0注意，地图的原点应该与地图的大小相对应。例如，默认的地图大小是30 * 30，因此原点将是[-15，-15，0]，即地图大小的一半。
AMCL包
当机器人在地图上导航时，自适应蒙特卡罗定位(AMCL)在一段时间内动态调整粒子的数量。这种自适应过程比MCL提供了显著的计算优势。
ROS AMCL包(http://wiki.ros.org/amcl)实现了此变体，将此包与机器人集成，以将其定位在提供的地图中。
ROS利用启动文件来运行ROS节点。在下面开始为AMCL包构建启动文件!
4. AMCL launch 文件
首先，在包的launch文件夹中为AMCL节点创建一个launch文件:
$ cd /home/workspace/catkin_ws/src/myrobotb/launch/
$ vi amcl.launch # nano, gedit可随意使用编辑器:vi、nano或gedit。
在amcl.launch文件中添加根元素<launch>标签:
<launch>
<!-- TODO: Add nodes here -->
</launch>现在，向这个amcl.launch文件中添加节点,下面提供了一个节点列表，作为进度跟踪。现在不需要全部完成------当添加了所有三个节点时再回来比对一下!
下面是AMCL包运行所需的节点列表:
5.AMCL Launch 文件：Map Server Node_files
Map Server Node地图服务器节点
第一个节点是map_server节点(map_server) - ROS Wiki)。map_server节点将地图数据作为ROS服务提供给其他节点(如amcl节点)。在这里，map_server节点将定位在map Setup步骤中创建的地图，并将其作为地图数据发送出去。
首先，在地图文件中添加一个路径参数，这样就可以很容易地改变加载的地图，避免再次输入长路径:
<arg name="map_file" default="$(find <YOUR PACKAGE NAME>)/maps/<YOUR MAP NAME>.yaml"/>然后，添加map_server节点，使用刚才创建的参数:
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />amcl.launch文件应该是这样的:
<launch>
<!-- Map Server -->
<arg name="map_file" ... />
<node name="map_server" ... />
</launch>示例如下：
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
</launch>设置了map_server节点后，再转到amcl节点。
AMCL Launch 文件：AMCL节点
amcl节点需要里程计和激光扫描数据来执行AMCL定位。
添加amcl节点
首先，在amcl.launch文件中添加amcl节点:
<launch>
<!-- Map Server -->
...
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
</node>
</launch>现在创建了amcl节点，还需要更多的信息来定位机器人!例如，激光雷达的传感器读数等。
Remap（重映射）scan主题
默认情况下，amcl包将为LiDAR数据查找扫描主题。在仿真中，Hokuyo LiDAR传感器实际上发布在<YOUR PACKAGE NAME>/laser/scan主题上。我们将使用remap标签将主题名称扫描重新映射到实际的主题名称，以便amcl包可以使用它!
将此remap重映射行添加到amcl.launch文件中的amcl节点:
...
<!-- AMCL Node -->
<node name="amcl" ...>
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>
</node>有关remap的更多信息，请查看ROS Wiki: http://wiki.ros.org/roslaunch/XML/remap
添加AMCL参数
AMCL节点还需要一组参数来连接world(地图帧)和机器人(odom帧)。
将以下参数标签添加到amcl.launch文件中的amcl节点:
...
<!-- AMCL Node -->
<node name="amcl" ...>
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
</node>从ROS Wiki (http://wiki.ros.org/amcl)中，可以找到上面添加的参数的用途:
odom_frame_id (string, default: "odom"): 用于里程计的帧odom_model_type (string, default: "diff" ): 使用哪个模型，"diff"， "omni"， "diff-corrected"或"omni-corrected"global_frame_id (string, default: "map"): 定位系统发布的坐标帧名称记住:AMCL包将机器人(odom帧)与world(地图帧)“链接”，这些参数是amcl包在world内实现机器人定位所必需的。
可选:设置“初始位置”
可以使用RViz 2D Pose Estimate函数为AMCL提供一个姿态估计作为位置，也可以在launch启动文件中定义它。
将以下参数添加到AMCL节点，这些值应该与world.launch文件相对应:
<param name="initial_pose_x" value="<YOUR X VALUE>"/>
<param name="initial_pose_y" value="<YOUR Y VALUE>"/>启动文件
现在，amcl.launch启动文件应该是这样的:
<launch>
<!-- Map Server -->
<arg name="map_file" ... />
<node name="map_server" ... />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<!-- If you choose to define initial pose here -->
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
</node>
</launch>示例如下：
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<remap from="scan" to="my_robotb/laser/scan"/>
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<!-- If you choose to define initial pose here -->
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
</node>
</launch>现在，机器人有一个地图和amcl包来进行定位。但它如何导航到其他位置来收集更多关于周围环境的信息呢?答案是ROS Navigation Stack（导航堆栈）。
6.AMCL Launch 文件：Move Base Node移动基础节点
使用move_base包，可以在地图中为机器人定义导航目标位置，机器人将导航到该目标位置。注意，如果选择使用teleop node来控制和定位机器人，则此步骤是可选的。
move_base包是一个非常强大的工具。它利用了costmap——地图的每个部分都被划分为被占用的区域，如墙壁或障碍物，以及未占用的区域。当机器人四处移动时，与全局costmap相关的局部costmap会不断更新，允许包定义机器人移动的连续路径。
让这个包更引人注目的是，它有一些内置的纠正行为或操作。根据特定的情况，比如检测到一个特定的障碍，或者机器人被卡住了，它会让机器人绕过障碍，或者旋转机器人，直到找到前方的清晰路径。
添加ROS move_base节点
让我们将move_base节点添加到amcl.launch文件:
<launch>
<!-- Map Server -->
...
<!-- AMCL Node -->
...
<!-- Move Base -->
<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen">
</node>
</launch>重映射 scan 主题
同样，需要将scan主题remap到正确的主题。
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>为move_base节点添加参数
与amcl节点类似，move_base节点也有一组参数可调节，可以使用param标签来指定参数。但是当有大量参数时，可以使用rosparam标签来包含配置文件，以直接设置多个参数!
规划
将以下参数标签添加到move_base节点:
<param name="base_global_planner" value="navfn/NavfnROS" />
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>加载配置文件（Config Files ）
然后需要创建/获取配置文件，并将它们加载到 launch启动文件中:
$ cd ..
$ mkdir config
$ cd config从Udacity S3下载配置文件，并将其添加到config文件夹:
wget https://s3-us-west-1.amazonaws.com/udacity-robotics/Resource/where_am_i/config.zip
unzip config.zip
rm config.zip如果您选择在测试中向机器人发送导航目标，这些配置文件具有为导航机器人定义的预设参数。
现在已经准备好了配置文件，我们将把它们包含在launch启动文件，在move_base节点中:
<rosparam file="$(find udacity_bot)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find udacity_bot)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find udacity_bot)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find udacity_bot)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find udacity_bot)/config/base_local_planner_params.yaml" command="load" />完成以上步骤后，amcl.launch文件示例如下所示：
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
<param name="initial_pose_a" value="-1.57"/>
</node>
<!-- Move Base -->
<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen">
<remap from="scan" to="my_robotb/laser/scan"/>
<param name="base_global_planner" value="navfn/NavfnROS" />
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find my_robotb)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>
8.可选: Teleop Package
如果希望像在实验室中那样控制机器人来帮助它定位自己，则需要将teleop节点添加到包中。多亏了ROS社区，我们可以使用ROS -teleop包通过键盘或控制器向机器人发送命令。
将ros-teleop包克隆到src文件夹:
cd /home/workspace/catkin_ws/src
git clone https://github.com/ros-teleop/teleop_twist_keyboard构建包并设置来源脚本:
cd ..
catkin_make
source devel/setup.bash现在可以像README文件中描述的那样运行teleop脚本:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py9.定位:启动Launching
机器人，地图，定位和导航节点全部齐备，启动并测试！首先，启动模拟:
$ cd /home/workspace/catkin_ws/
$ roslaunch <YOUR PACKAGE NAME> <YOUR WORLD>.launch即：
$ cd /home/workspace/catkin_ws/
$ roslaunch myrobotb Robotroom1.launch在另一个终端中，启动amcl launch文件:
$ roslaunch <YOUR PACKAGE NAME> amcl.launch即：
$ roslaunch myrobotb amcl.launchRviz配置
正如在前一节中所做的那样，通过添加必要的显示并选择所需的主题来可视化机器人和地图，来设置RViz。
按显示类型添加
在Rviz,
topic/map: 列表中的第二个和第三个主题将显示全局costmap和本地costmap，两者都有助于优化参数PoseArray和选择topic/particlecloud: 在机器人周围显示一组箭头每个箭头本质上都是一个粒子，定义了定位包创建的机器人的姿态。您的目标是添加/调优参数，以帮助更好地定位机器人，从而改进姿态数组。
注意:可以将上面的RViz设置保存在配置文件中，并每次使用相同的配置启动RViz。这将使过程更有效率!点击file -> save config保存当前配置。
按主题添加
在RViz中添加元素时，还可以选择按主题选项卡。在这里，所有有效的主题都将显示出来，可以更快地找到所需要的!
Transform Timeout和Map Update Loop如果收到关于Transform Timeout（转换超时）和Map Update Loop（地图更新循环）的警告，则可能需要配置相应的参数。即AMCL节点的transform_tolerance值变大，配置文件中的update_frequency和publish_frequency值变小。
10.定位:参数
下面为amcl.launch文件中的amcl节点确定和调优参数，以达到更好的效果。
AMCL参数
amcl包（amcl - ROS Wiki）有很多参数可供选择。不同的参数集对算法的不同方面有贡献。广义上讲，它们可以分为三类——整体滤波、激光和里程计，开始时需要考虑的参数或关注的细节。
整体过滤
min_particles and max_particles - 当amcl在每次迭代时动态调整它的粒子时，它期望一个范围内的粒子数量作为输入。通常，这个范围是根据系统规范进行调优的。对于低端系统来说，较大的范围和较高的最大值可能会导致计算量过大。initial_pose - 对于项目，应该将位置设置为[0,0]，可随意摆弄平均偏航值。update_min* - amcl依赖于传入的激光扫描。在收到扫描后，它检查update_min_a和update_min_d的值，并与机器人移动的距离进行比较。基于这种比较，它决定是更新过滤器还是丢弃扫描数据。丢弃数据可能会导致较差的定位结果，对于快速移动的机器人来说，过于频繁的过滤器更新也可能导致计算问题。激光
有两种不同类型的模型来考虑——likelihood_field和光束。每一个模型都定义了激光测距传感器如何估计与机器人相关的障碍物。
对于正在使用的这种环境，likelihood - field模型通常在计算上更高效、更可靠。因此，可以专注于特定模型的参数，如-
laser_*_rangelaser_max_beamslaser_z_hit和laser_z_rand这些参数的调整必须是实验性的。在调整它们的同时，观察RViz中的激光扫描信息，并尝试确保激光扫描与实际地图匹配或对齐，以及随着机器人的移动它是如何更新的。对障碍物位置的估计越好，定位结果就越好。
里程计
odom_model_type—因为正在使用差动驱动移动机器人，所以最好使用差动校正类型。还有一些特定于这种类型的附加参数——odom_alphas(1到4)。这些参数定义了机器人在地图中导航时，它的移动/运动预计会产生多少噪声。
注:本项目里程计信息直接从Gazebo接收，与地面真值等效(无噪声)。因此，不需要调优这些参数，保持默认值。但是请随意试验一些值，看看是否注意到任何变化。
重要的是:上面的一组参数应该可以帮助开始，但不是唯一可以改善结果的参数。可阅读相关文档，确定哪些参数可以帮助改进定位结果，并使用它们进行实验。其余参数和相应的文档都可以在ROS wiki的amcl页面上（amcl - ROS Wiki）找到。识别和调优所有这些参数需要花费时间和精力，根据所提供的信息和资源，有助于正面解决问题!
完成以上步骤后，amcl.launch文件示例如下所示：
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
<param name="initial_pose_a" value="-1.57"/>
<!-- Tune parameters-->
<param name="transform_tolerance" value="0.2"/>
<param name="min_particles" value="100"/>
<param name="max_particles" value="3000"/>
<param name="update_min_a" value="pi/6.0"/>
<param name="update_min_d" value="0.1"/>
<param name="laser_min_range" value="0.1"/>
<param name="laser_max_range" value="10"/>
<param name="laser_max_beams" value="30"/>
<param name="laser_z_hit" value="0.95"/>
<param name="laser_z_rand" value="0.05"/>
<param name="odom_alpha1" value="0.02"/>
<param name="odom_alpha2" value="0.02"/>
<param name="odom_alpha3" value="0.02"/>
<param name="odom_alpha4" value="0.02"/>
<param name="odom_alpha5" value="0.02"/>
</node>
<!-- Move Base -->
<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen">
<remap from="scan" to="my_robotb/laser/scan"/>
<param name="base_global_planner" value="navfn/NavfnROS" />
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find my_robotb)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>
11.定位:测试
现在，测试一下AMCL包的性能!定位时有两个选项控制机器人:
通过RViz发送navigation goal（导航目标）
teleop包发送移动命令导航机器人，观察它的性能并为AMCL调整参数!
选项1:发送2D导航目标
第一个选择是从RViz发送一个2D导航目标。move_base将尝试基于定位来导航机器人。在新观测和测程的基础上，机器人进一步进行定位。
点击工具栏中的2D Nav Goal按钮，然后在地图上点击并拖动，将目标发送给机器人。它将开始移动并进行定位过程。如果你想给amcl节点轻推动作，请使用2D Pose Estimate在地图上给机器人一个初始位置估计。
选项2:使用teleop Node
如果在Optional: Teleop Package(可选的teleop Package部分)中设置了机器人，还可以使用teleop节点来控制机器人并观察它在环境中的自身定位。
打开另一个终端并启动teleop脚本:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
现在你可以通过键盘命令来控制你的机器人。
