Mapping and navigation task creation

This guide builds a complete GRW workflow: create a map, save a GRW map archive, load that archive with an initial pose, and navigate the robot to a target pose.

Part 1 builds mapping_task.py to prepare and save a map. Part 2 builds navigation_task.py, which takes the archive saved in Part 1 as its input and runs one single-goal navigation task.

Each part builds one script step by step: every stage states the complete new method and the class it belongs to, and the complete example at the end of each part is those pieces combined verbatim.

Interfaces used: /slam/set_mode, /clear_map, /map, /save_composite_map, /slam/load_map, navigate_to_pose, /cancel_current_action, and /get_current_action.

Prerequisites

1. The navigation service is running and the development environment uses the same ROS 2 / DDS domain as the robot.
2. ROS 2 Humble with rclpy, fourier_msgs, fourier_msgs_pod, map_manager, nav2_msgs, and geometry_msgs is installed.
3. Before running Part 2, a GRW map archive has been saved by Part 1 or by /save_composite_map. The map path must name the archive directory itself.
4. Test only in a clear, safe area. The initial pose and target must be in traversable map space.

Environment Preparation

Prepare the environment in every new terminal (see Environment Preparation) and put the robot namespace into ROBOT_NAMESPACE:

Bash

source /opt/fftai/fourierdds/shells/discovery_env.sh --ros2
export ROS_DOMAIN_ID=123
export ROBOT_NAMESPACE=/GRW01AA0001  # replace with your robot's namespace

Both scripts in this guide use relative interface names; the run commands point them at the robot with --ros-args -r __ns:=$ROBOT_NAMESPACE. The command-line examples use the $ROBOT_NAMESPACE prefix directly.

Part 1: Create a mapping task

A map used for navigation must first be built by GRW and saved as a reloadable archive. The current public interfaces support switching to mapping mode, clearing the runtime map, inspecting the occupancy grid, and saving an archive, but they do not provide automatic exploration or automatic environment coverage. The script therefore only prepares and saves the map; covering the environment must be done manually by the operator in a safe area.

Mapping task overview

Stage	Operation	Change to the script
1	Connect interfaces	Create the mode-switch and map-save clients and the clear-map publisher.
2	Enter mapping mode	Add switch_to_mapping().
3	Create an empty map	Optionally add clear_map().
4	Cover the environment manually	Inspect /map; the script does not drive the robot.
5	Save the archive	Add save_map_archive().
6	Run the task	Execute the start or save phase.

Mapping stage 1: Connect to the robot

Create mapping_task.py and start with the complete imports, the node class, and its initializer. All three endpoints below are interfaces that GRW currently exposes.

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")
        # The /clear_map subscriber is BEST_EFFORT + TRANSIENT_LOCAL; the
        # publisher must match that QoS or the command is not processed.
        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)

The code in the following mapping stages is indented by four spaces and added to the MappingTask class.

Mapping stage 2: Enter mapping mode

Append the complete mode-switch method to the MappingTask class. Continue only when the service response success is true.

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

Mapping stage 3: Create an empty map

To start over from an empty map, append the following method to the class and call it when starting the task. GRW does not parse the message content; the call only clears the runtime map and does not delete saved archives.

Python

    def clear_map(self):
        # Wait for the DDS publisher and subscriber to match, then publish
        # the clear-map command once.
        rclpy.spin_once(self, timeout_sec=1.0)
        self.clear_map_publisher.publish(ShortString())
        # One more spin so the command is delivered before the process exits
        rclpy.spin_once(self, timeout_sec=1.0)
        self.get_logger().info("clear-map command sent")

To keep extending an existing runtime map, omit --clear and the current map is preserved.

Mapping stage 4: Cover the environment manually

In mapping mode, the operator should use the existing manual control method to cover doorways, corridors, corners, and any other area navigation must pass through, driving at low speed. The current public navigation interfaces have no automatic exploration, and this guide sends no autonomous-driving or velocity commands.

In another terminal, check that the 2D occupancy grid keeps updating:

Bash

ros2 topic echo $ROBOT_NAMESPACE/map

/map uses the map frame. In the grid data, -1, 0, and 100 mean unknown, free, and occupied. Do not save the map before coverage is complete.

Mapping stage 5: Save the map archive

Append the following method to the class. It uses /save_composite_map to save a complete GRW map archive; that directory is the --map-path input to the navigation task in Part 2. An existing target directory is overwritten by the new archive.

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

Do not use map_saver_cli output as a replacement for this archive: map.pgm and map.yaml are standard 2D exports and cannot be passed to GRW's /slam/load_map.

Mapping stage 6: Run the task

Append the entry point after the class definition. Run --phase start --clear first to enter mapping mode with an empty map; after covering the environment manually, run --phase save to save the archive.

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")
    # Pass through --ros-args (e.g. -r __ns:=) for rclpy.init namespace parsing
    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())

