Architecture

In the live path, positions come only from the LiDAR driver on /cones. Color comes from projecting those centroids into camera images inside point_to_pixel. Image detections without a matching projected LiDAR point are ignored.

Hesai UDP
  → custom_ros_driver
  → /cones
  → point_to_pixel (+ cameras + /filter/twist)
  → /associated_colored_cones
  → planning

Empty /cones with healthy YOLO output is a driver or filtering problem, not a classifier problem.

Topics

Topic	Publisher	Notes
/cones	driver	Centroid list in metres. Input to point-to-pixel.
/lidar/ground_filtered	driver	Post-GnC cloud for visualization.
/lidar/log	driver	Drop counters, timing, health (LidarLog).
/lidar/raw_points	driver	Converted returns when published.
/cone_array	driver	Same centroids as ConeArray. Debug only; coloring uses /cones.
/filter/twist	state estimator	Motion model and history propagation.
/colored_cones	point-to-pixel	Pre-history debug (publish_pre_cone_history_debug).
/associated_colored_cones	point-to-pixel	Planning output.

Timing and frames

point_to_pixel consumes /cones, camera frames (SeeCam by default), and /filter/twist. Twist covers LiDAR-to-camera stamp skew; at speed, uncompensated skew moves the projected pixel off the cone.

• Driver internals are millimetres; ROS messages are metres.
• Planning ConeArray keeps LiDAR-frame $x,y$ and sets $z = 0$.
• Projection matrices live in point_to_pixel/config/params.yaml.
• imu_to_lidar converts IMU-centered twist into the LiDAR frame.

LiDAR and cameras are not hardware time-synced. Point-to-pixel selects the nearest buffered left/right frames and integrates twist across the gap. Missing twist or bad stamps produce systematic projection offset, worse when moving.

Failure mapping

Observation	Area
Missing or sparse /cones	Driver, UDP 2368, correction, GnC, DBSCAN, cutoffs (ops)
Good /cones, wrong colors	Projection, calibration, camera frames, YOLO/HSV
Pre-history OK, post-history wrong	Cone history. Gap-fill only on feature/perc/propagate_history
Close range OK, far range bad	Projection error, centroid noise, timing, history ghosts
Offset at rest	Extrinsics / params.yaml / camera YAML (physical calibration)
Offset only at speed	Twist, imu_to_lidar, empty image deques

Package docs

• LiDAR driver
• Point-to-pixel
• Physical calibration
• Regression harness (feature/perc/bag-extractor)
• Ops