robot_navigator.py

#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
# Copyright 2025 Open Navigation LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
 
 
from enum import Enum
import time
from typing import Any, Optional, Union
 
from action_msgs.msg import GoalStatus
from builtin_interfaces.msg import Duration
from geographic_msgs.msg import GeoPose
from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped
from lifecycle_msgs.srv import GetState
from nav2_msgs.action import (AssistedTeleop, BackUp, ComputeAndTrackRoute,
                              ComputePathThroughPoses, ComputePathToPose, ComputeRoute, DockRobot,
                              DriveOnHeading, FollowGPSWaypoints, FollowPath, FollowWaypoints,
                              NavigateThroughPoses, NavigateToPose, SmoothPath, Spin, UndockRobot)
from nav2_msgs.msg import Route
from nav2_msgs.srv import (ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap,
                           GetCostmap, LoadMap, ManageLifecycleNodes)
from nav_msgs.msg import Goals, OccupancyGrid, Path
import rclpy
from rclpy.action import ActionClient  # type: ignore[attr-defined]
from rclpy.action.client import ClientGoalHandle
from rclpy.duration import Duration as rclpyDuration
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
from rclpy.task import Future
from rclpy.type_support import GetResultServiceResponse
 
 
# Task Result enum for the result of the task being executed
class TaskResult(Enum):
    UNKNOWN = 0
    SUCCEEDED = 1
    CANCELED = 2
    FAILED = 3
 
 
# Task enum for the task being executed, if its a long-running task to be able to obtain
# necessary contextual information in `isTaskComplete` and `getFeedback` regarding the task
# which is running.
class RunningTask(Enum):
    NONE = 0
    NAVIGATE_TO_POSE = 1
    NAVIGATE_THROUGH_POSES = 2
    FOLLOW_PATH = 3
    FOLLOW_WAYPOINTS = 4
    FOLLOW_GPS_WAYPOINTS = 5
    SPIN = 6
    BACKUP = 7
    DRIVE_ON_HEADING = 8
    ASSISTED_TELEOP = 9
    DOCK_ROBOT = 10
    UNDOCK_ROBOT = 11
    COMPUTE_AND_TRACK_ROUTE = 12
 
 
