所用的学习链接：

【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程P289-314

【以上视频笔记见http://www.autolabor.com.cn/book/ROSTutorials/】

一、介绍

官方链接：https://www.ros.org/reps/rep-0105.html

1.实现导航的关键技术

 （1）全局地图

（全局概览图：定位+路径规划）

• SLAM（实现地图构建和即时定位）,也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。（如红警起始一片黑，随着摸索出现地图）

ROS中保存地图的功能包是 map_server

• 传感器：如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如: 激光雷达、摄像头、RGB-D摄像头

（2）自身定位

（确定在地图中的位置）

• amcl(adaptiveMonteCarloLocalization)自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。它实现了自适应（或KLD采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。
• 导航开始和导航过程中，机器人都需要确定当前自身的位置，室外可用GPS。室内、隧道、地下或一些特殊的屏蔽和弱化 GPS 信号的区域，前面的 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包amcl

（3）路径规划

（全局+局部路径规划）

• 全局路径规划(gloable_planner)

  根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线

• 局部路径规划(local_planner)

  在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如:机器人在运行中，可能会随时出现一定的障碍物... 本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径

（4）运动控制

（控制速度和方向）

 导航功能包集假定它可以通过话题"cmd_vel"发布geometry_msgs/Twist类型的消息，这个消息基于机器人的基座坐标系，它传递的是运动命令。这意味着必须有一个节点订阅"cmd_vel"话题， 将该话题上的速度命令转换为电机命令并发送。

（5）环境感知

（感知周围环境）

感知周围环境信息，比如: 摄像头、激光雷达、编码器...，摄像头、激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。

在导航功能包集中，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块诸如: SLAM、amcl、move_base 都需要依赖于环境感知。

 2.坐标系

（1）里程计定位（odom）

• 里程计定位：时时收集机器人的速度信息，计算并发布机器人坐标系与父级参考系的相对关系。
• 优点：里程计定位信息是连续的，没有离散的跳跃。
• 缺点：里程计存在累计误差，不利于长距离或长期定位。

【根据自己向什么方向走了多少路判断位置】

（2）传感器定位（map）

• 传感器定位：通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系。
• 优点：比里程计定位更精准；
• 缺点：传感器定位会出现跳变的情况，且传感器定位在标志物较少的环境下，其定位精度会大打折扣。

 【根据自己周围环境判断位置】

（3）坐标系变换

上述两种定位实现中，机器人坐标系一般使用机器人模型中的根坐标系(base_link 或 base_footprint)

里程计定位时，父级坐标系一般称之为 odom

传感器定位时，父级参考系一般称之为 map。

当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中"单继承"的原则的，所以，一般会将转换关系设置为: map -> doom -> base_link 或 base_footprint。

3.硬软件需求

（1）硬件

虽然导航功能包集被设计成尽可能的通用，在使用时仍然有三个主要的硬件限制：

• 它是为差速驱动的轮式机器人设计的。它假设底盘受到理想的运动命令的控制并可实现预期的结果，命令的格式为：x速度分量，y速度分量，角速度(theta)分量。 

• 它需要在底盘上安装一个单线激光雷达。这个激光雷达用于构建地图和定位。

• 导航功能包集是为正方形的机器人开发的，所以方形或圆形的机器人将是性能最好的。 它也可以工作在任意形状和大小的机器人上，但是较大的机器人将很难通过狭窄的空间。

（2）软件 

• 安装 ROS

• 当前导航基于仿真环境，先保证上一章的机器人系统仿真可以正常执行

• 在仿真环境下，机器人可以正常接收 /cmd_vel 消息，并发布里程计消息，传感器消息发布也正常，也即导航模块中的运动控制和环境感知实现完毕

二、导航实现

1.准备工作

（1）安装功能包

• gmapping 包（用于构建地图）

# sudo apt install ros-<ROS版本>-gmapping
sudo apt install ros-noetic-gmapping

• 地图服务包(用于保存与读取地图)

# sudo apt install ros-<ROS版本>-map-server
sudo apt install ros-noetic-map-server

