跳到主要内容

建图与导航任务创建

本文以完整工作流为例，演示如何通过 GR-Navigation 的 ROS 2 接口创建地图、保存 GRW 地图归档，再加载该归档、设置初始位姿，并将机器人导航到指定目标点。

第一部分构建 mapping_task.py，用于准备和保存地图；第二部分构建 navigation_task.py，将第一部分保存的归档作为输入执行单点导航。

本文按同一个 navigation_task.py 脚本逐步构建：每个阶段都明确新增的完整方法及其所属类，最后的完整示例由前述代码直接组合而成。

涉及接口： /slam/set_mode、/clear_map、/map、/save_composite_map、/slam/load_map、navigate_to_pose、/cancel_current_action、/get_current_action

前置条件​

  1. 机器人导航服务已启动，开发环境与机器人使用相同的 ROS 2 / DDS domain。
  2. 已安装 ROS 2 Humble 的 rclpy、fourier_msgs、fourier_msgs_pod、map_manager、nav2_msgs 和 geometry_msgs。
  3. 执行第二部分前，已通过第一部分或 /save_composite_map 保存 GRW 地图归档；地图路径必须指向归档目录本身。
  4. 请在安全、空旷的区域测试，且初始位姿与目标点均位于地图可通行区域。

环境准备​

每个新终端先初始化机器人环境（详见 环境准备），并把机器人命名空间写入 ROBOT_NAMESPACE：

Bash
source /opt/fftai/fourierdds/shells/discovery_env.sh --ros2
export ROS_DOMAIN_ID=123
export ROBOT_NAMESPACE=/GRW01AA0001  # 替换为实际机器人命名空间

本文两个脚本内部均使用相对接口名，运行命令通过 --ros-args -r __ns:=$ROBOT_NAMESPACE 指向目标机器人；命令行示例则直接使用 $ROBOT_NAMESPACE 前缀。

第一部分：创建建图任务​

导航所需地图必须先由 GRW 建图并保存为可加载归档。当前公开接口支持切换建图模式、清空运行时地图、观察栅格地图和保存归档，但不提供自动探索或自动覆盖环境的接口。因此，程序只负责准备和保存地图，环境覆盖必须由操作者在安全区域手动完成。

建图任务概览​

阶段操作对脚本的改动
1连接接口创建模式切换、地图保存客户端和清图发布器。
2进入建图模式加入 switch_to_mapping()。
3创建空地图可选加入 clear_map()。
4人工覆盖环境观察 /map，不由脚本自动驾驶。
5保存归档加入 save_map_archive()。
6运行任务按 start 或 save 阶段执行。

建图阶段 1：连接机器人​

新建 mapping_task.py，先写入完整导入、节点类和初始化方法。这里的三个端点均是当前 GRW 已公开接口。

Python
#!/usr/bin/env python3
import argparse

import rclpy
from fourier_msgs.srv import SetMode
from fourier_msgs_pod.msg import ShortString
from map_manager.srv import SaveCompositeMap
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
from rclpy.node import Node


class MappingTask(Node):
    def __init__(self):
        super().__init__("grw_mapping_task")
        self.mode_client = self.create_client(SetMode, "slam/set_mode")
        self.save_client = self.create_client(
            SaveCompositeMap, "save_composite_map")
        # /clear_map 的订阅端为 BEST_EFFORT + TRANSIENT_LOCAL，
        # 发布端必须使用相同 QoS，否则清图命令不会被处理。
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.clear_map_publisher = self.create_publisher(
            ShortString, "clear_map", qos)

后续建图阶段的代码都应以四个空格缩进，添加到 MappingTask 类中。

建图阶段 2：进入建图模式​

向 MappingTask 类追加完整的模式切换方法。仅在服务响应的 success 为真时才继续。

