机器人 / 系统课时
用 Python 提交多路点导航
用 Python 提交多路点导航
使用 ROS 2 Jazzy 的 BasicNavigator 一次提交多个地图位姿,读取当前路点反馈,并在结束时核对哪些路点真正完成。示例还会配置遇到不可达路点时继续执行后续目标,避免把整批 Action 成功误认为每个点都已到达。
学习目标
- 使用
PoseStamped和followWaypoints()提交有序路点。 - 读取
current_waypoint、TaskResult和未完成路点清单。 - 配置跳过失败路点,并在 RViz 中定位不可达原因。
核对导航环境
示例适用于 ROS 2 Jazzy、已启动的 Nav2、AMCL 定位与仿真时钟。确保地图已加载、机器人初始位置可确定、/clock 在发布;运行脚本的终端也应加载 ROS 2 环境。检查路点 Action 服务:
source /opt/ros/jazzy/setup.bash
ros2 action list -t | grep '/follow_waypoints'预期输出(无命名空间时):
/follow_waypoints [nav2_msgs/action/FollowWaypoints]followWaypoints() 使用 Waypoint Follower 按顺序调用 NavigateToPose,并在到达每点后执行已配置的路点任务插件;它与只负责经过多个位姿的 goThroughPoses() 不完全相同。下文的坐标仅作示例,必须按自己的 map 地图选取自由区域。
配置不可达路点的处理方式
Waypoint Follower 的 stop_on_failure 在 Jazzy 中默认为 true:一个路点失败,整批任务终止。巡检允许漏检并继续时,可以在节点已激活后改为 false,后续路点将继续尝试。
ros2 param set /waypoint_follower stop_on_failure false
ros2 param get /waypoint_follower stop_on_failure预期输出:
Set parameter successful
Boolean value is: False
用 BasicNavigator 提交三个路点
将以下完整程序保存为 multi_waypoints.py。map_pose() 设置地图坐标和朝向,朝向由弧度转为四元数;waitUntilNav2Active() 等待 AMCL 与导航器进入可用状态。仿真节点使用 use_sim_time=True,避免时间戳与 TF 时间基准不一致。
import math
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
from rclpy.parameter import Parameter
def map_pose(nav, x, y, yaw):
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.header.stamp = nav.get_clock().now().to_msg()
pose.pose.position.x = float(x)
pose.pose.position.y = float(y)
pose.pose.orientation.z = math.sin(yaw / 2.0)
pose.pose.orientation.w = math.cos(yaw / 2.0)
return pose
def main():
rclpy.init()
nav = BasicNavigator()
nav.set_parameters([Parameter('use_sim_time', value=True)])
try:
# 初始位姿和目标点都应按实际地图调整到自由区域
nav.setInitialPose(map_pose(nav, 0.0, 0.0, 0.0))
nav.waitUntilNav2Active()
goals = [
map_pose(nav, 0.8, 0.0, 0.0),
map_pose(nav, 1.6, 0.0, 1.57),
map_pose(nav, 1.6, 1.0, 3.14),
]
if not nav.followWaypoints(goals):
print('路点请求被拒绝')
return
last_index = -1
while not nav.isTaskComplete():
feedback = nav.getFeedback()
if feedback and feedback.current_waypoint != last_index:
last_index = feedback.current_waypoint
print(f'正在处理路点 {last_index + 1}/{len(goals)}')
status = nav.getResult()
# Jazzy 的 Action 原始结果可区分整批结束与个别漏点
raw_result = nav.result_future.result().result
missed = [(item.index + 1, item.error_code)
for item in raw_result.missed_waypoints]
print(f'任务状态: {status.name}')
print(f'未完成路点(编号,错误码): {missed}')
if status == TaskResult.SUCCEEDED and missed:
print('存在漏点,整条巡检路线尚未完全验收')
elif status != TaskResult.SUCCEEDED:
print('整批任务未正常结束,请检查导航服务器日志')
finally:
nav.destroyNode()
rclpy.shutdown()
if __name__ == '__main__':
main()保存后执行 python3 multi_waypoints.py。预期输出(三个目标均可达;Nav2 自身日志省略,路点反馈显示次数可能随执行速度变化):
正在处理路点 1/3
正在处理路点 2/3
正在处理路点 3/3
任务状态: SUCCEEDED
未完成路点(编号,错误码): []验证漏点与排查失败
在仿真地图中把第二个目标移入明确的占据栅格或障碍膨胀区,再运行一次。配置为 stop_on_failure=false 时,Waypoint Follower 会尝试第三点;最终可能仍返回 SUCCEEDED,但 missed_waypoints 中应有索引 1,脚本显示为第 2 个路点。错误码由实际失败原因决定,不要硬编码判断某一个数字就是“不可达”。
- 只有第二点失败:检查该点在 RViz 的 Global Costmap 中是否位于致命障碍、膨胀高代价区域或地图外;将目标移到可通行区域后重新执行。
- 多个点同时失败:先检查 AMCL 定位、
map→odom→base_linkTF、/scan与地图障碍层,不要继续盲目跳过。 - 取消或失败:
TaskResult.CANCELED表示任务被取消,FAILED表示 Action 中止;两者都不能算完整路线成功。
验收时同时看 RViz 中机器人是否依次经过可达点,以及控制台的失败清单是否为空。对于必须全部完成的任务,恢复 stop_on_failure=true 并把漏点作为任务失败处理。