• navigation 包(用于定位以及路径规划) 

# sudo apt install ros-<ROS版本>-navigation
sudo apt install ros-noetic-navigation

（2）新建功能包

①右键 7.19_demo01/src ，新建package catkin ，命名【nav_demo】，导入dependences： gmapping map_server amcl move_base

②在nav_demo文件夹下新建文件夹launch、map、param、config

2.SLAM建图

官方链接：http://wiki.ros.org/gmapping

（1）gmapping相关

SLAM算法有多种，当前我们选用gmapping。

gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图。

gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

• 订阅的Topic

tf (tf/tfMessage)：用于雷达、底盘与里程计之间的坐标变换消息。

scan(sensor_msgs/LaserScan)：SLAM所需的雷达信息。

• 发布的Topic

map_metadata(nav_msgs/MapMetaData)：地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。

map(nav_msgs/OccupancyGrid)：地图栅格数据，一般会在rviz中以图形化的方式显示。

~entropy(std_msgs/Float64)：机器人姿态分布熵估计(值越大，不确定性越大)。

• 服务

dynamic_map(nav_msgs/GetMap)：用于获取地图数据。

• 常用参数

~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)：激光探测的最大可用范围(超出此阈值，被截断)。

 激光探测的最大范围。

• 所需的坐标变换

雷达坐标系→基坐标系：一般由 robot_state_publisher 或 static_transform_publisher 发布。

基坐标系→里程计坐标系：一般由里程计节点发布。

• 发布的坐标变换

地图坐标系→里程计坐标系：地图到里程计坐标系之间的变换。

（2）建图代码

在nav_demo01/launch文件夹下新建 t1_slam.launch

<launch>
    <!-- 设置为true表示当前环境是仿真环境 -->
    <param name="use_sim_time" value="true" />
    <!-- gamping 节点 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
        <!-- 设置雷达话题 -->
        <remap from="scan" to="scan" />

        <!-- 关键参数：坐标系 -->
        <param name="base_frame" value="base_footprint" /> <!--底盘坐标系-->
        <param name="odom_frame" value="odom" /> <!--里程计坐标系-->
        <param name="map_frame" value="map" /> <!--地图坐标系-->

        <param name="map_update_interval" value="5.0" />
        <param name="maxUrange" value="16.0" />
        <param name="sigma" value="0.05" />
        <param name="kernelSize" value="1" />
        <param name="lstep" value="0.05" />
        <param name="astep" value="0.05" />
        <param name="iterations" value="5" />
        <param name="lsigma" value="0.075" />
        <param name="ogain" value="3.0" />
        <param name="lskip" value="0" />
        <param name="srr" value="0.1" />
        <param name="srt" value="0.2" />
        <param name="str" value="0.1" />
        <param name="stt" value="0.2" />
        <param name="linearUpdate" value="1.0" />
        <param name="angularUpdate" value="0.5" />
        <param name="temporalUpdate" value="3.0" />
        <param name="resampleThreshold" value="0.5" />
        <param name="particles" value="30" />
        <param name="xmin" value="-50.0" />
        <param name="ymin" value="-50.0" />
        <param name="xmax" value="50.0" />
        <param name="ymax" value="50.0" />
        <param name="delta" value="0.05" />
        <param name="llsamplerange" value="0.01" />
        <param name="llsamplestep" value="0.01" />
        <param name="lasamplerange" value="0.005" />
        <param name="lasamplestep" value="0.005" />
    </node>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
    <!-- 使用之前保存过的 rviz 配置-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf_gazebo)/config/t7_car.rviz" />

</launch>

（3）启动 

①运行相关启动命令

ctrl+shift+b   # 编译

# 终端1
roscore

# 终端2（启动之前的gazebo）
roslaunch urdf_gazebo t7_gazebo.launch

# 终端3（启动新建的地图绘制 launch ）
roslaunch nav_demo t1_slam.launch

# 外部新终端1（启动键盘控制）
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

 ②在Rviz中 Add → Map → Topic ：/map

