diff --git a/data/navigator_default.rviz b/data/navigator_default.rviz index 3c31e798d..92991b80d 100644 --- a/data/navigator_default.rviz +++ b/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 diff --git a/launches/launch.perception.py b/launches/launch.perception.py index 3af931166..3545678f7 100644 --- a/launches/launch.perception.py +++ b/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, ]) diff --git a/launches/launch.vehicle.py b/launches/launch.vehicle.py index 74db8185f..269205e7e 100644 --- a/launches/launch.vehicle.py +++ b/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, diff --git a/launches/launch_node_definitions.py b/launches/launch_node_definitions.py index 696ff2077..21a9bd8a4 100644 --- a/launches/launch_node_definitions.py +++ b/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', diff --git a/param/autonomous_cruise_params.yaml b/param/autonomous_cruise_params.yaml index c418d3a8e..f44485579 100644 --- a/param/autonomous_cruise_params.yaml +++ b/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 diff --git a/src/autonomous_cruise/autonomous_cruise/autonomous_cruise_node.py b/src/autonomous_cruise/autonomous_cruise/autonomous_cruise_node.py old mode 100644 new mode 100755 index f9d3a4505..29258ac8d --- a/src/autonomous_cruise/autonomous_cruise/autonomous_cruise_node.py +++ b/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,10 +374,7 @@ 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( @@ -301,6 +382,19 @@ def control_loop(self): 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() diff --git a/src/autonomous_cruise/autonomous_cruise/lateral_controller.py b/src/autonomous_cruise/autonomous_cruise/lateral_controller.py index 5d1153569..b455a713f 100644 --- a/src/autonomous_cruise/autonomous_cruise/lateral_controller.py +++ b/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') diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 385e99da0..114f8d895 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/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) diff --git a/src/msg/navigator_msgs/msg/LaneGrid.msg b/src/msg/navigator_msgs/msg/LaneGrid.msg new file mode 100644 index 000000000..a1f5ff561 --- /dev/null +++ b/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 diff --git a/src/perception/segmentation/segmentation/bev_geometry.py b/src/perception/segmentation/segmentation/bev_geometry.py new file mode 100644 index 000000000..fbf37342b --- /dev/null +++ b/src/perception/segmentation/segmentation/bev_geometry.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +""" +bev_geometry.py — shared camera-to-BEV projection geometry. + +Extracted from perception_drivable_grid_node.py so every grid-publishing node +(drivable grid, lane grid, ...) stays pixel-aligned on the same 300x300 +@0.2m/cell base_link grid and the same hardcoded camera extrinsics, instead of +each node re-deriving its own projection. + +Cameras (hardcoded from carla_objects.json — camera TF frames do not exist): + K: fx=fy=571.12, cx=400, cy=300 (fov=70, 800x600) + R_cb(theta) = [[sin t, 0, cos t], [-cos t, 0, sin t], [0, -1, 0]] + front: t=(0.7,-0.15,1.88) yaw=0 /semantic/front + right: t=(0.7,-0.15,1.88) yaw=-70 /semantic/right + left: t=(0.7, 0.15,1.88) yaw=+70 /semantic/left + back: t=(-1.5,0.0, 1.88) yaw=180 /semantic/back (needs rgb_back in carla_objects.json) +""" + +import math + +import numpy as np + +GRID_SIZE = 300 +RESOLUTION = 0.2 +ORIGIN_X = -20.0 +ORIGIN_Y = -30.0 +MAX_PROJ_DIST = 40.0 + +VEHICLE_COL = int((0.0 - ORIGIN_X) / RESOLUTION) # 100 +VEHICLE_ROW = int((0.0 - ORIGIN_Y) / RESOLUTION) # 150 + +FX = FY = 571.12 +CX, CY = 400.0, 300.0 +IMG_W, IMG_H = 800, 600 + +K = np.array([[FX, 0.0, CX], [0.0, FY, CY], [0.0, 0.0, 1.0]], dtype=np.float64) +K_INV = np.linalg.inv(K) + + +def R_cb(yaw_deg): + t = math.radians(yaw_deg) + s, c = math.sin(t), math.cos(t) + return np.array([[s, 0, c], [-c, 0, s], [0, -1, 0]], dtype=np.float64) + + +CAMERAS = [ + ('front', '/semantic/front', np.array([ 0.70, -0.15, 1.88]), R_cb( 0.0)), + ('right', '/semantic/right', np.array([ 0.70, -0.15, 1.88]), R_cb(-70.0)), + ('left', '/semantic/left', np.array([ 0.70, 0.15, 1.88]), R_cb( 70.0)), + ('back', '/semantic/back', np.array([-1.50, 0.00, 1.88]), R_cb(180.0)), +] + + +class CamLUT: + """Precomputed per-pixel -> BEV grid cell mapping for one camera.""" + + __slots__ = ('gc', 'gr', 'valid', 'img_h', 'img_w') + + def __init__(self, t_base, R_cb_mat, img_h=IMG_H, img_w=IMG_W): + us, vs = np.arange(img_w, dtype=np.float64), np.arange(img_h, dtype=np.float64) + uu, vv = np.meshgrid(us, vs) + uvh = np.stack([uu.ravel(), vv.ravel(), np.ones(img_h * img_w)]) + rays_cam = (K_INV @ uvh).T + ray_base = (R_cb_mat @ rays_cam.T).T.astype(np.float32) + + rz = ray_base[:, 2] + safe = np.abs(rz) > 1e-6 + with np.errstate(divide='ignore', invalid='ignore'): + lam = np.where(safe, -float(t_base[2]) / rz, 0.0) + + px = float(t_base[0]) + lam * ray_base[:, 0] + py = float(t_base[1]) + lam * ray_base[:, 1] + + self.gc = ((px - ORIGIN_X) / RESOLUTION).astype(np.int32) + self.gr = ((py - ORIGIN_Y) / RESOLUTION).astype(np.int32) + d2d = np.hypot(px, py) + self.valid = (safe & (lam > 0.0) & (d2d < MAX_PROJ_DIST) + & (self.gc >= 0) & (self.gc < GRID_SIZE) + & (self.gr >= 0) & (self.gr < GRID_SIZE)) + self.img_h, self.img_w = img_h, img_w diff --git a/src/perception/segmentation/segmentation/hybrid_drivable_grid_node.py b/src/perception/segmentation/segmentation/hybrid_drivable_grid_node.py index 068f813c5..fbf0d9b57 100644 --- a/src/perception/segmentation/segmentation/hybrid_drivable_grid_node.py +++ b/src/perception/segmentation/segmentation/hybrid_drivable_grid_node.py @@ -76,7 +76,7 @@ def __init__(self): self.pub = self.create_publisher(OccupancyGrid, '/grid/drivable', 10) - self.create_timer(0.1, self._publish_loop) + self.create_timer(0.05, self._publish_loop) self._last_hdmap_stamp = None diff --git a/src/perception/segmentation/segmentation/hybrid_perception_grid_node.py b/src/perception/segmentation/segmentation/hybrid_perception_grid_node.py index 4e7f8ea6b..99ac8a800 100644 --- a/src/perception/segmentation/segmentation/hybrid_perception_grid_node.py +++ b/src/perception/segmentation/segmentation/hybrid_perception_grid_node.py @@ -143,7 +143,7 @@ def __init__(self): # 1-Hz LUT rebuild timer -- retries until all cameras have good LUTs self.create_timer(1.0, self._lut_timer) # 10-Hz publish timer - self.create_timer(0.1, self._publish_loop) + self.create_timer(0.05, self._publish_loop) self.get_logger().info('HybridPerceptionGridNode ready (4-camera + segmented LiDAR).') diff --git a/src/perception/segmentation/segmentation/image_segmentation_node.py b/src/perception/segmentation/segmentation/image_segmentation_node.py index 07a9f1e12..173de6691 100644 --- a/src/perception/segmentation/segmentation/image_segmentation_node.py +++ b/src/perception/segmentation/segmentation/image_segmentation_node.py @@ -60,8 +60,8 @@ class ImageSegmentationNode(Node): def __init__(self): super().__init__('image_segmentation_node') - self.get_logger().info('Loading PSPNet on CPU…') - self.model = init_model(_CONFIG, _CKPT, device='cpu') + self.get_logger().info("Loading PSPNet on GPU (cuda:1, kept off GPU0 to avoid contending with CARLA rendering)…") + self.model = init_model(_CONFIG, _CKPT, device="cuda:1") self.bridge = CvBridge() self.get_logger().info('PSPNet ready.') @@ -98,12 +98,21 @@ def _loop(self): import time n = len(self._order) self.get_logger().info('Inference thread running (round-robin 4 cameras).') + # Track the last frame stamp we actually ran inference on per camera. + # On GPU, inference is fast enough that without this check the loop + # re-processes and re-publishes the same cached frame hundreds of + # times a second while waiting for the camera's next real frame, + # flooding downstream perception nodes and the DDS graph. Only run + # inference when a genuinely new frame has arrived. + last_stamp = {cam: None for cam in self._order} while True: cam = self._order[self._idx] with self._lock: msg, pub = self._latest[cam] - if msg is not None: + stamp = msg.header.stamp if msg is not None else None + if msg is not None and stamp != last_stamp[cam]: + last_stamp[cam] = stamp try: img = self.bridge.imgmsg_to_cv2(msg, 'rgb8')[:, :, :3] result = inference_model(self.model, img) @@ -115,7 +124,7 @@ def _loop(self): except Exception as e: self.get_logger().error(f'Inference error on {cam}: {e}') else: - time.sleep(0.05) + time.sleep(0.01) self._idx = (self._idx + 1) % n diff --git a/src/perception/segmentation/segmentation/lane_grid_node.py b/src/perception/segmentation/segmentation/lane_grid_node.py new file mode 100644 index 000000000..4464c27e2 --- /dev/null +++ b/src/perception/segmentation/segmentation/lane_grid_node.py @@ -0,0 +1,259 @@ +#!/usr/bin/env python3 +""" +lane_grid_node.py — lane-indexed BEV grid, built without map_management. + +Pipeline (10 Hz, same rate/geometry as perception_drivable_grid_node so the +two grids overlay exactly): + 1. Parse /lidar/filtered ground-level points, score lane-marking-ness by + LiDAR intensity (lane paint reads brighter than bare asphalt — + independent of the camera lighting/shadow failures that make + /grid/drivable/segmented imperfect). + 2. Temporally blend that per-frame marking evidence (same EMA + + pose-compensation pattern as perception_drivable_grid_node's + evidence grid) so sparse per-frame LiDAR hits accumulate into stable + marking lines instead of flickering. + 3. Cut the latest /grid/drivable/segmented mask along those marking + lines and label the connected components as lanes + (lane_segmentation.segment_lanes), ordered by lateral (row) position + at the ego's column. + 4. Publish /grid/lane (navigator_msgs/LaneGrid) with per-cell lane id + + confidence plus total_lane_count / ego_lane_index / ego_lane_width_m. + +/lane_types/detections (lane_type_detector) is used only as a validation +cross-check (both are independently noisy, per-frame heuristics) — logged, +not fused into the published grid. +""" + +import math +import threading + +import cv2 +import numpy as np +import rclpy +from nav_msgs.msg import OccupancyGrid, Odometry +from navigator_msgs.msg import AllLaneDetections, LaneGrid +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import PointCloud2, PointField + +from segmentation.bev_geometry import GRID_SIZE, RESOLUTION, ORIGIN_X, ORIGIN_Y +from segmentation.lane_segmentation import ( + intensity_lane_evidence, segment_lanes, count_and_locate_ego, +) + +DRIVABLE_OCC_MAX = 50 # /grid/drivable/segmented cell counts as drivable if occ < this + +ALPHA_BLEND = 0.65 # same constant family as perception_drivable_grid_node +VALIDATION_LOG_PERIOD = 10 # log lane-count agreement every N publish ticks (~1 Hz at 10 Hz) + +# LaneGrid.msg has no native RViz display (it's a custom message type), so we +# also publish a colorized PointCloud2 debug view on /grid/lane/viz — one +# distinct color per lane id, viewable with a stock rviz_default_plugins/ +# PointCloud2 display (Color Transformer: RGB8), same pattern already used +# for /lidar/filtered in navigator_default.rviz. +_LANE_COLORS = np.array([ + (230, 25, 75), (60, 180, 75), (255, 225, 25), (0, 130, 200), + (245, 130, 48), (145, 30, 180), (70, 240, 240), (240, 50, 230), +], dtype=np.uint8) + + +def _build_lane_viz_cloud(lane_id_grid, stamp): + rows, cols = np.nonzero(lane_id_grid >= 0) + msg = PointCloud2() + msg.header.stamp = stamp + msg.header.frame_id = 'base_link' + msg.height = 1 + msg.fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), + ] + msg.is_bigendian = False + msg.point_step = 16 + msg.is_dense = True + if rows.size == 0: + msg.width = 0 + msg.row_step = 0 + msg.data = b'' + return msg + + xs = (ORIGIN_X + (cols + 0.5) * RESOLUTION).astype(np.float32) + ys = (ORIGIN_Y + (rows + 0.5) * RESOLUTION).astype(np.float32) + zs = np.zeros_like(xs) + + lane_ids = lane_id_grid[rows, cols].astype(np.int64) + palette = _LANE_COLORS[lane_ids % len(_LANE_COLORS)] + rgb_uint32 = (palette[:, 0].astype(np.uint32) << 16 | + palette[:, 1].astype(np.uint32) << 8 | + palette[:, 2].astype(np.uint32)) + rgb_float = rgb_uint32.view(np.float32) + + pts = np.column_stack([xs, ys, zs, rgb_float]).astype(np.float32) + msg.width = pts.shape[0] + msg.row_step = msg.point_step * pts.shape[0] + msg.data = pts.tobytes() + return msg + + +def _parse_xyzi(msg): + """Extract an Nx4 (x, y, z, intensity) array from a PointCloud2, reading + field offsets dynamically (mirrors perception_drivable_grid_node's + _lidar_evidence_grid) rather than assuming a fixed struct layout. + Returns None if the cloud has no intensity field.""" + n = msg.width * msg.height + if n == 0: + return None + offsets = {} + for f in msg.fields: + if f.name in ('x', 'y', 'z', 'intensity'): + offsets[f.name] = f.offset + if not {'x', 'y', 'z', 'intensity'} <= offsets.keys(): + return None + ps = msg.point_step + raw = np.frombuffer(bytes(msg.data), dtype=np.uint8).reshape(n, ps) + cols = [] + for name in ('x', 'y', 'z', 'intensity'): + off = offsets[name] + cols.append(np.frombuffer(raw[:, off:off + 4].tobytes(), dtype=np.float32)) + points = np.stack(cols, axis=1) + ok = np.isfinite(points).all(axis=1) + return points[ok] + + +class LaneGridNode(Node): + + def __init__(self): + super().__init__('lane_grid_node') + self._lock = threading.Lock() + + self._marking_evidence = np.zeros((GRID_SIZE, GRID_SIZE), dtype=np.float32) + self._drivable_mask = np.zeros((GRID_SIZE, GRID_SIZE), dtype=bool) + + self._last_x = self._last_y = self._last_yaw = None + self._latest_detector_count = None + self._tick = 0 + + qos_be = rclpy.qos.QoSProfile( + reliability=rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT, + history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST, depth=1) + + self.create_subscription(PointCloud2, '/lidar/filtered', self._cb_lidar, qos_be) + self.create_subscription(OccupancyGrid, '/grid/drivable/segmented', self._cb_drivable, qos_be) + self.create_subscription(Odometry, '/gnss/odometry', self._cb_odom, qos_be) + self.create_subscription(AllLaneDetections, '/lane_types/detections', + self._cb_lane_detections, qos_be) + + self.pub = self.create_publisher(LaneGrid, '/grid/lane', 10) + self.viz_pub = self.create_publisher(PointCloud2, '/grid/lane/viz', 10) + self.create_timer(0.05, self._publish_loop) + self.get_logger().info('LaneGridNode ready — /grid/lane, /grid/lane/viz') + + # ── callbacks ───────────────────────────────────────────────────────────── + + def _cb_lidar(self, msg): + points = _parse_xyzi(msg) + if points is None: + return + frame_marking = intensity_lane_evidence(points) + with self._lock: + # Lane markings default to "absent" whether observed or not, so a + # single EMA (no separate unobserved-decay branch) is enough — + # unlike drivable-area evidence, there's no neutral prior to hold. + self._marking_evidence = (ALPHA_BLEND * self._marking_evidence + + (1 - ALPHA_BLEND) * frame_marking) + + def _cb_drivable(self, msg): + grid = np.array(msg.data, dtype=np.int16).reshape(GRID_SIZE, GRID_SIZE) + with self._lock: + self._drivable_mask = grid < DRIVABLE_OCC_MAX + + def _cb_lane_detections(self, msg): + if not msg.lane_detections: + return + with self._lock: + self._latest_detector_count = msg.lane_detections[-1].totallanecount + + def _cb_odom(self, msg): + p, q = msg.pose.pose.position, msg.pose.pose.orientation + x, y = p.x, p.y + _, _, yaw = Rotation.from_quat([q.x, q.y, q.z, q.w]).as_euler('xyz') + with self._lock: + if self._last_x is None: + self._last_x, self._last_y, self._last_yaw = x, y, yaw + return + self._compensate_pose(x, y, yaw) + self._last_x, self._last_y, self._last_yaw = x, y, yaw + + def _compensate_pose(self, x, y, yaw): + """Warp the persistent marking-evidence grid for ego motion between + frames — identical pattern to perception_drivable_grid_node's + _compensate_pose, so both grids stay aligned the same way.""" + dx = x - self._last_x + dy = y - self._last_y + dyaw = (yaw - self._last_yaw + math.pi) % (2 * math.pi) - math.pi + if abs(dx) < 0.02 and abs(dy) < 0.02 and abs(dyaw) < 0.008: + return + sc = -dx / RESOLUTION + sr = -dy / RESOLUTION + vc = (0.0 - ORIGIN_X) / RESOLUTION + vr = (0.0 - ORIGIN_Y) / RESOLUTION + ca, sa = math.cos(-dyaw), math.sin(-dyaw) + m = np.float32([[ca, -sa, sc + vc * (1 - ca) + vr * sa], + [sa, ca, sr - vc * sa + vr * (1 - ca)]]) + self._marking_evidence = cv2.warpAffine( + self._marking_evidence, m, (GRID_SIZE, GRID_SIZE), + flags=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT, borderValue=0.0) + + # ── publish loop ────────────────────────────────────────────────────────── + + def _publish_loop(self): + with self._lock: + drivable_mask = self._drivable_mask.copy() + marking_evidence = self._marking_evidence.copy() + detector_count = self._latest_detector_count + + lane_id_grid, confidence_grid = segment_lanes(drivable_mask, marking_evidence) + total_lane_count, ego_lane_index, ego_lane_width_m = count_and_locate_ego(lane_id_grid) + + self._tick += 1 + if detector_count is not None and self._tick % VALIDATION_LOG_PERIOD == 0: + agree = 'yes' if detector_count == total_lane_count else 'no' + self.get_logger().info( + f'lane count check — grid={total_lane_count} ' + f'lane_type_detector={detector_count} agree={agree}') + + msg = LaneGrid() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'base_link' + msg.width = GRID_SIZE + msg.height = GRID_SIZE + msg.resolution = RESOLUTION + msg.origin.position.x = ORIGIN_X + msg.origin.position.y = ORIGIN_Y + msg.origin.position.z = 0.0 + msg.origin.orientation.w = 1.0 + msg.cell_lane_id = lane_id_grid.ravel().tolist() + msg.cell_confidence = confidence_grid.ravel().tolist() + msg.total_lane_count = total_lane_count + msg.ego_lane_index = ego_lane_index + msg.ego_lane_width_m = ego_lane_width_m + self.pub.publish(msg) + + self.viz_pub.publish(_build_lane_viz_cloud(lane_id_grid, msg.header.stamp)) + + +def main(args=None): + rclpy.init(args=args) + node = LaneGridNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception/segmentation/segmentation/lane_segmentation.py b/src/perception/segmentation/segmentation/lane_segmentation.py new file mode 100644 index 000000000..e3a1341fa --- /dev/null +++ b/src/perception/segmentation/segmentation/lane_segmentation.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 +""" +lane_segmentation.py — pure numpy/scipy lane-indexing algorithm. + +No ROS/rclpy/cv2 dependency so this can be unit-tested standalone. The ROS +node wrapper (lane_grid_node.py) supplies real sensor data and handles +temporal smoothing; this module only reasons about a single frame. + +Grid axis convention (matches perception_drivable_grid_node.py — see its +FOOTPRINT_HALF_W being applied to the row range and FOOTPRINT_REAR/FRONT to +the column range): row = lateral offset, column = longitudinal/forward +offset. Lanes sit side by side across ROWS at a given column, so "which lane +am I in" and "how many lanes are there" are read off a COLUMN slice (fixed +column, varying row) — not a row slice. + +Pipeline for one frame: + 1. intensity_lane_evidence — turn ground-level LiDAR intensity into a + per-cell "lane marking" score. Retro-reflective paint reads back + brighter than bare asphalt, independent of camera lighting/shadows, + so this is a channel the drivable grid's camera-driven false + positives/negatives don't share. + 2. segment_lanes — cut the drivable mask along marking-evidence barriers + and label the resulting connected components as lanes, ordered by + increasing row (lateral position) at the ego column. + 3. count_and_locate_ego — read off total lane count, ego's lane index, + and ego lane width from the labeled grid. +""" + +import numpy as np +from scipy.ndimage import label as sp_label + +from segmentation.bev_geometry import ( + GRID_SIZE, RESOLUTION, ORIGIN_X, ORIGIN_Y, VEHICLE_COL, VEHICLE_ROW, +) + +# Ground band for LiDAR points considered for lane-marking evidence — same +# convention as perception_drivable_grid_node's LIDAR_GROUND_Z/-0.30 floor. +GROUND_Z_MIN = -0.30 +GROUND_Z_MAX = 0.35 + +MARKING_MIN_HITS = 2 # need >= 2 ground returns in a cell to trust its intensity ratio +MARKING_PERCENTILE = 85.0 # "bright" = top 15% of intensities in this frame's ground points + +MARKING_THRESHOLD = 0.5 # marking_evidence >= this cuts the drivable mask + +CONF_ASSIGNED = 90 # cell belongs to a lane component connected to the ego column +CONF_UNASSIGNED = 30 # cell is drivable but not connected to any ego-column lane +CONF_NONE = 0 # cell is not drivable + + +def intensity_lane_evidence(points_xyzi, grid_size=GRID_SIZE): + """points_xyzi: Nx4 array of (x, y, z, intensity) in base_link. + + Returns a (grid_size, grid_size) float32 grid in [0, 1]: the fraction of + ground-level LiDAR returns in each cell whose intensity is in the top + MARKING_PERCENTILE of this frame's ground returns. Cells with fewer than + MARKING_MIN_HITS ground returns are left at 0 (no evidence either way). + """ + evidence = np.zeros((grid_size, grid_size), dtype=np.float32) + if points_xyzi is None or len(points_xyzi) == 0: + return evidence + + x, y, z, intensity = (points_xyzi[:, 0], points_xyzi[:, 1], + points_xyzi[:, 2], points_xyzi[:, 3]) + ground = (z >= GROUND_Z_MIN) & (z < GROUND_Z_MAX) & np.isfinite(x) & np.isfinite(y) + x, y, intensity = x[ground], y[ground], intensity[ground] + if len(intensity) == 0: + return evidence + + gc = ((x - ORIGIN_X) / RESOLUTION).astype(np.int32) # column = longitudinal (x) + gr = ((y - ORIGIN_Y) / RESOLUTION).astype(np.int32) # row = lateral (y) + inb = (gc >= 0) & (gc < grid_size) & (gr >= 0) & (gr < grid_size) + gc, gr, intensity = gc[inb], gr[inb], intensity[inb] + if len(intensity) == 0: + return evidence + + thresh = np.percentile(intensity, MARKING_PERCENTILE) + bright = intensity >= thresh + + total_cnt = np.zeros((grid_size, grid_size), dtype=np.int32) + bright_cnt = np.zeros((grid_size, grid_size), dtype=np.int32) + np.add.at(total_cnt, (gr, gc), 1) + if bright.any(): + np.add.at(bright_cnt, (gr[bright], gc[bright]), 1) + + trusted = total_cnt >= MARKING_MIN_HITS + with np.errstate(divide='ignore', invalid='ignore'): + ratio = np.where(trusted, bright_cnt / np.maximum(total_cnt, 1), 0.0) + evidence[:] = np.clip(ratio, 0.0, 1.0) + return evidence + + +def segment_lanes(drivable_mask, marking_evidence, + ego_row=VEHICLE_ROW, ego_col=VEHICLE_COL, + marking_threshold=MARKING_THRESHOLD): + """drivable_mask: (H, W) bool, True where the drivable grid says drivable. + marking_evidence: (H, W) float in [0, 1] from intensity_lane_evidence. + + Cuts drivable_mask along cells where marking_evidence >= marking_threshold, + labels the remaining connected components (4-connectivity), and assigns + lane ids by increasing row (lateral position) to whichever components + touch the ego's column — components that don't touch the ego column + (e.g. a visible cross-street) are left unassigned (-1) rather than + guessed at. + + Returns (lane_id_grid int16, confidence_grid uint8), both (H, W). + """ + h, w = drivable_mask.shape + lane_id_grid = np.full((h, w), -1, dtype=np.int16) + confidence_grid = np.zeros((h, w), dtype=np.uint8) + + barrier = marking_evidence >= marking_threshold + traversable = drivable_mask & ~barrier + labeled, _ = sp_label(traversable) + + veh_label = labeled[ego_row, ego_col] if traversable[ego_row, ego_col] else 0 + if veh_label == 0: + # Ego cell itself isn't traversable (e.g. sitting on/near a marking) — + # fall back to the nearest traversable cell in the ego column. + col_labels = labeled[:, ego_col] + nonzero_rows = np.flatnonzero(col_labels) + if len(nonzero_rows): + nearest_row = nonzero_rows[np.argmin(np.abs(nonzero_rows - ego_row))] + veh_label = col_labels[nearest_row] + + confidence_grid[drivable_mask] = CONF_UNASSIGNED + + if veh_label != 0: + col_labels = labeled[:, ego_col] + # Increasing-row order of distinct components touching the ego column. + ordered_labels = [] + seen = set() + for lbl in col_labels: + if lbl != 0 and lbl not in seen: + seen.add(lbl) + ordered_labels.append(lbl) + label_to_lane = {lbl: i for i, lbl in enumerate(ordered_labels)} + + for lbl, lane_id in label_to_lane.items(): + mask = labeled == lbl + lane_id_grid[mask] = lane_id + confidence_grid[mask] = CONF_ASSIGNED + + confidence_grid[~drivable_mask] = CONF_NONE + return lane_id_grid, confidence_grid + + +def count_and_locate_ego(lane_id_grid, ego_row=VEHICLE_ROW, ego_col=VEHICLE_COL, + resolution=RESOLUTION): + """Reads total lane count, ego's lane index, and ego lane width off the + ego column of a labeled lane_id_grid. Returns -1/-1/0.0 if the ego cell + isn't assigned to any lane.""" + col = lane_id_grid[:, ego_col] + valid_ids = sorted(int(v) for v in np.unique(col) if v >= 0) + total_lane_count = len(valid_ids) + + ego_id = int(lane_id_grid[ego_row, ego_col]) + if ego_id < 0: + return total_lane_count, -1, 0.0 + + ego_lane_index = valid_ids.index(ego_id) + lane_width_cells = int(np.count_nonzero(col == ego_id)) + lane_width_m = lane_width_cells * resolution + return total_lane_count, ego_lane_index, lane_width_m diff --git a/src/perception/segmentation/segmentation/perception_drivable_grid_node.py b/src/perception/segmentation/segmentation/perception_drivable_grid_node.py index 6c8c2ee00..0ca6b24a2 100644 --- a/src/perception/segmentation/segmentation/perception_drivable_grid_node.py +++ b/src/perception/segmentation/segmentation/perception_drivable_grid_node.py @@ -59,8 +59,11 @@ CC_UNCERTAIN_HI = 0.65 CC_FILL_VALUE = 0.70 # evidence assigned to flood-fill-reached uncertain cells -ALPHA_BLEND = 0.65 -DECAY_RATE = 0.992 +ALPHA_BLEND = 0.8062 # sqrt(0.65): publish loop moved 10Hz->20Hz; + # rescaled so the EMA keeps the same real-time + # smoothing constant instead of reacting 2x faster + # to per-frame camera noise +DECAY_RATE = 0.9960 # sqrt(0.992), same reasoning INIT_EVIDENCE = 0.50 PATH_HISTORY_MAXLEN = 50 # ~12 s at 4 Hz odom = 800 bytes @@ -180,7 +183,7 @@ def __init__(self): self.create_subscription(Odometry, '/gnss/odometry', self._cb_odom, qos_be) self.pub = self.create_publisher(OccupancyGrid, '/grid/drivable/segmented', 10) - self.create_timer(0.1, self._publish_loop) + self.create_timer(0.05, self._publish_loop) self.get_logger().info('PerceptionDrivableGridNode ready — /grid/drivable/segmented') # ── callbacks ───────────────────────────────────────────────────────────── diff --git a/src/perception/segmentation/setup.py b/src/perception/segmentation/setup.py index e0b4fe511..87ec51630 100755 --- a/src/perception/segmentation/setup.py +++ b/src/perception/segmentation/setup.py @@ -27,7 +27,8 @@ 'image_projection_node = segmentation.image_projection_node:main', 'hybrid_perception_grid_node = segmentation.hybrid_perception_grid_node:main', 'perception_drivable_grid_node = segmentation.perception_drivable_grid_node:main', - 'hybrid_drivable_grid_node = segmentation.hybrid_drivable_grid_node:main', + "hybrid_drivable_grid_node = segmentation.hybrid_drivable_grid_node:main", + "lane_grid_node = segmentation.lane_grid_node:main", ], }, ) diff --git a/src/perception/segmentation/test/test_lane_segmentation.py b/src/perception/segmentation/test/test_lane_segmentation.py new file mode 100644 index 000000000..269ab6812 --- /dev/null +++ b/src/perception/segmentation/test/test_lane_segmentation.py @@ -0,0 +1,159 @@ +"""Unit tests for lane_segmentation.py — pure numpy/scipy, no ROS required. + +Run with: python3 -m pytest src/perception/segmentation/test/test_lane_segmentation.py +""" + +import os +import sys + +import numpy as np +import pytest + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) + +from segmentation.bev_geometry import GRID_SIZE, RESOLUTION, ORIGIN_X, ORIGIN_Y, VEHICLE_COL, VEHICLE_ROW +from segmentation import lane_segmentation as ls + + +def make_three_lane_drivable_mask(lane_width_cells=15, gap_cells=1): + """3 lanes side by side across rows, centered on VEHICLE_ROW, spanning + all columns. Returns (drivable_mask, lane_row_ranges) where + lane_row_ranges is a list of (start, end) row ranges, one per lane, + ego-lane in the middle.""" + mask = np.zeros((GRID_SIZE, GRID_SIZE), dtype=bool) + lane_row_ranges = [] + r = VEHICLE_ROW - (3 * lane_width_cells + 2 * gap_cells) // 2 + for _ in range(3): + start = r + end = r + lane_width_cells + mask[start:end, :] = True + lane_row_ranges.append((start, end)) + r = end + gap_cells + return mask, lane_row_ranges + + +def marking_lines_at_gaps(lane_row_ranges, grid_size=GRID_SIZE): + """Synthetic marking-evidence grid: a barrier row between each lane gap.""" + evidence = np.zeros((grid_size, grid_size), dtype=np.float32) + for i in range(len(lane_row_ranges) - 1): + boundary_row = lane_row_ranges[i][1] # the gap row between lane i and i+1 + evidence[boundary_row, :] = 1.0 + return evidence + + +def test_intensity_lane_evidence_flags_bright_ground_points(): + # Two "paint lines" at y = -2m and y = +2m (rows), dim background elsewhere. + # Step (0.05m) finer than RESOLUTION (0.2m) so multiple points land in + # each grid cell -> clears the MARKING_MIN_HITS trust threshold. + xs = np.tile(np.arange(-15.0, 15.0, 0.05), 3) + n_per_line = len(np.arange(-15.0, 15.0, 0.05)) + ys = np.concatenate([np.full(n_per_line, -2.0), + np.full(n_per_line, 0.0), + np.full(n_per_line, 2.0)]) + zs = np.zeros_like(xs) + intensity = np.concatenate([np.full(n_per_line, 200.0), # bright paint line + np.full(n_per_line, 10.0), # dim asphalt + np.full(n_per_line, 200.0)]) # bright paint line + points = np.stack([xs, ys, zs, intensity], axis=1) + + evidence = ls.intensity_lane_evidence(points) + + gr_bright1 = int((-2.0 - ORIGIN_Y) / RESOLUTION) + gr_dim = int((0.0 - ORIGIN_Y) / RESOLUTION) + gr_bright2 = int((2.0 - ORIGIN_Y) / RESOLUTION) + + assert evidence[gr_bright1, VEHICLE_COL] > 0.5 + assert evidence[gr_bright2, VEHICLE_COL] > 0.5 + assert evidence[gr_dim, VEHICLE_COL] < 0.5 + + +def test_intensity_lane_evidence_empty_input(): + evidence = ls.intensity_lane_evidence(np.zeros((0, 4))) + assert evidence.shape == (GRID_SIZE, GRID_SIZE) + assert not evidence.any() + + evidence_none = ls.intensity_lane_evidence(None) + assert not evidence_none.any() + + +def test_segment_lanes_splits_three_clean_lanes(): + mask, lane_row_ranges = make_three_lane_drivable_mask() + marking = marking_lines_at_gaps(lane_row_ranges) + + lane_id_grid, confidence_grid = ls.segment_lanes(mask, marking) + total, ego_idx, width_m = ls.count_and_locate_ego(lane_id_grid) + + assert total == 3 + assert ego_idx == 1 # ego sits in the middle lane + assert width_m == pytest.approx(15 * RESOLUTION) + assert confidence_grid[VEHICLE_ROW, VEHICLE_COL] == ls.CONF_ASSIGNED + + +def test_segment_lanes_robust_to_false_negative_gap_in_drivable_mask(): + """Drivable-grid false negative: a chunk of the ego's own lane is + incorrectly marked non-drivable (e.g. a shadow). The lane should still + be found and correctly counted since most of the lane region survives.""" + mask, lane_row_ranges = make_three_lane_drivable_mask() + marking = marking_lines_at_gaps(lane_row_ranges) + + # Punch a hole in the ego lane away from the ego's own column. + mask[lane_row_ranges[1][0]:lane_row_ranges[1][1], 40:60] = False + + lane_id_grid, confidence_grid = ls.segment_lanes(mask, marking) + total, ego_idx, width_m = ls.count_and_locate_ego(lane_id_grid) + + assert total == 3 + assert ego_idx == 1 + assert confidence_grid[VEHICLE_ROW, VEHICLE_COL] == ls.CONF_ASSIGNED + + +def test_segment_lanes_robust_to_false_positive_blob_outside_road(): + """Drivable-grid false positive: an isolated drivable blob far from the + road (e.g. misclassified sidewalk) must not get counted as a 4th lane, + since it's not connected to the ego's lane component.""" + mask, lane_row_ranges = make_three_lane_drivable_mask() + marking = marking_lines_at_gaps(lane_row_ranges) + + # Isolated blob, disconnected from the road (surrounded by non-drivable). + mask[250:260, 250:260] = True + + lane_id_grid, confidence_grid = ls.segment_lanes(mask, marking) + total, ego_idx, width_m = ls.count_and_locate_ego(lane_id_grid) + + assert total == 3 + assert ego_idx == 1 + # The blob is drivable but disconnected from any ego-column lane -> unassigned. + assert lane_id_grid[255, 255] == -1 + assert confidence_grid[255, 255] == ls.CONF_UNASSIGNED + + +def test_segment_lanes_no_drivable_area(): + mask = np.zeros((GRID_SIZE, GRID_SIZE), dtype=bool) + marking = np.zeros((GRID_SIZE, GRID_SIZE), dtype=np.float32) + + lane_id_grid, confidence_grid = ls.segment_lanes(mask, marking) + total, ego_idx, width_m = ls.count_and_locate_ego(lane_id_grid) + + assert total == 0 + assert ego_idx == -1 + assert width_m == 0.0 + assert not confidence_grid.any() + + +def test_segment_lanes_single_lane_no_markings_detected(): + """If no marking evidence is found (e.g. worn-out paint) and the drivable + strip is contiguous (no physical gap), it stays one lane rather than + being split incorrectly.""" + mask = np.zeros((GRID_SIZE, GRID_SIZE), dtype=bool) + mask[VEHICLE_ROW - 22:VEHICLE_ROW + 23, :] = True # one wide contiguous strip + marking = np.zeros((GRID_SIZE, GRID_SIZE), dtype=np.float32) # no markings found + + lane_id_grid, confidence_grid = ls.segment_lanes(mask, marking) + total, ego_idx, width_m = ls.count_and_locate_ego(lane_id_grid) + + assert total == 1 + assert ego_idx == 0 + + +if __name__ == '__main__': + sys.exit(pytest.main([__file__, '-v'])) diff --git a/src/planning/costs/costs/route_costmap_node.py b/src/planning/costs/costs/route_costmap_node.py index 7071558e3..675d8e715 100644 --- a/src/planning/costs/costs/route_costmap_node.py +++ b/src/planning/costs/costs/route_costmap_node.py @@ -81,7 +81,7 @@ def __init__(self): # DiagnosticStatus, '/node_status', 1) # self.status = DiagnosticStatus() - self.costmap_timer = self.create_timer(0.15, self.buildRouteCostmap, callback_group=MutuallyExclusiveCallbackGroup()) + self.costmap_timer = self.create_timer(0.05, self.buildRouteCostmap, callback_group=MutuallyExclusiveCallbackGroup()) self.clock_sub = self.create_subscription( Clock, '/clock', self.clockCb, 1) @@ -118,10 +118,21 @@ def _is_drivable(self, x_bl: float, y_bl: float) -> bool: return False return int(self._drivable_grid[row, col]) < 90 + # Perception evidence is EMA-smoothed and continuous (see + # perception_drivable_grid_node's ALPHA_BLEND) — a genuinely confirmed + # cell asymptotically approaches but rarely lands on *exactly* 0 + # (that needs evidence > 0.995, ~20+ consecutive high-confidence frames + # on the same cell). Requiring exact equality made "confirmed" flicker + # almost at random as the vehicle moved and cells entered/left view, + # which was the real cause of the wild goal jumps / constant pathfinding + # failures. Use a tolerant threshold instead, consistent with + # _is_drivable's `< 90` style check. + CONFIRMED_THRESHOLD = 30 + def _is_camera_confirmed_drivable(self, x_bl: float, y_bl: float) -> bool: - """Return True only if drivable_grid == 0 at this cell. - drivable == 0 means perception actively classified it as road surface. - drivable == 50 means HD-map default / unmapped — beyond camera horizon.""" + """Return True if drivable_grid is confidently road surface at this cell + (<= CONFIRMED_THRESHOLD). drivable == 50 means HD-map default / + unmapped — beyond camera horizon.""" if self._drivable_grid is None: return False # conservative: no perception data yet col = int(round((x_bl + 20.0) / 0.2)) @@ -130,7 +141,7 @@ def _is_camera_confirmed_drivable(self, x_bl: float, y_bl: float) -> bool: return False if not (0 <= col < self._drivable_grid.shape[1]): return False - return int(self._drivable_grid[row, col]) == 0 + return int(self._drivable_grid[row, col]) <= self.CONFIRMED_THRESHOLD # TODO: currently implemented, the route cannot be changed once it is first received def routeCb(self, msg: Path): @@ -285,24 +296,30 @@ def buildRouteCostmap(self): # Gradient corridor: centerline lowest cost, padding slightly higher. # Path hugs the exact route center; deviates only when an obstacle # (occupancy=100 -> sc=100 via np.maximum) blocks the centerline. - HALF_W = 6 # padding half-width cells (1.2m at 0.2m/cell) + HALF_W = 10 # lane half-width cells (2.0m at 0.2m/cell) CENTER_CONFIRMED = 0 # exact route centerline, camera confirmed CENTER_UNCONFIRMED = 20 # exact route centerline, HD-map only - SIDE_CONFIRMED = 10 # side padding band, camera confirmed - SIDE_UNCONFIRMED = 30 # side padding band, HD-map only + EDGE_CONFIRMED = 10 # lane-edge cost, camera confirmed + EDGE_UNCONFIRMED = 30 # lane-edge cost, HD-map only for r in range(len(gridxs)): confirmed = self._is_camera_confirmed_drivable(gridxs[r], gridys[r]) center_val = CENTER_CONFIRMED if confirmed else CENTER_UNCONFIRMED - side_val = SIDE_CONFIRMED if confirmed else SIDE_UNCONFIRMED + edge_val = EDGE_CONFIRMED if confirmed else EDGE_UNCONFIRMED ci = int(round((gridys[r] + veh_lat) / gridres)) cj = int(round((gridxs[r] + veh_long) / gridres)) - # Paint padding band first, then stamp centerline on top + # Radial gradient: cost is lowest exactly on the centerline and + # ramps up smoothly to edge_val at the lane edge (HALF_W cells + # out), instead of a flat padding band. Biases the planner to + # hug lane center and only drift outward when something (an + # obstacle) forces it to. for di in range(-HALF_W, HALF_W + 1): for dj in range(-HALF_W, HALF_W + 1): ni, nj = ci + di, cj + dj if 0 <= ni < grid_rows and 0 <= nj < grid_cols: - if routemap[ni, nj] > side_val: - routemap[ni, nj] = side_val + frac = min(1.0, np.sqrt(di * di + dj * dj) / HALF_W) + cell_val = center_val + (edge_val - center_val) * frac + if routemap[ni, nj] > cell_val: + routemap[ni, nj] = cell_val # Exact centerline always lowest cost if 0 <= ci < grid_rows and 0 <= cj < grid_cols: if routemap[ci, cj] > center_val: diff --git a/src/planning/path_planners/path_planners/dijkstra_path_planner.py b/src/planning/path_planners/path_planners/dijkstra_path_planner.py index d0592c613..b8a44bbb1 100644 --- a/src/planning/path_planners/path_planners/dijkstra_path_planner.py +++ b/src/planning/path_planners/path_planners/dijkstra_path_planner.py @@ -7,28 +7,51 @@ def __init__(self): pass def shortest_path(self, costmap, start, end, obstacle_threshold=90): - # Fast heapq Dijkstra — no NetworkX graph construction. - # Operates directly on the numpy costmap array. + # A* with Euclidean heuristic + bounding-box pruning. + # Replaces plain Dijkstra — same interface, ~20x faster on 300x300 grids. + # Admissible: h = Euclidean distance, min edge cost = 1.0 (cardinal step, cell=0). if costmap[start] >= obstacle_threshold or costmap[end] >= obstacle_threshold: return None rows, cols = costmap.shape + MARGIN = 40 # cell buffer beyond start/goal bbox to allow obstacle detours + + r_lo = max(0, min(start[0], end[0]) - MARGIN) + r_hi = min(rows - 1, max(start[0], end[0]) + MARGIN) + c_lo = max(0, min(start[1], end[1]) - MARGIN) + c_hi = min(cols - 1, max(start[1], end[1]) + MARGIN) + dist = np.full((rows, cols), np.inf, dtype=np.float64) dist[start] = 0.0 prev = {} - heap = [(0.0, start)] DIRS = [(-1,-1),(-1,0),(-1,1),(0,-1),(0,1),(1,-1),(1,0),(1,1)] SQRT2 = 1.4142135623730951 + er, ec = end + + # Weighted A*: the true per-step cost is (cell_val + 1), which for + # off-corridor cells (~50) is far larger than the unit-cost Euclidean + # heuristic assumes, so the heuristic is very loose and the search + # expands close to the full MARGIN bounding box every call. Scaling + # the heuristic keeps it goal-directed so it actually finishes inside + # the 20Hz planning budget; bounded suboptimality is an acceptable + # trade here since the costmap gradient still pulls the path to the + # lane center. + WEIGHT = 3.0 + + def h(r, c): + return WEIGHT * ((r - er) ** 2 + (c - ec) ** 2) ** 0.5 + + heap = [(h(*start), 0.0, start)] while heap: - d, (r, c) = heapq.heappop(heap) + _, d, (r, c) = heapq.heappop(heap) if (r, c) == end: break if d > dist[r, c]: continue for dr, dc in DIRS: nr, nc = r + dr, c + dc - if not (0 <= nr < rows and 0 <= nc < cols): + if not (r_lo <= nr <= r_hi and c_lo <= nc <= c_hi): continue cell_val = int(costmap[nr, nc]) if cell_val >= obstacle_threshold: @@ -38,7 +61,7 @@ def shortest_path(self, costmap, start, end, obstacle_threshold=90): if nd < dist[nr, nc]: dist[nr, nc] = nd prev[(nr, nc)] = (r, c) - heapq.heappush(heap, (nd, (nr, nc))) + heapq.heappush(heap, (nd + h(nr, nc), nd, (nr, nc))) if end not in prev and start != end: return None diff --git a/src/planning/path_planners/path_planners/path_planner_node.py b/src/planning/path_planners/path_planners/path_planner_node.py index 3959ff238..07239a3d6 100644 --- a/src/planning/path_planners/path_planners/path_planner_node.py +++ b/src/planning/path_planners/path_planners/path_planner_node.py @@ -132,7 +132,7 @@ def __init__(self): self.origin_x = 20.0 # Origin offset in X self.origin_y = 30.0 # Origin offset in Y self.obstacle_threshold = 90 # Values above this are considered obstacles - self.obstacle_padding = 1 # Cells to pad around obstacles (1 = 0.2m margin) + self.obstacle_padding = 5 # Cells to pad around obstacles (5 = 1.0m, ~vehicle half-width) # Path re-use: cache the last valid Dijkstra result. # Replan only when an obstacle blocks the cached path or the goal moves. @@ -182,7 +182,7 @@ def __init__(self): # Planning timer self.path_timer = self.create_timer( - 0.1, self.generate_path, callback_group=MutuallyExclusiveCallbackGroup() + 0.05, self.generate_path, callback_group=MutuallyExclusiveCallbackGroup() ) # Initialize path planners @@ -312,6 +312,7 @@ def generate_path(self): self.obstacle_padding ) + # ---------------------------- # Goal snap: if goal landed in an obstacle cell (e.g. grid edge # dilation or boundary), walk back along the line toward start @@ -341,35 +342,6 @@ def generate_path(self): throttle_duration_sec=2.0) return - # ---------------------------- - # PATH RE-USE: skip Dijkstra when cached path is still obstacle-free - # and the goal hasn't moved significantly. - # Replan only when: (a) a new obstacle blocks the path, or - # (b) the goal shifted > GOAL_THRESHOLD cells. - # ---------------------------- - GOAL_REPLAN_THRESHOLD = 10 # cells (~2 m) - must_replan = True - - if (self._cached_path_cells is not None - and self._cached_goal is not None - and len(self._cached_path_cells) > 5): - gi_old, gj_old = self._cached_goal - goal_shifted = (abs(gi_old - goal_i) > GOAL_REPLAN_THRESHOLD - or abs(gj_old - goal_j) > GOAL_REPLAN_THRESHOLD) - if not goal_shifted: - path_blocked = any( - padded_costmap[r, c] >= self.obstacle_threshold - for (r, c) in self._cached_path_cells - ) - if not path_blocked: - must_replan = False - else: - self.get_logger().info( - "Obstacle on path — replanning", throttle_duration_sec=1.0) - else: - self.get_logger().info( - "Goal changed — replanning", throttle_duration_sec=1.0) - # ---------------------------- # Run Planner # ---------------------------- @@ -384,15 +356,12 @@ def generate_path(self): path = self.planner.arastar() elif isinstance(self.planner, DijkstraPathPlanner): - if must_replan: - path = self.planner.shortest_path( - padded_costmap, - (start_i, start_j), - (goal_i, goal_j), - self.obstacle_threshold - ) - else: - path = self._cached_path_cells + path = self.planner.shortest_path( + padded_costmap, + (start_i, start_j), + (goal_i, goal_j), + self.obstacle_threshold + ) elif isinstance(self.planner, DPPathPlanner): self.planner.costmap_data = padded_costmap @@ -424,11 +393,6 @@ def generate_path(self): throttle_duration_sec=2.0) return - # Cache the valid path - if must_replan: - self._cached_path_cells = list(path) - self._cached_goal = (goal_i, goal_j) - # ---------------------------- # Smooth Path # ----------------------------