Initial Commit

src/control/mission_planner.py

@@ -0,0 +1,236 @@
+#!/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()