feat(acc): Autonomous Cruise Controller — lateral/longitudinal control with LiDAR braking and intersection compliance

data/navigator_default.rviz

@@ -29,6 +29,38 @@ Panels:
 Visualization Manager:
   Class: ""
   Displays:
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaneGrid
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 6
+      Size (m): 0.15000000596046
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /grid/lane/viz
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
     - Alpha: 0.30000001192092896
       Cell Size: 1
       Class: rviz_default_plugins/Grid

launches/launch.perception.py

@@ -40,10 +40,11 @@ def generate_launch_description():
         # occupancy_grid_node,
         # lane_type_detector,
         # pedestrian_skeleton
-        road_user_detector,
-        image_segmentation,
+        # road_user_detector,      # YOLO model required - disabled until model present
+        # image_segmentation,      # mmseg/PSPNet required - disabled until installed
         ground_seg,
         static_grid,
         hybrid_grid,
         perception_drivable_grid,
+        hybrid_drivable_grid,
     ])

launches/launch.vehicle.py

@@ -68,18 +68,18 @@ def generate_launch_description():
                         # rqt,
                         # camera_streamer,
                         # PERCEPTION
-                        #*perception_launch_entities,
+                        *perception_launch_entities,
                         # PLANNING
                         # routing_monitor,
                         routing_hardcoded,  # Use this to load a manual route saved as a csv. Comment out routing_monitor
                         grid_route_costmap,
                         grid_summation,
-                        #intersection_manager,
+                        intersection_manager,
                         # junction_manager,
                         path_planner,
                         # *nav2_launch_entities,
                         # path_planner_nav2,
-                        #pure_pursuit_controller,
+                        autonomous_cruise_controller,
                         # SAFETY
                         ##airbags,
                         ##guardian,

launches/launch_node_definitions.py

@@ -218,7 +218,7 @@
    	parameters=[],
 )
 
-image_segmentation = Node(
+image_seg_yolo = Node(
     package='image_segmentation',
     executable='image_seg_node'
 )
@@ -280,6 +280,13 @@
     output="screen",
 )
 
+hybrid_drivable_grid = Node(
+    package="segmentation",
+    executable="hybrid_drivable_grid_node",
+    name="hybrid_drivable_grid_node",
+    output="screen",
+)
+
 autonomous_cruise_intersection_controller = Node(
     package='autonomous_cruise',
     executable='autonomous_cruise_intersection_node',

param/autonomous_cruise_params.yaml

@@ -14,13 +14,13 @@ autonomous_cruise_controller:
     wheelbase: 2.875  # meters (typical sedan)
 
     # Lateral control (Pure Pursuit) parameters
-    min_lookahead: 3.0  # meters
+    min_lookahead: 6.0  # meters
     max_lookahead: 15.0  # meters
-    lookahead_gain: 0.5  # dimensionless (lookahead = min + gain * speed)
+    lookahead_gain: 0.3  # dimensionless (lookahead = min + gain * speed)
     max_steer: 1.0  # radians (±57 degrees)
 
     # Longitudinal control (PID) parameters
-    target_speed: 6.0  # m/s (~13.4 mph, ~21.6 km/h)
+    target_speed: 3.5  # m/s (~13.4 mph, ~21.6 km/h)
     kp: 1.2  # Proportional gain
     ki: 0.15  # Integral gain
     kd: 0.05  # Derivative gain

src/autonomous_cruise/autonomous_cruise/autonomous_cruise_node.py

@@ -14,11 +14,13 @@
 from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
 
 from nav_msgs.msg import Odometry, Path
-from navigator_msgs.msg import VehicleControl, Object3DArray
+from sensor_msgs.msg import PointCloud2
+from navigator_msgs.msg import VehicleControl, Object3DArray, VehicleSpeed, IntersectionBehavior
 from std_msgs.msg import String
 from geometry_msgs.msg import Pose
 
 import math
+import numpy as np
 from typing import Optional, Tuple
 
 from autonomous_cruise.lateral_controller import PurePursuitController
@@ -77,6 +79,10 @@ def __init__(self):
         self.current_path: Optional[Path] = None
         self.current_odom: Optional[Odometry] = None
         self.current_objects: Optional[Object3DArray] = None
+        self.current_speed: float = 0.0
+        self.intersection_action: str = 'Proceed'  # 'Wait' = stop, 'Proceed' = go
+        self.traffic_light_red: bool = False
+        self.lidar_obstacle_distance: float = float('inf')  # m to nearest forward obstacle
         self.last_control_time = self.get_clock().now()
         self.enabled = True
 
@@ -115,6 +121,33 @@ def __init__(self):
             qos_reliable
         )
 
