ros2 Navigation 学习笔记 第四章（the construct 网站）

第四章 如何在ROS2中部署路径规划

本章内容

路径规划是什么意思

在ROS2中怎么部署路径规划

Nav2 planner

Nav2 controller

Nav2 bt-navigator

Navigation behaviors 包括recovery behaviors

路径规划是什么意思

在之前的单元学习了建图和定位。现在这些在哪里使用以及如何使用？本课程即将结束，将获得关于机器人导航的全部内容。

路径规划是指规划从a点到b点的轨迹，避开沿途的障碍物。

在ros Nav2中启动路径规划

要启动路径规划，将启动多个节点。除此之外，还需要一些路径规划所需的其他节点。

要求

启动路径规划系统之前，需要运行以下节点：

nav2_map_server 节点

nav2_amcl 节点

为了启动路径规划

还需要启动：

planner、controller、manager of recovery behaviors、bahavior tree navigator、nav2_lifecycle_manager

1、启动planner

planner节点相当于ROS1中的Global Planner。目标是找到一条从a点到b点的路径。

它能够计算路径同时避开地图上的已知障碍物。一旦接收到2d_Goal_Pose就会构建路径计算。

Planner还可以访问全局环境表示和缓冲到其中的传感器数据（比如全局成本地图）

目前ROS2中只有一个planner算法可用，即Nav2Fn_Planner

在节点中需要指出的字段：

nav2_planner提供的controller

可执行文件叫做planner_server

            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[nav2_yaml])
            

它需要的参数包括：

1、包含节点所有参数的yaml文件

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

2、启动controller

ROS Nav2 controller相当于ROS1中的local planner。他的主要任务是执行从当前位置到前方几米（传感器范围）的反应路径规划。然后建立一条轨迹，避开动态障碍物（这些障碍物不出现在地图上，但可以借助传感器数据进行检测），同时遵循全局计划。

他还负责产生轮子的命令，使机器人遵循轨迹。

目前有两个可用：

dwb_controller:通常用于差速驱动机器人

TEB Controller:用于车型机器人

在节点启动中需要指出的字段有：

nav2_controller提供控制器

可执行文件名为controller_server

它需要的参数节点包括yaml文件

            package='nav2_controller',
            executable='controller_server',
            output='screen',
            parameters=[controller_yaml])
            

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] 
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

3、启动navigation coordinator

在上述两节点之外，接下来是协调节点的节点。该节点调用path planner和controller，是bt_navigator

以下是需要在节点启动中指出的字段

            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml])
            

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    default_nav_to_pose_bt_xml: "/home/user/ros2_ws/src/path_planner_server/config/behavior.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node

重要提示：bt_navigator被一个XML文件所包含的行为树里的行为所定义。该行为树包括何时调用路径规划器、何时调用控制器以及何时激活恢复行为。本节课程会有一个例子

更多的详细配置在Advanced Nav2中可以获得（博主之后可能会做该课程的学习笔记）

<!--
  This Behavior Tree replans the global path periodically at 1 Hz, and has
  recovery actions. Obtained from the official Nav2 package
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <SequenceStar name="RecoveryActions">
        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="5"/>
      </SequenceStar>
    </RecoveryNode>
  </BehaviorTree>
</root>

4、启动recoveries_server

更新：目前nav2包中已不存在recoveries的包和节点，包名应更改为nav2_behavior,可执行文件应更改为behavior_server。

如果机器人无法到达所提供的有效路径或者卡在中间怎么办。

这种情况需要使用recovery behaviors。这种简单的、预先定义的动作通常通过行为树来清除情况。recoveries_server是负责执行回复行为的节点。例如，当bt_navigator认为机器人被卡住时，它将执行recoveries_server。

            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            parameters=[recovery_yaml],
            output='screen'),

目前有三种可用恢复行为：

自旋、备份（执行给定距离的线性平移）、等待（静止）

在配置文件中，可以指明要加载哪些行为，以便bt_navigator可以调用这些行为。

一个典型例子

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

练习4.1

（a）在工作空间创建一个名为path_planner_server的新包

（b）在包的路径中创建启动和配置目录

（c）启动文件pathplanner.launch.py，所有的路径规划器节点在其中正确启动

（d）config中创建YAML文件

（e）修改setup.py

（f）启动上节课的定位系统

（g）打开RVIZ

此外，加载localization_rviz_config文件，添加路径显示并配置为/plan主题

