准备工作：请先安装相关的ROS功能包
安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping
安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server
安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation
新建功能包，并导入依赖: gmapping map_server amcl move_base
2.1 导航实现-SLAM建图
（1）gmapping简介
gmapping 是 ROS 开源社区中常用且较为成熟的 SLAM 算法之一。它能够利用移动机器人提供的里程计数据和激光雷达数据，构建出精确的二维栅格地图。因此，**gmapping**对于机器人硬件配置有一定要求：
①移动机器人必须能够发布里程计消息 ，通过跟踪机器人底盘的移动信息（如位移和角度），为 **gmapping**提供机器人的运动数据，以便正确推算机器人的位姿。
②机器人需要能够发布激光雷达消息 ，通常通过水平固定安装的激光雷达采集环境信息，帮助 gmapping 构建周围的栅格地图。
③除了使用激光雷达，也可以通过深度相机 的深度数据转换为类似激光雷达的消息，以替代激光雷达发布的雷达数据。这种方式可以扩展不同传感器的使用，为 gmapping 提供灵活的数据来源。
（2）gmapping节点说明
gmapping 功能包中的核心节点是：slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。
1）订阅的Topic
tf (tf/tfMessage)
用于雷达、底盘与里程计之间的坐标变换消息。
scan(sensor_msgs/LaserScan)
SLAM所需的雷达信息。
2）发布的Topic
map_metadata(nav_msgs/MapMetaData)
地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。
map(nav_msgs/OccupancyGrid)
地图栅格数据，一般会在rviz中以图形化的方式显示。
~entropy(std_msgs/Float64)
机器人姿态分布熵估计(值越大，不确定性越大)。
3）服务
dynamic_map(nav_msgs/GetMap)
用于获取地图数据。
4）参数
base_frame(string, default:"base_link")
机器人基坐标系。
map_frame(string, default:"map")
地图坐标系。
odom_frame(string, default:"odom")
里程计坐标系。
map_update_interval(float, default: 5.0)
地图更新频率，根据指定的值设计更新间隔。
maxUrange(float, default: 80.0)
激光探测的最大可用范围(超出此阈值，被截断)。
maxRange(float)
激光探测的最大范围。
....
5）所需的坐标变换
雷达坐标系→基坐标系
一般由 robot_state_publisher 或 static_transform_publisher 发布。
基坐标系→里程计坐标系
一般由里程计节点发布。
6）发布的坐标变换
地图坐标系→里程计坐标系
地图到里程计坐标系之间的变换。
（3）gmapping使用
launch文件编写可以参考 github 的演示 launch文件：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch
2.2 导航实现-地图服务
（1）map_server简介
map_server功能包中提供了两个节点： map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。
（2）map_server使用之地图保存节点(map_saver)
1）map_saver节点说明
订阅的topic：map(nav_msgs/OccupancyGrid)
订阅此话题用于生成地图文件。
2）地图保存launch文件
地图保存的语法比较简单，编写一个launch文件，内容如下:
<launch>
<arg name="filename" value="$(find mycar_nav)/map/nav" />
<node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>
其中filename是指地图的保存路径以及保存的文件名称。
map_server 中障碍物计算规则：
地图中的每一个像素取值在**[0,255]**之间，白色为 255，黑色为 0，该值设为 x；
map_server 会将像素值作为判断是否是障碍物的依据，首先计算比例: p = (255 - x) / 255.0，白色为0，黑色为1(negate为true，则p = x / 255.0)；
根据步骤2计算的比例判断是否是障碍物，如果 p > occupied_thresh 那么视为障碍物，如果 p < free_thresh 那么视为无物。
output 99 * (p - free_thresh) / (occupied_thresh - free_thresh)
（3）map_server使用之地图服务(map_server)
1）map_server节点说明
发布的话题
map_metadata（nav_msgs / MapMetaData）
发布地图元数据。
map（nav_msgs / OccupancyGrid）
地图数据。
服务
static_map（nav_msgs / GetMap）
通过此服务获取地图。
参数
frame_id（字符串，默认值："map"）
地图坐标系。
2）地图读取
通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件如下:
<launch>
<!-- 设置地图的配置文件 -->
<arg name="map" default="nav.yaml" />
<!-- 运行地图服务器，并且加载设置的地图-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
</launch>
其中参数是地图描述文件的资源路径，执行该launch文件，该节点会发布话题：map(nav_msgs/OccupancyGrid)。
2.3 导航实现-定位
所谓定位，是指推算机器人在全局地图中的准确位置。虽然SLAM（同步定位与建图）算法中也包含了定位功能，但SLAM的定位主要用于构建全局地图，通常是在导航开始之前的地图构建阶段完成的。而当前讨论的定位，是在导航过程中用于机器人实时定位，以确保机器人能够沿着设定的路径运动。通过定位，系统能够判断机器人的实际运动轨迹是否符合预期。
在ROS的导航功能包集中，navigation 包提供了一个专门用于实现机器人导航定位的功能包，即**amcl** （自适应蒙特卡洛定位）。**amcl**通过粒子滤波的方式，根据机器人的传感器数据（如激光雷达或里程计）实时推算出机器人在已知地图中的位置，并与设定的目标路径进行对比，确保机器人准确跟随全局规划路径，从而实现精确的导航定位。
（1）amcl简介
AMCL（自适应蒙特卡洛定位，Adaptive Monte Carlo Localization）是用于2D移动机器人的一种概率定位系统，基于粒子滤波器推算机器人在已知地图中的位置。AMCL采用自适应蒙特卡洛方法（或称为KLD采样），通过动态调整粒子数量来提高计算效率，确保机器人在不同环境复杂度下都能准确定位。
AMCL 通过结合机器人传感器数据（如激光雷达、里程计等）与地图信息，实时推算出机器人在全局地图中的位置，并根据传感器反馈调整位置估计的精度。这一方法能够适应不同环境变化和传感器噪声，从而实现更加稳健的定位效果。
AMCL 功能包已经集成在ROS的**navigation**包中，能够与其他导航组件（如全局路径规划器、局部路径规划器等）无缝协作，确保机器人在导航过程中始终保持精确的定位能力。这使得AMCL成为ROS导航系统中实现自主定位和移动的重要模块。
（2）amcl节点说明
roscd amcl
ls examples
1）坐标变换
里程计（Odometry） 在机器人定位中也能起到一定的辅助作用，能够根据轮子转动的角度和速度推算机器人在局部坐标系中的位置和姿态。然而，里程计的定位方法存在一个主要问题：累计误差 。随着时间的推移，误差会不断累积，尤其在一些特殊情况下，如车轮打滑或地面不平时，里程计的定位可能会出现显著的偏差。
为了解决这一问题，AMCL（自适应蒙特卡洛定位）可以通过粒子滤波的方式，结合地图信息，实时估算机器人在全局坐标系下的姿态。AMCL在进行定位时，会综合考虑传感器数据（如激光雷达）和地图匹配结果，有效减小里程计带来的累计误差。此外，AMCL还能与里程计数据相结合，通过融合两者的优点，提升整体定位的精度与稳定性。
里程计定位：只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
amcl定位：可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。
（3）amcl使用
关于launch文件的实现，在amcl功能包下的example目录已经给出了示例，可以作为参考，具体实现：
1）订阅的Topic
scan(sensor_msgs/LaserScan)
激光雷达数据。
tf(tf/tfMessage)
坐标变换消息。
initialpose(geometry_msgs/PoseWithCovarianceStamped)
用来初始化粒子滤波器的均值和协方差。
map(nav_msgs/OccupancyGrid)
获取地图数据。
2）发布的Topic
amcl_pose(geometry_msgs/PoseWithCovarianceStamped)
机器人在地图中的位姿估计。
particlecloud(geometry_msgs/PoseArray)
位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。
tf(tf/tfMessage)
发布从 odom 到 map 的转换。
3）服务
global_localization(std_srvs/Empty)
初始化全局定位的服务。
request_nomotion_update(std_srvs/Empty)
手动执行更新和发布更新的粒子的服务。
set_map(nav_msgs/SetMap)
手动设置新地图和姿态的服务。
4）调用的服务
static_map(nav_msgs/GetMap)
调用此服务获取地图数据。
5）参数
odom_model_type(string, default:"diff")
里程计模型选择: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向轮)
odom_frame_id(string, default:"odom")
里程计坐标系。
base_frame_id(string, default:"base_link")
机器人极坐标系。
global_frame_id(string, default:"map")
地图坐标系。
该目录下会列出两个文件: amcl_diff.launch 和 amcl_omni.launch 文件，前者适用于差分移动机器人，后者适用于全向移动机器人，可以按需选择。
2.4 导航实现-路径规划
ROS的导航功能包集navigation中提供了move_base功能包，用于实现此功能。
（1）move_base简介
move_base 功能包提供了一种基于动作（action）的路径规划和控制实现，它能够根据给定的目标点，自动规划路径并控制机器人底盘运动到目标位置。在机器人运动的过程中，move_base 会持续反馈机器人当前的姿态、位置以及与目标点的状态信息。这种反馈机制确保机器人能够在动态环境中做出及时的调整和避障。
（2）move_base节点说明
1）动作
动作订阅
move_base/goal(move_base_msgs/MoveBaseActionGoal)
move_base 的运动规划目标。
move_base/cancel(actionlib_msgs/GoalID)
取消目标。
动作发布
move_base/feedback(move_base_msgs/MoveBaseActionFeedback)
连续反馈的信息，包含机器人底盘坐标。
move_base/status(actionlib_msgs/GoalStatusArray)
发送到move_base的目标状态信息。
move_base/result(move_base_msgs/MoveBaseActionResult)
操作结果(此处为空)。
2）订阅的Topic
move_base_simple/goal(geometry_msgs/PoseStamped)
运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。
3）发布的Topic
cmd_vel(geometry_msgs/Twist)
输出到机器人底盘的运动控制消息。
4）服务
make_plan(nav_msgs/GetPlan)
请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。
clear_unknown_space(std_srvs/Empty)
允许用户直接清除机器人周围的未知空间。
clear_costmaps(std_srvs/Empty)
允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。
5）参数
参考：http://wiki.ros.org/move_base
（3）move_base与代价地图
1）概念
机器人导航，特别是路径规划模块，依赖于地图信息。之前在SLAM中已经介绍了地图的生成过程。在ROS中，地图实际上是一张图片，其中包含宽度、高度、分辨率等元数据信息。地图通过灰度值来表示不同区域的状态，例如空闲区域、障碍物区域等，灰度值反映了障碍物存在的概率。
动态障碍物的变化 ：SLAM生成的静态地图反映的是机器人构建地图时的环境状态，而在导航过程中，障碍物信息可能会随时发生变化。例如，某些障碍物可能被移走，或者新的障碍物被添加。如果依赖于静态地图，机器人无法及时感知这些变化。因此，导航过程中需要实时获取障碍物信息，以动态更新环境感知，确保安全的路径规划。
障碍物边缘的安全问题 ：在静态地图中，某些靠近障碍物边缘的区域可能被标记为空闲区域，但机器人进入这些区域时可能会面临风险。例如，机器人由于惯性、形状不规则或在转弯时，可能会与障碍物发生碰撞。为了提高安全性，通常在地图的障碍物边缘设置警戒区 ，也称为膨胀区域，这个区域可以通过膨胀算法扩展障碍物范围，确保机器人尽量远离障碍物，防止碰撞。
因此，SLAM生成的静态地图在导航中需要进行扩展和处理，才能满足实际应用需求。在静态地图的基础上，需要添加一些辅助信息，例如通过传感器实时获取的动态障碍物数据，以及基于静态地图生成的膨胀区域等。这种处理可以使导航系统在复杂和变化的环境中更加鲁棒和安全。
2）组成
在ROS的导航系统中，代价地图有两种类型：global_costmap（全局代价地图） 和 local_costmap（本地代价地图）。全局代价地图用于全局路径规划，帮助机器人在整个环境中找到从当前位置到目标点的最优路径；而本地代价地图用于本地路径规划，实时处理机器人周围的障碍物信息，确保机器人能够在动态环境中避障并沿着全局路径前进。
代价地图由多个层（layer）叠加构成，每个层级负责处理不同的信息。通常情况下，代价地图包含以下几层：
Static Map Layer（静态地图层）：这一层是SLAM构建的静态地图，代表环境的基本结构，如房间的墙壁和固定障碍物。它为全局路径规划提供了基础的地图信息。
Obstacle Map Layer（障碍物地图层）：这一层由机器人传感器（如激光雷达、深度相机等）获取的障碍物信息实时更新，用于检测并标记机器人周围动态或新的障碍物。它在本地路径规划中尤为重要，帮助机器人避开动态障碍。
Inflation Layer（膨胀层）：这一层基于静态地图层和障碍物地图层生成，主要作用是通过"膨胀"障碍物的区域来增加安全距离。该层将障碍物向外扩展，确保机器人不会因为惯性或外壳尺寸与障碍物发生碰撞。膨胀层在路径规划中提供了额外的安全边界。
Other Layers（其他层）：用户可以根据实际需求自定义其他层，添加特定的代价地图信息，例如指定禁行区域、优先通行区域等，进一步增强导航系统的灵活性和定制化能力。
多个代价地图层可以按需自由搭配使用，以满足不同的导航场景。各层通过叠加形成完整的代价地图，使机器人能够综合考虑静态结构、动态障碍物以及安全距离，确保路径规划的有效性和安全性。
3）碰撞算法
在代价地图中，栅格的灰度值用于表示机器人周围区域的安全程度，上图中的横轴表示距离机器人中心的距离，纵轴表示栅格在代价地图中的灰度值。不同的灰度值代表了机器人与障碍物之间的相对位置及可能发生碰撞的风险。具体分类如下：
致命障碍： 栅格值为 254，此时障碍物与机器人的中心重叠，意味着必然会发生碰撞。该区域表示极度危险，机器人无法通过。
内切障碍： 栅格值为 253，障碍物位于机器人的内切圆内，机器人必然会与该障碍物碰撞。这代表障碍物已非常接近机器人的核心区域。
外切障碍： 栅格值范围为 [128, 252]，表示障碍物位于机器人的外切圆内，处于碰撞的临界状态。这时可能会发生碰撞，具体情况取决于机器人运动的方向、惯性等因素。
非自由空间： 栅格值范围为 (0, 127]，此时机器人接近障碍物，处于危险警戒区。虽然还没有发生碰撞，但进入此区域意味着未来有较高的碰撞风险。
自由区域： 栅格值为 0，表示该区域没有障碍物，机器人可以安全通过，无碰撞风险。
未知区域： 栅格值为 255，表示该区域还没有被探测到，系统尚未确定是否存在障碍物。这类区域通常代表尚未被传感器覆盖的空间。
在路径规划中，膨胀空间的设置可以参考非自由空间的范围，确保机器人在靠近障碍物时，仍能保持足够的安全距离，以避免潜在的碰撞风险。膨胀空间通过扩大障碍物的影响范围，使机器人更加谨慎地绕过障碍物，确保安全。
（4）move_base使用
1）先编写launch文件模板
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find 功能包)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find 功能包)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find 功能包)/param/base_local_planner_params.yaml" command="load" />
</node>
</launch>
launch文件解释:
启动了 move_base 功能包下的 move_base 节点，respawn 为 false，意味着该节点关闭后，不会被重启；clear_params 为 true，意味着每次启动该节点都要清空私有参数然后重新载入；通过 rosparam 会载入若干 yaml 文件用于配置参数。
2）编写配置文件
关于配置文件的编写，可以参考一些成熟的机器人的路径规划实现，比如： turtlebot3，github链接：https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_navigation/param，先下载这些配置文件备用。
在功能包下新建 param 目录，复制下载的文件到此目录： costmap_common_params_burger.yaml、local_costmap_params.yaml、global_costmap_params.yaml、base_local_planner_params.yaml，并将costmap_common_params_burger.yaml 重命名为:costmap_common_params.yaml。
costmap_common_params.yaml
该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括：机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下：
#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状
obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物
#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0
#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
global_costmap_params.yaml
该文件用于全局代价地图参数设置：
global_costmap:
global_frame: map #地图坐标系
robot_base_frame: base_footprint #机器人坐标系
# 以此实现坐标变换
update_frequency: 1.0 #代价地图更新频率
publish_frequency: 1.0 #代价地图的发布频率
transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间
static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.
local_costmap_params.yaml
local_costmap:
global_frame: odom #里程计坐标系
robot_base_frame: base_footprint #机器人坐标系
update_frequency: 10.0 #代价地图更新频率
publish_frequency: 10.0 #代价地图的发布频率
transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间
static_map: false #不需要静态地图，可以提升导航效果
rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
width: 3 # 局部地图宽度 单位是 m
height: 3 # 局部地图高度 单位是 m
resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致
base_local_planner_params
在配置基本的局部路径规划器时，通常需要设定机器人的最大速度、最小速度以及加速度阈值，以确保机器人能够平稳、安全地导航。
TrajectoryPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.5 # X 方向最大速度
min_vel_x: 0.1 # X 方向最小速速
max_vel_theta: 1.0 #
min_vel_theta: -1.0
min_in_place_vel_theta: 1.0
acc_lim_x: 1.0 # X 加速限制
acc_lim_y: 0.0 # Y 加速限制
acc_lim_theta: 0.6 # 角速度加速限制
# Goal Tolerance Parameters，目标公差
xy_goal_tolerance: 0.10
yaw_goal_tolerance: 0.05
# Differential-drive robot configuration
# 是否是全向移动机器人
holonomic_robot: false
# Forward Simulation Parameters，前进模拟参数
sim_time: 0.8
vx_samples: 18
vtheta_samples: 20
sim_granularity: 0.05
参数配置技巧
在实际操作中，机器人可能会遇到本地路径规划与全局路径规划不一致的情况，导致机器人进入膨胀区域，从而出现"假死"现象。为尽量避免这种情况，可以针对全局路径规划和本地路径规划分别进行参数调整，适配它们不同的职责和功能。
虽然全局路径规划和本地路径规划可以使用相同的参数，但两者在路径规划和避障策略上有所不同，因此可以采取以下不同的参数设置策略：
全局代价地图：在全局代价地图中，可以适当增大膨胀半径和障碍物系数。这将使全局路径远离障碍物，保证整体路径的安全性。
本地代价地图：在本地代价地图中，膨胀半径和障碍物系数可以相对减小。这使得本地路径规划即使在机器人偏离全局路径时，依然能够在障碍物附近保留较大的自由空间，确保机器人可以灵活避障，减少"假死"现象的发生。
通过这种参数差异化的设置，能够在全局规划中避免过于接近障碍物的路径选择，同时在本地规划中保留足够的动态避障空间，提升机器人整体导航的可靠性和灵活性。
3）集成导航相关的launch文件
<launch>
<!-- 设置地图的配置文件 -->
<arg name="map" default="nav.yaml" />
<!-- 运行地图服务器，并且加载设置的地图-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
<!-- 启动AMCL节点 -->
<include file="$(find mycar_nav)/launch/amcl.launch" />
<!-- 运行move_base节点 -->
<include file="$(find mycar_nav)/launch/path.launch" />
<!-- 运行rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mycar_nav)/rviz/nav.rviz" />
</launch>
4）测试（可以在导航过程中，添加新的障碍物，机器人也可以自动躲避障碍物。）
2.5 导航与SLAM建图
在SLAM建图过程中，SLAM算法本身会实时发布地图信息，因此无需额外使用**map_server** 来发布地图数据。SLAM系统已经通过话题**/map**发布了实时生成的地图消息。此外，导航过程中通常需要定位模块来获取位置信息，而SLAM不仅能够生成地图，还可以实现自身定位的功能，因此可以满足导航系统对定位的需求。
（1）编写launc文件
<launch>
<!-- 启动SLAM节点 -->
<include file="$(find mycar_nav)/launch/slam.launch" />
<!-- 运行move_base节点 -->
<include file="$(find mycar_nav)/launch/path.launch" />
<!-- 运行rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mycar_nav)/rviz/nav.rviz" />
</launch>
（2）测试
