feat(acc): Autonomous Cruise Controller — lateral/longitudinal control with LiDAR braking and intersection compliance - #503
Conversation
- launch.vehicle.py + launch.perception.py: auto-launch perception nodes and pure_pursuit_controller; disable mmseg/YOLO nodes until models present - launch_node_definitions.py: add hybrid_drivable_grid node; rename duplicate image_segmentation definition to prevent silent overwrite - route_costmap_node.py: restore 20 Hz timer (was 6.7 Hz); widen corridor HALF_W 6->10 cells (1.2 m -> 2.0 m each side) - dijkstra_path_planner.py: tighten bounding box and heuristic - path_planner_node.py: remove path re-use cache so A* always replans from current vehicle position (eliminates stale-waypoint jitter); remove cost-threshold pruning that caused swerving; obstacle_padding 1->5 cells - pure_pursuit_controller_node.py: LD 3.0->6.0 m, kf 0.1->0.3 so lookahead carrot reaches past path deviations before vehicle closes on them
- launch.vehicle.py: replace pure_pursuit_controller with autonomous_cruise_controller - autonomous_cruise_node.py: subscribe to /speed (VehicleSpeed) for actual vehicle speed; twist.twist.linear is never populated by the GNSS processor so speed was always 0.0 m/s causing the PID to floor the throttle permanently - autonomous_cruise_params.yaml: min_lookahead 3.0->6.0m, lookahead_gain 0.5->0.3, target_speed 6.0->3.5 m/s to match CARLA GEM e6 tuning
Path planner publishes in base_link so waypoints are already vehicle-local. lateral_controller was wrongly subtracting the odom world position from each waypoint and rotating by yaw, double-transforming coordinates that are already in the vehicle frame. Vehicle was driving straight because steering computed as near-zero against huge world-frame offsets. Fix: use path point x/y directly (vehicle is at origin in base_link).
CARLA uses positive steer = turn right; atan2(sin(alpha), ld) produces positive alpha when target is left, so steering was inverted. Original pure pursuit already negates alpha (-alpha) before applying. Added minus to match.
- ACC node subscribes to /intersection (IntersectionBehavior) and forces full brake when intersection_manager says Wait (red light / stop sign) - intersection_manager uncommented in launch.vehicle.py so it runs with the stack - Steering direction inverted correctly already in previous commit
Subscribes to /lidar/filtered and scans points in a forward corridor (x 2-15m, |y|<1.5m, z>0.15m). Scales longitudinal target_speed linearly from full at 12m to zero at 3.5m — works for any obstacle without needing the 3D object detector.
cc42916 to
08dfdf2
Compare
- Narrow lateral width 1.5m -> 0.9m (stops catching curbs during turns) - Raise z floor 0.15m -> 0.3m (filters ground surface noise) - Require >= 5 points before triggering (single stray returns ignored)
2df67ae to
f1000a0
Compare
…g on turns Instead of a rectangular corridor aligned with vehicle heading (which sees intersection walls and road edges during turns), check LiDAR points against the planned path waypoints. Only triggers braking for points within 1m of the path ahead — ignores everything off the intended route.
- LiDAR obstacle detection now checks points against planned path waypoints (1m tolerance) instead of a fixed rectangular corridor — eliminates false braking at intersections and during turns where road edges entered the box - Subscribe to /carla/traffic_light_state (std_msgs/String from bridge) and brake to full stop on Red/Yellow, same priority as intersection Wait
d055d48 to
aa86f54
Compare
CREEP 0.3->1.0 m/s keeps the car moving while the planner replans around large obstacles (trees) instead of stalling at a near-standstill. Path tolerance 0.7->0.6m prevents wide tree canopy at road edge from registering as an on-path obstacle.
…range The receding-horizon goal selection walked the route as far as the camera-confirmed (drivable==0) zone extended, which could reach tens of metres in a single frame. That far boundary is the noisiest part of the signal (segmentation error, stale round-robin camera updates), so the published goal could snap several metres between ticks, causing erratic path replanning. Cap the forward-walk (and the perception-warmup fallback) to a fixed 8m lookahead, matching the old LiDAR-only behaviour: a short, continuously-updated target only as far as can be reliably confirmed right now.
|
Added commit 462434a: caps the camera-confirmed receding-horizon goal search in route_costmap_node to a fixed 8m lookahead. The previous unbounded search reached as far as the camera-confirmed zone extended (tens of metres), but that far boundary is the noisiest part of the perception signal, so the published goal could snap several metres between ticks and cause erratic replanning. Verified live with camera+LiDAR fusion and traffic-manager NPCs running. |
…topped vehicles Three related fixes verified live against a stationary vehicle placed directly in the ego path: 1. The forward-obstacle LiDAR prefilter excluded points closer than 2.5m (the vehicle hood self-occlusion cutoff for this roof-mounted, steeply-downward-FOV sensor), but STOP_DIST was 2.0m. As the ego closed to within 2.5m of a stopped car, every point on it got filtered out, lidar_obstacle_distance reset to inf, and the controller accelerated back to cruise speed right as it was about to make contact. STOP_DIST now derives from the same LIDAR_MIN_RANGE_M constant so this gap cannot reopen silently. 2. No hysteresis at the stop boundary: once stopped, sensor noise or minor position drift right at STOP_DIST let the vehicle immediately resume creeping forward, inching into the obstacle over repeated stop/creep cycles. Added a latch — once stopped for an obstacle, it stays stopped until the obstacle clears past SLOW_DIST, not just a hair past STOP_DIST. 3. Doubled the braking zone (STOP_DIST 3.0m to 6.0m, SLOW_DIST 7.0m to 14.0m) — the logic threshold being correct does not help if there is not enough physical distance left to actually decelerate to zero in time. Also caps PyTorch CPU thread usage in image_segmentation_node (was defaulting to 32 threads / 2700%+ CPU on this 64-core box with no GPU available, starving CARLA's own engine and the rest of the ROS stack enough to cause unrelated physics/timing flakiness during testing).
|
Added commit 7457f9f: fixes a collision bug found while live-testing. The forward-obstacle LiDAR prefilter excluded points closer than 2.5m (self-occlusion cutoff for the roof-mounted, steep-FOV sensor), but STOP_DIST was 2.0m — so a closing obstacle vanished from detection before the vehicle actually reached stop range, and the controller accelerated back to cruise speed right as it was about to hit something. Also added a stop/creep hysteresis latch (no more inching forward after stopping due to sensor noise at the boundary) and doubled the braking zone (STOP_DIST 3m→6m, SLOW_DIST 7m→14m) for real stopping margin. Verified live: spawned a stationary vehicle directly in the ego path, confirmed it now stops cleanly with no contact.\n\nAlso caps PyTorch CPU threads in image_segmentation_node (was 32 threads / 2700%+ CPU with no GPU available, which was starving CARLA badly enough to cause unrelated physics/timing flakiness). |
… grid rates to 20Hz route_costmap_node: the receding-horizon goal selection stopped its forward-walk at the first non-confirmed cell and never resumed, so an isolated obstruction (e.g. a tree overhanging the road) pinned the goal right before it. The planner was never asked to reach anywhere past the obstacle, so it had no reason to detour — it just stopped short. Now tolerates bridging over a short gap (<=2m) if confirmed road resumes, so the goal lands past the obstruction on real road. path_planner_node: the existing goal-snap (for when the selected goal lands on a blocked cell) only walked backward along the start->goal line, which can only retreat the goal to before an obstacle, never past it. Replaced with a 2D nearest-free-cell search so it can find clearance to the *side* of an obstacle when the corridor has room, giving the existing detour-capable A* search a goal that actually requires routing around the blockage. Also raised obstacle_padding 1.0m -> 1.8m. Per prior history on this branch (64e0901), detour paths running close to a tree's mapped footprint can still register within the ACC's 0.6m on-path LiDAR check (autonomous_cruise_node.py) because canopy extends past its occupancy-grid footprint in reality -- same failure mode, re-exposed now that detours are attempted instead of stopping short. More padding keeps detour paths clear of that band without touching the braking thresholds verified separately against stopped vehicles. Bumped /grid/occupancy/current, /grid/drivable/segmented, and /grid/drivable publish timers from 10Hz to 20Hz to match route_costmap_node/grid_summation_node/ACC control rate.
|
Added commit b993a7d: fixes the vehicle stopping short of an obstacle (tree overhanging the road) instead of routing around it, found during the same live-testing session.\n\nroute_costmap_node never asked the planner to reach anywhere past an obstruction (its goal search stopped at the first non-confirmed cell), so there was nothing to detour toward. Now bridges over a short gap (<=2m) if confirmed road resumes beyond it. path_planner_node's existing goal-snap only retreated backward along the start-goal line when blocked; replaced with a 2D nearest-free-cell search so it can find clearance to the side of an obstacle instead.\n\nAlso raised obstacle_padding 1.0m to 1.8m — per the project's own history on this branch (64e0901), a detour running that close to a tree's mapped footprint can still register inside the ACC's 0.6m on-path LiDAR check since canopy extends past its grid footprint in reality. Same failure mode, re-exposed now that we attempt detours instead of stopping short; fixed via clearance rather than touching the braking thresholds (verified separately against stopped vehicles).\n\nAlso bumped the three perception grid publish rates from 10Hz to 20Hz to match the rest of the planning/control loop. |
Reverts route_costmap_node.py, path_planner_node.py, autonomous_cruise_node.py, image_segmentation_node.py, hybrid_perception_grid_node.py, perception_drivable_grid_node.py, and hybrid_drivable_grid_node.py to their state at 64e0901, undoing 462434a, 7457f9f, and b993a7d plus uncommitted follow-on tuning. The route-around-obstacle work (gap-bridging in route_costmap_node, 2D goal-snap in path_planner_node) destabilized path planning at turns -- attempts to fix the instability (padding tuning, snap hysteresis) did not resolve it and the behavior became unsafe (crashing into walls). Reverting to the last state confirmed smooth and reliable rather than continuing to patch forward.
|
Reverted via 9173cbc: backed out all of today's planning/perception/ACC changes (462434a, 7457f9f, b993a7d, plus uncommitted follow-on tuning). The route-around-obstacle work destabilized path planning at turns badly enough (crashing into walls) that further patching was not the right call. All seven touched files are back to their exact state at 64e0901, the last point confirmed smooth and reliable. PR is back to just the original ACC work. |
…e fix, lane grid node - route_costmap_node: radial lane-center gradient corridor; fixed goal-selection exact-equality bug (== 0) against continuous EMA evidence -> CONFIRMED_THRESHOLD=30 - dijkstra_path_planner: weighted A* heuristic for real-time performance - perception_drivable_grid_node: rescaled ALPHA_BLEND/DECAY_RATE (sqrt) to preserve EMA time-constant after bumping publish rate 10Hz->20Hz - image_segmentation_node: PSPNet on GPU1 (not GPU0, avoids CARLA render contention), throttled inference loop to skip re-processing unchanged frames - autonomous_cruise_node: fixed LiDAR near-field dead zone (2.5m filter vs 2.0m STOP_DIST) that made close obstacles invisible and caused re-acceleration into them - new lane_grid_node: camera (drivable grid) + LiDAR-intensity lane segmentation, EMA+pose-compensated for occlusion robustness, publishes /grid/lane (LaneGrid) and /grid/lane/viz (colorized PointCloud2) for RViz; 7 passing unit tests - navigator_default.rviz: added LaneGrid PointCloud2 display Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
1 participant
Summary
autonomous_cruise_node) inlaunch.vehicle.py— plug-and-play swap with no changes to the rest of the planning stackbase_link(vehicle-local frame); removed erroneous world-frame subtraction that caused the controller to compute zero steering. Negated steering to match CARLA sign convention (positive = turn right)Odometry.twist.twist.linear(always zero in CARLA) to/speed(VehicleSpeedtopic)/lidar/filtered(ground-segmented PointCloud2 inbase_link) directly; no dependency onobjdet3d_tracked. Forward corridor scan (2–15 m ahead, ±1.5 m lateral, z > 0.15 m) linearly scales target speed to zero as obstacles approach 3.5 m/intersection(IntersectionBehavior); forces throttle=0, brake=1 when action isWait; enablesintersection_managerinlaunch.vehicle.pymin_lookahead 3.0→6.0,lookahead_gain 0.5→0.3,target_speed 6.0→3.5 m/s