可以看见rviz中出现了地图。控制小车移动，可以进一步绘制地图

③打开TF → Frames → 勾选 map、odom、base_footprint

勾选后会显示起始点的 map 和 odm 坐标，base_footprint可显示当前位置和原点的距离关系，其他选项根据需要自行调整

 3.地图保存和读取

官方链接：http://wiki.ros.org/map_server

map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。

（1）map_sarver相关

• 订阅的Topic

map(nav_msgs/OccupancyGrid)：订阅此话题用于生成地图文件。

（2）map_server相关

• 发布的Topic

map_metadata（nav_msgs / MapMetaData）：发布地图元数据。

map（nav_msgs / OccupancyGrid）：地图数据。

• 服务

static_map（nav_msgs / GetMap）：通过此服务获取地图。

• 常用参数

〜frame_id（字符串，默认值：“map”）：地图坐标系。

（3）地图保存

①新建用于地图保存的 t2_map_save.launch文件

<launch>
    <!-- nav 文件夹可以不创建 -->
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

②建完图后，运行 t2_map_save.launch

# 新终端
roslaunch nav_demo t2_map_save.launch

③查看

在nav_demo/map目录下，会看到生成了两个地图文件

 

• xxx.pgm 本质是一张图片

直接使用图片查看程序即可打开。

• xxx.yaml 保存的是地图的元数据信息

用于描述图片，内容格式如下:

# 图片路径，可以是绝对路径也可以是相对路径
image: /home/netceor_ros/ROS/7.19_demo1/src/nav_demo/map/nav.pgm
# 分辨率(单位: m/像素)、地图刻度尺单位
resolution: 0.050000
# 地图位姿，相对于rviz中原点位姿。（x，y，偏航）偏航为逆时针旋转（偏航= 0表示无旋转）
origin: [-50.000000, -50.000000, 0.000000]
# 取反：是否应该颠倒白色/黑色自由/占用的语义
negate: 0
# 占用阈值：占用概率大于此阈值的像素被视为完全占用
occupied_thresh: 0.65
# 空闲阈值：占用率小于此阈值的像素被视为完全空闲
free_thresh: 0.196

• map_server 中障碍物计算规则

a.地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x；

b.map_server 会将像素值作为判断是否是障碍物的依据，首先计算比例: p = (255 - x) / 255.0，白色为0，黑色为1(negate为true，则p = x / 255.0)；

c.根据步骤2计算的比例判断是否是障碍物，如果 p > occupied_thresh 那么视为障碍物，如果 p < free_thresh 那么视为无物。

（4）地图读取

①新建用于地图读取的 t3_map_load.launch文件

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)" />
</launch>

②启动命令

ctrl+shift+b  # 编译

# 集成终端1，运行完后地图信息就被发布
roslaunch nav_demo t3_map_load.launch

# 外部新终端2，开启一个rviz
rviz

# 外部新终端3,查看话题，如果有map相关就说明成功发布
# rostopic list

③ Rviz 中 → Add → Map → Topic：/map

【这里我第一次启动的时候没有map话题，把vscode重新启动后就有用了】

4.定位

官方链接：http://wiki.ros.org/amcl

（1）amcl 相关

定位就是推算机器人自身在全局地图中的位置。SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中。导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

• 订阅的Topic

scan(sensor_msgs/LaserScan)：激光雷达数据。

tf(tf/tfMessage)：坐标变换消息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)：用来初始化粒子滤波器的均值和协方差。

map(nav_msgs/OccupancyGrid)：获取地图数据。

• 发布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)：机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)：位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)：发布从 odom 到 map 的转换。

• 服务

global_localization(std_srvs/Empty)：初始化全局定位的服务。

request_nomotion_update(std_srvs/Empty)：手动执行更新和发布更新的粒子的服务。

set_map(nav_msgs/SetMap)：手动设置新地图和姿态的服务。

• 调用的服务

static_map(nav_msgs/GetMap)：调用此服务获取地图数据。

• 常用参数

~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")：地图坐标系。

（2）坐标变换

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

