【SLAM建图和导航仿真实例】（二）- 根据已知地图进行定位和导航
引言
在这个-SLAM建图和导航仿真实例-项目中，主要分为三个部分，分别是
- （一）模型构建
- （二）根据已知地图进行定位和导航
- （三）使用RTAB-MAP进行建图和导航
该项目的slam_bot已经上传我的Github。
这是第二部分。
二、 根据已知地图进行定位和导航
1.添加地图
在slam_bot目录中创建一个新文件夹。
$ cd ~/catkin_ws/src/slam_bot/
$ mkdir maps
$ cd maps
从Github中将jackal_race.pgm和jackal_race.yaml复制到“ maps”文件夹中。
$ cd ..
$ cd worlds
将文件jackal_race.world从Github复制到“ worlds”文件夹中。
接下来，修改slam.launch文件并更新此新地图/世界的路径。
$ cd ..
$ cd launch/
$ nano slam_bot.launch
修改参数world_name，使其指向jackal_race.world。现在可以在新地图中启动了。
$ cd ~/catkin_ws/
$ roslaunch slam_bot/slam.launch
2.导航堆栈
导航堆栈(Navigation Stack)在概念层面上相当简单。它从里程表(odometry )和传感器流中获取信息，并输出速度指令发送到移动基地。然而，在机器人上使用导航堆栈则要复杂一些。作为导航堆栈使用的前提条件，机器人必须运行ROS，有一个TF转换树，并使用正确的ROS消息类型发布传感器数据。同时，导航堆栈需要对机器人的形状和动态进行配置，以使机器人的性能达到较高的水平。
导航堆栈需求:
虽然导航堆栈旨在尽可能地通用，但存在三个主要的硬件要求限制了它的使用：
- 它仅适用于差速驱动和完整轮式机器人。假定通过发送所需的速度命令来控制移动基座，以达到以下形式：x速度，y速度，theta速度。
- 它需要将平面激光器安装在活动基座上的某个位置。该激光器用于地图构建和定位。
- 导航堆栈是在方形机器人上开发的，因此其性能在接近正方形或圆形的机器人上将是最好的。它可以在任意形状和大小的机器人上运行，但是在狭窄的空间（如门口）中使用大型矩形机器人可能会遇到困难。
更多可以查看ros wiki navigation。
move_base包
使用move_base程序包，通过该程序包我们可以在地图中为机器人定义目标位置，然后机器人将导航至该目标位置。
move_base软件包是一个非常强大的工具。它利用成本图（costmap）将地图的每个部分划分为占用的区域（如墙壁或障碍物）和未占用的区域。当机器人四处移动时，相对于全局成本图的局部成本图会不断更新，从而使程序包可以为机器人定义连续的移动路径。
move_base程序包具有一些内置的纠正行为或操作。根据特定条件，例如检测到特定障碍物或机器人是否被卡住，它将围绕障碍物导航机器人或旋转机器人，直到找到合适的路径。
3.AMCL包
在之前的博客中介绍了蒙特卡洛定位算法（MCL）。当机器人在地图中四处导航时，自适应蒙特卡洛定位（AMCL）可在一段时间内动态调整粒子数量。与MCL相比，这种自适应过程具有明显的计算优势。
ROS amcl软件包实现了此变体，我们将使用此软件包与机器人集成在一起以在提供的地图中对其进行定位。
创建一个新的启动文件。
$ cd ~/catkin_ws/src/slam_bot/launch/
$ nano amcl.launch
该启动文件具有三个节点，其中一个用于amcl包。
map_server包
使用map_server包的节点加载提供的地图。robot_state_publisher根据URDF文件构建了机器人的整个tf树。但是它并没有通过链接“map”框架来扩展tf树。amcl软件包通过链接“ map”和“ odom”框架自动完成此操作。
amcl包
添加一个将启动amcl包的节点。该软件包具有自己的一组参数，这些参数定义了它在RViz中的行为以及一切与机器人和所提供地图的关系，以便机器人可以有效地对其自身进行定位。amcl软件包完全依赖于机器人的odom和激光扫描数据。
move_base包
move_base具有自己的一组必需参数，以帮助其有效执行。可以指定remap特定的主题，以使他们能够从里程计或激光数据中获取输入，还可以定义一些参数文件或配置文件，这些文件涉及与成本图有关的参数或定义，以及用于创建路径并沿该路径导航机器人的本地计划器。
首先添加以下配置文件。
在Github中复制以下文件，并将其添加到config文件夹：
- local_costmap_params.yaml
- global_costmap_params.yaml
- costmap_common_params.yaml
- base_local_planner_params.yaml
costmap_common_params.yaml配置文件
map_type: costmap
obstacle_range: 5
raytrace_range: 20.0
transform_tolerance: 1.0
inflation_radius: 1.5
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: /hokuyo, data_type: LaserScan, topic: /slam_bot/laser/scan, marking: true, clearing: true}
这些参数对于定义你的成本图如何更新障碍物信息以及你的机器人在导航时如何响应障碍物信息是非常重要的。
obstacle_range - 例如，如果设置为0.1，这意味着如果激光传感器检测到的障碍物距离机器人的底部0.1米以内，该障碍物将被添加到成本图中。调整这个参数可以帮助丢弃噪声、错误检测到障碍物，甚至是计算成本。
raytrace_range - 这个参数用于在机器人移动时清除和更新成本图中的自由空间。
inflation_radius - 这个参数决定了机器人几何体和障碍物之间的最小距离。试着为这个参数设置一个非常高的值，然后启动项目并选择全局成本图选定。你会发现障碍物似乎被 "膨胀 "了，如下图所示。这个参数的适当值可以保证机器人顺利地在地图上导航，不会撞到墙面上，不会被卡住，甚至可以通过任何狭窄的通道。
4.运行amcl.launch
首先运行我们的模型
$ cd ~/catkin_ws/
$ roslaunch slam_bot slam.launch
在新的终端中
$ roslaunch slam_bot amcl.launch
报错：
ERROR: cannot launch node of type [map_server/map_server]: map_server
ROS path [0]=/opt/ros/kinetic/share/ros
ROS path [1]=/home/stan/catkin_ws/src
ROS path [2]=/opt/ros/kinetic/share
ERROR: cannot launch node of type [amcl/amcl]: amcl
ROS path [0]=/opt/ros/kinetic/share/ros
ROS path [1]=/home/stan/catkin_ws/src
ROS path [2]=/opt/ros/kinetic/share
ERROR: cannot launch node of type [move_base/move_base]: move_base
ROS path [0]=/opt/ros/kinetic/share/ros
ROS path [1]=/home/stan/catkin_ws/src
ROS path [2]=/opt/ros/kinetic/share
解决办法
$ sudo apt-get install ros-kinetic-map-server
$ sudo apt-get install ros-kinetic-amcl
$ sudo apt-get install ros-kinetic-move-base
在RViz中
- 选择“ odom”作为固定框
- 点击“Add”按钮，然后
- 添加“ RobotModel”
- 添加“Map”并选择第一个topic/map
- 列表中的第二个和第三个主题将显示global costmap和local costmap
- 添加“ PoseArray”并选择主题/ particlecloud
- 这将在机器人周围显示一组箭头
可以在RViz工具栏中，选择“ 2D Nav Goal”（2D导航目标），在地图上的其他任何地方单击，然后从那里拖动以定义目标位置以及机器人在目标位置处的方向。
5. 测试
在Github中提供了一个C++节点，该节点可以将机器人导航到目标位置。
需要为此创建一个新文件夹。
$ cd ~/catkin_ws/src/slam_bot
$ mkdir src
$ cd src
将navigation_goal.cpp文件复制到此文件夹。
为了使用或启动此节点，首先需要对其进行编译,所以需要为此修改CMakeLists.txt文件。
$ cd ~/catkin_ws/src/slam_bot
nano CMakeLists.txt
将以下内容添加到CMakeLists.txt中
find_package(catkin REQUIRED COMPONENTS
message_generation
geometry_msgs
std_msgs
actionlib
move_base_msgs
roscpp
)
foreach(dir config launch meshes urdf)
install(DIRECTORY ${dir}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(navigation_goal src/navigation_goal.cpp)
target_link_libraries(navigation_goal ${catkin_LIBRARIES})
修改完成后，对slam_bot进行编译
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
在slam_bot.launch和amcl.launch运行的情况下，打开一个新的terminal
$ cd ~/catkin_ws
$ rosrun slam_bot navigation_goal
上面的代码将运行该节点，机器人会移动至目标位置。
[ INFO] [1590585650.579740467, 1608.259000000]: Waiting for the move_base action server
[ INFO] [1590585650.827041070, 1608.334000000]: Connected to move_base server
[ INFO] [1590585650.827097987, 1608.334000000]: Sending goal
[ INFO] [1590585964.490917946, 1727.796000000]: Excellent! Your robot has reached the goal position.