diff --git a/vastu-slam/configs/config.toml b/vastu-slam/configs/config.toml index c71afa7..6e2ab5a 100644 --- a/vastu-slam/configs/config.toml +++ b/vastu-slam/configs/config.toml @@ -26,10 +26,7 @@ auto_expand = true # Sensor Configuration # ============================================================================ [sensor.lidar] -# Sensor offset from robot center (x forward, y left) -offset_x = -0.110 -offset_y = 0.0 - +# Note: Lidar offset is now handled by SangamIO (hardware config) # Range limits min_range = 0.15 max_range = 8.0 diff --git a/vastu-slam/examples/scenario_slam.rs b/vastu-slam/examples/scenario_slam.rs index fadea5c..5ab9c00 100644 --- a/vastu-slam/examples/scenario_slam.rs +++ b/vastu-slam/examples/scenario_slam.rs @@ -280,12 +280,9 @@ fn main() -> Result<(), Box> { let mut map = OccupancyGridMap::new(map_config); // Get matcher config from loaded configuration - let mut matcher_config = vastu_config.correlative_matcher_config(); - // Apply sensor offset from sensor section - matcher_config.sensor_offset = ( - vastu_config.sensor.lidar.offset_x, - vastu_config.sensor.lidar.offset_y, - ); + // Note: sensor_offset defaults to (0,0) - SangamIO now sends lidar data + // already transformed to robot center coordinates + let matcher_config = vastu_config.correlative_matcher_config(); // Tracking state let mut trajectory: Vec = vec![]; diff --git a/vastu-slam/src/config/README.md b/vastu-slam/src/config/README.md index 8ae4f41..67a6d27 100644 --- a/vastu-slam/src/config/README.md +++ b/vastu-slam/src/config/README.md @@ -97,7 +97,7 @@ let config = VastuConfig::default(); // Direct access to sections let resolution = config.grid.resolution; let robot_radius = config.sensor.robot.radius; -let lidar_offset = config.sensor.lidar.offset_x; +let lidar_range = config.sensor.lidar.max_range; // Convenient methods let radius = config.robot_radius(); diff --git a/vastu-slam/src/config/defaults.rs b/vastu-slam/src/config/defaults.rs index 42c3e97..d25618e 100644 --- a/vastu-slam/src/config/defaults.rs +++ b/vastu-slam/src/config/defaults.rs @@ -16,10 +16,6 @@ pub fn enabled() -> bool { true } -pub fn lidar_offset_x() -> f32 { - -0.110 -} - pub fn min_range() -> f32 { 0.15 } diff --git a/vastu-slam/src/config/sensor.rs b/vastu-slam/src/config/sensor.rs index c7ec596..2fb2f44 100644 --- a/vastu-slam/src/config/sensor.rs +++ b/vastu-slam/src/config/sensor.rs @@ -2,7 +2,6 @@ use serde::{Deserialize, Serialize}; -use crate::core::WorldPoint; use crate::grid::SensorConfig; use super::defaults; @@ -32,7 +31,6 @@ impl SensorSection { pub fn to_sensor_config(&self) -> SensorConfig { SensorConfig { robot_radius: self.robot.radius, - lidar_offset: WorldPoint::new(self.lidar.offset_x, self.lidar.offset_y), confidence_threshold: 3, max_lidar_range: self.lidar.max_range, min_lidar_range: self.lidar.min_range, @@ -41,16 +39,11 @@ impl SensorSection { } /// LiDAR settings +/// +/// Note: Lidar mounting offset is now handled by SangamIO. +/// VastuSLAM receives robot-centered lidar data directly. #[derive(Clone, Debug, Serialize, Deserialize)] pub struct LidarSettings { - /// Offset X from robot center - #[serde(default = "defaults::lidar_offset_x")] - pub offset_x: f32, - - /// Offset Y from robot center - #[serde(default)] - pub offset_y: f32, - /// Minimum valid range #[serde(default = "defaults::min_range")] pub min_range: f32, @@ -63,8 +56,6 @@ pub struct LidarSettings { impl Default for LidarSettings { fn default() -> Self { Self { - offset_x: -0.110, - offset_y: 0.0, min_range: 0.15, max_range: 8.0, } diff --git a/vastu-slam/src/grid/README.md b/vastu-slam/src/grid/README.md index 14fe7dd..d7d1088 100644 --- a/vastu-slam/src/grid/README.md +++ b/vastu-slam/src/grid/README.md @@ -142,9 +142,9 @@ pub struct GridConfig { } // Robot and sensor parameters +// Note: Lidar offset is now handled by SangamIO pub struct SensorConfig { pub robot_radius: f32, // 0.17m - pub lidar_offset: WorldPoint, // (-0.11, 0) from center pub max_lidar_range: f32, // 8.0m pub min_lidar_range: f32, // 0.15m } diff --git a/vastu-slam/src/grid/config.rs b/vastu-slam/src/grid/config.rs index 665f5a5..55331ee 100644 --- a/vastu-slam/src/grid/config.rs +++ b/vastu-slam/src/grid/config.rs @@ -85,14 +85,14 @@ impl GridConfig { } /// Sensor configuration for grid updates +/// +/// Note: Lidar mounting offset is now handled by SangamIO. +/// VastuSLAM receives robot-centered lidar data directly. #[derive(Clone, Debug, Serialize, Deserialize)] pub struct SensorConfig { /// Robot radius in meters (for obstacle inflation) pub robot_radius: f32, - /// Lidar mounting offset from robot center (x, y) in robot frame - pub lidar_offset: WorldPoint, - /// How many observations needed for a cell to be "confident" pub confidence_threshold: u8, @@ -107,7 +107,6 @@ impl Default for SensorConfig { fn default() -> Self { Self { robot_radius: 0.17, - lidar_offset: WorldPoint::new(-0.110, 0.0), // From SangamIO CRL-200S config confidence_threshold: 3, max_lidar_range: 8.0, min_lidar_range: 0.15, diff --git a/vastu-slam/src/grid/lidar_update.rs b/vastu-slam/src/grid/lidar_update.rs index e907d3d..95974d1 100644 --- a/vastu-slam/src/grid/lidar_update.rs +++ b/vastu-slam/src/grid/lidar_update.rs @@ -43,8 +43,9 @@ pub fn update_from_lidar( // Pre-allocate with typical capacity to avoid reallocations let mut new_wall_coords: Vec = Vec::with_capacity(32); - // Calculate sensor position in world frame - let sensor_pos = robot_pose.transform_point(sensor_config.lidar_offset); + // Use robot position directly - lidar data is already robot-centered + // (SangamIO transforms lidar measurements to robot center before streaming) + let sensor_pos = WorldPoint::new(robot_pose.x, robot_pose.y); let mut sensor_coord = storage.world_to_grid(sensor_pos); // Process each ray @@ -275,9 +276,9 @@ mod tests { assert!(result.cells_floor > 0); // Check walls in 4 directions - // Note: sensor offset moves the lidar backwards + // Lidar data is now robot-centered (no offset adjustment needed) let check_points = [ - WorldPoint::new(1.0 - 0.110, 0.0), // Forward (adjusted for lidar offset) + WorldPoint::new(1.0, 0.0), // Forward (at 1m from robot center) ]; for point in &check_points { diff --git a/vastu-slam/src/io/mod.rs b/vastu-slam/src/io/mod.rs index 9acf93b..2b474a4 100644 --- a/vastu-slam/src/io/mod.rs +++ b/vastu-slam/src/io/mod.rs @@ -59,4 +59,4 @@ pub use pgm::{export_pgm, export_ros_map, export_yaml, write_pgm}; #[cfg(any(test, feature = "scenario"))] pub use scenario::{Command, Scenario, ScenarioError, SeedConfig, StartPose, SvgOutputConfig}; -pub use svg::{SvgConfig, SvgVisualizer, markers_by_distance, markers_by_time}; +pub use svg::{DebugMarker, SvgConfig, SvgVisualizer, markers_by_distance, markers_by_time}; diff --git a/vastu-slam/src/io/svg.rs b/vastu-slam/src/io/svg.rs index fc0a925..b6e3c52 100644 --- a/vastu-slam/src/io/svg.rs +++ b/vastu-slam/src/io/svg.rs @@ -82,6 +82,19 @@ pub struct Trajectory { pub marker_indices: Vec, } +/// Debug marker for visualization +#[derive(Clone, Debug)] +pub struct DebugMarker { + /// Position in world coordinates + pub position: WorldPoint, + /// Radius in pixels + pub radius: f32, + /// Fill color + pub color: String, + /// Optional label + pub label: Option, +} + /// SVG visualization builder pub struct SvgVisualizer { config: SvgConfig, @@ -95,6 +108,12 @@ pub struct SvgVisualizer { odom_trajectory: Option, /// Title to display title: Option, + /// Debug markers (waypoints, frontiers, etc.) + debug_markers: Vec, + /// Whether frontiers were added (for legend) + has_frontiers: bool, + /// Whether waypoints were added (for legend) + has_waypoints: bool, } impl SvgVisualizer { @@ -107,6 +126,9 @@ impl SvgVisualizer { est_trajectory: None, odom_trajectory: None, title: None, + debug_markers: Vec::new(), + has_frontiers: false, + has_waypoints: false, } } @@ -149,6 +171,44 @@ impl SvgVisualizer { self } + /// Add waypoints as small markers + pub fn with_waypoints(mut self, waypoints: Vec) -> Self { + if !waypoints.is_empty() { + self.has_waypoints = true; + for wp in waypoints { + self.debug_markers.push(DebugMarker { + position: wp, + radius: 3.0, + color: "#FF8800".to_string(), // Orange for waypoints + label: None, + }); + } + } + self + } + + /// Add frontier targets as larger markers + pub fn with_frontiers(mut self, frontiers: Vec) -> Self { + if !frontiers.is_empty() { + self.has_frontiers = true; + for (i, frontier) in frontiers.into_iter().enumerate() { + self.debug_markers.push(DebugMarker { + position: frontier, + radius: 6.0, + color: "#00AA00".to_string(), // Green for frontiers + label: Some(format!("F{}", i + 1)), + }); + } + } + self + } + + /// Add custom debug markers + pub fn with_markers(mut self, markers: Vec) -> Self { + self.debug_markers.extend(markers); + self + } + /// Render to SVG string pub fn render(&self) -> String { let mut svg = String::new(); @@ -160,7 +220,25 @@ impl SvgVisualizer { let padding = self.config.padding; let title_height = if self.title.is_some() { 30.0 } else { 0.0 }; - let legend_height = 100.0; + + // Calculate dynamic legend height based on entries + let mut left_entries = 0; + if self.gt_trajectory.is_some() { + left_entries += 1; + } + if self.est_trajectory.is_some() { + left_entries += 1; + } + if self.odom_trajectory.is_some() { + left_entries += 1; + } + if self.has_frontiers { + left_entries += 1; + } + if self.has_waypoints { + left_entries += 1; + } + let legend_height = (left_entries.max(3) * 20 + 25) as f32 + 10.0; // +10 for margin let width = map_width_px + 2.0 * padding; let height = map_height_px + 2.0 * padding + title_height + legend_height; @@ -213,6 +291,9 @@ impl SvgVisualizer { self.render_trajectory(&mut svg, traj, min_world, map_height_px); } + // Render debug markers (waypoints, frontiers) + self.render_debug_markers(&mut svg, min_world, map_height_px); + writeln!(&mut svg, " ").unwrap(); // Render legend @@ -337,6 +418,38 @@ impl SvgVisualizer { writeln!(svg, " ").unwrap(); } + /// Render debug markers (waypoints, frontiers, etc.) + fn render_debug_markers(&self, svg: &mut String, min_world: WorldPoint, height_px: f32) { + if self.debug_markers.is_empty() { + return; + } + + writeln!(svg, r#" "#).unwrap(); + + for marker in &self.debug_markers { + let px = (marker.position.x - min_world.x) * self.config.scale; + let py = height_px - (marker.position.y - min_world.y) * self.config.scale; + + // Draw circle marker + writeln!( + svg, + r#" "#, + px, py, marker.radius, marker.color + ).unwrap(); + + // Draw label if present + if let Some(ref label) = marker.label { + writeln!( + svg, + r##" {}"##, + px, py, marker.color, label + ).unwrap(); + } + } + + writeln!(svg, " ").unwrap(); + } + /// Render legend fn render_legend(&self, svg: &mut String, svg_width: f32, y_offset: f32) { writeln!( @@ -345,17 +458,39 @@ impl SvgVisualizer { y_offset ).unwrap(); + // Count left-side entries to calculate legend height + let mut left_entries = 0; + if self.gt_trajectory.is_some() { + left_entries += 1; + } + if self.est_trajectory.is_some() { + left_entries += 1; + } + if self.odom_trajectory.is_some() { + left_entries += 1; + } + if self.has_frontiers { + left_entries += 1; + } + if self.has_waypoints { + left_entries += 1; + } + + // Legend height: 20px per entry + 20px padding + let legend_height = (left_entries.max(3) * 20 + 25) as f32; + // Legend background writeln!( svg, - r##" "##, - svg_width - 20.0 + r##" "##, + svg_width - 20.0, + legend_height ).unwrap(); let mut entry_y = 20.0; - // Legend entries - let entries = [ + // Trajectory legend entries (lines) + let traj_entries = [ ( self.gt_trajectory.as_ref(), "Ground Truth", @@ -373,7 +508,7 @@ impl SvgVisualizer { ), ]; - for (traj_opt, label, color) in entries { + for (traj_opt, label, color) in traj_entries { if traj_opt.is_some() { // Color line writeln!( @@ -395,6 +530,38 @@ impl SvgVisualizer { } } + // Debug marker legend entries (circles) + if self.has_frontiers { + // Green circle for frontiers + writeln!( + svg, + r##" "##, + entry_y + ).unwrap(); + writeln!( + svg, + r##" Frontier Targets"##, + entry_y + 4.0 + ) + .unwrap(); + entry_y += 20.0; + } + + if self.has_waypoints { + // Orange circle for waypoints + writeln!( + svg, + r##" "##, + entry_y + ).unwrap(); + writeln!( + svg, + r##" Path Waypoints"##, + entry_y + 4.0 + ) + .unwrap(); + } + // Map legend (right side) let map_legend_x = svg_width - 150.0; writeln!( diff --git a/vastu-slam/tests/quality.rs b/vastu-slam/tests/quality.rs index 3469227..bfd272f 100644 --- a/vastu-slam/tests/quality.rs +++ b/vastu-slam/tests/quality.rs @@ -143,13 +143,13 @@ fn test_map_quality_with_room() { let config = MapConfig::default(); let mut map = OccupancyGridMap::new(config); - // Create a simple room map - let scan = common::room_scan(6.0, 6.0, 3.0, 3.0, 360); + // Create room with robot at center + // Multiple observations at same position to accumulate log-odds + let pose = Pose2D::new(3.0, 3.0, 0.0); - // Multiple observations for better quality - for i in 0..10 { - let angle = i as f32 * 0.1; - let pose = Pose2D::new(3.0, 3.0, angle); + for _ in 0..10 { + // Generate fresh scan for each observation (robot-centered) + let scan = common::room_scan(6.0, 6.0, 3.0, 3.0, 360); map.observe_lidar(&scan, pose); }