+        self.speed_sub = self.create_subscription(
+            VehicleSpeed,
+            '/speed',
+            self.speed_callback,
+            qos_best_effort
+        )
+
+        self.intersection_sub = self.create_subscription(
+            IntersectionBehavior,
+            '/intersection',
+            self.intersection_callback,
+            qos_reliable
+        )
+        self.traffic_light_sub = self.create_subscription(
+            String,
+            "/carla/traffic_light_state",
+            self.traffic_light_callback,
+            qos_reliable
+        )
+
+        self.lidar_sub = self.create_subscription(
+            PointCloud2,
+            '/lidar/filtered',
+            self.lidar_callback,
+            qos_best_effort
+        )
+
         # Publishers
         self.control_pub = self.create_publisher(
             VehicleControl,
@@ -260,6 +293,57 @@ def objects_callback(self, msg: Object3DArray):
             f'Received {len(msg.objects)} detected objects'
         )
 
+    def speed_callback(self, msg: VehicleSpeed):
+        """Callback for vehicle speed from GNSS processor."""
+        self.current_speed = msg.speed
+
+    def intersection_callback(self, msg: IntersectionBehavior):
+        """Callback for intersection manager commands (Wait / Proceed)."""
+        self.intersection_action = msg.action
+
+    def traffic_light_callback(self, msg: String):
+        self.traffic_light_red = (msg.data == "Red")
+
+    def lidar_callback(self, msg: PointCloud2):
+        """Scan ground-segmented LiDAR for obstacles on the planned path only."""
+        if msg.width == 0 or msg.point_step == 0:
+            return
+        step = msg.point_step // 4
+        raw = np.frombuffer(bytes(msg.data), dtype=np.float32)
+        if len(raw) < step:
+            return
+        xs = raw[0::step]
+        ys = raw[1::step]
+        zs = raw[2::step]
+
+        # Pre-filter: ahead of bumper, not too far, above ground
+        # Near-field floor kept below STOP_DIST=2.0m: the old 2.5m cutoff
+        # made obstacles between 2.0-2.5m invisible, so the controller saw
+        # 'no obstacle' and re-accelerated into whatever it had just stopped
+        # for. 0.5m still excludes ego-vehicle/hood returns.
+        pre = (xs > 0.5) & (xs < 20.0) & (zs > 0.3)
+        if not pre.any():
+            self.lidar_obstacle_distance = float('inf')
+            return
+        xs_f, ys_f = xs[pre], ys[pre]
+
+        path = self.current_path
+        if path is not None and len(path.poses) > 1:
+            wx = np.array([p.pose.position.x for p in path.poses])
+            wy = np.array([p.pose.position.y for p in path.poses])
+            ahead = (wx > 1.0) & (wx < 20.0)
+            if ahead.any():
+                wx, wy = wx[ahead], wy[ahead]
+                dx = xs_f[:, None] - wx[None, :]
+                dy = ys_f[:, None] - wy[None, :]
+                on_path = np.sqrt(dx**2 + dy**2).min(axis=1) < 0.6
+                self.lidar_obstacle_distance = float(xs_f[on_path].min()) if on_path.sum() >= 5 else float('inf')
+                return
+
+        # Fallback: tight rectangular corridor when no path available
+        mask = (np.abs(ys_f) < 0.9)
+        self.lidar_obstacle_distance = float(xs_f[mask].min()) if mask.sum() >= 5 else float('inf')
+
     def control_loop(self):
         """Main control loop executed at control_rate Hz."""
         if not self.enabled:
@@ -290,17 +374,27 @@ def control_loop(self):
 
         # Extract current state
         current_pose = self.current_odom.pose.pose
-        current_velocity = self.current_odom.twist.twist.linear
-        current_speed = math.sqrt(
-            current_velocity.x**2 + current_velocity.y**2
-        )
+        current_speed = self.current_speed
 
         # Lateral control (steering)
         steer, cross_track_error = self.lateral_controller.compute_steering(
             current_pose,
             current_speed
         )
 
+        # LiDAR-based speed limit — scale down as obstacle approaches
+        SLOW_DIST = 7.0   # m — begin decelerating
+        STOP_DIST = 2.0   # m — full stop
+        CREEP = 1.0       # m/s — minimum speed while obstacle present (lets planner replan)
+        d = self.lidar_obstacle_distance
+        if d < STOP_DIST:
+            self.longitudinal_controller.target_speed = 0.0
+        elif d < SLOW_DIST:
+            ratio = (d - STOP_DIST) / (SLOW_DIST - STOP_DIST)
+            self.longitudinal_controller.target_speed = max(CREEP, self.target_speed * ratio)
+        else:
+            self.longitudinal_controller.target_speed = self.target_speed
+
         # Longitudinal control (throttle/brake)
         throttle, brake, target_speed = (
             self.longitudinal_controller.compute_control(
@@ -310,6 +404,11 @@ def control_loop(self):
             )
         )
 
