ROS笔记3.路径规划1

• 在 Rviz 中可视化路径规划
• move_base 节点的基本概念
• 什么是Global Planner？
• 什么是Global Costmap？

在 Rviz 中可视化路径规划

对于本章，您基本上需要使用 RViz 的 3 个元素：

• Map Display (Costmaps)
• Path Displays (Plans)
• 2D 工具

练习

a) 执行下一个命令以启动 move_base 节点。

roslaunch husky_navigation move_base_demo.launch

b) 打开图形界面并执行以下命令来启动 RViz。

rosrun rviz rviz

c) 正确配置 RViz 以便可视化必要的部分。

可视化代价地图

• 单击“Displays”下的“Add”按钮并选择“地图”元素。
• 将主题设置为 /move_base/global_costmap/costmap，以便可视化全局成本图

• 将主题更改为 /move_base/local_costmap/costmap，以便可视化本地成本图。

• 您可以有 2 个地图显示，每个成本图一个。

可视化规划

• 单击“Displays”下的“Add”按钮并选择“Path”元素。
• 将主题设置为 /move_base/NavfnROS/plan，以便可视化全局计划。
• 将主题更改为 /move_base/DWAPlannerROS/local_plan，以便可视化本地计划。
• 您还可以有 2 个路径显示，每个计划一个。

d) 使用 2D 姿势估计工具为机器人提供初始姿势

e) 使用 2D Nav Goal 工具向机器人发送目标姿势。

检查以下注意事项以完成练习：

note 1：请记住，如果您不设置 2D 导航目标，则规划过程将不会启动。这意味着，除非您设置了 2D 导航目标，否则您将无法在 RViz 中可视化任何规划。

note 2：为了使 2D 工具正常工作，必须将 Rviz 中的固定框架设置为地图。

note 3：您可以在 RViz 配置中的配色方案选项中更改用于显示成本地图的配色方案：

note 4：此外，您还可以在 RViz 配置中的颜色选项中更改用于显示路径的颜色。

move_base 包

move_base 包含了 move_base 节点。move_base 节点是 ROS 导航栈中的一个重要组成部分，它连接了导航过程中所有的元素。可以说，它就像《黑客帝国》中的建筑师，或者《星际大战》中的原力。没有这个节点，ROS 导航栈将毫无意义！

好的！我们明白 move_base 节点非常重要，但它究竟是什么呢？它的作用是什么？这是个好问题！

move_base 节点的主要功能是将机器人从当前位置移动到目标位置。基本上，这个节点是 SimpleActionServer 的一个实现，它接受类型为 geometry_msgs/PoseStamped 的目标位姿消息。因此，我们可以通过使用 SimpleActionClient 向这个节点发送位置目标。

这个 Action Server 提供了名为 move_base/goal 的话题，这是导航栈的输入。这个话题用于提供目标位姿。

练习

a) 在 WebShell 中，可视化 move_base/goal 主题。

b) 像上一个练习中一样，使用 RViz 中的 2D Nav Goal 工具向机器人发送目标。

c) 检查您正在收听的主题中发生了什么。

因此，每次使用 RViz 中的 2D Nav Goal 工具设置 Pose Goal 时，实际上都会有一条新消息发布到 move_base/goal 主题中。

无论如何，这不是 move_base Action Server 提供的唯一主题。与每个动作服务器一样，它提供以下 5 个主题：

move_base/goal (move_base_msgs/MoveBaseActionGoal)

发送导航目标位置和方向。
move_base/cancel (actionlib_msgs/GoalID)

取消当前目标。
move_base/feedback (move_base_msgs/MoveBaseActionFeedback)

获取关于目标执行的进度反馈。
move_base/status (actionlib_msgs/GoalStatusArray)

获取当前所有目标的状态信息。
move_base/result (move_base_msgs/MoveBaseActionResult)

获取目标的执行结果。

练习