Complete mapping example

The following is the complete mapping_task.py, combining mapping stages 1 through 6 verbatim:

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")
        # The /clear_map subscriber is BEST_EFFORT + TRANSIENT_LOCAL; the
        # publisher must match that QoS or the command is not processed.
        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())
        # One more spin so the command is delivered before the process exits
        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")
    # Pass through --ros-args (e.g. -r __ns:=) for rclpy.init namespace parsing
    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())

Start a fresh mapping task:

Bash

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

Save the archive after covering the environment manually:

Bash

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

Part 2: Create a single-goal navigation task

Navigation task overview

Stage	Operation	Change to the script
1	Connect interfaces	Create the node, the map-load service client, and the navigation Action client.
2	Load the map and localize	Add load_map() to the class.
3	Create a navigation task	Add send_goal() and the goal-acceptance callback.
4	Monitor execution	Add the feedback and result callbacks.
5	Cancel the task	Add the Action cancellation request and its confirmation callback.
6	Run the task	Add the command-line arguments and main().

Constant definitions

Replace the values below with your own archive path and safe coordinates. Angles are in radians.

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

Stage 1: Connect to the robot

Create navigation_task.py and start with the complete imports, the node class, and its initializer. The self.load_map_client and self.navigation_client created here are used by the methods added in later stages; self.result lets the main loop wait for the Action terminal state.

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

The code in the following stages is indented by four spaces and added to the NavigationTask class.

Stage 2: Load the map and set the initial pose

Append the following complete load_map() method to the NavigationTask class. It waits for the service, loads the complete GRW map archive, and sets the initial pose in the same call through the request x, y, and yaw. It reports success only when result == 0; z is a compatibility field that GRW does not use.

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 must be the archive directory saved by save_composite_map, not the map.yaml, map.pgm, or any other individual file inside it.

Stage 3: Create a navigation task

Append the following two methods to the NavigationTask class. The goal uses the map frame; yaw is converted to the quaternion z and w through sin(yaw / 2) and cos(yaw / 2); behavior_tree must be left empty.

GRW accepts only one unfinished navigation goal at a time. While a goal is running, a new goal is rejected; wait for the previous terminal state, or cancel the previous goal first.

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)

Stage 4: Monitor the navigation task

Append the feedback and result callbacks to the same NavigationTask class. Current GRW feedback reliably populates only current_pose.pose.position.x and y; it cannot be used to derive remaining distance, ETA, complete orientation, or recovery count. The final navigation outcome must be taken from the Action terminal state.

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 means completed, CANCELED means canceled, and ABORTED means aborted or failed. For the numbering of the terminal status codes, see ros_api.md section 4.1.

Stage 5: Cancel the navigation task

Append the following cancellation methods to the class. They are triggered on a timer by the --cancel-after argument added later, and send the cancellation request through the current Action goal handle. After the cancellation request is accepted, still wait for on_result() from stage 4 to receive the terminal state.

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)")

A management client that does not hold the Action goal handle can call cancel_current_action to ask the navigation module to cancel the current action; a successful response there only means the cancellation request was accepted, and the final state must still be confirmed from the Action result. Publishing a non-zero manual velocity during navigation also cancels the current navigation task.

Stage 6: Run the task

Append the argument parsing and the entry point after the class definition. The main loop keeps processing ROS 2 callbacks while self.result has not been filled in by an Action callback; it returns 0 only when navigation succeeds.

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")
    # Pass through --ros-args (e.g. -r __ns:=) for rclpy.init namespace parsing
    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())

Complete navigation example

The following is the complete navigation_task.py, combining stages 1 through 6 verbatim:

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")
    # Pass through --ros-args (e.g. -r __ns:=) for rclpy.init namespace parsing
    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())

Normal execution:

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

Request cancellation five seconds after the goal is accepted:

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

Troubleshooting

Map loading fails

Ensure that --map-path names the complete GRW map archive directory saved by save_composite_map, not the map.yaml, map.pgm, or any other individual file inside it. See load_map.

Localization or navigation direction is wrong

Check that --initial-x, --initial-y, and --initial-yaw match the real robot pose; both the initial pose and the target use the map frame. Subscribe to /robot_pose to inspect the current localization result.

Navigation goal is rejected

GRW rejects a new goal while an existing navigation task has not finished. Wait for the previous Action terminal state, or cancel that goal through the Action client; you can also call cancel_current_action to request cancellation. Use get_current_action to inspect the current action.

Navigation is canceled or aborted

Start from the CANCELED or ABORTED Action terminal state. A non-zero manual velocity makes navigation yield to manual control and cancels it. The current interface does not expose the underlying failure reason, remaining distance, or ETA; investigate with /robot_pose, /plan, and the runtime logs.

Further reading

• GR-Navigation overview
• ROS 2 API
• load_map
• NavigateToPose action
• cancel_current_action
• get_current_action