+        # Intersection / traffic light override — stop on red
+        if self.intersection_action == 'Wait' or self.traffic_light_red:
+            throttle = 0.0
+            brake = 1.0
+
         # Create and publish control message
         control_msg = VehicleControl()
         control_msg.header.stamp = current_time.to_msg()

src/autonomous_cruise/autonomous_cruise/lateral_controller.py

@@ -141,16 +141,14 @@ def _find_target_point(
         if self.path is None:
             return None, -1
 
-        current_x = current_pose.position.x
-        current_y = current_pose.position.y
-
+        # Path is in base_link frame; vehicle is at the origin
         # Find the closest point on the path
         min_dist = float('inf')
         closest_idx = 0
 
         for i, pose_stamped in enumerate(self.path.poses):
-            dx = pose_stamped.pose.position.x - current_x
-            dy = pose_stamped.pose.position.y - current_y
+            dx = pose_stamped.pose.position.x
+            dy = pose_stamped.pose.position.y
             dist = math.sqrt(dx * dx + dy * dy)
 
             if dist < min_dist:
@@ -160,8 +158,8 @@ def _find_target_point(
         # Search forward from closest point for lookahead point
         for i in range(closest_idx, len(self.path.poses)):
             pose_stamped = self.path.poses[i]
-            dx = pose_stamped.pose.position.x - current_x
-            dy = pose_stamped.pose.position.y - current_y
+            dx = pose_stamped.pose.position.x
+            dy = pose_stamped.pose.position.y
             dist = math.sqrt(dx * dx + dy * dy)
 
             if dist >= lookahead_dist:
@@ -188,18 +186,9 @@ def _compute_pure_pursuit_steering(
         Returns:
             Steering angle (radians).
         """
-        # Transform target point to vehicle frame
-        dx = target_point.x - current_pose.position.x
-        dy = target_point.y - current_pose.position.y
-
-        # Get vehicle heading from quaternion
-        yaw = self._quaternion_to_yaw(current_pose.orientation)
-
-        # Rotate to vehicle frame
-        cos_yaw = math.cos(-yaw)
-        sin_yaw = math.sin(-yaw)
-        target_x = dx * cos_yaw - dy * sin_yaw
-        target_y = dx * sin_yaw + dy * cos_yaw
+        # Path is in base_link frame; target_point is already vehicle-local
+        target_x = target_point.x
+        target_y = target_point.y
 
         # Compute lookahead distance
         ld = math.sqrt(target_x * target_x + target_y * target_y)
@@ -210,7 +199,7 @@ def _compute_pure_pursuit_steering(
         # Pure Pursuit formula: steering = atan(2 * L * sin(alpha) / ld)
         # where alpha is the angle to the target point
         alpha = math.atan2(target_y, target_x)
-        steering = math.atan2(2.0 * self.wheelbase * math.sin(alpha), ld)
+        steering = -math.atan2(2.0 * self.wheelbase * math.sin(alpha), ld)
 
         return steering
 
@@ -227,8 +216,9 @@ def _compute_cross_track_error(self, current_pose: Pose) -> float:
         if self.path is None or len(self.path.poses) < 2:
             return 0.0
 
-        current_x = current_pose.position.x
-        current_y = current_pose.position.y
+        # Vehicle is at origin in base_link frame
+        current_x = 0.0
+        current_y = 0.0
 
         min_dist = float('inf')
 

src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py

@@ -24,9 +24,9 @@
 
 class Constants:
     # Look ahead distance in meters
-    LD: float = 3.0
+    LD: float = 6.0
     # Look forward gain in meters (gain in look ahead distance per m/s of speed)
-    kf: float = 0.1
+    kf: float = 0.3
     # Wheel base (distance between front and rear wheels) in meter
     WHEEL_BASE: float = 3.5
     # Max throttle and acceleration (out of 1)

src/msg/navigator_msgs/msg/LaneGrid.msg

@@ -0,0 +1,15 @@
+std_msgs/Header header
+uint32 width
+uint32 height
+float32 resolution
+geometry_msgs/Pose origin
+
+# Row-major, size width*height. -1 = unknown/non-drivable, 0..N-1 = lane index left-to-right.
+int16[] cell_lane_id
+
+# Row-major, size width*height. Per-cell confidence, 0-100.
+uint8[] cell_confidence
+
+int32 total_lane_count
+int32 ego_lane_index
+float32 ego_lane_width_m