Python
    def switch_to_mapping(self) -> bool:
        if not self.mode_client.wait_for_service(timeout_sec=10.0):
            self.get_logger().error("slam/set_mode service is unavailable")
            return False

        request = SetMode.Request()
        request.mode = "mapping"
        future = self.mode_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        response = future.result()
        if response is None or not response.success:
            message = "no response" if response is None else response.message
            self.get_logger().error(f"failed to enter mapping mode: {message}")
            return False

        self.get_logger().info("mapping mode enabled")
        return True

建图阶段 3：创建空地图​

需要从空地图重新开始时，向类中追加以下方法并在启动任务时调用。消息内容不参与 GRW 解析；该调用只清除运行时地图，不删除已保存归档。

Python
    def clear_map(self):
        # 等待 DDS 发布器与订阅端完成发现，再发布一次清图命令。
        rclpy.spin_once(self, timeout_sec=1.0)
        self.clear_map_publisher.publish(ShortString())
        # 再等待一拍，确保命令发出后进程退出前完成 DDS 传输
        rclpy.spin_once(self, timeout_sec=1.0)
        self.get_logger().info("clear-map command sent")

已有运行时地图需要继续扩展时，不使用 --clear 即可保留当前地图。

建图阶段 4：人工覆盖环境​

在建图模式下，操作者应通过已有的手动控制方式，以低速覆盖门口、走廊、转角等需要导航经过的区域。当前公开导航接口没有自动探索功能，本实践不发送任何自动驾驶或速度指令。

另开终端观察二维栅格地图是否持续更新：

Bash
ros2 topic echo $ROBOT_NAMESPACE/map

/map 的坐标系为 map；栅格数据中 -1、0、100 分别表示未知、空闲和占用。覆盖完成前不要保存地图。

建图阶段 5：保存地图归档​

向类中追加以下方法。它使用 /save_composite_map 保存完整 GRW 地图归档；该目录将作为第二部分导航任务的 --map-path 输入。目标目录已存在时会被新归档覆盖。

Python
    def save_map_archive(self, map_path: str) -> bool:
        if not self.save_client.wait_for_service(timeout_sec=10.0):
            self.get_logger().error("save_composite_map service is unavailable")
            return False

        request = SaveCompositeMap.Request()
        request.map_path = map_path
        request.compress = False
        future = self.save_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        response = future.result()
        if response is None or not response.success:
            message = "no response" if response is None else response.message
            self.get_logger().error(f"map save failed: {message}")
            return False

        self.get_logger().info(f"map archive saved to: {response.saved_path}")
        return True

不要使用 map_saver_cli 生成的 map.pgm、map.yaml 替代该归档：它们是标准二维地图导出文件，不能作为 GRW 的 /slam/load_map 输入。

建图阶段 6：运行任务​

在类定义结束后追加入口。首次使用 --phase start --clear 进入空白建图模式；人工完成环境覆盖后，第二次使用 --phase save 保存归档。

Python
def parse_mapping_args():
    parser = argparse.ArgumentParser(description="Create or save one GRW map")
    parser.add_argument("--map-path", default="/data/maps/office",
                        help="GRW map archive directory to create or save")
    parser.add_argument("--phase", choices=("start", "save"), required=True,
                        help="start: enter mapping mode; save: save the current map")
    parser.add_argument("--clear", action="store_true",
                        help="clear the current runtime map before mapping")
    # 透传 --ros-args（如 -r __ns:=）给 rclpy.init 解析命名空间
    args, _ = parser.parse_known_args()
    return args


def main():
    args = parse_mapping_args()
    rclpy.init()
    node = MappingTask()
    try:
        if args.phase == "start":
            if not node.switch_to_mapping():
                return 1
            if args.clear:
                node.clear_map()
            node.get_logger().info(
                "manually drive the robot to cover the environment, then run --phase save")
            return 0
        return 0 if node.save_map_archive(args.map_path) else 1
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    raise SystemExit(main())

完整建图示例​

以下是将建图阶段 1 至阶段 6 原样组合后的完整 mapping_task.py：

Python
#!/usr/bin/env python3
import argparse