不使用 Rviz，向 move_base 节点发送位姿目标。

a) 使用命令行工具将此目标发送到 move_base 节点的操作服务器。

发送目标：

b) 通过 webshel​​l 可视化操作中涉及的所有主题，并在操作进行时和完成时检查它们的输出。

# /move_base/goal：检查这个话题以确认目标已被发布。
rostopic echo /move_base/goal

# /move_base/feedback：观察这个话题以接收关于目标执行过程中的反馈信息。
rostopic echo /move_base/feedback

# /move_base/status：检查这个话题以查看目标的状态，例如是否处于激活、成功或失败状态。
rostopic echo /move_base/status

# /move_base/result：该话题将显示目标完成后的结果（成功或失败）。
rostopic echo /move_base/result

输出反馈

 Echo status accepted:

输出被接受的状态

输出到达的状态

输出结果

练习

a) 创建一个名为 send_goals 的新包。添加 rospy 作为依赖项。

cd ~/catkin_ws/src
catkin_create_pkg send_goals rospy move_base_msgs

b) 在此包内，创建一个名为 send_goal_client.py 的文件。在此文件中写入 Action Client 的代码，以便向 move_base 节点的 Action Server 发送消息。

mkdir -p scripts
touch scripts/send_goal_client.py

#!/usr/bin/env python

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

def send_goal_to_move_base(x, y, theta):
    # 初始化节点
    rospy.init_node('send_goal_client')

    # 创建 Action Client
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    
    # 等待 Action Server 启动
    rospy.loginfo("等待 move_base action server 启动...")
    client.wait_for_server()

    rospy.loginfo("已连接到 move_base server")

    # 创建新目标
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.position.z = 0.0
    goal.target_pose.pose.orientation.x = 0.0
    goal.target_pose.pose.orientation.y = 0.0
    goal.target_pose.pose.orientation.z = theta
    goal.target_pose.pose.orientation.w = 1.0

    # 发送目标
    rospy.loginfo(f"发送目标: ({x}, {y}, {theta})")
    client.send_goal(goal)

    # 等待结果
    client.wait_for_result()
    rospy.loginfo("目标已到达。")

if __name__ == '__main__':
    try:
        # 定义目标位姿
        poses = [
            (1.0, 1.0, 0.0),
            (2.0, 2.0, 0.0),
            (3.0, 1.0, 0.0)
        ]
        
        # 循环移动到每个目标
        while not rospy.is_shutdown():
            for pose in poses:
                x, y, theta = pose
                send_goal_to_move_base(x, y, theta)
    except rospy.ROSInterruptException:
        rospy.loginfo("导航测试结束。")

chmod +x scripts/send_goal_client.py

c) 使用此 Action Client，将机器人移动到地图的三个不同姿势。当机器人达到这 3 个姿势时，重新开始创建一个循环，这样机器人就会不断重复这 3 个姿势。

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roslaunch husky_navigation move_base_demo.launch
rosrun send_goals send_goal_client.py

此时，您已检查可以通过向其动作服务器的 /move_base/goal 主题发送消息来将姿势目标发送到 move_base 节点。

当此节点收到目标姿势时，它会链接到全局规划器、局部规划器、恢复行为和成本图等组件，并生成输出，该输出是消息类型为 geometry_msgs/Twist 的速度命令，并将其发送到 /cmd_vel 主题以移动机器人。

正如您在前几章中看到的 slam_gmapping 和 amcl 节点一样，move_base 节点也具有您可以修改的参数。例如，您可以修改的参数之一是 move_base 节点将这些速度命令发送到基础控制器的频率。让我们通过一个快速练习来检查一下。

练习

a) 创建一个名为 my_move_base_launcher 的新包。在这个包中，创建 2 个目录，一个名为 launch，另一个名为 params。在 launch 目录中，创建一个名为 my_move_base.launch 的新文件。在 params 目录中，创建一个名为 my_move_base_params.yaml 的新文件。

