Initial Commit

src/navigation/__init__.py

@@ -0,0 +1 @@
+"""Navigation module for local path planning and waypoint following."""

src/navigation/local_planner.py

@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+"""Local Planner - Path planning using relative coordinates only."""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped, Twist, Point
+from nav_msgs.msg import Path, Odometry
+from visualization_msgs.msg import Marker
+import numpy as np
+
+
+class LocalPlanner(Node):
+    
+    def __init__(self):
+        super().__init__('local_planner')
+        
+        self.declare_parameter('max_velocity', 2.0)
+        self.declare_parameter('max_acceleration', 1.0)
+        self.declare_parameter('lookahead_distance', 1.0)
+        self.declare_parameter('goal_tolerance', 0.3)
+        
+        self.max_velocity = self.get_parameter('max_velocity').value
+        self.max_acceleration = self.get_parameter('max_acceleration').value
+        self.lookahead_distance = self.get_parameter('lookahead_distance').value
+        self.goal_tolerance = self.get_parameter('goal_tolerance').value
+        
+        self.current_pose = None
+        self.current_velocity = np.zeros(3)
+        self.path = []
+        self.current_waypoint_idx = 0
+        
+        self.odom_sub = self.create_subscription(
+            Odometry, '/uav/local_position/odom', self.odom_callback, 10)
+        
+        self.goal_sub = self.create_subscription(
+            PoseStamped, '/uav/goal_pose', self.goal_callback, 10)
+        
+        self.path_sub = self.create_subscription(
+            Path, '/uav/planned_path', self.path_callback, 10)
+        
+        self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10)
+        self.setpoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10)
+        self.path_viz_pub = self.create_publisher(Path, '/uav/local_path', 10)
+        
+        self.timer = self.create_timer(0.05, self.control_loop)
+        
+        self.get_logger().info('Local Planner Started - GPS-denied navigation')
+    
+    def odom_callback(self, msg):
+        self.current_pose = msg.pose.pose
+        self.current_velocity = np.array([
+            msg.twist.twist.linear.x,
+            msg.twist.twist.linear.y,
+            msg.twist.twist.linear.z
+        ])
+    
+    def goal_callback(self, msg):
+        goal = np.array([
+            msg.pose.position.x,
+            msg.pose.position.y,
+            msg.pose.position.z
+        ])
+        self.path = [goal]
+        self.current_waypoint_idx = 0
+        self.get_logger().info(f'New goal received: [{goal[0]:.2f}, {goal[1]:.2f}, {goal[2]:.2f}]')
+    
+    def path_callback(self, msg):
+        self.path = []
+        for pose_stamped in msg.poses:
+            waypoint = np.array([
+                pose_stamped.pose.position.x,
+                pose_stamped.pose.position.y,
+                pose_stamped.pose.position.z
+            ])
+            self.path.append(waypoint)
+        self.current_waypoint_idx = 0
+        self.get_logger().info(f'Path received with {len(self.path)} waypoints')
+    
+    def control_loop(self):
+        if self.current_pose is None or len(self.path) == 0:
+            return
+        
+        if self.current_waypoint_idx >= len(self.path):
+            self.stop()
+            return
+        
+        current_pos = np.array([
+            self.current_pose.position.x,
+            self.current_pose.position.y,
+            self.current_pose.position.z
+        ])
+        
+        target = self.path[self.current_waypoint_idx]
+        distance = np.linalg.norm(target - current_pos)
+        
+        if distance < self.goal_tolerance:
+            self.current_waypoint_idx += 1
+            if self.current_waypoint_idx >= len(self.path):
+                self.get_logger().info('Path complete!')
+                self.stop()
+                return
+            target = self.path[self.current_waypoint_idx]
+        
+        direction = target - current_pos
+        distance = np.linalg.norm(direction)
+        
+        if distance > 0:
+            direction = direction / distance
+        
+        speed = min(self.max_velocity, distance * 0.5)
+        velocity = direction * speed
+        
+        self.publish_command(velocity, target)
+    
+    def publish_command(self, velocity, target):
+        cmd = Twist()
+        cmd.linear.x = float(velocity[0])
+        cmd.linear.y = float(velocity[1])
+        cmd.linear.z = float(velocity[2])
+        self.cmd_vel_pub.publish(cmd)
+        
+        setpoint = PoseStamped()
+        setpoint.header.stamp = self.get_clock().now().to_msg()
+        setpoint.header.frame_id = 'odom'
+        setpoint.pose.position.x = float(target[0])
+        setpoint.pose.position.y = float(target[1])
+        setpoint.pose.position.z = float(target[2])
+        setpoint.pose.orientation.w = 1.0
+        self.setpoint_pub.publish(setpoint)
+    
+    def stop(self):
+        cmd = Twist()
+        self.cmd_vel_pub.publish(cmd)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LocalPlanner()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/navigation/obstacle_avoidance.py