• 里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
• amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。

 （3）使用amcl和运行

①amcl节点相关的 t4_amcl.launch文件

# 可查看模板
# roscd amcl
# ls examples
# 该目录下会列出两个文件: amcl_diff.launch 和 amcl_omni.launch 文件，前者适用于差分移动机器人，后者适用于全向移动机器人
# gedit examples/amcl_diff.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" /> <!-- 里程计odom坐标系 -->
        <param name="base_frame_id" value="base_footprint" /> <!-- 添加机器人基坐标系 -->
        <param name="global_frame_id" value="map" /> <!-- 添加地图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>

 ②编写测试 amcl 的 t5_amcl_test.launch 文件

amcl文件无法单独运行，需要新建一个启动文件

<launch>
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <!-- 加载地图服务 -->
    <include file="$(find nav_demo)/launch/t3_map_load.launch" />
    <!-- 启动AMCL节点 -->
    <include file="$(find nav_demo)/launch/t4_amcl.launch" />
</launch>

③运行

ctrl+shift+b  # 编译

# 启动gazebo
roslaunch urdf_gazebo t7_gazebo.launch

# 启动键盘
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

# 启动 t5_amcl_test.launch
roslaunch nav_demo t5_amcl_test.launch

④Rviz 中 → Add → Map → Topic：/map

⑤Rviz 中 → Add → RobotModel 

⑥Rviz 中 → Add → PoseArray → Topic：/particlecloud

控制小车运动起来，红色线越密集处，小车处于该位置的概率越大。

5.路径规划

官方链接：http://wiki.ros.org/move_base

（1）move_base相关

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。move_base主要由全局路径规划与本地路径规划组成。

move_base已经被集成到了navigation包

【动作可以为机器人的行动提供实时反馈】

• 动作订阅

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)：操作结果(此处为空)。

• 订阅的Topic

move_base_simple/goal(geometry_msgs/PoseStamped)：运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。

• 发布的Topic

cmd_vel(geometry_msgs/Twist)：输出到机器人底盘的运动控制消息。

• 服务

~make_plan(nav_msgs/GetPlan)：请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。

~clear_unknown_space(std_srvs/Empty)：允许用户直接清除机器人周围的未知空间。

~clear_costmaps(std_srvs/Empty)：允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。

（2）代价地图相关

机器人导航(尤其是路径规划模块)是依赖于地图的。ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用的，因为：

• SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；
• 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入...

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。

代价地图有两张：global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，前者用于全局路径规划，后者用于本地路径规划。

两张代价地图都可以多层叠加,一般有以下层级:

• Static Map Layer：静态地图层——SLAM构建的静态地图。

• Obstacle Map Layer：障碍地图层——导航中传感器感知的障碍物信息。

• Inflation Layer：膨胀层——在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

• Other Layers：自定义costmap——根据业务设置的地图数据，临时模拟。

多个layer可以按需自由搭配。

（3）碰撞算法

在ROS中，计算代价值的方法如图

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。

• 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
• 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
• 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
• 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
• 自由区域:栅格值为0，此处机器人可以自由通过；
• 未知区域:栅格值为255，还没探明是否有障碍物。

膨胀空间的设置可以参考非自由空间。

（4）配置和代码

 路径规划算法在move_base功能包的move_base节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同....最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数。

①配置文件 costmap_common_params.yaml

在 param 文件夹下新建 costmap_common_params.yaml

#机器人几何参，如果机器人是圆形，设置 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

在 param 文件夹下新建 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

在 param 文件夹下新建 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.yaml

在 param 文件夹下新建 base_local_planner_params.yaml

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

⑤参数配置技巧（如果跟着步骤，则无需操作）

以上配置在实操中，可能会出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况。

全局路径规划与本地路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略:

• 全局代价地图可以将膨胀半径和障碍物系数设置的偏大一些；
• 本地代价地图可以将膨胀半径和障碍物系数设置的偏小一些。

这样，在全局路径规划时，规划的路径会尽量远离障碍物，而本地路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形。

 以及几个可能会调整的参数