cd ~/catkin_ws/src
catkin_create_pkg my_move_base_launcher rospy move_base_msgs
cd my_move_base_launcher
mkdir -p launch params
touch launch/my_move_base.launch
touch params/my_move_base_params.yaml

b) 查看 husky_navigation 包的 move_base_demo.launch 和 move_base.launch 文件。检查它们的结构并查看它们的工作原理。

move_base_demo.launch

<launch>
  <!-- Run the map server -->
  <arg name="map_file" default="$(find husky_navigation)/maps/my_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- Run AMCL -->
  <include file="$(find husky_navigation)/launch/amcl.launch" />

  <!-- Run Move Base -->
  <include file="$(find husky_navigation)/launch/move_base.launch" />
</launch>

move_base.launch

<launch>
  <arg name="no_static_map" default="false"/>
  <arg name="base_global_planner" default="navfn/NavfnROS"/>
  <arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/>
</launch>

c) 将 move_base.launch 文件的内容复制到文件 my_move_base.launch。

d) 修改 my_move_base.launch 文件，使其也加载 map_server 和 amcl 节点。注意在 move_base_demo.launch 文件中是如何完成的。

<launch>
  <!-- Run the map server -->
  <arg name="map_file" default="$(find my_move_base_launcher)/maps/my_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- Run AMCL -->
  <node name="amcl" pkg="amcl" type="amcl" output="screen">
    <param name="odom_model_type" value="diff"/>
  </node>

  <!-- Run Move Base -->
  <node name="move_base" pkg="move_base" type="move_base" output="screen"/>

</launch>

e) 现在，查看 husky_navigation 包的 planner.yaml 文件。

# 控制器频率，单位是赫兹（Hz）。通常设置为机器人控制更新的频率。
controller_frequency: 5.0

# 是否启用恢复行为。布尔值。
recovery_behaviour_enabled: true

# 全局规划器配置
NavfnROS:
  # 是否允许全局规划器在未知空间中生成路径。布尔值。
  allow_unknown: true
  
  # 目标点的容差，单位是米（m）。
  default_tolerance: 0.1

# 轨迹规划器配置
TrajectoryPlannerROS:
  # 机器人配置参数
  # 机器人在x方向上的加速度限制，单位是米每平方秒（m/s²）。
  acc_lim_x: 2.5
  # 机器人在旋转方向上的加速度限制，单位是弧度每平方秒（rad/s²）。
  acc_lim_theta: 3.2
  
  # 机器人在x方向上的最大速度，单位是米每秒（m/s）。
  max_vel_x: 1.0
  # 机器人在x方向上的最小速度，单位是米每秒（m/s）。
  min_vel_x: 0.0
  
  # 机器人旋转方向上的最大速度，单位是弧度每秒（rad/s）。
  max_vel_theta: 1.0
  # 机器人旋转方向上的最小速度，单位是弧度每秒（rad/s）。
  min_vel_theta: -1.0
  # 机器人原地旋转的最小速度，单位是弧度每秒（rad/s）。
  min_in_place_vel_theta: 0.2
  
  # 是否为全向机器人。布尔值。
  holonomic_robot: false
  # 逃逸速度，单位是米每秒（m/s）。
  escape_vel: -0.1

  # 目标容差参数
  # 目标点的偏航角容差，单位是弧度（rad）。
  yaw_goal_tolerance: 0.1
  # 目标点的xy容差，单位是米（m）。
  xy_goal_tolerance: 0.2
  # 是否固定目标点的xy容差。布尔值。
  latch_xy_goal_tolerance: false

  # 前向模拟参数
  # 模拟时间，单位是秒（s）。
  sim_time: 2.0
  # 模拟粒度，单位是秒（s）。
  sim_granularity: 0.02
  # 角度模拟粒度，单位是弧度（rad）。
  angular_sim_granularity: 0.02
  # 前进速度样本数量。
  vx_samples: 6
  # 角速度样本数量。
  vtheta_samples: 20
  # 控制器频率，单位是赫兹（Hz）。
  controller_frequency: 20.0

  # 轨迹评分参数
  # 是否按米来打分。布尔值。
  meter_scoring: true
  # 避障权重。
  occdist_scale: 0.1
  # 路径距离权重。
  pdist_scale: 0.75
  # 目标距离权重。
  gdist_scale: 1.0

  # 评分时的前瞻距离，单位是米（m）。
  heading_lookahead: 0.325
  # 是否根据机器人朝向来打分。布尔值。
  heading_scoring: false
  # 使用朝向评分时的前瞻时间，单位是秒（s）。
  heading_scoring_timestep: 0.8
  # 是否使用动态窗口方法（DWA）。布尔值。
  dwa: true
  # 是否使用简单吸引器。布尔值。
  simple_attractor: false
  # 是否发布代价网格点云。布尔值。
  publish_cost_grid_pc: true

  # 振荡预防参数
  # 机器人在重置振荡标志之前必须移动的距离，单位是米（m）。
  oscillation_reset_dist: 0.25
  # 逃逸重置距离，单位是米（m）。
  escape_reset_dist: 0.1
  # 逃逸重置角度，单位是弧度（rad）。
  escape_reset_theta: 0.1