import rclpy
from fourier_msgs.srv import SetMode
from fourier_msgs_pod.msg import ShortString
from map_manager.srv import SaveCompositeMap
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
from rclpy.node import Node


class MappingTask(Node):
    def __init__(self):
        super().__init__("grw_mapping_task")
        self.mode_client = self.create_client(SetMode, "slam/set_mode")
        self.save_client = self.create_client(
            SaveCompositeMap, "save_composite_map")
        # /clear_map 的订阅端为 BEST_EFFORT + TRANSIENT_LOCAL，
        # 发布端必须使用相同 QoS，否则清图命令不会被处理。
        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.clear_map_publisher = self.create_publisher(
            ShortString, "clear_map", qos)

    def switch_to_mapping(self) -> bool:
        if not self.mode_client.wait_for_service(timeout_sec=10.0):
            self.get_logger().error("slam/set_mode service is unavailable")
            return False
        request = SetMode.Request()
        request.mode = "mapping"
        future = self.mode_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        response = future.result()
        if response is None or not response.success:
            message = "no response" if response is None else response.message
            self.get_logger().error(f"failed to enter mapping mode: {message}")
            return False
        self.get_logger().info("mapping mode enabled")
        return True

    def clear_map(self):
        rclpy.spin_once(self, timeout_sec=1.0)
        self.clear_map_publisher.publish(ShortString())
        # 再等待一拍，确保命令发出后进程退出前完成 DDS 传输
        rclpy.spin_once(self, timeout_sec=1.0)
        self.get_logger().info("clear-map command sent")

    def save_map_archive(self, map_path: str) -> bool:
        if not self.save_client.wait_for_service(timeout_sec=10.0):
            self.get_logger().error("save_composite_map service is unavailable")
            return False
        request = SaveCompositeMap.Request()
        request.map_path = map_path
        request.compress = False
        future = self.save_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        response = future.result()
        if response is None or not response.success:
            message = "no response" if response is None else response.message
            self.get_logger().error(f"map save failed: {message}")
            return False
        self.get_logger().info(f"map archive saved to: {response.saved_path}")
        return True


def parse_mapping_args():
    parser = argparse.ArgumentParser(description="Create or save one GRW map")
    parser.add_argument("--map-path", default="/data/maps/office",
                        help="GRW map archive directory to create or save")
    parser.add_argument("--phase", choices=("start", "save"), required=True,
                        help="start: enter mapping mode; save: save the current map")
    parser.add_argument("--clear", action="store_true",
                        help="clear the current runtime map before mapping")
    # 透传 --ros-args（如 -r __ns:=）给 rclpy.init 解析命名空间
    args, _ = parser.parse_known_args()
    return args


def main():
    args = parse_mapping_args()
    rclpy.init()
    node = MappingTask()
    try:
        if args.phase == "start":
            if not node.switch_to_mapping():
                return 1
            if args.clear:
                node.clear_map()
            node.get_logger().info(
                "manually drive the robot to cover the environment, then run --phase save")
            return 0
        return 0 if node.save_map_archive(args.map_path) else 1
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    raise SystemExit(main())

启动新的空白建图任务：

Bash
python3 mapping_task.py --phase start --clear --map-path /data/maps/office \
  --ros-args -r __ns:=$ROBOT_NAMESPACE

人工完成环境覆盖后保存归档：

Bash
python3 mapping_task.py --phase save --map-path /data/maps/office \
  --ros-args -r __ns:=$ROBOT_NAMESPACE

第二部分：创建单点导航任务​

导航任务概览​

阶段操作对脚本的改动
1连接接口创建节点、地图加载服务客户端和导航 Action 客户端。
2加载地图并定位向类中加入 load_map()。
3创建导航任务加入 send_goal() 和目标接受回调。
4监控执行加入反馈和结果回调。
5取消任务加入 Action 取消请求与确认回调。
6运行任务加入命令行参数和 main()。

常量定义​

请使用实际地图归档和安全坐标替换以下值。角度单位为弧度。