class BasicNavigator(Node):
 
    def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> None:
        super().__init__(node_name=node_name, namespace=namespace)
        self.initial_poseinitial_pose = PoseStamped()
        self.initial_poseinitial_pose.header.frame_id = 'map'
 
        self.goal_handlegoal_handle: Optional[ClientGoalHandle[Any, Any, Any]] = None
        self.result_futureresult_future: \
            Optional[Future[GetResultServiceResponse[Any]]] = None
        self.feedbackfeedback: Any = None
        self.statusstatus: Optional[int] = None
 
        # Since the route server's compute and track action server is likely
        # to be running simultaneously with another (e.g. controller, WPF) server,
        # we must track its futures and feedback separately. Additionally, the
        # route tracking feedback is uniquely important to be complete and ordered
        self.route_goal_handleroute_goal_handle: Optional[ClientGoalHandle[Any, Any, Any]] = None
        self.route_result_futureroute_result_future: \
            Optional[Future[GetResultServiceResponse[Any]]] = None
        self.route_feedback: list[Any] = []
 
        # Error code and messages from servers
        self.last_action_error_codelast_action_error_code = 0
        self.last_action_error_msglast_action_error_msg = ''
 
        amcl_pose_qos = QoSProfile(
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=1,
        )
 
        self.initial_pose_receivedinitial_pose_received = False
        self.nav_through_poses_clientnav_through_poses_client = ActionClient(
            self, NavigateThroughPoses, 'navigate_through_poses'
        )
        self.nav_to_pose_clientnav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        self.follow_waypoints_clientfollow_waypoints_client = ActionClient(
            self, FollowWaypoints, 'follow_waypoints'
        )
        self.follow_gps_waypoints_clientfollow_gps_waypoints_client = ActionClient(
            self, FollowGPSWaypoints, 'follow_gps_waypoints'
        )
        self.follow_path_clientfollow_path_client = ActionClient(self, FollowPath, 'follow_path')
        self.compute_path_to_pose_clientcompute_path_to_pose_client = ActionClient(
            self, ComputePathToPose, 'compute_path_to_pose'
        )
        self.compute_path_through_poses_clientcompute_path_through_poses_client = ActionClient(
            self, ComputePathThroughPoses, 'compute_path_through_poses'
        )
        self.smoother_clientsmoother_client = ActionClient(self, SmoothPath, 'smooth_path')
        self.compute_route_clientcompute_route_client = ActionClient(self, ComputeRoute, 'compute_route')
        self.compute_and_track_route_clientcompute_and_track_route_client = ActionClient(self, ComputeAndTrackRoute,
                                                           'compute_and_track_route')
        self.spin_clientspin_client = ActionClient(self, Spin, 'spin')
        self.backup_clientbackup_client = ActionClient(self, BackUp, 'backup')
        self.drive_on_heading_clientdrive_on_heading_client = ActionClient(
            self, DriveOnHeading, 'drive_on_heading'
        )
        self.assisted_teleop_clientassisted_teleop_client = ActionClient(
            self, AssistedTeleop, 'assisted_teleop'
        )
        self.docking_clientdocking_client = ActionClient(self, DockRobot, 'dock_robot')
        self.undocking_clientundocking_client = ActionClient(self, UndockRobot, 'undock_robot')
        self.localization_pose_sublocalization_pose_sub = self.create_subscription(
            PoseWithCovarianceStamped,
            'amcl_pose',
            self._amclPoseCallback_amclPoseCallback,
            amcl_pose_qos,
        )
        self.initial_pose_pubinitial_pose_pub = self.create_publisher(
            PoseWithCovarianceStamped, 'initialpose', 10
        )
        self.change_maps_srvchange_maps_srv = self.create_client(LoadMap, 'map_server/load_map')
        self.clear_costmap_global_srvclear_costmap_global_srv = self.create_client(
            ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap'
        )
        self.clear_costmap_local_srvclear_costmap_local_srv = self.create_client(
            ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap'
        )
        self.clear_costmap_except_region_srvclear_costmap_except_region_srv = self.create_client(
            ClearCostmapExceptRegion, 'local_costmap/clear_costmap_except_region'
        )
        self.clear_costmap_around_robot_srvclear_costmap_around_robot_srv = self.create_client(
            ClearCostmapAroundRobot, 'local_costmap/clear_costmap_around_robot'
        )
        self.get_costmap_global_srvget_costmap_global_srv = self.create_client(
            GetCostmap, 'global_costmap/get_costmap'
        )
        self.get_costmap_local_srvget_costmap_local_srv = self.create_client(
            GetCostmap, 'local_costmap/get_costmap'
        )
 
    def destroyNode(self) -> None:
        self.destroy_nodedestroy_node()
 
    def destroy_node(self) -> None:
        self.nav_through_poses_clientnav_through_poses_client.destroy()
        self.nav_to_pose_clientnav_to_pose_client.destroy()
        self.follow_waypoints_clientfollow_waypoints_client.destroy()
        self.follow_path_clientfollow_path_client.destroy()
        self.compute_path_to_pose_clientcompute_path_to_pose_client.destroy()
        self.compute_path_through_poses_clientcompute_path_through_poses_client.destroy()
        self.compute_and_track_route_clientcompute_and_track_route_client.destroy()
        self.compute_route_clientcompute_route_client.destroy()
        self.smoother_clientsmoother_client.destroy()
        self.spin_clientspin_client.destroy()
        self.backup_clientbackup_client.destroy()
        self.drive_on_heading_clientdrive_on_heading_client.destroy()
        self.assisted_teleop_clientassisted_teleop_client.destroy()
        self.follow_gps_waypoints_clientfollow_gps_waypoints_client.destroy()
        self.docking_clientdocking_client.destroy()
        self.undocking_clientundocking_client.destroy()
        super().destroy_node()
 
    def setInitialPose(self, initial_pose: PoseStamped) -> None:
        """Set the initial pose to the localization system."""
        self.initial_pose_receivedinitial_pose_received = False
        self.initial_poseinitial_pose = initial_pose
        self._setInitialPose_setInitialPose()
 
    def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> Optional[RunningTask]:
        """Send a `NavThroughPoses` action request."""
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'NavigateThroughPoses' action server")
        while not self.nav_through_poses_clientnav_through_poses_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'NavigateThroughPoses' action server not available, waiting...")
 
        goal_msg = NavigateThroughPoses.Goal()
        goal_msg.poses = poses
        goal_msg.behavior_tree = behavior_tree
 
        self.infoinfo(f'Navigating with {len(goal_msg.poses)} goals....')
        send_goal_future = self.nav_through_poses_clientnav_through_poses_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = f'NavigateThroughPoses request with {len(poses)} was rejected!'
            self.setTaskErrorsetTaskError(NavigateThroughPoses.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.NAVIGATE_THROUGH_POSES
 
    def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> Optional[RunningTask]:
        """Send a `NavToPose` action request."""
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'NavigateToPose' action server")
        while not self.nav_to_pose_clientnav_to_pose_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'NavigateToPose' action server not available, waiting...")
 
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = pose
        goal_msg.behavior_tree = behavior_tree
 
        self.infoinfo(
            'Navigating to goal: '
            + str(pose.pose.position.x)
            + ' '
            + str(pose.pose.position.y)
            + '...'
        )
        send_goal_future = self.nav_to_pose_clientnav_to_pose_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = (
                'NavigateToPose goal to '
                + str(pose.pose.position.x)
                + ' '
                + str(pose.pose.position.y)
                + ' was rejected!'
            )
            self.setTaskErrorsetTaskError(NavigateToPose.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.NAVIGATE_TO_POSE
 
    def followWaypoints(self, poses: list[PoseStamped]) -> Optional[RunningTask]:
        """Send a `FollowWaypoints` action request."""
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'FollowWaypoints' action server")
        while not self.follow_waypoints_clientfollow_waypoints_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'FollowWaypoints' action server not available, waiting...")
 
        goal_msg = FollowWaypoints.Goal()
        goal_msg.poses = poses
 
        self.infoinfo(f'Following {len(goal_msg.poses)} goals....')
        send_goal_future = self.follow_waypoints_clientfollow_waypoints_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = f'Following {len(poses)} waypoints request was rejected!'
            self.setTaskErrorsetTaskError(FollowWaypoints.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.FOLLOW_WAYPOINTS
 
    def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> Optional[RunningTask]:
        """Send a `FollowGPSWaypoints` action request."""
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'FollowWaypoints' action server")
        while not self.follow_gps_waypoints_clientfollow_gps_waypoints_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'FollowWaypoints' action server not available, waiting...")
 
        goal_msg = FollowGPSWaypoints.Goal()
        goal_msg.gps_poses = gps_poses
 
        self.infoinfo(f'Following {len(goal_msg.gps_poses)} gps goals....')
        send_goal_future = self.follow_gps_waypoints_clientfollow_gps_waypoints_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = f'Following {len(gps_poses)} gps waypoints request was rejected!'
            self.setTaskErrorsetTaskError(FollowGPSWaypoints.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.FOLLOW_GPS_WAYPOINTS
 
    def spin(
            self, spin_dist: float = 1.57, time_allowance: int = 10,
            disable_collision_checks: bool = False) -> Optional[RunningTask]:
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'Spin' action server")
        while not self.spin_clientspin_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'Spin' action server not available, waiting...")
        goal_msg = Spin.Goal()
        goal_msg.target_yaw = spin_dist
        goal_msg.time_allowance = Duration(sec=time_allowance)
        goal_msg.disable_collision_checks = disable_collision_checks
 
        self.infoinfo(f'Spinning to angle {goal_msg.target_yaw}....')
        send_goal_future = self.spin_clientspin_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'Spin request was rejected!'
            self.setTaskErrorsetTaskError(Spin.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.SPIN
 
    def backup(
            self, backup_dist: float = 0.15, backup_speed: float = 0.025,
            time_allowance: int = 10,
            disable_collision_checks: bool = False) -> Optional[RunningTask]:
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'Backup' action server")
        while not self.backup_clientbackup_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'Backup' action server not available, waiting...")
        goal_msg = BackUp.Goal()
        goal_msg.target = Point(x=float(backup_dist))
        goal_msg.speed = backup_speed
        goal_msg.time_allowance = Duration(sec=time_allowance)
        goal_msg.disable_collision_checks = disable_collision_checks
 
        self.infoinfo(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....')
        send_goal_future = self.backup_clientbackup_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'Backup request was rejected!'
            self.setTaskErrorsetTaskError(BackUp.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.BACKUP
 
    def driveOnHeading(
            self, dist: float = 0.15, speed: float = 0.025,
            time_allowance: int = 10,
            disable_collision_checks: bool = False) -> Optional[RunningTask]:
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'DriveOnHeading' action server")
        while not self.backup_clientbackup_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'DriveOnHeading' action server not available, waiting...")
        goal_msg = DriveOnHeading.Goal()
        goal_msg.target = Point(x=float(dist))
        goal_msg.speed = speed
        goal_msg.time_allowance = Duration(sec=time_allowance)
        goal_msg.disable_collision_checks = disable_collision_checks
 
        self.infoinfo(f'Drive {goal_msg.target.x} m on heading at {goal_msg.speed} m/s....')
        send_goal_future = self.drive_on_heading_clientdrive_on_heading_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'Drive On Heading request was rejected!'
            self.setTaskErrorsetTaskError(DriveOnHeading.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.DRIVE_ON_HEADING
 
    def assistedTeleop(self, time_allowance: int = 30) -> Optional[RunningTask]:
 
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Wanting for 'assisted_teleop' action server")
 
        while not self.assisted_teleop_clientassisted_teleop_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'assisted_teleop' action server not available, waiting...")
        goal_msg = AssistedTeleop.Goal()
        goal_msg.time_allowance = Duration(sec=time_allowance)
 
        self.infoinfo("Running 'assisted_teleop'....")
        send_goal_future = self.assisted_teleop_clientassisted_teleop_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'Assisted Teleop request was rejected!'
            self.setTaskErrorsetTaskError(AssistedTeleop.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.ASSISTED_TELEOP
 
    def followPath(self, path: Path, controller_id: str = '',
                   goal_checker_id: str = '') -> Optional[RunningTask]:
        self.clearTaskErrorclearTaskError()
        """Send a `FollowPath` action request."""
        self.debugdebug("Waiting for 'FollowPath' action server")
        while not self.follow_path_clientfollow_path_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'FollowPath' action server not available, waiting...")
 
        goal_msg = FollowPath.Goal()
        goal_msg.path = path
        goal_msg.controller_id = controller_id
        goal_msg.goal_checker_id = goal_checker_id
 
        self.infoinfo('Executing path...')
        send_goal_future = self.follow_path_clientfollow_path_client.send_goal_async(
            goal_msg, self._feedbackCallback_feedbackCallback
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'FollowPath goal was rejected!'
            self.setTaskErrorsetTaskError(FollowPath.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.FOLLOW_PATH
 
    def dockRobotByPose(self, dock_pose: PoseStamped,
                        dock_type: str = '', nav_to_dock: bool = True) -> Optional[RunningTask]:
        self.clearTaskErrorclearTaskError()
        """Send a `DockRobot` action request."""
        self.infoinfo("Waiting for 'DockRobot' action server")
        while not self.docking_clientdocking_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo('"DockRobot" action server not available, waiting...')
 
        goal_msg = DockRobot.Goal()
        goal_msg.use_dock_id = False
        goal_msg.dock_pose = dock_pose
        goal_msg.dock_type = dock_type
        goal_msg.navigate_to_staging_pose = nav_to_dock  # if want to navigate before staging
 
        self.infoinfo('Docking at pose: ' + str(dock_pose) + '...')
        send_goal_future = self.docking_clientdocking_client.send_goal_async(goal_msg,
                                                               self._feedbackCallback_feedbackCallback)
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'DockRobot request was rejected!'
            self.setTaskErrorsetTaskError(DockRobot.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.DOCK_ROBOT
 
    def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> Optional[RunningTask]:
        """Send a `DockRobot` action request."""
        self.clearTaskErrorclearTaskError()
        self.infoinfo("Waiting for 'DockRobot' action server")
        while not self.docking_clientdocking_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo('"DockRobot" action server not available, waiting...')
 
        goal_msg = DockRobot.Goal()
        goal_msg.use_dock_id = True
        goal_msg.dock_id = dock_id
        goal_msg.navigate_to_staging_pose = nav_to_dock  # if want to navigate before staging
 
        self.infoinfo('Docking at dock ID: ' + str(dock_id) + '...')
        send_goal_future = self.docking_clientdocking_client.send_goal_async(goal_msg,
                                                               self._feedbackCallback_feedbackCallback)
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'DockRobot request was rejected!'
            self.setTaskErrorsetTaskError(DockRobot.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.DOCK_ROBOT
 
    def undockRobot(self, dock_type: str = '') -> Optional[RunningTask]:
        """Send a `UndockRobot` action request."""
        self.clearTaskErrorclearTaskError()
        self.infoinfo("Waiting for 'UndockRobot' action server")
        while not self.undocking_clientundocking_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo('"UndockRobot" action server not available, waiting...')
 
        goal_msg = UndockRobot.Goal()
        goal_msg.dock_type = dock_type
 
        self.infoinfo('Undocking from dock of type: ' + str(dock_type) + '...')
        send_goal_future = self.undocking_clientundocking_client.send_goal_async(goal_msg,
                                                                 self._feedbackCallback_feedbackCallback)
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            msg = 'UndockRobot request was rejected!'
            self.setTaskErrorsetTaskError(UndockRobot.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        return RunningTask.UNDOCK_ROBOT
 
    def cancelTask(self) -> None:
        """Cancel pending task request of any type."""
        self.infoinfo('Canceling current task.')
        if self.result_futureresult_future:
            if self.goal_handlegoal_handle is not None:
                future = self.goal_handlegoal_handle.cancel_goal_async()
                rclpy.spin_until_future_complete(self, future)
            else:
                self.errorerror('Cancel task failed, goal handle is None')
                self.setTaskErrorsetTaskError(0, 'Cancel task failed, goal handle is None')
                return
        if self.route_result_futureroute_result_future:
            if self.route_goal_handleroute_goal_handle is not None:
                future = self.route_goal_handleroute_goal_handle.cancel_goal_async()
                rclpy.spin_until_future_complete(self, future)
            else:
                self.errorerror('Cancel route task failed, goal handle is None')
                self.setTaskErrorsetTaskError(0, 'Cancel route task failed, goal handle is None')
                return
        self.clearTaskErrorclearTaskError()
        return
 
    def isTaskComplete(self, task: RunningTask = RunningTask.NONE) -> bool:
        """Check if the task request of any type is complete yet."""
        # Find the result future to spin
        if task is None:
            self.errorerror('Task is None, cannot check for completion')
            return False
 
        result_future = None
        if task != RunningTask.COMPUTE_AND_TRACK_ROUTE:
            result_future = self.result_futureresult_future
        else:
            result_future = self.route_result_futureroute_result_future
        if not result_future:
            # task was cancelled or completed
            return True
 
        # Get the result of the future, if complete
        rclpy.spin_until_future_complete(self, result_future, timeout_sec=0.10)
        result_response = result_future.result()
 
        if result_response:
            self.statusstatus = result_response.status
            if self.statusstatus != GoalStatus.STATUS_SUCCEEDED:
                result = result_response.result
                if result is not None:
                    self.setTaskErrorsetTaskError(result.error_code, result.error_msg)
                    self.debugdebug(
                        'Task with failed with'
                        f' status code:{self.status}'
                        f' error code:{result.error_code}'
                        f' error msg:{result.error_msg}')
                    return True
                else:
                    self.setTaskErrorsetTaskError(0, 'No result received')
                    self.debugdebug('Task failed with no result received')
                    return True
        else:
            # Timed out, still processing, not complete yet
            return False
 
        self.debugdebug('Task succeeded!')
        return True
 
    def getFeedback(self, task: RunningTask = RunningTask.NONE) -> Any:
        """Get the pending action feedback message."""
        if task != RunningTask.COMPUTE_AND_TRACK_ROUTE:
            return self.feedbackfeedback
        if len(self.route_feedback) > 0:
            return self.route_feedback.pop(0)
        return None
 
    def getResult(self) -> TaskResult:
        """Get the pending action result message."""
        if self.statusstatus == GoalStatus.STATUS_SUCCEEDED:
            return TaskResult.SUCCEEDED
        elif self.statusstatus == GoalStatus.STATUS_ABORTED:
            return TaskResult.FAILED
        elif self.statusstatus == GoalStatus.STATUS_CANCELED:
            return TaskResult.CANCELED
        else:
            return TaskResult.UNKNOWN
 
    def clearTaskError(self) -> None:
        self.last_action_error_codelast_action_error_code = 0
        self.last_action_error_msglast_action_error_msg = ''
 
    def setTaskError(self, error_code: int, error_msg: str) -> None:
        self.last_action_error_codelast_action_error_code = error_code
        self.last_action_error_msglast_action_error_msg = error_msg
 
    def getTaskError(self) -> tuple[int, str]:
        return (self.last_action_error_codelast_action_error_code, self.last_action_error_msglast_action_error_msg)
 
    def waitUntilNav2Active(self, navigator: str = 'bt_navigator',
                            localizer: str = 'amcl') -> None:
        """Block until the full navigation system is up and running."""
        if localizer != 'robot_localization':  # non-lifecycle node
            self._waitForNodeToActivate_waitForNodeToActivate(localizer)
        if localizer == 'amcl':
            self._waitForInitialPose_waitForInitialPose()
        self._waitForNodeToActivate_waitForNodeToActivate(navigator)
        self.infoinfo('Nav2 is ready for use!')
        return
 
    def _getPathImpl(
        self, start: PoseStamped, goal: PoseStamped,
        planner_id: str = '', use_start: bool = False
    ) -> ComputePathToPose.Result:
        """
        Send a `ComputePathToPose` action request.
 
        Internal implementation to get the full result, not just the path.
        """
        self.debugdebug("Waiting for 'ComputePathToPose' action server")
        while not self.compute_path_to_pose_clientcompute_path_to_pose_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'ComputePathToPose' action server not available, waiting...")
 
        goal_msg = ComputePathToPose.Goal()
        goal_msg.start = start
        goal_msg.goal = goal
        goal_msg.planner_id = planner_id
        goal_msg.use_start = use_start
 
        self.infoinfo('Getting path...')
        send_goal_future = self.compute_path_to_pose_clientcompute_path_to_pose_client.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            self.errorerror('Get path was rejected!')
            self.statusstatus = GoalStatus.UNKNOWN
            result = ComputePathToPose.Result()
            result.error_code = ComputePathToPose.UNKNOWN
            result.error_msg = 'Get path was rejected'
            return result
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, self.result_futureresult_future)
        self.statusstatus = self.result_futureresult_future.result().status  # type: ignore[union-attr]
 
        return self.result_futureresult_future.result().result  # type: ignore[union-attr]
 
    def getPath(
        self, start: PoseStamped, goal: PoseStamped,
            planner_id: str = '', use_start: bool = False) -> Path:
        """Send a `ComputePathToPose` action request."""
        self.clearTaskErrorclearTaskError()
        rtn = self._getPathImpl_getPathImpl(start, goal, planner_id, use_start)
 
        if self.statusstatus == GoalStatus.STATUS_SUCCEEDED:
            return rtn.path
        else:
            self.setTaskErrorsetTaskError(rtn.error_code, rtn.error_msg)
            self.warnwarn('Getting path failed with'
                      f' status code:{self.status}'
                      f' error code:{rtn.error_code}'
                      f' error msg:{rtn.error_msg}')
            return None
 
    def _getPathThroughPosesImpl(
        self, start: PoseStamped, goals: Goals,
            planner_id: str = '', use_start: bool = False
    ) -> ComputePathThroughPoses.Result:
        """
        Send a `ComputePathThroughPoses` action request.
 
        Internal implementation to get the full result, not just the path.
        """
        self.debugdebug("Waiting for 'ComputePathThroughPoses' action server")
        while not self.compute_path_through_poses_clientcompute_path_through_poses_client.wait_for_server(
            timeout_sec=1.0
        ):
            self.infoinfo(
                "'ComputePathThroughPoses' action server not available, waiting..."
            )
 
        goal_msg = ComputePathThroughPoses.Goal()
        goal_msg.start = start
        goal_msg.goals.header.frame_id = 'map'
        goal_msg.goals.header.stamp = self.get_clock().now().to_msg()
        goal_msg.goals.goals = goals
        goal_msg.planner_id = planner_id
        goal_msg.use_start = use_start
 
        self.infoinfo('Getting path...')
        send_goal_future = self.compute_path_through_poses_clientcompute_path_through_poses_client.send_goal_async(
            goal_msg
        )
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            self.errorerror('Get path was rejected!')
            result = ComputePathThroughPoses.Result()
            result.error_code = ComputePathThroughPoses.UNKNOWN
            result.error_msg = 'Get path was rejected!'
            return result
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, self.result_futureresult_future)
        self.statusstatus = self.result_futureresult_future.result().status  # type: ignore[union-attr]
 
        return self.result_futureresult_future.result().result  # type: ignore[union-attr]
 
    def getPathThroughPoses(
        self, start: PoseStamped, goals: Goals,
            planner_id: str = '', use_start: bool = False) -> Path:
        """Send a `ComputePathThroughPoses` action request."""
        self.clearTaskErrorclearTaskError()
        rtn = self._getPathThroughPosesImpl_getPathThroughPosesImpl(start, goals, planner_id, use_start)
 
        if self.statusstatus == GoalStatus.STATUS_SUCCEEDED:
            return rtn.path
        else:
            self.setTaskErrorsetTaskError(rtn.error_code, rtn.error_msg)
            self.warnwarn('Getting path failed with'
                      f' status code:{self.status}'
                      f' error code:{rtn.error_code}'
                      f' error msg:{rtn.error_msg}')
            return None
 
    def _getRouteImpl(
        self, start: Union[int, PoseStamped],
        goal: Union[int, PoseStamped], use_start: bool = False
    ) -> ComputeRoute.Result:
        """
        Send a `ComputeRoute` action request.
 
        Internal implementation to get the full result, not just the sparse route and dense path.
        """
        self.debugdebug("Waiting for 'ComputeRoute' action server")
        while not self.compute_route_clientcompute_route_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'ComputeRoute' action server not available, waiting...")
 
        goal_msg = ComputeRoute.Goal()
        goal_msg.use_start = use_start
 
        # Support both ID based requests and PoseStamped based requests
        if isinstance(start, int) and isinstance(goal, int):
            goal_msg.start_id = start
            goal_msg.goal_id = goal
            goal_msg.use_poses = False
        elif isinstance(start, PoseStamped) and isinstance(goal, PoseStamped):
            goal_msg.start = start
            goal_msg.goal = goal
            goal_msg.use_poses = True
        else:
            self.errorerror('Invalid start and goal types. Must be PoseStamped for pose or int for ID')
            result = ComputeRoute.Result()
            result.error_code = ComputeRoute.UNKNOWN
            result.error_msg = 'Request type fields were invalid!'
            return result
 
        self.infoinfo('Getting route...')
        send_goal_future = self.compute_route_clientcompute_route_client.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            self.errorerror('Get route was rejected!')
            result = ComputeRoute.Result()
            result.error_code = ComputeRoute.UNKNOWN
            result.error_msg = 'Get route was rejected!'
            return result
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, self.result_futureresult_future)
        self.statusstatus = self.result_futureresult_future.result().status  # type: ignore[union-attr]
 
        return self.result_futureresult_future.result().result  # type: ignore[union-attr]
 
    def getRoute(
            self, start: Union[int, PoseStamped],
            goal: Union[int, PoseStamped],
            use_start: bool = False) -> Optional[list[Union[Path, Route]]]:
        """Send a `ComputeRoute` action request."""
        self.clearTaskErrorclearTaskError()
        rtn = self._getRouteImpl_getRouteImpl(start, goal, use_start=False)
 
        if self.statusstatus != GoalStatus.STATUS_SUCCEEDED:
            self.setTaskErrorsetTaskError(rtn.error_code, rtn.error_msg)
            self.warnwarn(
                'Getting route failed with'
                f' status code:{self.status}'
                f' error code:{rtn.error_code}'
                f' error msg:{rtn.error_msg}')
            return None
 
        return [rtn.path, rtn.route]
 
    def getAndTrackRoute(
        self, start: Union[int, PoseStamped],
        goal: Union[int, PoseStamped], use_start: bool = False
    ) -> Optional[RunningTask]:
        """Send a `ComputeAndTrackRoute` action request."""
        self.clearTaskErrorclearTaskError()
        self.debugdebug("Waiting for 'ComputeAndTrackRoute' action server")
        while not self.compute_and_track_route_clientcompute_and_track_route_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'ComputeAndTrackRoute' action server not available, waiting...")
 
        goal_msg = ComputeAndTrackRoute.Goal()
        goal_msg.use_start = use_start
 
        # Support both ID based requests and PoseStamped based requests
        if isinstance(start, int) and isinstance(goal, int):
            goal_msg.start_id = start
            goal_msg.goal_id = goal
            goal_msg.use_poses = False
        elif isinstance(start, PoseStamped) and isinstance(goal, PoseStamped):
            goal_msg.start = start
            goal_msg.goal = goal
            goal_msg.use_poses = True
        else:
            self.setTaskErrorsetTaskError(ComputeAndTrackRoute.UNKNOWN, 'Request type fields were invalid!')
            self.errorerror('Invalid start and goal types. Must be PoseStamped for pose or int for ID')
            return None
 
        self.infoinfo('Computing and tracking route...')
        send_goal_future = self.compute_and_track_route_clientcompute_and_track_route_client.send_goal_async(goal_msg,
            self._routeFeedbackCallback_routeFeedbackCallback)  # noqa: E128
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.route_goal_handleroute_goal_handle = send_goal_future.result()
 
        if not self.route_goal_handleroute_goal_handle or not self.route_goal_handleroute_goal_handle.accepted:
            msg = 'Compute and track route was rejected!'
            self.setTaskErrorsetTaskError(ComputeAndTrackRoute.UNKNOWN, msg)
            self.errorerror(msg)
            return None
 
        self.route_result_futureroute_result_future = self.route_goal_handleroute_goal_handle.get_result_async()
        return RunningTask.COMPUTE_AND_TRACK_ROUTE
 
    def _smoothPathImpl(
        self, path: Path, smoother_id: str = '',
        max_duration: float = 2.0, check_for_collision: bool = False
    ) -> SmoothPath.Result:
        """
        Send a `SmoothPath` action request.
 
        Internal implementation to get the full result, not just the path.
        """
        self.debugdebug("Waiting for 'SmoothPath' action server")
        while not self.smoother_clientsmoother_client.wait_for_server(timeout_sec=1.0):
            self.infoinfo("'SmoothPath' action server not available, waiting...")
 
        goal_msg = SmoothPath.Goal()
        goal_msg.path = path
        goal_msg.max_smoothing_duration = rclpyDuration(seconds=max_duration).to_msg()
        goal_msg.smoother_id = smoother_id
        goal_msg.check_for_collisions = check_for_collision
 
        self.infoinfo('Smoothing path...')
        send_goal_future = self.smoother_clientsmoother_client.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(self, send_goal_future)
        self.goal_handlegoal_handle = send_goal_future.result()
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            self.errorerror('Smooth path was rejected!')
            result = SmoothPath.Result()
            result.error_code = SmoothPath.UNKNOWN
            result.error_msg = 'Smooth path was rejected'
            return result
 
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, self.result_futureresult_future)
        self.statusstatus = self.result_futureresult_future.result().status  # type: ignore[union-attr]
 
        return self.result_futureresult_future.result().result  # type: ignore[union-attr]
 
    def smoothPath(
        self, path: Path, smoother_id: str = '',
            max_duration: float = 2.0, check_for_collision: bool = False) -> Path:
        """Send a `SmoothPath` action request."""
        self.clearTaskErrorclearTaskError()
        rtn = self._smoothPathImpl_smoothPathImpl(path, smoother_id, max_duration, check_for_collision)
 
        if self.statusstatus == GoalStatus.STATUS_SUCCEEDED:
            return rtn.path
        else:
            self.setTaskErrorsetTaskError(rtn.error_code, rtn.error_msg)
            self.warnwarn('Getting path failed with'
                      f' status code:{self.status}'
                      f' error code:{rtn.error_code}'
                      f' error msg:{rtn.error_msg}')
            return None
 
    def changeMap(self, map_filepath: str) -> bool:
        """Change the current static map in the map server."""
        while not self.change_maps_srvchange_maps_srv.wait_for_service(timeout_sec=1.0):
            self.infoinfo('change map service not available, waiting...')
        req = LoadMap.Request()
        req.map_url = map_filepath
        future = self.change_maps_srvchange_maps_srv.call_async(req)
        rclpy.spin_until_future_complete(self, future)
 
        future_result = future.result()
        if future_result is None:
            self.errorerror('Change map request failed!')
            return False
 
        result = future_result.result
        if result != LoadMap.Response().RESULT_SUCCESS:
            if result == LoadMap.RESULT_MAP_DOES_NOT_EXIST:
                reason = 'Map does not exist'
            elif result == LoadMap.INVALID_MAP_DATA:
                reason = 'Invalid map data'
            elif result == LoadMap.INVALID_MAP_METADATA:
                reason = 'Invalid map metadata'
            elif result == LoadMap.UNDEFINED_FAILURE:
                reason = 'Undefined failure'
            else:
                reason = 'Unknown'
            self.setTaskErrorsetTaskError(result, reason)
            self.errorerror(f'Change map request failed:{reason}!')
            return False
        else:
            self.infoinfo('Change map request was successful!')
            return True
 
    def clearAllCostmaps(self) -> None:
        """Clear all costmaps."""
        self.clearLocalCostmapclearLocalCostmap()
        self.clearGlobalCostmapclearGlobalCostmap()
        return
 
    def clearLocalCostmap(self) -> None:
        """Clear local costmap."""
        while not self.clear_costmap_local_srvclear_costmap_local_srv.wait_for_service(timeout_sec=1.0):
            self.infoinfo('Clear local costmaps service not available, waiting...')
        req = ClearEntireCostmap.Request()
        future = self.clear_costmap_local_srvclear_costmap_local_srv.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return
 
    def clearGlobalCostmap(self) -> None:
        """Clear global costmap."""
        while not self.clear_costmap_global_srvclear_costmap_global_srv.wait_for_service(timeout_sec=1.0):
            self.infoinfo('Clear global costmaps service not available, waiting...')
        req = ClearEntireCostmap.Request()
        future = self.clear_costmap_global_srvclear_costmap_global_srv.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return
 
    def clearCostmapExceptRegion(self, reset_distance: float) -> None:
        """Clear the costmap except for a specified region."""
        while not self.clear_costmap_except_region_srvclear_costmap_except_region_srv.wait_for_service(timeout_sec=1.0):
            self.infoinfo('ClearCostmapExceptRegion service not available, waiting...')
        req = ClearCostmapExceptRegion.Request()
        req.reset_distance = reset_distance
        future = self.clear_costmap_except_region_srvclear_costmap_except_region_srv.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return
 
    def clearCostmapAroundRobot(self, reset_distance: float) -> None:
        """Clear the costmap around the robot."""
        while not self.clear_costmap_around_robot_srvclear_costmap_around_robot_srv.wait_for_service(timeout_sec=1.0):
            self.infoinfo('ClearCostmapAroundRobot service not available, waiting...')
        req = ClearCostmapAroundRobot.Request()
        req.reset_distance = reset_distance
        future = self.clear_costmap_around_robot_srvclear_costmap_around_robot_srv.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return
 
    def getGlobalCostmap(self) -> OccupancyGrid:
        """Get the global costmap."""
        while not self.get_costmap_global_srvget_costmap_global_srv.wait_for_service(timeout_sec=1.0):
            self.infoinfo('Get global costmaps service not available, waiting...')
        req = GetCostmap.Request()
        future = self.get_costmap_global_srvget_costmap_global_srv.call_async(req)
        rclpy.spin_until_future_complete(self, future)
 
        result = future.result()
        if result is None:
            self.errorerror('Get global costmap request failed!')
            return None
 
        return result.map
 
    def getLocalCostmap(self) -> OccupancyGrid:
        """Get the local costmap."""
        while not self.get_costmap_local_srvget_costmap_local_srv.wait_for_service(timeout_sec=1.0):
            self.infoinfo('Get local costmaps service not available, waiting...')
        req = GetCostmap.Request()
        future = self.get_costmap_local_srvget_costmap_local_srv.call_async(req)
        rclpy.spin_until_future_complete(self, future)
 
        result = future.result()
 
        if result is None:
            self.errorerror('Get local costmap request failed!')
            return None
 
        return result.map
 
    def lifecycleStartup(self) -> None:
        """Startup nav2 lifecycle system."""
        self.infoinfo('Starting up lifecycle nodes based on lifecycle_manager.')
        for srv_name, srv_type in self.get_service_names_and_types():
            if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
                self.infoinfo(f'Starting up {srv_name}')
                mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
                while not mgr_client.wait_for_service(timeout_sec=1.0):
                    self.infoinfo(f'{srv_name} service not available, waiting...')
                req = ManageLifecycleNodes.Request()
                req.command = ManageLifecycleNodes.Request().STARTUP
                future = mgr_client.call_async(req)
 
                # starting up requires a full map->odom->base_link TF tree
                # so if we're not successful, try forwarding the initial pose
                while True:
                    rclpy.spin_until_future_complete(self, future, timeout_sec=0.10)
                    if not future:
                        self._waitForInitialPose_waitForInitialPose()
                    else:
                        break
        self.infoinfo('Nav2 is ready for use!')
        return
 
    def lifecycleShutdown(self) -> None:
        """Shutdown nav2 lifecycle system."""
        self.infoinfo('Shutting down lifecycle nodes based on lifecycle_manager.')
        for srv_name, srv_type in self.get_service_names_and_types():
            if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
                self.infoinfo(f'Shutting down {srv_name}')
                mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
                while not mgr_client.wait_for_service(timeout_sec=1.0):
                    self.infoinfo(f'{srv_name} service not available, waiting...')
                req = ManageLifecycleNodes.Request()
                req.command = ManageLifecycleNodes.Request().SHUTDOWN
                future = mgr_client.call_async(req)
                rclpy.spin_until_future_complete(self, future)
                future.result()
        return
 
    def _waitForNodeToActivate(self, node_name: str) -> None:
        # Waits for the node within the tester namespace to become active
        self.debugdebug(f'Waiting for {node_name} to become active..')
        node_service = f'{node_name}/get_state'
        state_client = self.create_client(GetState, node_service)
        while not state_client.wait_for_service(timeout_sec=1.0):
            self.infoinfo(f'{node_service} service not available, waiting...')
 
        req = GetState.Request()
        state = 'unknown'
        while state != 'active':
            self.debugdebug(f'Getting {node_name} state...')
            future = state_client.call_async(req)
            rclpy.spin_until_future_complete(self, future)
 
            result = future.result()
            if result is not None:
                state = result.current_state.label
                self.debugdebug(f'Result of get_state: {state}')
            time.sleep(2)
        return
 
    def _waitForInitialPose(self) -> None:
        while not self.initial_pose_receivedinitial_pose_received:
            self.infoinfo('Setting initial pose')
            self._setInitialPose_setInitialPose()
            self.infoinfo('Waiting for amcl_pose to be received')
            rclpy.spin_once(self, timeout_sec=1.0)
        return
 
    def _amclPoseCallback(self, msg: PoseWithCovarianceStamped) -> None:
        self.debugdebug('Received amcl pose')
        self.initial_pose_receivedinitial_pose_received = True
        return
 
    def _feedbackCallback(self, msg: NavigateToPose.Feedback) -> None:
        self.debugdebug('Received action feedback message')
        self.feedbackfeedback = msg.feedback
        return
 
    def _routeFeedbackCallback(self, msg: ComputeAndTrackRoute.Feedback) -> None:
        self.debugdebug('Received route action feedback message')
        self.route_feedback.append(msg.feedback)
        return
 
    def _setInitialPose(self) -> None:
        msg = PoseWithCovarianceStamped()
        msg.pose.pose = self.initial_poseinitial_pose.pose
        msg.header.frame_id = self.initial_poseinitial_pose.header.frame_id
        msg.header.stamp = self.initial_poseinitial_pose.header.stamp
        self.infoinfo('Publishing Initial Pose')
        self.initial_pose_pubinitial_pose_pub.publish(msg)
        return
 
    def info(self, msg: str) -> None:
        self.get_logger().info(msg)
        return
 
    def warn(self, msg: str) -> None:
        self.get_logger().warn(msg)
        return
 
    def error(self, msg: str) -> None:
        self.get_logger().error(msg)
        return
 
    def debug(self, msg: str) -> None:
        self.get_logger().debug(msg)
        return