# 动态窗口方法规划器配置
DWAPlannerROS:
  # 机器人配置参数
  # 机器人在x方向上的加速度限制，单位是米每平方秒（m/s²）。
  acc_lim_x: 2.5
  # 机器人在y方向上的加速度限制，单位是米每平方秒（m/s²）。
  acc_lim_y: 0
  # 机器人旋转方向上的加速度限制，单位是弧度每平方秒（rad/s²）。
  acc_lim_th: 3.2

  # 机器人在x方向上的最大速度，单位是米每秒（m/s）。
  max_vel_x: 0.5
  # 机器人在x方向上的最小速度，单位是米每秒（m/s）。
  min_vel_x: 0.0
  # 机器人在y方向上的最大速度，单位是米每秒（m/s）。
  max_vel_y: 0
  # 机器人在y方向上的最小速度，单位是米每秒（m/s）。
  min_vel_y: 0

  # 机器人在平移方向上的最大速度，单位是米每秒（m/s）。
  max_vel_trans: 0.5
  # 机器人在平移方向上的最小速度，单位是米每秒（m/s）。
  min_vel_trans: 0.1
  # 机器人旋转方向上的最大速度，单位是弧度每秒（rad/s）。
  max_vel_theta: 1.0
  # 机器人旋转方向上的最小速度，单位是弧度每秒（rad/s）。
  min_vel_theta: 0.2

  # 目标容差参数
  # 目标点的偏航角容差，单位是弧度（rad）。
  yaw_goal_tolerance: 0.1
  # 目标点的xy容差，单位是米（m）。
  xy_goal_tolerance: 0.2
  # 是否固定目标点的xy容差。布尔值。
  latch_xy_goal_tolerance: false


  # 前向模拟参数（已注释）
  # sim_time: 2.0
  # sim_granularity: 0.02
  # vx_samples: 6
  # vy_samples: 0
  # vtheta_samples: 20
  # penalize_negative_x: true

  # 轨迹评分参数（已注释）
  # path_distance_bias: 32.0
  # goal_distance_bias: 24.0
  # occdist_scale: 0.01
  # forward_point_distance: 0.325
  # stop_time_buffer: 0.2
  # scaling_speed: 0.25
  # max_scaling_factor: 0.2

  # 振荡预防参数（已注释）
  # oscillation_reset_dist: 0.25

f) 将 planner.yaml 文件的内容复制到文件 my_move_base_params.yaml。

g) 修改 my_move_base.launch 文件，以便它加载参数文件 my_move_base_params.yaml。