Python
DEFAULT_MAP_PATH = "/data/maps/office"
DEFAULT_INITIAL_X, DEFAULT_INITIAL_Y, DEFAULT_INITIAL_YAW = 1.0, 2.0, 0.0
DEFAULT_TARGET_X, DEFAULT_TARGET_Y, DEFAULT_TARGET_YAW = 3.0, 2.0, 0.0

阶段 1：连接机器人​

新建 navigation_task.py，先写入完整导入、节点类和初始化方法。这里创建的 self.load_map_client 与 self.navigation_client 供后续阶段的方法使用；self.result 用于让主循环等待 Action 终态。

Python
#!/usr/bin/env python3
import argparse
import math

import rclpy
from action_msgs.msg import GoalStatus
from fourier_msgs.srv import LoadMap
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from rclpy.node import Node


class NavigationTask(Node):
    def __init__(self, cancel_after: float):
        super().__init__("grw_navigation_task")
        self.load_map_client = self.create_client(LoadMap, "slam/load_map")
        self.navigation_client = ActionClient(
            self, NavigateToPose, "navigate_to_pose")
        self.cancel_after = cancel_after
        self.goal_handle = None
        self.result = None
        self.cancel_timer = None

后续阶段的代码都应以四个空格缩进，添加到 NavigationTask 类中。

阶段 2：加载地图并设置初始位姿​

在 NavigationTask 类中追加以下完整的 load_map() 方法。它等待服务、加载完整 GRW 地图归档，并同时通过请求中的 x、y、yaw 设置初始位姿。仅在 result == 0 时返回成功；z 是兼容字段，GRW 不使用它。

Python
    def load_map(self, map_path: str, x: float, y: float, yaw: float) -> bool:
        if not self.load_map_client.wait_for_service(timeout_sec=10.0):
            self.get_logger().error("slam/load_map service is unavailable")
            return False

        request = LoadMap.Request()
        request.map_path = map_path
        request.x = x
        request.y = y
        request.z = 0.0
        request.yaw = yaw
        future = self.load_map_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        response = future.result()
        if response is None or response.result != 0:
            code = None if response is None else response.result
            self.get_logger().error(f"map load failed: result={code}")
            return False

        self.get_logger().info("map loaded and initial pose set")
        return True

map_path 必须是由 save_composite_map 保存的归档目录，而不是其中的 map.yaml、map.pgm 或其他单个文件。

阶段 3：创建导航任务​

继续向 NavigationTask 类追加以下两个方法。目标使用 map 坐标系；yaw 通过 sin(yaw / 2)、cos(yaw / 2) 转为四元数 z、w；behavior_tree 必须留空。

GRW 一次只接受一个未结束的导航目标。已有目标执行中时，新目标会被拒绝；应先等待旧目标终态，或先取消旧目标。

Python
    def send_goal(self, x: float, y: float, yaw: float):
        if not self.navigation_client.wait_for_server(timeout_sec=10.0):
            raise RuntimeError("navigate_to_pose action server is unavailable")

        goal = NavigateToPose.Goal()
        goal.pose = PoseStamped()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = x
        goal.pose.pose.position.y = y
        goal.pose.pose.position.z = 0.0
        goal.pose.pose.orientation.z = math.sin(yaw / 2.0)
        goal.pose.pose.orientation.w = math.cos(yaw / 2.0)
        goal.behavior_tree = ""

        future = self.navigation_client.send_goal_async(
            goal, feedback_callback=self.on_feedback)
        future.add_done_callback(self.on_goal_response)

    def on_goal_response(self, future):
        self.goal_handle = future.result()
        if self.goal_handle is None:
            self.get_logger().error("no goal response from the action server")
            self.result = "UNKNOWN"
            return
        if not self.goal_handle.accepted:
            self.get_logger().error("navigation goal was rejected")
            self.result = "REJECTED"
            return

        self.get_logger().info("navigation goal accepted")
        if self.cancel_after > 0.0:
            self.cancel_timer = self.create_timer(
                self.cancel_after, self.cancel_navigation)
        self.goal_handle.get_result_async().add_done_callback(self.on_result)