•  costmap_common_params.yaml

a.robot_radius【如圆形机器人大小0.1，设置为0.12】

b.sensor_frame

•  global_costmap_params.yaml

a.global_frame

d.robot_base_frame

•  local_costmap_params.yaml

a.global_frame

d.robot_base_frame

⑥ 新建 t6_movebase.launch 文件

<launch>
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" />
    </node>
</launch>

⑦ 新建 t7_346.launch 文件

<launch>
    <!-- 启动rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <!-- 地图服务 -->
    <include file="$(find nav_demo)/launch/t3_map_load.launch" />
    <!-- 启动AMCL节点 -->
    <include file="$(find nav_demo)/launch/t4_amcl.launch" />
    <!-- 运行move_base节点 -->
    <include file="$(find nav_demo)/launch/t6_movebase.launch" />
</launch>

（5）自动导航

①编译、运行gazebo、运行rviz

ctrl+shift+b  # 编译

# 启动gazebo
roslaunch urdf_gazebo t7_gazebo.launch

# 启动 rviz
roslaunch nav_demo t7_346.launch

② Rviz 中 → Add 添加组件

• RobotModel 
• Map → Topic：/map
• PoseArray → Topic：/particlecloud
•  LaserScan → Topic：/scan
•  Odometry → Topic：/odom → 取消勾选 Covariance
• Map（重命名为Map_global）→  Topic：/move_base/global_costmap/costmap → Color Scheme：costmap【与前面的map不同，可以看到全局代价地图的结果】
• Map（重命名为Map_local）→  Topic：/move_base/local_costmap/costmap → Color Scheme：costmap【与前面的map不同，可以看到局部代价地图的结果】
• Path（重命名为Path_global）→ Topic：/move_base/NavfnROS/plan【会出现指向目的地的线条】
• Path（重命名为Path_local）→ Topic：/move_base/TrajectoryPlannerROS/local_plan【会出现小段前进方向的线条，可调整颜色为方便区分】

③自动导航实现

选中菜单栏 " 2D Nav Goal "，任意选择一个位置，机器人开始自动导航

三、导航建图

在前文的建图中，我们通过键盘控制小车运动，得到完整的地图模型，学习了自动导航之后，可以开始尝试使用自动导航来实现地图构建

1. launch文件

由于SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。

因此当前launch文件实现，无需调用map_server的相关节点，只需要启动SLAM节点与move_base节点。

新建一个 t8_slam_auto.launch 来集成slam和自动导航。

由于slam文件中已有打开rviz的语句，因此在t8中不需要这个语句。

但要修改 t1_slam.launch文件中关于启动rviz的相关参数，导入上一博客中保存好的 nav_test.rviz 环境

• 新建 t8_slam_auto.launch

<!-- 集成slam和导航，机器人自动导航建图 -->
<launch>
    <!-- 启动SLAM节点 -->
    <include file="$(find nav_demo)/launch/t1_slam.launch" />
    <!-- 运行move_base节点 -->
    <include file="$(find nav_demo)/launch/t6_movebase.launch" />
</launch>

• 修改 t1_slam.launch

# 原句子
# <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf_gazebo)/config/t7_car.rviz" />

# 替换为    
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav_test.rviz" />

2.自动导航建图

 （1）启动

ctrl+shift+b  # 编译

# 启动gazebo
roslaunch urdf_gazebo t7_gazebo.launch

# 启动 rviz
roslaunch nav_demo t8_slam_auto.launch

（2） 自动导航

在rviz中通过2D Nav Goal设置目标点，机器人开始自主移动并建图

（3）保存地图

建图完成后，使用map_server保存地图，详情参考本文的二、3

机器人模型搭建参考链接：

ROS入门(五)——仿真机器人一（URDF+Rviz）

ROS入门(六)——仿真机器人二（Xacro+Rviz+Arbotix小车运动）

ROS入门(七)——仿真机器人三（Gazebo+Xacro）

ROS入门(八)——仿真机器人四（Gazebo+Rviz+雷达、摄像头、kinet仿真显示）