机器人 / 系统课时

用 Python 提交多路点导航

用 Python 提交多路点导航

使用 ROS 2 Jazzy 的 BasicNavigator 一次提交多个地图位姿,读取当前路点反馈,并在结束时核对哪些路点真正完成。示例还会配置遇到不可达路点时继续执行后续目标,避免把整批 Action 成功误认为每个点都已到达。

学习目标

  • 使用 PoseStamped 和 followWaypoints() 提交有序路点。
  • 读取 current_waypoint、TaskResult 和未完成路点清单。
  • 配置跳过失败路点,并在 RViz 中定位不可达原因。

核对导航环境

示例适用于 ROS 2 Jazzy、已启动的 Nav2、AMCL 定位与仿真时钟。确保地图已加载、机器人初始位置可确定、/clock 在发布;运行脚本的终端也应加载 ROS 2 环境。检查路点 Action 服务:

bash
source /opt/ros/jazzy/setup.bash
ros2 action list -t | grep '/follow_waypoints'

预期输出(无命名空间时):

text
/follow_waypoints [nav2_msgs/action/FollowWaypoints]

followWaypoints() 使用 Waypoint Follower 按顺序调用 NavigateToPose,并在到达每点后执行已配置的路点任务插件;它与只负责经过多个位姿的 goThroughPoses() 不完全相同。下文的坐标仅作示例,必须按自己的 map 地图选取自由区域。

配置不可达路点的处理方式

Waypoint Follower 的 stop_on_failure 在 Jazzy 中默认为 true:一个路点失败,整批任务终止。巡检允许漏检并继续时,可以在节点已激活后改为 false,后续路点将继续尝试。

bash
ros2 param set /waypoint_follower stop_on_failure false
ros2 param get /waypoint_follower stop_on_failure

预期输出:

text
Set parameter successful
Boolean value is: False

多路点执行及失败跳过分支

用 BasicNavigator 提交三个路点

将以下完整程序保存为 multi_waypoints.py。map_pose() 设置地图坐标和朝向,朝向由弧度转为四元数;waitUntilNav2Active() 等待 AMCL 与导航器进入可用状态。仿真节点使用 use_sim_time=True,避免时间戳与 TF 时间基准不一致。

python
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 自身日志省略,路点反馈显示次数可能随执行速度变化):

text
正在处理路点 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_link TF、/scan 与地图障碍层,不要继续盲目跳过。
  • 取消或失败:TaskResult.CANCELED 表示任务被取消,FAILED 表示 Action 中止;两者都不能算完整路线成功。

验收时同时看 RViz 中机器人是否依次经过可达点,以及控制台的失败清单是否为空。对于必须全部完成的任务,恢复 stop_on_failure=true 并把漏点作为任务失败处理。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 Python 提交多路点导航

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通