第四章 如何在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()