使用线条样式展示path路径。billboards展示并且线宽为0.05

在RVIZ配置保存在src目录并名为pathplanning_rviz_config

（h）用RVIZ提供机器人起始点

（i）启动path planner

注释

需要包含nav2_lifecycle_manager的启动，该启动需要包含所有的路径规划器节点，此生命周期管理器的名称必须与在定位中启动的名称不同。

通过RVIZ发送导航目标

一切都运行后，试着给机器人一个导航目标

（j）单机RVIZ中的Navigation2_goal按钮并且电机地图上的任何一点

你应该看到一个绿色的线，只是机器人从当前位置到目标的路径。你还有个看到机器人朝着目标前进。

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    controller_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'controller.yaml')
    bt_navigator_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'bt_navigator.yaml')
    planner_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'planner_server.yaml')
    recovery_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'recovery.yaml')

    
    return LaunchDescription([     
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[controller_yaml]),

        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_yaml]),
            
        Node(
            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            parameters=[recovery_yaml],
            output='screen'),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml]),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_pathplanner',
            output='screen',
            parameters=[{'autostart': True},
                        {'node_names': ['planner_server',
                                        'controller_server',
                                        'recoveries_server',
                                        'bt_navigator']}])
    ])

ROS2中路径规划的参数

1、planners的参数

1.1 planner_server参数

expected_planner_frequency:double 20计算路径的频率

planner_plugins:vector,GridBased 参数和处理请求的映射插件名称列表。此列表中定义的插件名称空间都需要有一个插件参数，定义要加载到名称空间中的插件类型，具体方式如下：

    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"

1.2 The Nav2Fn_Planner

Nav2Fn是Nav2 Planner Server的插件。它可以使用A*或Dijkstra算法来查找两点之间的路径。

在Dijkstra模式下保证任何条件下找到最短路径。

A*模式下不能保证最短路径，它使用启发式方法将潜在领域扩展到目标，从而更快地找到可能的路径。

所有的nav2Fn_planner参数

GridBased.tolerance:double 0.5目标姿势和终点路径之间的容限

GridBased.use_astar:bool,false用A*还是Dijkstra算法

GridBased.allow_unknown:bool,true：是否允许在未知空间进行规划。

2、controllers的参数

2.1 controller server 参数

controller_frequency double 20.0hz 运行控制器频率

min_x_velocity_threshold double 0.01m/s 控制器最小X速度，低于此值被认为是0

min_y_velocity_threshold double 0.0m/s 控制器最小Y速度，低于此值被认为是0

min_theta_velocity_threshold double 0.1 rad/s 控制器最小角速度，低于此值被认为是0

failure tolerance double 0.5m 在followpath操作失败之前，被调用的控制器插件可fail的最长持续时间。如果是-1就是无限，0将禁用，任何正直都是适当的超时。

progress_checker_plugin string 'progress_checker' 用于检查机器人进度的进度检查器插件名称

goal_checker_plugins string ['general_goal_checker'] 用于检查是否达到目标的目标检查其插件的映射名称列表

controller_plugins vector ['followpath'] 用于处理请求喝参数的控制器插件的映射名称列表

2.2 DWB_Controller

最重要的参数是不同轴的速度和加速度的限制，它们控制机器人移动的速度

xy_goal_tolerance double 0.25m 达到目标位置时可以接受多少误差

2.3 progress checker

检查机器人是否卡住或在完成目标方面取得了进展

required_movement_radius double 0.5m 机器人必须移动以达到目标的最小量

movement_time_allowance double 10.0s 机器人必须移动最小半径的最长时间

2.4 goal checker

检查机器人是否达到目标姿势

xy_goal_tolerance double 0.25m 满足目标完成标准的公差 米

yaw_goal_tolerance double 0.25rad 同上，角度

3、recovery 参数

3.1 Recovery server

costmap_topic string local_costmap/costmap_raw 用于冲突检查的原始成本图主题

footprint_topic string local_costmap/published_footprint 成本图中的足迹主题

cycle_frequency double 10.0 运行恢复插件的帧率

transform_timeout double 0.1 TF 变换公差

global_frame string odom 参考框架

robot_base_frame string base_link 机器人底座

recovery_plugins vector[string] {"spin","back_up","wait"} 要使用的插件名称列表，也与操作服务器名称匹配

3.2 BT navigator