@@ -0,0 +1,160 @@
+#!/usr/bin/env python3
+"""Obstacle Avoidance - Vision-based obstacle detection and avoidance."""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image, LaserScan
+from geometry_msgs.msg import Twist, TwistStamped, Vector3
+from std_msgs.msg import Bool
+from cv_bridge import CvBridge
+import cv2
+import numpy as np
+
+
+class ObstacleAvoidance(Node):
+    
+    def __init__(self):
+        super().__init__('obstacle_avoidance')
+        
+        self.bridge = CvBridge()
+        
+        self.declare_parameter('detection_range', 5.0)
+        self.declare_parameter('safety_margin', 1.0)
+        self.declare_parameter('avoidance_gain', 1.0)
+        self.declare_parameter('enable', True)
+        
+        self.detection_range = self.get_parameter('detection_range').value
+        self.safety_margin = self.get_parameter('safety_margin').value
+        self.avoidance_gain = self.get_parameter('avoidance_gain').value
+        self.enabled = self.get_parameter('enable').value
+        
+        self.obstacle_detected = False
+        self.obstacle_direction = np.zeros(3)
+        self.obstacle_distance = float('inf')
+        
+        self.depth_sub = self.create_subscription(
+            Image, '/uav/camera/depth/image_raw', self.depth_callback, 10)
+        
+        self.scan_sub = self.create_subscription(
+            LaserScan, '/uav/scan', self.scan_callback, 10)
+        
+        self.cmd_vel_sub = self.create_subscription(
+            Twist, '/uav/cmd_vel', self.cmd_vel_callback, 10)
+        
+        self.enable_sub = self.create_subscription(
+            Bool, '/obstacle_avoidance/enable', self.enable_callback, 10)
+        
+        self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel_safe', 10)
+        self.obstacle_pub = self.create_publisher(Bool, '/obstacle_avoidance/obstacle_detected', 10)
+        self.avoidance_vec_pub = self.create_publisher(Vector3, '/obstacle_avoidance/avoidance_vector', 10)
+        
+        self.current_cmd = Twist()
+        
+        self.timer = self.create_timer(0.05, self.avoidance_loop)
+        
+        self.get_logger().info('Obstacle Avoidance Node Started')
+    
+    def enable_callback(self, msg):
+        self.enabled = msg.data
+    
+    def depth_callback(self, msg):
+        try:
+            depth_image = self.bridge.imgmsg_to_cv2(msg, 'passthrough')
+            
+            height, width = depth_image.shape
+            center_region = depth_image[height//3:2*height//3, width//3:2*width//3]
+            
+            valid_depths = center_region[center_region > 0]
+            if len(valid_depths) > 0:
+                min_distance = np.min(valid_depths)
+                
+                if min_distance < self.detection_range:
+                    self.obstacle_detected = True
+                    self.obstacle_distance = min_distance
+                    
+                    left_region = depth_image[:, :width//3]
+                    right_region = depth_image[:, 2*width//3:]
+                    
+                    left_clear = np.mean(left_region[left_region > 0]) if np.any(left_region > 0) else float('inf')
+                    right_clear = np.mean(right_region[right_region > 0]) if np.any(right_region > 0) else float('inf')
+                    
+                    if left_clear > right_clear:
+                        self.obstacle_direction = np.array([0.0, 1.0, 0.0])
+                    else:
+                        self.obstacle_direction = np.array([0.0, -1.0, 0.0])
+                else:
+                    self.obstacle_detected = False
+                    self.obstacle_distance = float('inf')
+                    
+        except Exception as e:
+            self.get_logger().error(f'Depth processing error: {e}')
+    
+    def scan_callback(self, msg):
+        ranges = np.array(msg.ranges)
+        valid_ranges = ranges[np.isfinite(ranges)]
+        
+        if len(valid_ranges) > 0:
+            min_range = np.min(valid_ranges)
+            min_idx = np.argmin(ranges)
+            
+            if min_range < self.detection_range:
+                self.obstacle_detected = True
+                self.obstacle_distance = min_range
+                
+                angle = msg.angle_min + min_idx * msg.angle_increment
+                self.obstacle_direction = np.array([
+                    -np.cos(angle),
+                    -np.sin(angle),
+                    0.0
+                ])
+            else:
+                self.obstacle_detected = False
+    
+    def cmd_vel_callback(self, msg):
+        self.current_cmd = msg
+    
+    def avoidance_loop(self):
+        obstacle_msg = Bool()
+        obstacle_msg.data = self.obstacle_detected
+        self.obstacle_pub.publish(obstacle_msg)
+        
+        if not self.enabled:
+            self.cmd_vel_pub.publish(self.current_cmd)
+            return
+        
+        safe_cmd = Twist()
+        
+        if self.obstacle_detected and self.obstacle_distance < self.safety_margin:
+            repulsion = self.avoidance_gain * (self.safety_margin / self.obstacle_distance)
+            avoidance = self.obstacle_direction * repulsion
+            
+            safe_cmd.linear.x = self.current_cmd.linear.x + float(avoidance[0])
+            safe_cmd.linear.y = self.current_cmd.linear.y + float(avoidance[1])
+            safe_cmd.linear.z = self.current_cmd.linear.z + float(avoidance[2])
+            safe_cmd.angular = self.current_cmd.angular
+            
+            vec_msg = Vector3()
+            vec_msg.x = float(avoidance[0])
+            vec_msg.y = float(avoidance[1])
+            vec_msg.z = float(avoidance[2])
+            self.avoidance_vec_pub.publish(vec_msg)
+        else:
+            safe_cmd = self.current_cmd
+        
+        self.cmd_vel_pub.publish(safe_cmd)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ObstacleAvoidance()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/navigation/waypoint_follower.py

@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+"""Waypoint Follower - Follows relative waypoints without GPS."""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped, Twist
+from nav_msgs.msg import Odometry, Path
+from std_msgs.msg import Int32, String, Bool
+import numpy as np
+from enum import Enum
+
+
+class WaypointState(Enum):
+    IDLE = 0
+    NAVIGATING = 1
+    REACHED = 2
+    PAUSED = 3
+
+
+class WaypointFollower(Node):
+    
+    def __init__(self):
+        super().__init__('waypoint_follower')
+        
+        self.declare_parameter('waypoint_radius', 0.5)
+        self.declare_parameter('max_velocity', 2.0)
+        self.declare_parameter('kp_linear', 0.8)
+        self.declare_parameter('kp_angular', 1.0)
+        
+        self.waypoint_radius = self.get_parameter('waypoint_radius').value
+        self.max_velocity = self.get_parameter('max_velocity').value
+        self.kp_linear = self.get_parameter('kp_linear').value
+        self.kp_angular = self.get_parameter('kp_angular').value
+        
+        self.waypoints = []
+        self.current_idx = 0
+        self.state = WaypointState.IDLE
+        self.current_pose = None
+        
+        self.odom_sub = self.create_subscription(
+            Odometry, '/uav/local_position/odom', self.odom_callback, 10)
+        
+        self.waypoint_sub = self.create_subscription(
+            PoseStamped, '/waypoint_follower/add_waypoint', self.add_waypoint_callback, 10)
+        
+        self.path_sub = self.create_subscription(
+            Path, '/waypoint_follower/set_path', self.set_path_callback, 10)
+        
+        self.command_sub = self.create_subscription(
+            String, '/waypoint_follower/command', self.command_callback, 10)
+        
+        self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10)
+        self.current_waypoint_pub = self.create_publisher(Int32, '/waypoint_follower/current_index', 10)
+        self.status_pub = self.create_publisher(String, '/waypoint_follower/status', 10)
+        self.reached_pub = self.create_publisher(Bool, '/waypoint_follower/waypoint_reached', 10)
+        
+        self.timer = self.create_timer(0.05, self.control_loop)
+        
+        self.get_logger().info('Waypoint Follower Started - Using LOCAL coordinates only')
+    
+    def odom_callback(self, msg):
+        self.current_pose = msg.pose.pose
+    
+    def add_waypoint_callback(self, msg):
+        waypoint = np.array([
+            msg.pose.position.x,
+            msg.pose.position.y,
+            msg.pose.position.z
+        ])
+        self.waypoints.append(waypoint)
+        self.get_logger().info(f'Added waypoint {len(self.waypoints)}: {waypoint}')
+    
+    def set_path_callback(self, msg):
+        self.waypoints = []
+        for pose_stamped in msg.poses:
+            waypoint = np.array([
+                pose_stamped.pose.position.x,
+                pose_stamped.pose.position.y,
+                pose_stamped.pose.position.z
+            ])
+            self.waypoints.append(waypoint)
+        self.current_idx = 0
+        self.get_logger().info(f'Set path with {len(self.waypoints)} waypoints')
+    
+    def command_callback(self, msg):
+        cmd = msg.data.lower()
+        
+        if cmd == 'start':
+            if len(self.waypoints) > 0:
+                self.state = WaypointState.NAVIGATING
+                self.get_logger().info('Starting waypoint navigation')
+        elif cmd == 'pause':
+            self.state = WaypointState.PAUSED
+            self.stop()
+        elif cmd == 'resume':
+            self.state = WaypointState.NAVIGATING
+        elif cmd == 'stop':
+            self.state = WaypointState.IDLE
+            self.stop()
+        elif cmd == 'clear':
+            self.waypoints = []
+            self.current_idx = 0
+            self.state = WaypointState.IDLE
+        elif cmd == 'next':
+            if self.current_idx < len(self.waypoints) - 1:
+                self.current_idx += 1
+        elif cmd == 'prev':
+            if self.current_idx > 0:
+                self.current_idx -= 1
+    
+    def control_loop(self):
+        self.publish_status()
+        
+        if self.state != WaypointState.NAVIGATING:
+            return
+        
+        if self.current_pose is None or len(self.waypoints) == 0:
+            return
+        
+        if self.current_idx >= len(self.waypoints):
+            self.state = WaypointState.IDLE
+            self.stop()
+            self.get_logger().info('All waypoints reached!')
+            return
+        
+        current_pos = np.array([
+            self.current_pose.position.x,
+            self.current_pose.position.y,
+            self.current_pose.position.z
+        ])
+        
+        target = self.waypoints[self.current_idx]
+        error = target - current_pos
+        distance = np.linalg.norm(error)
+        
+        if distance < self.waypoint_radius:
+            reached_msg = Bool()
+            reached_msg.data = True
+            self.reached_pub.publish(reached_msg)
+            
+            self.get_logger().info(f'Reached waypoint {self.current_idx + 1}/{len(self.waypoints)}')
+            self.current_idx += 1
+            return
+        
+        direction = error / distance
+        speed = min(self.kp_linear * distance, self.max_velocity)
+        velocity = direction * speed
+        
+        cmd = Twist()
+        cmd.linear.x = float(velocity[0])
+        cmd.linear.y = float(velocity[1])
+        cmd.linear.z = float(velocity[2])
+        
+        self.cmd_vel_pub.publish(cmd)
+    
+    def stop(self):
+        cmd = Twist()
+        self.cmd_vel_pub.publish(cmd)
+    
+    def publish_status(self):
+        idx_msg = Int32()
+        idx_msg.data = self.current_idx
+        self.current_waypoint_pub.publish(idx_msg)
+        
+        status_msg = String()
+        status_msg.data = f'{self.state.name}|{self.current_idx}/{len(self.waypoints)}'
+        self.status_pub.publish(status_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WaypointFollower()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()