<launch>
  <!-- Run the map server -->
  <arg name="map_file" default="$(find my_move_base_launcher)/maps/my_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- Run AMCL -->
  <node name="amcl" pkg="amcl" type="amcl" output="screen">
    <param name="odom_model_type" value="diff"/>
    <!-- 其他 AMCL 参数配置 -->
  </node>

  <!-- Run Move Base -->
  <node name="move_base" pkg="move_base" type="move_base" output="screen">
    <rosparam file="$(find my_move_base_launcher)/params/my_move_base_params.yaml" command="load"/>
  </node>
</launch>

i) 启动 my_move_base.launch 文件，并向您的机器人发送目标！

全局路径规划

当 move_base 节点收到新目标时，该目标会立即发送给全局规划器。然后，全局规划器负责计算一条安全路径以到达该目标姿势。这条路径是在机器人开始移动之前计算出来的，因此它不会考虑机器人传感器在移动过程中的读数。

每次全局规划器规划一条新路径时，这条路径都会发布到 /plan 主题中。让我们做一个练习来检查一下。

a) 打开 Rviz 并添加 Display，以便能够可视化 Global Plan。

b) 订阅 Global Planner 发布其计划路径的主题并查看。

c) 使用 2D Nav Goal 工具，向 move_base 节点发送新目标。

您可能已经注意到，当您发送目标以可视化全局规划器制定的路径规划时，机器人会自动开始执行此规划。发生这种情况的原因是，通过发送此目标姿势，您将启动整个导航过程。

在某些情况下，您可能只对可视化全局规划感兴趣，但不想执行该计划。对于这种情况，move_base 节点提供了一个名为 /make_plan 的服务。此服务允许您计算全局规划，而无需让机器人执行路径。让我们在下一个练习中检查它是如何工作的。

练习

创建一个服务客户端，它将调用上面介绍的服务之一，以使计划达到给定的姿势，而不会导致机器人移动。

a) 创建一个名为 make_plan 的新包。添加 rospy 作为依赖项。

cd ~/catkin_ws/src
catkin_create_pkg make_plan rospy
cd ~/catkin_ws
catkin_make
cd ~/catkin_ws/src/make_plan

b) 在此包中，创建一个名为 make_plan_caller.py 的文件。将服务客户端的代码写入此文件。

mkdir -p scripts
touch scripts/make_plan_caller.py
chmod +x scripts/make_plan_caller.py

make_plan_caller.py 

#!/usr/bin/env python

import rospy
from nav_msgs.srv import GetPlan
from geometry_msgs.msg import PoseStamped

def call_service(start_pose, goal_pose):
    rospy.wait_for_service('/move_base/make_plan')
    try:
        get_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
        response = get_plan(start=start_pose, goal=goal_pose, tolerance=0.0)
        return response.plan
    except rospy.ServiceException as e:
        rospy.logerr("服务调用失败: %s" % e)
        return None

if __name__ == '__main__':
    rospy.init_node('make_plan_client')

    # 定义起始姿态和目标姿态
    start_pose = PoseStamped()
    start_pose.header.frame_id = "map"
    start_pose.header.stamp = rospy.Time.now()
    start_pose.pose.position.x = 0.0
    start_pose.pose.position.y = 0.0
    start_pose.pose.orientation.w = 1.0

    goal_pose = PoseStamped()
    goal_pose.header.frame_id = "map"
    goal_pose.header.stamp = rospy.Time.now()
    goal_pose.pose.position.x = 1.0
    goal_pose.pose.position.y = 1.0
    goal_pose.pose.orientation.w = 1.0

    plan = call_service(start_pose, goal_pose)
    
    if plan:
        rospy.loginfo("收到的路径包含 %d 个姿态" % len(plan.poses))
    else:
        rospy.loginfo("未收到路径")

rosrun make_plan make_plan_caller.py

