#!/usr/bin/env python3 """Mission Planner - Coordinates missions using local waypoints.""" import rclpy from rclpy.node import Node from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Path from std_msgs.msg import String, Bool import yaml from enum import Enum class MissionState(Enum): IDLE = 0 LOADING = 1 EXECUTING = 2 PAUSED = 3 COMPLETED = 4 ABORTED = 5 class MissionPlanner(Node): def __init__(self): super().__init__('mission_planner') self.declare_parameter('mission_file', '') self.mission = [] self.current_action_idx = 0 self.state = MissionState.IDLE self.uav_goal_reached = False self.ugv_goal_reached = False self.command_sub = self.create_subscription( String, '/mission/command', self.command_callback, 10) self.uav_status_sub = self.create_subscription( String, '/uav/controller/status', self.uav_status_callback, 10) self.ugv_status_sub = self.create_subscription( String, '/ugv/controller/status', self.ugv_status_callback, 10) self.uav_reached_sub = self.create_subscription( Bool, '/uav/waypoint_follower/waypoint_reached', self.uav_reached_callback, 10) self.ugv_reached_sub = self.create_subscription( Bool, '/ugv/goal_reached', self.ugv_reached_callback, 10) self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10) self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10) self.uav_waypoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10) self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10) self.uav_path_pub = self.create_publisher(Path, '/waypoint_follower/set_path', 10) self.status_pub = self.create_publisher(String, '/mission/status', 10) self.mission_timer = self.create_timer(0.5, self.mission_loop) self.get_logger().info('Mission Planner Started - LOCAL coordinates only') def command_callback(self, msg): cmd = msg.data.lower().split(':') action = cmd[0] if action == 'load': if len(cmd) > 1: self.load_mission_file(cmd[1]) else: self.load_demo_mission() elif action == 'start': self.start_mission() elif action == 'pause': self.state = MissionState.PAUSED elif action == 'resume': self.state = MissionState.EXECUTING elif action == 'abort': self.abort_mission() elif action == 'clear': self.mission = [] self.current_action_idx = 0 self.state = MissionState.IDLE def uav_status_callback(self, msg): pass def ugv_status_callback(self, msg): pass def uav_reached_callback(self, msg): if msg.data: self.uav_goal_reached = True def ugv_reached_callback(self, msg): if msg.data: self.ugv_goal_reached = True def load_demo_mission(self): self.mission = [ {'type': 'uav_command', 'command': 'arm'}, {'type': 'uav_command', 'command': 'takeoff'}, {'type': 'wait', 'duration': 3.0}, {'type': 'uav_waypoint', 'x': 10.0, 'y': 0.0, 'z': 5.0}, {'type': 'uav_waypoint', 'x': 10.0, 'y': 10.0, 'z': 5.0}, {'type': 'ugv_goal', 'x': 5.0, 'y': 5.0}, {'type': 'wait_for_vehicles'}, {'type': 'uav_waypoint', 'x': 0.0, 'y': 0.0, 'z': 5.0}, {'type': 'ugv_goal', 'x': 0.0, 'y': 0.0}, {'type': 'wait_for_vehicles'}, {'type': 'uav_command', 'command': 'land'}, ] self.current_action_idx = 0 self.state = MissionState.IDLE self.get_logger().info(f'Demo mission loaded: {len(self.mission)} actions') def load_mission_file(self, filepath): try: with open(filepath, 'r') as f: data = yaml.safe_load(f) self.mission = data.get('mission', []) self.current_action_idx = 0 self.state = MissionState.IDLE self.get_logger().info(f'Mission loaded from {filepath}') except Exception as e: self.get_logger().error(f'Failed to load mission: {e}') def start_mission(self): if len(self.mission) == 0: self.get_logger().warn('No mission loaded') return self.current_action_idx = 0 self.state = MissionState.EXECUTING self.uav_goal_reached = False self.ugv_goal_reached = False self.get_logger().info('Mission started') def abort_mission(self): self.state = MissionState.ABORTED stop_msg = String() stop_msg.data = 'hold' self.uav_cmd_pub.publish(stop_msg) stop_msg.data = 'stop' self.ugv_cmd_pub.publish(stop_msg) self.get_logger().warn('Mission aborted') def mission_loop(self): status_msg = String() status_msg.data = f'{self.state.name}|{self.current_action_idx}/{len(self.mission)}' self.status_pub.publish(status_msg) if self.state != MissionState.EXECUTING: return if self.current_action_idx >= len(self.mission): self.state = MissionState.COMPLETED self.get_logger().info('Mission completed!') return action = self.mission[self.current_action_idx] completed = self.execute_action(action) if completed: self.current_action_idx += 1 self.uav_goal_reached = False self.ugv_goal_reached = False def execute_action(self, action): action_type = action.get('type', '') if action_type == 'uav_command': cmd_msg = String() cmd_msg.data = action['command'] self.uav_cmd_pub.publish(cmd_msg) return True elif action_type == 'ugv_command': cmd_msg = String() cmd_msg.data = action['command'] self.ugv_cmd_pub.publish(cmd_msg) return True elif action_type == 'uav_waypoint': pose = PoseStamped() pose.header.stamp = self.get_clock().now().to_msg() pose.header.frame_id = 'odom' pose.pose.position.x = float(action['x']) pose.pose.position.y = float(action['y']) pose.pose.position.z = float(action['z']) pose.pose.orientation.w = 1.0 self.uav_waypoint_pub.publish(pose) return self.uav_goal_reached elif action_type == 'ugv_goal': pose = PoseStamped() pose.header.stamp = self.get_clock().now().to_msg() pose.header.frame_id = 'odom' pose.pose.position.x = float(action['x']) pose.pose.position.y = float(action['y']) pose.pose.orientation.w = 1.0 self.ugv_goal_pub.publish(pose) return self.ugv_goal_reached elif action_type == 'wait': if not hasattr(self, '_wait_start'): self._wait_start = self.get_clock().now() elapsed = (self.get_clock().now() - self._wait_start).nanoseconds / 1e9 if elapsed >= action['duration']: delattr(self, '_wait_start') return True return False elif action_type == 'wait_for_vehicles': return self.uav_goal_reached and self.ugv_goal_reached return True def main(args=None): rclpy.init(args=args) node = MissionPlanner() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()