阶段 4：监控导航任务​

向同一个 NavigationTask 类追加反馈和结果回调。当前 GRW feedback 仅可靠填充 current_pose.pose.position.x 与 y，不能用于推导剩余距离、预计到达时间、完整朝向或恢复次数。导航最终状态必须以 Action 终态为准。

Python
    def on_feedback(self, feedback_message):
        position = feedback_message.feedback.current_pose.pose.position
        self.get_logger().info(
            f"feedback position: x={position.x:.2f}, y={position.y:.2f}")

    def on_result(self, future):
        wrapped = future.result()
        states = {
            GoalStatus.STATUS_SUCCEEDED: "SUCCEEDED",
            GoalStatus.STATUS_CANCELED: "CANCELED",
            GoalStatus.STATUS_ABORTED: "ABORTED",
        }
        self.result = states.get(wrapped.status, "UNKNOWN")
        self.get_logger().info(f"navigation {self.result}")

SUCCEEDED 表示完成，CANCELED 表示已取消，ABORTED 表示中止或失败。终态状态码的编号说明见 ros_api.md 第 4.1 节。

阶段 5：取消导航任务​

向类中追加以下取消方法。它由后续 --cancel-after 参数定时调用，并使用当前 Action goal handle 发送取消请求。取消请求获得确认后，仍应等待阶段 4 的 on_result() 收到终态。

Python
    def cancel_navigation(self):
        if self.cancel_timer is not None:
            self.cancel_timer.cancel()
        if self.goal_handle is None:
            return

        self.get_logger().warn("requesting navigation cancellation")
        future = self.goal_handle.cancel_goal_async()
        future.add_done_callback(self.on_cancel_response)

    def on_cancel_response(self, future):
        response = future.result()
        self.get_logger().info(
            f"cancel request accepted for {len(response.goals_canceling)} goal(s)")

没有持有 Action goal handle 的管理端可调用 cancel_current_action 请求导航模块取消当前动作；该服务成功仅表示取消请求被接受，最终状态仍应等待 Action 结果确认。导航期间发布非零手动速度也会取消当前导航任务。

阶段 6：运行任务​

在类定义结束后追加参数解析和入口函数。主循环在 self.result 尚未被 Action 回调填充时持续处理 ROS 2 回调；仅在导航成功时返回 0。

Python
def parse_args():
    parser = argparse.ArgumentParser(description="Create one GRW navigation task")
    parser.add_argument("--map-path", default=DEFAULT_MAP_PATH,
                        help="GRW map archive directory produced by mapping_task.py")
    parser.add_argument("--initial-x", type=float, default=DEFAULT_INITIAL_X,
                        help="robot initial x position in the map frame, in meters")
    parser.add_argument("--initial-y", type=float, default=DEFAULT_INITIAL_Y,
                        help="robot initial y position in the map frame, in meters")
    parser.add_argument("--initial-yaw", type=float, default=DEFAULT_INITIAL_YAW,
                        help="robot initial yaw in the map frame, in radians")
    parser.add_argument("--target-x", type=float, default=DEFAULT_TARGET_X,
                        help="navigation target x position in the map frame, in meters")
    parser.add_argument("--target-y", type=float, default=DEFAULT_TARGET_Y,
                        help="navigation target y position in the map frame, in meters")
    parser.add_argument("--target-yaw", type=float, default=DEFAULT_TARGET_YAW,
                        help="navigation target yaw in the map frame, in radians")
    parser.add_argument("--cancel-after", type=float, default=0.0,
                        help="seconds before canceling an accepted goal; 0 waits")
    # 透传 --ros-args（如 -r __ns:=）给 rclpy.init 解析命名空间
    args, _ = parser.parse_known_args()
    return args