所以，您现在知道，导航过程的第一步是计算一个安全计划，以便您的机器人能够到达用户指定的目标姿势。但是……这条路径是如何计算的？

存在不同的全局规划器。根据您的设置（您使用的机器人、它导航的环境等），您可以使用其中一种或另一种。让我们来看看最重要的一些。

Navfn

Navfn 规划器可能是 ROS 导航中最常用的全局规划器。它使用 Dijkstra 算法来计算初始姿势和目标姿势之间的最短路径。下面，您可以看到该算法如何工作的动画。

CarrotPlanner

CarrotPlanner采用目标姿势并检查该目标是否处于障碍物中。然后，如果它处于障碍物中，它会沿着目标和机器人之间的矢量向后走，直到找到不在障碍物中的目标点。然后，它将这个目标点作为计划传递给本地规划器或控制器。因此，该规划器不进行任何全局路径规划。如果您需要机器人靠近给定的目标，即使无法到达目标，它也很有用。在复杂的室内环境中，这个规划器不太实用。

例如，如果您希望机器人尽可能靠近障碍物（例如桌子），则此算法很有用。

GlobalPlanner

GlobalPlanner 是我们之前看到的 Navfn 规划器的更灵活的替代品。它允许您更改 Navfn 使用的算法（Dijkstra 算法）来计算其他算法的路径。这些选项包括对 A∗ 的支持、切换二次近似和切换网格路径。

特性	NavfnPlanner	CarrotPlanner	GlobalPlanner
算法	Dijkstra（广度优先搜索）	简单的直线路径	A*算法（启发式搜索）
障碍物处理	避开障碍物，生成全局最优路径	不考虑障碍物，仅生成直线路径	避开障碍物，但更快速生成路径
适用场景	静态环境，障碍较少的场景	需要与局部规划器结合	动态环境，复杂地图
路径平滑性	路径相对平滑	路径为直线，不平滑	路径可能较不平滑
计算复杂度	较高，耗时较长	非常低，实时性强	较低，计算速度较快
优缺点	优点：可靠，路径最优 缺点：效率较低	优点：简单快速 缺点：缺乏障碍物避让能力	优点：高效处理动态环境 缺点：路径不一定是全局最优

修改Global Planner

move_base 节点使用的全局规划器在 base_global_planner 参数中指定。它可以在参数文件中设置，如下例所示：

base_global_planner: "navfn/NavfnROS" # 设置 Navfn Planner

base_global_planner: "carrot_planner/CarrotPlanner" # 设置 CarrotPlanner

base_global_planner: "global_planner/GlobalPlanner" # 设置 GlobalPlanner

或者可以直接在启动文件中设置，就像我们的例子一样：

<arg name="base_global_planner" default="navfn/NavfnROS"/>

练习

a) 打开 Rviz 并添加显示，以便能够可视化全局规划。

为确保您已正确更改全局规划器，您可以使用以下命令：

rosparam get /move_base/base_global_planner

b) 使用 2D Nav Goal 工具发送目标。此目标必须位于障碍物“内部”。检查会发生什么。

Navfn planner response:

 将目标设置在障碍物内：

c) 修改 my_move_base.launch 文件，使其现在使用 CarrotPlanner。

 Carrot Planner Response:

d) 重复步骤 b，并检查现在会发生什么。

每个规划器也有自己的参数，以便自定义其行为。这些参数通常位于 YAML 文件中。根据您使用的全局规划器，要设置的参数会有所不同。在本课程中，我们将了解 Navfn 规划器的参数，因为它是最常用的。如果您有兴趣检查可以为其他规划器设置的参数，可以在此处查看：

carrot 规划器：http://wiki.ros.org/carrot_planner

global规划器：http://wiki.ros.org/global_planner

Navfn参数

如果您检查包 my_move_base_launcher 中的文件 my_move_base_params.yaml，您将看到 Navfn 规划器所看到的参数：

