Initial Commit

src/control/__init__.py

@@ -0,0 +1 @@
+"""Control module for UAV and UGV controllers."""

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

src/control/uav_controller.py

@@ -0,0 +1,249 @@
+#!/usr/bin/env python3
+"""UAV Controller - Flight control using local position only."""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped, TwistStamped, Twist
+from mavros_msgs.srv import CommandBool, SetMode, CommandTOL
+from mavros_msgs.msg import State
+from std_msgs.msg import String, Bool
+import numpy as np
+from enum import Enum
+
+
+class FlightState(Enum):
+    DISARMED = 0
+    ARMED = 1
+    TAKING_OFF = 2
+    HOVERING = 3
+    NAVIGATING = 4
+    LANDING = 5
+    EMERGENCY = 6
+
+
+class UAVController(Node):
+    
+    def __init__(self):
+        super().__init__('uav_controller')
+        
+        self.declare_parameter('takeoff_altitude', 5.0)
+        self.declare_parameter('position_tolerance', 0.3)
+        self.declare_parameter('namespace', '/uav')
+        
+        self.takeoff_altitude = self.get_parameter('takeoff_altitude').value
+        self.position_tolerance = self.get_parameter('position_tolerance').value
+        ns = self.get_parameter('namespace').value
+        
+        self.state = FlightState.DISARMED
+        self.mavros_state = None
+        self.current_pose = None
+        self.target_pose = None
+        self.home_position = None
+        
+        self.state_sub = self.create_subscription(
+            State, f'{ns}/mavros/state', self.state_callback, 10)
+        
+        self.local_pose_sub = self.create_subscription(
+            PoseStamped, f'{ns}/mavros/local_position/pose', self.pose_callback, 10)
+        
+        self.cmd_sub = self.create_subscription(
+            String, f'{ns}/controller/command', self.command_callback, 10)
+        
+        self.setpoint_sub = self.create_subscription(
+            PoseStamped, f'{ns}/setpoint_position', self.setpoint_callback, 10)
+        
+        self.vel_sub = self.create_subscription(
+            Twist, f'{ns}/cmd_vel_safe', self.velocity_callback, 10)
+        
+        self.setpoint_pub = self.create_publisher(
+            PoseStamped, f'{ns}/mavros/setpoint_position/local', 10)
+        
+        self.vel_pub = self.create_publisher(
+            TwistStamped, f'{ns}/mavros/setpoint_velocity/cmd_vel', 10)
+        
+        self.status_pub = self.create_publisher(
+            String, f'{ns}/controller/status', 10)
+        
+        self.arming_client = self.create_client(CommandBool, f'{ns}/mavros/cmd/arming')
+        self.set_mode_client = self.create_client(SetMode, f'{ns}/mavros/set_mode')
+        self.takeoff_client = self.create_client(CommandTOL, f'{ns}/mavros/cmd/takeoff')
+        self.land_client = self.create_client(CommandTOL, f'{ns}/mavros/cmd/land')
+        
+        self.setpoint_timer = self.create_timer(0.05, self.publish_setpoint)
+        self.status_timer = self.create_timer(0.5, self.publish_status)
+        
+        self.get_logger().info('UAV Controller Started - GPS-denied mode')
+    
+    def state_callback(self, msg):
+        self.mavros_state = msg
+        if msg.armed and self.state == FlightState.DISARMED:
+            self.state = FlightState.ARMED
+    
+    def pose_callback(self, msg):
+        self.current_pose = msg
+        if self.home_position is None and self.mavros_state and self.mavros_state.armed:
+            self.home_position = np.array([
+                msg.pose.position.x,
+                msg.pose.position.y,
+                msg.pose.position.z
+            ])
+            self.get_logger().info(f'Home position set: {self.home_position}')
+    
+    def setpoint_callback(self, msg):
+        self.target_pose = msg
+    
+    def velocity_callback(self, msg):
+        if self.state not in [FlightState.NAVIGATING, FlightState.HOVERING]:
+            return
+        
+        vel_msg = TwistStamped()
+        vel_msg.header.stamp = self.get_clock().now().to_msg()
+        vel_msg.header.frame_id = 'base_link'
+        vel_msg.twist = msg
+        self.vel_pub.publish(vel_msg)
+    
+    def command_callback(self, msg):
+        cmd = msg.data.lower()
+        
+        if cmd == 'arm':
+            self.arm()
+        elif cmd == 'disarm':
+            self.disarm()
+        elif cmd == 'takeoff':
+            self.takeoff()
+        elif cmd == 'land':
+            self.land()
+        elif cmd == 'rtl':
+            self.return_to_launch()
+        elif cmd == 'hold':
+            self.hold_position()
+        elif cmd == 'guided':
+            self.set_mode('GUIDED')
+    
+    def arm(self):
+        if not self.arming_client.wait_for_service(timeout_sec=5.0):
+            self.get_logger().error('Arming service not available')
+            return False
+        
+        req = CommandBool.Request()
+        req.value = True
+        future = self.arming_client.call_async(req)
+        future.add_done_callback(self.arm_response)
+        return True
+    
+    def arm_response(self, future):
+        try:
+            result = future.result()
+            if result.success:
+                self.state = FlightState.ARMED
+                self.get_logger().info('Vehicle armed')
+            else:
+                self.get_logger().error('Arming failed')
+        except Exception as e:
+            self.get_logger().error(f'Arming error: {e}')
+    
+    def disarm(self):
+        req = CommandBool.Request()
+        req.value = False
+        future = self.arming_client.call_async(req)
+        self.state = FlightState.DISARMED
+    
+    def set_mode(self, mode):
+        if not self.set_mode_client.wait_for_service(timeout_sec=5.0):
+            return False
+        
+        req = SetMode.Request()
+        req.custom_mode = mode
+        future = self.set_mode_client.call_async(req)
+        return True
+    
+    def takeoff(self):
+        self.set_mode('GUIDED')
+        self.arm()
+        
+        if self.current_pose is not None:
+            self.target_pose = PoseStamped()
+            self.target_pose.header.frame_id = 'odom'
+            self.target_pose.pose.position.x = self.current_pose.pose.position.x
+            self.target_pose.pose.position.y = self.current_pose.pose.position.y
+            self.target_pose.pose.position.z = self.takeoff_altitude
+            self.target_pose.pose.orientation.w = 1.0
+            
+            self.state = FlightState.TAKING_OFF
+            self.get_logger().info(f'Taking off to {self.takeoff_altitude}m')
+    
+    def land(self):
+        if self.current_pose is not None:
+            self.target_pose = PoseStamped()
+            self.target_pose.header.frame_id = 'odom'
+            self.target_pose.pose.position.x = self.current_pose.pose.position.x
+            self.target_pose.pose.position.y = self.current_pose.pose.position.y
+            self.target_pose.pose.position.z = 0.0
+            self.target_pose.pose.orientation.w = 1.0
+            
+            self.state = FlightState.LANDING
+            self.get_logger().info('Landing')
+    
+    def return_to_launch(self):
+        if self.home_position is not None:
+            self.target_pose = PoseStamped()
+            self.target_pose.header.frame_id = 'odom'
+            self.target_pose.pose.position.x = float(self.home_position[0])
+            self.target_pose.pose.position.y = float(self.home_position[1])
+            self.target_pose.pose.position.z = self.takeoff_altitude
+            self.target_pose.pose.orientation.w = 1.0
+            
+            self.state = FlightState.NAVIGATING
+            self.get_logger().info('Returning to local home position')
+    
+    def hold_position(self):
+        if self.current_pose is not None:
+            self.target_pose = PoseStamped()
+            self.target_pose.header = self.current_pose.header
+            self.target_pose.pose = self.current_pose.pose
+            self.state = FlightState.HOVERING
+    
+    def publish_setpoint(self):
+        if self.target_pose is None:
+            return
+        
+        self.target_pose.header.stamp = self.get_clock().now().to_msg()
+        self.setpoint_pub.publish(self.target_pose)
+        
+        if self.current_pose is not None:
+            error = np.array([
+                self.target_pose.pose.position.x - self.current_pose.pose.position.x,
+                self.target_pose.pose.position.y - self.current_pose.pose.position.y,
+                self.target_pose.pose.position.z - self.current_pose.pose.position.z
+            ])
+            distance = np.linalg.norm(error)
+            
+            if self.state == FlightState.TAKING_OFF and distance < self.position_tolerance:
+                self.state = FlightState.HOVERING
+                self.get_logger().info('Takeoff complete')
+            elif self.state == FlightState.LANDING and self.current_pose.pose.position.z < 0.2:
+                self.state = FlightState.ARMED
+                self.get_logger().info('Landing complete')
+    
+    def publish_status(self):
+        status_msg = String()
+        armed = 'ARMED' if (self.mavros_state and self.mavros_state.armed) else 'DISARMED'
+        mode = self.mavros_state.mode if self.mavros_state else 'UNKNOWN'
+        status_msg.data = f'{self.state.name}|{armed}|{mode}'
+        self.status_pub.publish(status_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = UAVController()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/control/ugv_controller.py

@@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+"""UGV Controller - Ground vehicle control using local position."""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist, PoseStamped
+from nav_msgs.msg import Odometry
+from std_msgs.msg import String, Bool
+import numpy as np
+from enum import Enum
+
+
+class UGVState(Enum):
+    IDLE = 0
+    MOVING = 1
+    ROTATING = 2
+    STOPPED = 3
+
+
+class UGVController(Node):
+    
+    def __init__(self):
+        super().__init__('ugv_controller')
+        
+        self.declare_parameter('max_linear_velocity', 1.0)
+        self.declare_parameter('max_angular_velocity', 1.5)
+        self.declare_parameter('kp_linear', 0.8)
+        self.declare_parameter('kp_angular', 1.5)
+        self.declare_parameter('position_tolerance', 0.3)
+        self.declare_parameter('angle_tolerance', 0.1)
+        
+        self.max_linear_vel = self.get_parameter('max_linear_velocity').value
+        self.max_angular_vel = self.get_parameter('max_angular_velocity').value
+        self.kp_linear = self.get_parameter('kp_linear').value
+        self.kp_angular = self.get_parameter('kp_angular').value
+        self.position_tolerance = self.get_parameter('position_tolerance').value
+        self.angle_tolerance = self.get_parameter('angle_tolerance').value
+        
+        self.state = UGVState.IDLE
+        self.current_odom = None
+        self.target_pose = None
+        self.home_position = None
+        
+        self.odom_sub = self.create_subscription(
+            Odometry, '/ugv/odom', self.odom_callback, 10)
+        
+        self.goal_sub = self.create_subscription(
+            PoseStamped, '/ugv/goal_pose', self.goal_callback, 10)
+        
+        self.cmd_sub = self.create_subscription(
+            String, '/ugv/controller/command', self.command_callback, 10)
+        
+        self.vel_sub = self.create_subscription(
+            Twist, '/ugv/cmd_vel_safe', self.velocity_callback, 10)
+        
+        self.cmd_vel_pub = self.create_publisher(Twist, '/ugv/cmd_vel', 10)
+        self.status_pub = self.create_publisher(String, '/ugv/controller/status', 10)
+        self.goal_reached_pub = self.create_publisher(Bool, '/ugv/goal_reached', 10)
+        
+        self.control_timer = self.create_timer(0.05, self.control_loop)
+        self.status_timer = self.create_timer(0.5, self.publish_status)
+        
+        self.get_logger().info('UGV Controller Started - GPS-denied mode')
+    
+    def odom_callback(self, msg):
+        self.current_odom = msg
+        if self.home_position is None:
+            self.home_position = np.array([
+                msg.pose.pose.position.x,
+                msg.pose.pose.position.y
+            ])
+            self.get_logger().info(f'Home position set: {self.home_position}')
+    
+    def goal_callback(self, msg):
+        self.target_pose = msg
+        self.state = UGVState.MOVING
+        self.get_logger().info(
+            f'Goal received: [{msg.pose.position.x:.2f}, {msg.pose.position.y:.2f}]')
+    
+    def command_callback(self, msg):
+        cmd = msg.data.lower()
+        
+        if cmd == 'stop':
+            self.stop()
+            self.state = UGVState.STOPPED
+        elif cmd == 'resume':
+            self.state = UGVState.MOVING
+        elif cmd == 'rtl':
+            self.return_to_launch()
+    
+    def velocity_callback(self, msg):
+        if self.state == UGVState.MOVING:
+            self.cmd_vel_pub.publish(msg)
+    
+    def control_loop(self):
+        if self.state != UGVState.MOVING:
+            return
+        
+        if self.current_odom is None or self.target_pose is None:
+            return
+        
+        current_pos = np.array([
+            self.current_odom.pose.pose.position.x,
+            self.current_odom.pose.pose.position.y
+        ])
+        
+        target_pos = np.array([
+            self.target_pose.pose.position.x,
+            self.target_pose.pose.position.y
+        ])
+        
+        error = target_pos - current_pos
+        distance = np.linalg.norm(error)
+        
+        if distance < self.position_tolerance:
+            self.stop()
+            self.state = UGVState.IDLE
+            
+            reached_msg = Bool()
+            reached_msg.data = True
+            self.goal_reached_pub.publish(reached_msg)
+            
+            self.get_logger().info('Goal reached!')
+            return
+        
+        target_angle = np.arctan2(error[1], error[0])
+        
+        quat = self.current_odom.pose.pose.orientation
+        current_yaw = np.arctan2(
+            2.0 * (quat.w * quat.z + quat.x * quat.y),
+            1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z)
+        )
+        
+        angle_error = self.normalize_angle(target_angle - current_yaw)
+        
+        cmd = Twist()
+        
+        if abs(angle_error) > self.angle_tolerance:
+            cmd.angular.z = np.clip(
+                self.kp_angular * angle_error,
+                -self.max_angular_vel,
+                self.max_angular_vel
+            )
+            
+            if abs(angle_error) < np.pi / 4:
+                speed = self.kp_linear * distance * (1.0 - abs(angle_error) / np.pi)
+                cmd.linear.x = np.clip(speed, 0.0, self.max_linear_vel)
+        else:
+            cmd.linear.x = np.clip(
+                self.kp_linear * distance,
+                0.0,
+                self.max_linear_vel
+            )
+        
+        self.cmd_vel_pub.publish(cmd)
+    
+    def stop(self):
+        cmd = Twist()
+        self.cmd_vel_pub.publish(cmd)
+    
+    def return_to_launch(self):
+        if self.home_position is not None:
+            self.target_pose = PoseStamped()
+            self.target_pose.header.frame_id = 'odom'
+            self.target_pose.pose.position.x = float(self.home_position[0])
+            self.target_pose.pose.position.y = float(self.home_position[1])
+            self.target_pose.pose.orientation.w = 1.0
+            
+            self.state = UGVState.MOVING
+            self.get_logger().info('Returning to local home')
+    
+    def normalize_angle(self, angle):
+        while angle > np.pi:
+            angle -= 2 * np.pi
+        while angle < -np.pi:
+            angle += 2 * np.pi
+        return angle
+    
+    def publish_status(self):
+        status_msg = String()
+        status_msg.data = self.state.name
+        self.status_pub.publish(status_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = UGVController()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()