def main():
    args = parse_args()
    rclpy.init()
    node = NavigationTask(args.cancel_after)
    try:
        if not node.load_map(args.map_path, args.initial_x,
                             args.initial_y, args.initial_yaw):
            return 1
        node.send_goal(args.target_x, args.target_y, args.target_yaw)
        while rclpy.ok() and node.result is None:
            rclpy.spin_once(node, timeout_sec=0.1)
        return 0 if node.result == "SUCCEEDED" else 1
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    raise SystemExit(main())

完整导航示例​

以下是将阶段 1 至阶段 6 原样组合后的完整 navigation_task.py：

Python
#!/usr/bin/env python3
import argparse
import math

import rclpy
from action_msgs.msg import GoalStatus
from fourier_msgs.srv import LoadMap
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from rclpy.node import Node

DEFAULT_MAP_PATH = "/data/maps/office"
DEFAULT_INITIAL_X, DEFAULT_INITIAL_Y, DEFAULT_INITIAL_YAW = 1.0, 2.0, 0.0
DEFAULT_TARGET_X, DEFAULT_TARGET_Y, DEFAULT_TARGET_YAW = 3.0, 2.0, 0.0


class NavigationTask(Node):
    def __init__(self, cancel_after: float):
        super().__init__("grw_navigation_task")
        self.load_map_client = self.create_client(LoadMap, "slam/load_map")
        self.navigation_client = ActionClient(self, NavigateToPose, "navigate_to_pose")
        self.cancel_after = cancel_after
        self.goal_handle = None
        self.result = None
        self.cancel_timer = None

    def load_map(self, map_path: str, x: float, y: float, yaw: float) -> bool:
        if not self.load_map_client.wait_for_service(timeout_sec=10.0):
            self.get_logger().error("slam/load_map service is unavailable")
            return False
        request = LoadMap.Request()
        request.map_path = map_path
        request.x, request.y, request.z, request.yaw = x, y, 0.0, yaw
        future = self.load_map_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        response = future.result()
        if response is None or response.result != 0:
            code = None if response is None else response.result
            self.get_logger().error(f"map load failed: result={code}")
            return False
        self.get_logger().info("map loaded and initial pose set")
        return True

    def send_goal(self, x: float, y: float, yaw: float):
        if not self.navigation_client.wait_for_server(timeout_sec=10.0):
            raise RuntimeError("navigate_to_pose action server is unavailable")
        goal = NavigateToPose.Goal()
        goal.pose = PoseStamped()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x, goal.pose.pose.position.y = x, y
        goal.pose.pose.orientation.z = math.sin(yaw / 2.0)
        goal.pose.pose.orientation.w = math.cos(yaw / 2.0)
        goal.behavior_tree = ""
        future = self.navigation_client.send_goal_async(
            goal, feedback_callback=self.on_feedback)
        future.add_done_callback(self.on_goal_response)

    def on_goal_response(self, future):
        self.goal_handle = future.result()
        if self.goal_handle is None:
            self.get_logger().error("no goal response from the action server")
            self.result = "UNKNOWN"
            return
        if not self.goal_handle.accepted:
            self.get_logger().error("navigation goal was rejected")
            self.result = "REJECTED"
            return
        self.get_logger().info("navigation goal accepted")
        if self.cancel_after > 0.0:
            self.cancel_timer = self.create_timer(
                self.cancel_after, self.cancel_navigation)
        self.goal_handle.get_result_async().add_done_callback(self.on_result)

    def on_feedback(self, feedback_message):
        position = feedback_message.feedback.current_pose.pose.position
        self.get_logger().info(
            f"feedback position: x={position.x:.2f}, y={position.y:.2f}")

    def on_result(self, future):
        wrapped = future.result()
        states = {
            GoalStatus.STATUS_SUCCEEDED: "SUCCEEDED",
            GoalStatus.STATUS_CANCELED: "CANCELED",
            GoalStatus.STATUS_ABORTED: "ABORTED",
        }
        self.result = states.get(wrapped.status, "UNKNOWN")
        self.get_logger().info(f"navigation {self.result}")

    def cancel_navigation(self):
        if self.cancel_timer is not None:
            self.cancel_timer.cancel()
        if self.goal_handle is None:
            return
        self.get_logger().warn("requesting navigation cancellation")
        future = self.goal_handle.cancel_goal_async()
        future.add_done_callback(self.on_cancel_response)

    def on_cancel_response(self, future):
        response = future.result()
        self.get_logger().info(
            f"cancel request accepted for {len(response.goals_canceling)} goal(s)")