# 全局规划器配置
NavfnROS:
  # 是否允许全局规划器在未知空间中生成路径。布尔值。
  allow_unknown: true
  
  # 目标点的容差，单位是米（m）。
  default_tolerance: 0.1

在此文件中，您还可以找到其他规划器（TrajectoryPlannerROS 和 DWAPlannerROS）的参数。现在不用担心这些，因为我们将在课程的下一个单元中检查它们。

让我们看看 Navfn 规划器最重要的参数是什么：

• /allow_unknown（默认值：true）：指定是否允许 navfn 创建遍历未知空间的规划。注意：如果您使用的是带有体素或障碍物层的分层 costmap_2d 代价地图，您还必须将该层的 track_unknown_space 参数设置为 true，否则它会将您所有的未知空间转换为自由空间（navfn 随后会顺利地穿过该空间）。
• /planner_window_x（默认值：0.0）：指定可选窗口的 x 大小以限制规划器。这对于限制 NavFn 在大型代价地图的小窗口中工作非常有用。
• /planner_window_y（默认值：0.0）：指定可选窗口的 y 大小以限制规划器。这对于限制 NavFn 在大型代价地图的小窗口中工作非常有用。
• /default_tolerance（默认值：0.0）：规划器的目标点容差。NavFn 将尝试创建一个尽可能接近指定目标的规划，但距离不会超过默认容差。
• /visualize_potential（默认值：false）：指定是否通过 PointCloud2 可视化 navfn 计算的潜在区域。

到目前为止，您已经看到存在一个全局规划器，它负责计算安全路径，以便将机器人从初始位置移动到目标位置。您还看到了有不同类型的全局规划器，您可以选择要使用的全局规划器。最后，您还看到每个规划器都有自己的参数，这些参数会修改规划器的行为方式。

但是现在，让我问你一个问题。当您规划轨迹时，必须根据地图来规划该轨迹，对吗？没有地图的路径是没有意义的。好的，那么……您能猜出全局规划器使用什么地图来计算其路径吗？

您可能会认为正在使用的地图是您在本课程的“映射”章节中创建的地图……但是，让我告诉你，这并不完全正确。

还有另一种类型的地图：成本地图。听起来熟悉吗？这是应该的，因为您在本章的第一个练习中已经对它进行了介绍。

Costmaps

成本图（costmap）是一种表示在网格单元中哪些地方对机器人是安全的地图。通常，成本图中的值是二进制的，表示空闲空间或机器人可能会发生碰撞的地方。

成本图中的每个单元格都有一个整数值，范围在 {0,255} 之间。这个范围中经常使用一些特殊值，它们的功能如下：

• 255（NO_INFORMATION）：保留给没有足够信息的单元格。
• 254（LETHAL_OBSTACLE）：表示在此单元格中检测到会导致碰撞的障碍物。
• 253（INSCRIBED_INFLATED_OBSTACLE）：表示没有障碍物，但将机器人中心移动到此位置会导致碰撞。
• 0（FREE_SPACE）：没有障碍物的单元格，因此将机器人中心移动到此位置不会导致碰撞。

存在两种类型的成本图：全局成本图（global costmap）和局部成本图（local costmap）。它们之间的主要区别在于它们的构建方式：

• 全局成本图是从静态地图创建的。
• 局部成本图是从机器人的传感器读数创建的。

现在，我们将重点关注全局成本图，因为它是全局规划器（global planner）使用的图。因此，全局规划器利用全局成本图来计算行进路径。

让我们做一个练习，以便更好地了解全局成本图的样子。

启动 Rviz 并添加必要的显示以可视化全局成本地图。

 

现在，您已经了解了全局成本地图的样子，让我们进一步了解它。 

Global Costmap

