Enuxia-Public/enuxia-vacuum
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Transform lidar data to robot-center frame in SangamIO

Browse Source 
Move lidar mounting configuration from mock simulation config to
hardware config section. SangamIO now transforms lidar measurements
from the physical lidar position to robot center before streaming,
ensuring clients receive consistent robot-centered data regardless
of hardware vs simulation.

- Add LidarMountingConfig with transform_to_robot_center() method
- Apply transformation in Delta2DDriver before accumulating points
- Simplify mock simulator to generate robot-centered data directly
- Consolidate mounting parameters in sangamio.toml [device.hardware]
This commit is contained in:
codetiger
2026-01-06 21:18:31 +05:30
parent d7837b9abb
commit 64020cf4dd
7 changed files with 176 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sangam-io/mock.toml
+2 -4
View File
@@ -31,15 +31,13 @@ collision_mode = "stop" # "stop", "slide", "passthrough" - stop for re
slide_friction = 0.3 # Used when collision_mode = "slide"
# Lidar configuration (Delta-2D specifications)
# Note: Mounting offset is handled by hardware config, not simulation.
# The mock simulator generates robot-centered lidar data directly.
[device.simulation.lidar]
num_rays = 360 # Rays per 360° scan
scan_rate_hz = 5.0 # Scans per second
min_range = 0.15 # Minimum detection range (meters)
max_range = 8.0 # Maximum detection range (meters)
mounting_x = -0.110 # Lidar X offset from robot center (negative = behind)
mounting_y = 0.0 # Lidar Y offset from robot center
optical_offset = 0.025 # Optical offset from lidar center
angle_offset = 0.2182 # Angle offset from robot forward (radians)
[device.simulation.lidar.noise]
range_stddev = 0.0 # Range measurement noise (disabled for testing)
sangam-io/sangamio.toml
+10
View File
@@ -35,6 +35,16 @@ heartbeat_interval_ms = 20
# Default: 60 (provides ~330 RPM / 5.5 Hz scan rate)
lidar_pwm = 60
# Lidar mounting configuration
# Defines the physical mounting position of the lidar relative to robot center.
# SangamIO uses this to transform lidar measurements to robot-centered coordinates
# before streaming to clients.
[device.hardware.lidar_mounting]
offset_x = -0.110 # Lidar X offset from robot center (meters, negative = behind)
offset_y = 0.0 # Lidar Y offset from robot center (meters, positive = left)
optical_offset = 0.025 # Optical offset from lidar rotation center (meters)
angle_offset = 0.2182 # Angle offset from robot forward (radians, ~12.5 degrees)
# Coordinate frame transforms (ROS REP-103)
# Transforms raw sensor data to standard robot frame:
# X = forward (direction robot drives)
sangam-io/src/config.rs
+119
View File
@@ -192,6 +192,115 @@ pub struct FrameTransforms {
pub imu_accel: AxisTransform3D,
}
// ============================================================================
// Lidar Mounting Configuration
// ============================================================================
/// Lidar mounting configuration for coordinate frame transformation.
///
/// SangamIO transforms lidar measurements from the physical lidar position
/// to robot center before streaming. This ensures downstream applications
/// receive robot-centered data without needing hardware-specific knowledge.
///
/// # Transformation
///
/// Given a measurement (angle, range) from the lidar:
/// 1. Apply angle_offset to get adjusted angle
/// 2. Account for optical_offset (radial offset within lidar)
/// 3. Compute point in robot frame using offset_x, offset_y
/// 4. Convert back to polar coordinates from robot center
///
/// # CRL-200S Hardware Values
///
/// - offset_x = -0.110m (lidar is 110mm behind robot center)
/// - offset_y = 0.0m (centered laterally)
/// - optical_offset = 0.025m (25mm from lidar rotation center to laser)
/// - angle_offset = 0.2182 rad (~12.5° rotation adjustment)
#[derive(Debug, Clone, Deserialize)]
pub struct LidarMountingConfig {
/// X offset from robot center (meters, negative = behind)
#[serde(default = "default_lidar_offset_x")]
pub offset_x: f32,
/// Y offset from robot center (meters, positive = left)
#[serde(default)]
pub offset_y: f32,
/// Optical offset from lidar rotation center (meters)
/// Distance from the lidar's rotation axis to the laser emission point
#[serde(default = "default_optical_offset")]
pub optical_offset: f32,
/// Angle offset from robot forward (radians)
/// Applied after scale/offset transform, before XY transformation
#[serde(default = "default_angle_offset")]
pub angle_offset: f32,
}
fn default_lidar_offset_x() -> f32 {
-0.110
}
fn default_optical_offset() -> f32 {
0.025
}
fn default_angle_offset() -> f32 {
0.2182
}
impl Default for LidarMountingConfig {
fn default() -> Self {
Self {
offset_x: default_lidar_offset_x(),
offset_y: 0.0,
optical_offset: default_optical_offset(),
angle_offset: default_angle_offset(),
}
}
}
impl LidarMountingConfig {
/// Transform a lidar point from lidar frame to robot center frame.
///
/// Given a measurement (angle, range) from the physical lidar position,
/// computes the equivalent measurement from robot center.
///
/// # Arguments
/// - `angle`: Ray angle in lidar frame (radians, after scale/offset transform)
/// - `range`: Distance measured by lidar (meters)
///
/// # Returns
/// (new_angle, new_range) as if measured from robot center
#[inline]
pub fn transform_to_robot_center(&self, angle: f32, range: f32) -> (f32, f32) {
use std::f32::consts::TAU;
// Apply angle offset first
let adjusted_angle = angle + self.angle_offset;
// Account for optical offset (distance from lidar rotation center to laser)
// The actual measurement origin is offset radially from lidar mount point
let effective_ox = self.offset_x + self.optical_offset * adjusted_angle.cos();
let effective_oy = self.offset_y + self.optical_offset * adjusted_angle.sin();
// Point in robot frame
let x = effective_ox + range * adjusted_angle.cos();
let y = effective_oy + range * adjusted_angle.sin();
// Convert to polar from robot center
let new_range = (x * x + y * y).sqrt();
let mut new_angle = y.atan2(x);
// Normalize angle to [0, 2π)
if new_angle < 0.0 {
new_angle += TAU;
}
(new_angle, new_range)
}
}
// ============================================================================
// Hardware Configuration
// ============================================================================
@@ -244,6 +353,16 @@ pub struct HardwareConfig {
/// Lower values = slower motor = lower scan rate
#[serde(default = "default_lidar_pwm")]
pub lidar_pwm: u8,
/// Lidar mounting configuration for robot-center transformation
///
/// Defines the physical mounting position of the lidar relative to
/// robot center. SangamIO uses this to transform lidar measurements
/// to robot-centered coordinates before streaming to clients.
///
/// **Optional**: Yes (defaults to CRL-200S values)
#[serde(default)]
pub lidar_mounting: LidarMountingConfig,
}
fn default_lidar_pwm() -> u8 {
sangam-io/src/devices/crl200s/delta2d/mod.rs
+20 -5
View File
@@ -65,7 +65,7 @@
pub mod protocol;
use crate::config::AffineTransform1D;
use crate::config::{AffineTransform1D, LidarMountingConfig};
use crate::core::types::{SensorGroupData, SensorValue};
use crate::error::{Error, Result};
use protocol::{Delta2DPacketReader, ParseResult};
@@ -95,6 +95,8 @@ pub struct Delta2DDriver {
error_count: Arc<AtomicU64>,
/// Angle transform for coordinate frame conversion
angle_transform: AffineTransform1D,
/// Lidar mounting configuration for robot-center transformation
lidar_mounting: LidarMountingConfig,
}
impl Delta2DDriver {
@@ -103,7 +105,12 @@ impl Delta2DDriver {
/// # Arguments
/// - `port_path`: Serial port path (e.g., "/dev/ttyS1")
/// - `angle_transform`: Transform applied to all lidar angles (use `AffineTransform1D::identity()` for no change)
pub fn new(port_path: &str, angle_transform: AffineTransform1D) -> Self {
/// - `lidar_mounting`: Mounting configuration for robot-center transformation
pub fn new(
port_path: &str,
angle_transform: AffineTransform1D,
lidar_mounting: LidarMountingConfig,
) -> Self {
Self {
port_path: port_path.to_string(),
shutdown: Arc::new(AtomicBool::new(false)),
@@ -111,6 +118,7 @@ impl Delta2DDriver {
scan_count: Arc::new(AtomicU64::new(0)),
error_count: Arc::new(AtomicU64::new(0)),
angle_transform,
lidar_mounting,
}
}
@@ -134,6 +142,7 @@ impl Delta2DDriver {
let scan_count = Arc::clone(&self.scan_count);
let error_count = Arc::clone(&self.error_count);
let angle_transform = self.angle_transform;
let lidar_mounting = self.lidar_mounting.clone();
self.reader_handle = Some(
thread::Builder::new()
@@ -146,6 +155,7 @@ impl Delta2DDriver {
scan_count,
error_count,
angle_transform,
lidar_mounting,
);
})
.map_err(|e| Error::Other(format!("Failed to spawn lidar thread: {}", e)))?,
@@ -163,6 +173,7 @@ impl Delta2DDriver {
scan_count: Arc<AtomicU64>,
error_count: Arc<AtomicU64>,
angle_transform: AffineTransform1D,
lidar_mounting: LidarMountingConfig,
) {
let mut reader = Delta2DPacketReader::with_transform(angle_transform);
let mut accumulated_points: Vec<(f32, f32, u8)> = Vec::with_capacity(400);
@@ -202,10 +213,14 @@ impl Delta2DDriver {
last_scan_time = Instant::now();
}
// Add this point to accumulator
// Transform point from lidar frame to robot center frame
let (new_angle, new_distance) = lidar_mounting
.transform_to_robot_center(point.angle, point.distance);
// Add transformed point to accumulator
accumulated_points.push((
point.angle,
point.distance,
new_angle,
new_distance,
point.quality,
));
last_angle = point.angle;
sangam-io/src/devices/crl200s/mod.rs
+5 -1
View File
@@ -131,7 +131,11 @@ impl DeviceDriver for CRL200SDriver {
// Initialize lidar driver
// Driver starts but lidar motor is OFF - will be enabled via command
let mut lidar = Delta2DDriver::new(&hardware.lidar_port, hardware.frame_transforms.lidar);
let mut lidar = Delta2DDriver::new(
&hardware.lidar_port,
hardware.frame_transforms.lidar,
hardware.lidar_mounting.clone(),
);
lidar.start(lidar_data)?;
self.lidar = Some(lidar);
log::info!("Lidar driver started (motor OFF - enable via command)");
sangam-io/src/devices/mock/config.rs
+4 -29
View File
@@ -277,6 +277,10 @@ impl Default for RobotConfig {
}
/// Lidar sensor configuration (Delta-2D specifications)
///
/// Note: Mounting offset configuration has been moved to hardware config.
/// The mock simulator generates lidar data as if measured from robot center,
/// matching the transformed output of real hardware.
#[derive(Debug, Clone, Deserialize)]
pub struct LidarConfig {
/// Number of rays per 360° scan
@@ -295,22 +299,6 @@ pub struct LidarConfig {
#[serde(default = "default_max_range")]
pub max_range: f32,
/// Lidar mounting X offset from robot center (meters, negative = behind)
#[serde(default = "default_mounting_x")]
pub mounting_x: f32,
/// Lidar mounting Y offset from robot center (meters)
#[serde(default)]
pub mounting_y: f32,
/// Optical offset from lidar center (meters)
#[serde(default = "default_optical_offset")]
pub optical_offset: f32,
/// Angle offset from robot forward (radians)
#[serde(default = "default_angle_offset")]
pub angle_offset: f32,
/// Noise configuration
#[serde(default)]
pub noise: LidarNoiseConfig,
@@ -328,15 +316,6 @@ fn default_min_range() -> f32 {
fn default_max_range() -> f32 {
8.0
}
fn default_mounting_x() -> f32 {
-0.110
}
fn default_optical_offset() -> f32 {
0.025
}
fn default_angle_offset() -> f32 {
0.2182
}
impl Default for LidarConfig {
fn default() -> Self {
@@ -345,10 +324,6 @@ impl Default for LidarConfig {
scan_rate_hz: default_scan_rate_hz(),
min_range: default_min_range(),
max_range: default_max_range(),
mounting_x: default_mounting_x(),
mounting_y: 0.0,
optical_offset: default_optical_offset(),
angle_offset: default_angle_offset(),
noise: LidarNoiseConfig::default(),
}
}
sangam-io/src/devices/mock/lidar_sim.rs
+16 -17
View File
@@ -1,6 +1,11 @@
//! Lidar simulator with ray-casting
//!
//! Simulates Delta-2D lidar sensor with configurable noise.
//!
//! The mock simulator generates lidar data as if measured from robot center,
//! matching the transformed output of real hardware. This ensures downstream
//! applications receive consistent robot-centered data regardless of whether
//! they're connected to real hardware or simulation.
use super::config::LidarConfig;
use super::map_loader::SimulationMap;
@@ -22,7 +27,12 @@ impl LidarSimulator {
}
}
/// Generate a complete 360° scan
/// Generate a complete 360° scan from robot center.
///
/// Unlike real hardware which measures from the physical lidar position,
/// the mock simulator directly generates robot-centered data. This matches
/// the output of real hardware after SangamIO applies the mounting offset
/// transformation.
///
/// Returns vector of (angle_rad, distance_m, quality) tuples.
pub fn generate_scan(
@@ -35,26 +45,15 @@ impl LidarSimulator {
let mut points = Vec::with_capacity(self.config.num_rays);
let angle_step = TAU / self.config.num_rays as f32;
// Compute lidar position in world frame
let lidar_x = robot_x + self.config.mounting_x * robot_theta.cos()
- self.config.mounting_y * robot_theta.sin();
let lidar_y = robot_y
+ self.config.mounting_x * robot_theta.sin()
+ self.config.mounting_y * robot_theta.cos();
for i in 0..self.config.num_rays {
// Local angle (in lidar frame)
// Local angle (in robot frame, 0 = forward)
let local_angle = i as f32 * angle_step;
// World angle (including robot orientation and lidar offset)
let world_angle = robot_theta + local_angle + self.config.angle_offset;
// World angle (robot orientation + local angle)
let world_angle = robot_theta + local_angle;
// Adjust for optical offset (radial offset from lidar center)
let ray_ox = lidar_x + self.config.optical_offset * world_angle.cos();
let ray_oy = lidar_y + self.config.optical_offset * world_angle.sin();
// Ray-cast against map
let mut distance = map.ray_cast(ray_ox, ray_oy, world_angle, self.config.max_range);
// Ray-cast from robot center
let mut distance = map.ray_cast(robot_x, robot_y, world_angle, self.config.max_range);
// Apply miss rate (random invalid readings)
if self.noise.chance(self.config.noise.miss_rate) {