def parse_args():
    parser = argparse.ArgumentParser(description="Create one GRW navigation task")
    parser.add_argument("--map-path", default=DEFAULT_MAP_PATH,
                        help="GRW map archive directory produced by mapping_task.py")
    parser.add_argument("--initial-x", type=float, default=DEFAULT_INITIAL_X,
                        help="robot initial x position in the map frame, in meters")
    parser.add_argument("--initial-y", type=float, default=DEFAULT_INITIAL_Y,
                        help="robot initial y position in the map frame, in meters")
    parser.add_argument("--initial-yaw", type=float, default=DEFAULT_INITIAL_YAW,
                        help="robot initial yaw in the map frame, in radians")
    parser.add_argument("--target-x", type=float, default=DEFAULT_TARGET_X,
                        help="navigation target x position in the map frame, in meters")
    parser.add_argument("--target-y", type=float, default=DEFAULT_TARGET_Y,
                        help="navigation target y position in the map frame, in meters")
    parser.add_argument("--target-yaw", type=float, default=DEFAULT_TARGET_YAW,
                        help="navigation target yaw in the map frame, in radians")
    parser.add_argument("--cancel-after", type=float, default=0.0,
                        help="seconds before canceling an accepted goal; 0 waits")
    # 透传 --ros-args（如 -r __ns:=）给 rclpy.init 解析命名空间
    args, _ = parser.parse_known_args()
    return args


def main():
    args = parse_args()
    rclpy.init()
    node = NavigationTask(args.cancel_after)
    try:
        if not node.load_map(args.map_path, args.initial_x,
                             args.initial_y, args.initial_yaw):
            return 1
        node.send_goal(args.target_x, args.target_y, args.target_yaw)
        while rclpy.ok() and node.result is None:
            rclpy.spin_once(node, timeout_sec=0.1)
        return 0 if node.result == "SUCCEEDED" else 1
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    raise SystemExit(main())

正常执行：

Bash
python3 navigation_task.py \
  --map-path /data/maps/office \
  --initial-x 1.0 --initial-y 2.0 --initial-yaw 0.0 \
  --target-x 3.0 --target-y 2.0 --target-yaw 0.0 \
  --ros-args -r __ns:=$ROBOT_NAMESPACE

在目标被接受 5 秒后请求取消：

Bash
python3 navigation_task.py \
  --map-path /data/maps/office \
  --target-x 3.0 --target-y 2.0 --cancel-after 5.0 \
  --ros-args -r __ns:=$ROBOT_NAMESPACE

常见问题​

地图加载失败​

确认 --map-path 指向由 save_composite_map 保存的完整 GRW 地图归档目录，而不是其中的 map.yaml、map.pgm 或其他单个文件。详见 load_map。

机器人定位不正确或导航方向异常​

检查 --initial-x、--initial-y、--initial-yaw 是否与机器人实际位置一致；初始位姿和目标均使用 map 坐标系。订阅 /robot_pose 可查看当前定位结果。

导航目标被拒绝​

当前已有导航任务尚未结束时，GRW 会拒绝新目标。等待前一个 Action 的终态，或通过 Action 客户端取消该 goal；也可调用 cancel_current_action 请求取消。使用 get_current_action 可查询当前动作概况。

导航被取消或中止​

先检查 Action 的 CANCELED 或 ABORTED 终态。非零手动速度会使导航让位于手动控制而取消。当前接口不提供底层失败原因、剩余距离或预计时间；请结合 /robot_pose、/plan 和运行日志排查。

推荐参考​