Llm freq

README.md

@@ -77,6 +77,3 @@ ros2 run teleop_twist_keyboard teleop_twist_keyboard
 
 Then, press `i` to move forward, `u` and `o` to move forward and turn, `,` to move backwards, `m` and `.` to move backwards and turn, and `k` to stop in the terminal window running the teleop node.
 
-## About Us
-
-S7our Squad team at the University of Ibn Tofail, we work on autonomous systems (cars), led by professor BOUKIR Khawla. Current members composed of OKHADIR Hamza, GHIATI Mustapha, KHATIB Mouad and EL-LOUH Youssef.

src/gap_follow/CMakeLists.txt

@@ -3,7 +3,7 @@ project(gap_follow)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -30,14 +30,14 @@ endforeach()
 include_directories(include)
 
 # Create Cpp executable
-add_executable(reactive_node src/reactive_node.cpp)
-ament_target_dependencies(reactive_node 
+add_executable(reactive_node_cpp src/reactive_node.cpp)
+ament_target_dependencies(reactive_node_cpp 
   rclcpp geometry_msgs ackermann_msgs nav_msgs sensor_msgs std_msgs
 )
 
 # Install Cpp executables
 install(TARGETS
-  reactive_node
+  reactive_node_cpp
   DESTINATION lib/${PROJECT_NAME})
 
 # Install Python modules

src/gap_follow/gap_follow/__init__.py

@@ -0,0 +1 @@
+self.prev_steering = 0.0

src/gap_follow/gap_follow/gap_follow_node.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+from geometry_msgs.msg import Twist
+import math
+import numpy as np
+
+class GapFollowNode(Node):
+    def __init__(self):
+        super().__init__('gap_follow')
+
+        # Subscribe to LiDAR scan
+        self.scan_subscription = self.create_subscription(
+            LaserScan,
+            '/scan',
+            self.scan_callback,
+            10
+        )
+
+        # Publish velocity commands
+        self.velocity_publisher = self.create_publisher(
+            Twist,
+            '/cmd_vel',
+            10
+        )
+
+        # Parameters
+        self.declare_parameter('linear_velocity', 0.5)
+        self.declare_parameter('gap_threshold', 0.5)  # meters
+        self.declare_parameter('min_gap_width', 5)    # degrees
+        self.declare_parameter('max_angular_velocity', 1.5)
+
+        self.linear_velocity = self.get_parameter('linear_velocity').value
+        self.gap_threshold = self.get_parameter('gap_threshold').value
+        self.min_gap_width = self.get_parameter('min_gap_width').value
+        self.max_angular_velocity = self.get_parameter('max_angular_velocity').value
+
+        self.get_logger().info('Gap Follow Node started')
+        self.get_logger().info(f'Target gap threshold: {self.gap_threshold}m')
+
+    def scan_callback(self, msg: LaserScan):
+        """
+        Process LiDAR scan and find largest gap.
+        """
+        try:
+            # Convert ranges to numpy for easier processing
+            ranges = np.array(msg.ranges)
+
+            # Replace infinities with max range
+            ranges = np.nan_to_num(ranges, nan=msg.range_max, 
+                                  posinf=msg.range_max, 
+                                  neginf=msg.range_max)
+
+            # Find gaps (free space where range > threshold)
+            gaps = self.find_gaps(ranges, msg)
+
+            if gaps:
+                # Get largest gap
+                largest_gap = max(gaps, key=lambda x: x[1] - x[0])
+                gap_center = (largest_gap[0] + largest_gap[1]) / 2
+
+                # Calculate angle to gap center
+                gap_angle = (msg.angle_min + 
+                           gap_center * msg.angle_increment)
+
+                # Generate movement command toward gap
+                cmd = Twist()
+                cmd.linear.x = self.linear_velocity
+                cmd.angular.z = self.saturate_angular_velocity(
+                    gap_angle * 0.5, 
+                    self.max_angular_velocity
+                )
+
+                self.velocity_publisher.publish(cmd)
+
+                self.get_logger().debug(
+                    f'Gap found at {math.degrees(gap_angle):.2f}°'
+                )
+            else:
+                # No gap found - stop
+                self.get_logger().warn('No gap found - stopping')
+                cmd = Twist()
+                self.velocity_publisher.publish(cmd)
+
+        except Exception as e:
+            self.get_logger().error(f'Error in scan callback: {str(e)}')
+
+    def find_gaps(self, ranges, msg):
+        """
+        Find all gaps in range data.
+        A gap is a continuous region where range > gap_threshold.
+        Returns list of (start_index, end_index) tuples.
+        """
+        gaps = []
+        gap_start = None
+
+        for i, r in enumerate(ranges):
+            if r > self.gap_threshold:  # Free space
+                if gap_start is None:
+                    gap_start = i
+            else:  # Obstacle
+                if gap_start is not None:
+                    # Check gap width
+                    gap_width = i - gap_start
+                    gap_width_degrees = gap_width * math.degrees(msg.angle_increment)
+
+                    if gap_width_degrees >= self.min_gap_width:
+                        gaps.append((gap_start, i))
+                    gap_start = None
+
+        # Handle gap that extends to end of scan
+        if gap_start is not None:
+            gap_width = len(ranges) - gap_start
+            gap_width_degrees = gap_width * math.degrees(msg.angle_increment)
+
+            if gap_width_degrees >= self.min_gap_width:
+                gaps.append((gap_start, len(ranges)))
+
+        return gaps
+
+    def saturate_angular_velocity(self, angular_vel, max_vel):
+        """Limit angular velocity to maximum value."""
+        if angular_vel > max_vel:
+            return max_vel
+        elif angular_vel < -max_vel:
+            return -max_vel
+        return angular_vel
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    gap_follow_node = GapFollowNode()
+
+    try:
+        rclpy.spin(gap_follow_node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        gap_follow_node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/gap_follow/package.xml

@@ -16,6 +16,7 @@
   <depend>sensor_msgs</depend>
   <depend>ackermann_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>ydlidar_ros2_driver</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/gap_follow/scripts/reactive_node.py

@@ -1,72 +1,154 @@
 #!/usr/bin/env python3
+
 import rclpy
 from rclpy.node import Node
-
 import numpy as np
+
 from sensor_msgs.msg import LaserScan
-from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive
+from ackermann_msgs.msg import AckermannDriveStamped
+
 
 class ReactiveFollowGap(Node):
-    """ 
-    Implement Wall Following on the car
-    This is just a template, you are free to implement your own node!
-    """
+
     def __init__(self):
-        super().__init__('reactive_node')
-        # Topics & Subs, Pubs
-        lidarscan_topic = '/scan'
-        drive_topic = '/drive'
+        super().__init__('reactive_follow_gap')
+
+        # Topics
+        self.scan_topic = '/scan'
+        self.drive_topic = '/drive'
+
+        # ===== CAR + LIDAR TUNING (FOR YOUR SETUP) =====
+        self.max_range = 4.0                 # meters (X2 reliable range)
+        self.bubble_radius = 0.45            # meters (1/10 car safety)
+        self.max_steering_angle = 0.34       # rad (~19.5 deg)
+        self.steering_gain = 0.9
+        self.front_fov_deg = 70              # +- degrees
+        self.prev_steering = 0.0
 
-        # TODO: Subscribe to LIDAR
-        # TODO: Publish to drive
+        # ROS interfaces
+        self.scan_sub = self.create_subscription(
+            LaserScan,
+            self.scan_topic,
+            self.lidar_callback,
+            10
+        )
 
+        self.drive_pub = self.create_publisher(
+            AckermannDriveStamped,
+            self.drive_topic,
+            10
+        )
+
+        self.get_logger().info("Follow-the-Gap initialized (YDLIDAR X2, 1/10 RC)")
+
+    # ------------------------------------------------
     def preprocess_lidar(self, ranges):
-        """ Preprocess the LiDAR scan array. Expert implementation includes:
-            1.Setting each value to the mean over some window
-            2.Rejecting high values (eg. > 3m)
-        """
-        proc_ranges = ranges
-        return proc_ranges
-
-    def find_max_gap(self, free_space_ranges):
-        """ Return the start index & end index of the max gap in free_space_ranges
-        """
-        return None
-
-    def find_best_point(self, start_i, end_i, ranges):
-        """Start_i & end_i are start and end indicies of max-gap range, respectively
-        Return index of best point in ranges
-	    Naive: Choose the furthest point within ranges and go there
-        """
-        return None
+        ranges = np.array(ranges)
+        ranges[np.isnan(ranges)] = 0.0
+        ranges[np.isinf(ranges)] = self.max_range
+        ranges = np.clip(ranges, 0.0, self.max_range)
+        return ranges
+
+    # ------------------------------------------------
+    def find_max_gap(self, ranges):
+        gaps = []
+        start = None
+
+        for i, r in enumerate(ranges):
+            if r > 0 and start is None:
+                start = i
+            elif r == 0 and start is not None:
+                gaps.append((start, i - 1))
+                start = None
+
+        if start is not None:
+            gaps.append((start, len(ranges) - 1))
+
+        if not gaps:
+            return None, None
+
+        return max(gaps, key=lambda g: g[1] - g[0])
 
+    # ------------------------------------------------
+    def find_best_point(self, start, end, ranges):
+        segment = ranges[start:end + 1]
+        return start + np.argmax(segment)
+
+    # ------------------------------------------------
     def lidar_callback(self, data):
-        """ Process each LiDAR scan as per the Follow Gap algorithm & publish an AckermannDriveStamped Message
-        """
-        ranges = data.ranges
-        proc_ranges = self.preprocess_lidar(ranges)
-
-        # TODO:
-        #Find closest point to LiDAR
+        ranges = self.preprocess_lidar(data.ranges)
+
+        # ===== FRONT-ONLY FOV =====
+        front_angle = np.deg2rad(self.front_fov_deg)
+
+        start_i = int((-front_angle - data.angle_min) / data.angle_increment)
+        end_i   = int(( front_angle - data.angle_min) / data.angle_increment)
+
+        start_i = max(start_i, 0)
+        end_i   = min(end_i, len(ranges) - 1)
+
+        ranges = ranges[start_i:end_i]
+
+        # ===== FIND CLOSEST OBSTACLE =====
+        closest_idx = np.argmin(ranges)
+        closest_dist = ranges[closest_idx]
+
+        # ===== DISTANCE-AWARE BUBBLE =====
+        if closest_dist > 0.0:
+            bubble_angle = int(
+                self.bubble_radius /
+                (closest_dist * data.angle_increment)
+            )
+            bubble_start = max(0, closest_idx - bubble_angle)
+            bubble_end   = min(len(ranges) - 1, closest_idx + bubble_angle)
+            ranges[bubble_start:bubble_end] = 0.0
 
-        #Eliminate all points inside 'bubble' (set them to zero) 
+        # ===== FIND GAP =====
+        gap_start, gap_end = self.find_max_gap(ranges)
+        if gap_start is None:
+            return
 
-        #Find max length gap 
+        best_idx = self.find_best_point(gap_start, gap_end, ranges)
 
-        #Find the best point in the gap 
+        # ===== STEERING =====
+        angle = (
+            data.angle_min +
+            (best_idx + start_i) * data.angle_increment
+        ) * self.steering_gain
 
-        #Publish Drive message
+        angle = np.clip(angle,
+                         -self.max_steering_angle,
+                          self.max_steering_angle)
+
+        # Steering smoothing (ESC & traction safety)
+        angle = 0.7 * self.prev_steering + 0.3 * angle
+        self.prev_steering = angle
+
+        # ===== SPEED CONTROL (CRITICAL FOR 4300KV) =====
+        min_dist = np.min(ranges)
+
+        if min_dist < 0.8:
+            speed = 0.7
+        elif min_dist < 1.2:
+            speed = 1.1
+        else:
+            speed = 1.6
+
+        # ===== PUBLISH =====
+        drive_msg = AckermannDriveStamped()
+        drive_msg.drive.steering_angle = angle
+        drive_msg.drive.speed = speed
+
+        self.drive_pub.publish(drive_msg)
 
 
 def main(args=None):
     rclpy.init(args=args)
-    print("WallFollow Initialized")
-    reactive_node = ReactiveFollowGap()
-    rclpy.spin(reactive_node)
-
-    reactive_node.destroy_node()
+    node = ReactiveFollowGap()
+    rclpy.spin(node)
+    node.destroy_node()
     rclpy.shutdown()
 
 
 if __name__ == '__main__':
-    main()
+    main()