全局代价地图是根据用户生成的静态地图（如我们在“地图绘制”一章中创建的地图）创建的。在这种情况下，代价地图被初始化为与静态地图提供的宽度、高度和障碍物信息相匹配。此配置通常与定位系统（如 amcl）结合使用。这是您用来初始化全局代价地图的方法。

全局代价地图也有自己的参数，这些参数在 YAML 文件中定义。接下来，您可以看到全局代价地图参数文件的示例。

global_frame: map
rolling_window: false

plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}

代价地图参数在 3 个不同的文件中定义：

• 一个 YAML 文件，用于设置全局代价地图（即您在上面看到的那个）的参数。我们将该文件命名为 global_costmap_params.yaml。
• 一个 YAML 文件，用于设置本地代价地图的参数。我们将该文件命名为 local_costmap_params.yaml。
• 一个 YAML 文件，用于设置全局和本地代价地图的参数。我们将该文件命名为 common_costmap_params.yaml。

现在，我们将重点介绍全局代价地图参数，因为它是全局规划器使用的代价地图。

Global Costmap 参数

您需要了解的参数如下：

• global_frame（默认值：“map”）：costmap 在其中运行的全局坐标系。
• robot_base_frame（默认值：“base_link”）：机器人底盘的坐标系名称。
• rolling_window（默认值：false）：是否使用 costmap 的滚动窗口版本。
• plugins：插件规范序列，每层一个。每个规范都是一个带有名称和类型字段的字典。名称用于定义插件的参数命名空间。然后，此名称将在 common_costmap_parameters.yaml 文件中定义，您将在下一个单元中看到该文件。类型字段实际上定义了将要使用的插件（源代码）。

让我们从讨论 rolling_window 参数开始，它是最重要的参数之一。因此，通过将 rolling_window 参数设置为 false，我们将通过从静态地图中获取数据来初始化 costmap。这是您想要初始化全局 costmap 的方式。

练习

a) 将名为 my_global_costmap_params.yaml 的文件添加到您创建的my_move_base_launcher包的 params 目录中。

b) 将 husky_navigation 包的 costmap_global_static.yaml 文件的内容复制到此文件中。

costmap_global_static.yaml

global_frame: map
rolling_window: false

plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}

c) 修改您在练习my_move_base_launcher中创建的 my_move_base.launch 文件，以便它加载您刚刚创建的全局代价地图参数文件。

d) 将 rolling_window 参数更改为 true 并再次启动 move_base 节点。

e) 检查您在全局代价地图可视化中看到的变化。

你需要了解的最后一个参数是插件区域（plugins area）。在插件区域中，我们将向成本图配置中添加层（layers）。好吧，那...什么是层呢？

为了简化（并明确）成本图的配置，ROS 使用了层（layers）。层就像是“块”（blocks）的一组相关参数。例如，静态地图、感知到的障碍物和膨胀（inflation）被分成了不同的层。这些层在 common_costmap_parameters.yaml 文件中定义，然后添加到 local_costmap_params.yaml 和 global_costmap_params.yaml 文件中。

要将层添加到成本图的配置文件中，你需要在插件区域指定它。请看下面的这一行：

plugins: 
    - {name: static_map,       type: "costmap_2d::StaticLayer"}

在这里，您将向 costmap 配置添加一个名为 static_map 的层，该层将使用 costmap_2d::StaticLayer 插件。您可以根据需要添加任意数量的层：

plugins: 
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: inflation,        type: "costmap_2d::InflationLayer"}

例如，您可以看到上面显示的本地代价地图参数文件的一个示例。对于全局代价地图，您通常会使用这 2 个层：

• costmap_2d::StaticLayer：用于从静态地图初始化代价地图。
• costmap_2d::InflationLayer：用于给障碍物膨胀。

您可能已经注意到这些层刚刚被添加到参数文件中。确实如此。在全局和本地代价地图参数文件中，这些层都是刚刚添加的。这些层的具体参数在通用代价地图参数文件中定义。我们将在下一个单元中稍后查看此文件。