Skip to main content
Version: 2.2.0

Execute Mission from Goal

Send the platform on an autonomous mission through the map, starting from a specific goal.

Action Name: autonomy/mission_from_goal

Action Type: clearpath_navigation_msgs/action/ExecuteMissionFromGoal

from action_msgs.msg import GoalStatus  # Import for reference to status constants
from clearpath_config.common.utils.yaml import read_yaml
from clearpath_navigation_msgs.action import ExecuteMissionFromGoal
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

ROBOT_CONFIG_PATH = '/etc/clearpath/robot.yaml'
LOGGING_NAME = 'ExecuteMissionFromGoal'

# TODO: Enter your mission, map and starting goal uuids
MISSION_ID: str = ''
MAP_ID: str = ''
GOAL_ID: str = ''
RUN_ON_START_TASKS: bool = True


class ExecuteMissionFromGoalActionClient(Node):

    def __init__(self, namespace: str):
        super().__init__('execute_mission_from_goal')
        self._namespace = namespace
        self._action_client = ActionClient(self, ExecuteMissionFromGoal, f'/{self._namespace}/autonomy/mission_from_goal')

        self._mission_from_goal_goal_handle = None
        self._mission_from_goal_goal_cancelled = False
        self._mission_running = False
        self._mission_completed = False

    #############################################
    # Mission From Goal Action Client Functions #
    #############################################
    def send_mission_from_goal_goal(self, mission_id, map_id, goal_id, run_on_start_tasks=True):
        # populate goal message
        goal_msg = ExecuteMissionFromGoal.Goal()
        goal_msg.mission_uuid = mission_id
        goal_msg.map_uuid = map_id
        goal_msg.goal_uuid = goal_id
        goal_msg.run_on_start_tasks = run_on_start_tasks

        if not self._action_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error(f'[{LOGGING_NAME}] /{self._namespace}/autonomy/mission_from_goal action server not available!')
            self.destroy_node()
            rclpy.shutdown()
            return

        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.mission_from_goal_feedback_cb)
        self._send_goal_future.add_done_callback(self.mission_from_goal_goal_response_cb)

    def mission_from_goal_goal_response_cb(self, future):
        self._mission_from_goal_goal_handle = future.result()
        if not self._mission_from_goal_goal_handle.accepted:
            self.get_logger().info(f'[{LOGGING_NAME}] Mission Goal rejected')
            self._mission_from_goal_goal_handle = None
            self.destroy_node()
            rclpy.shutdown()
            return

        self._mission_running = True
        self._mission_completed = False
        self._mission_goal_cancelled = False
        self.get_logger().info(f'[{LOGGING_NAME}] Mission Goal accepted')

        self._get_result_future = self._mission_from_goal_goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.mission_from_goal_get_result_cb)

    def mission_from_goal_feedback_cb(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(f'[{LOGGING_NAME}] Mission has been running for: {feedback.elapsed_time:.2f}s', throttle_duration_sec=60)

    def mission_from_goal_cancel_response_cb(self, future):
        cancel_response = future.result()
        self.get_logger().info(f'[{LOGGING_NAME}] Cancel response received: {cancel_response.return_code}')
        if len(cancel_response.goals_canceling) > 0:
            self.get_logger().info(f'[{LOGGING_NAME}] Canceling of mission goal complete')
        else:
            self.get_logger().warning(f'[{LOGGING_NAME}] Mission from goal Goal failed to cancel')

    def mission_from_goal_get_result_cb(self, future):
        result = future.result().result
        status = future.result().status

        # Compare the status integer against the constants defined in the message
        if status == GoalStatus.STATUS_SUCCEEDED:
            if result.success:
                self.get_logger().info(f'[{LOGGING_NAME}] [Mission from goal Goal Result] Succeeded! Result: Robot completed mission!')
                self._mission_completed = True
            else:
                self.get_logger().info(f'[{LOGGING_NAME}] [Mission from goal Goal Result] Succeeded! Result: Robot failed to complete mission')

        elif status == GoalStatus.STATUS_CANCELED:
            self.get_logger().info(f'[{LOGGING_NAME}] [Mission from goal Goal Result] Cancelled!')
            self._mission_goal_cancelled = True

        elif status == GoalStatus.STATUS_ABORTED:
            self.get_logger().info(f'[{LOGGING_NAME}] [Mission from goal Goal Result] Aborted!')
        else:
            self.get_logger().info(f'[{LOGGING_NAME}] [Mission from goal Goal Result] Finished with unknown status: {status}')

        self._mission_goal_handle = None
        self._mission_running = False

        self.destroy_node()
        rclpy.shutdown()


def main(args=None):
    rclpy.init(args=args)
    robot_config = read_yaml(ROBOT_CONFIG_PATH)
    namespace = robot_config['system']['ros2']['namespace']

    mission = ExecuteMissionFromGoalActionClient(namespace)
    mission.send_mission_from_goal_goal(MISSION_ID, MAP_ID, GOAL_ID, RUN_ON_START_TASKS)
    rclpy.spin(mission)


if __name__ == '__main__':
    main()

Last updated on