是一种基于行为树的导航实现，旨在允许导航任务的灵活性，并提供一种轻松指定复杂机器人行为（包括恢复）的方法

default_nav_to_pose_bt_xml string 指示包含用于导航到姿势的行为的文件的完整路径

default_nav_through_poses_bt_xml string 指示包含用于浏览姿势的行为的文件的完整路径

bt_loop_duration double 10.0 BT执行的每次迭代的持续时间 毫秒

robot_base_frame string "base_link" 机器人底座

global_frame "map" 全局参考框架

odom_topic string "odom" 发布里程计的主题

plugin_lib_names vector none 行为树节点共享库的列表

练习4.2

改变planner和controller的参数，观察机器人行为的变化。可以改变的参数包括

1. min_vel_x

2. max_vel_theta

3. use_astar

4. all goal tolerances

通过命令行发送导航目标，可以使用服务或者主题，两者非常相似，可以在需要对结果进行反馈时使用。

通过动作服务器

由于导航系统使用名为/anavigation_to_pose的动作服务器从RVIZ接收目标，因此可以使用命令行调用动作服务器并向机器人提供目标。

使用以下命令

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose: {header: {frame_id: map}, pose: {position: {x: 1.52, y: 1.92, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}"

你需要改变position和orientation的值更改为地图上要发送的机器人位置（方向需要四元数）。可以用RVIZ来标识位置的pose值

练习4.3

在本练习中，学习如何使用topic pub从终端向机器人提供导航目标：

1. 通过Publish Point从RVIZ获取点的坐标。单机Publish Point然后单击地图上的位置。该地图的坐标将显示在RVIZ终端中。

2. 用ros2 topic pub来将这些坐标发布到/goal_pose主题

3. 机器人应该开始向那个点移动

ros2 topic pub -1 /goal_pose geometry_msgs/PoseStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {position: {x: 2.2, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}"

练习4.4

在本练习中，学习如何使用动作客户端向导航动作服务器以编程的方式提供导航目标。

1. 在pathplanner_server包中，创建一个名为nav_to_pose_action_client.py的新python文件

2. 此文件将包含ROS2程序，因此请确保其放在正确位置，并在setup.py中添加一个条目以启动它

3. 你的python代码应该做两件事

        A. 订阅主题/clicked_point。您将使用该主题为机器人提供目标。主题数据是地图（x，y，z）坐标中的一个点。这些是你必须将机器人发送到的坐标。

        B. 编写在上一主题中发布新点时激活的动作客户端。然后使用（x,y,z）值创建导航目标并将其发送到导航操作服务器

4. 创建启动文件以启动程序

5. 编译并安装

6. 启动程序，然后转到RVIZ并使用发布点功能为程序提供位置。

import rclpy
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from rclpy.node import Node
from geometry_msgs.msg import PointStamped

class NavToPoseActionClient(Node):

    def __init__(self):
        super().__init__('Nav_To_Pose_Action_Client')
        self._action_client = ActionClient(self, NavigateToPose, '/navigate_to_pose')
        self.subscriber_ = self.create_subscription(PointStamped, 'clicked_point', self.callback, 1)
        self.subscriber_  # prevent unused variable warning

    def callback(self, msg):
        self.get_logger().info('Recieved Data:\n X : %f \n Y : %f \n Z : %f' % (msg.point.x, msg.point.y, msg.point.z))
        self.send_goal (msg.point.x, msg.point.y, 0.0)

    def send_goal(self, x ,y, theta):
        self.get_logger().info('sending goal to action server')
        goal_pose = NavigateToPose.Goal()
        goal_pose.pose.header.frame_id = 'map'
        goal_pose.pose.pose.position.x = x
        goal_pose.pose.pose.position.y = y
        goal_pose.pose.pose.position.z = theta

        self.get_logger().info('waiting for action server')
        self._action_client.wait_for_server()
        self.get_logger().info('action server detected')

        self._send_goal_future = self._action_client.send_goal_async(
            goal_pose,
            feedback_callback=self.feedback_callback)
        self.get_logger().info('goal sent')

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return
        self.get_logger().info('Goal accepted :)')
        self._get_result_future = goal_handle.get_result_async()

        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info('Result: {0}' + str(result))
        rclpy.shutdown()

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info('FEEDBACK:' + str(feedback) )

def main(args=None):
    rclpy.init(args=args)

    action_client = NavToPoseActionClient()

    rclpy.spin(action_client)


if __name__ == '__main__':
    main()