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

Remove lidar offset from VastuSLAM, add SVG debug markers

Browse Source 
Lidar offset handling is now done in SangamIO (commit 64020cf), which
transforms lidar data to robot-center coordinates before streaming.
This simplifies VastuSLAM by removing hardware-specific configuration.

Also adds debug visualization support to SVG output:
- DebugMarker struct for custom markers with position, radius, color
- with_waypoints() for path waypoints (orange)
- with_frontiers() for frontier targets (green, labeled)
- with_markers() for arbitrary debug markers
- Dynamic legend sizing based on active entries
This commit is contained in:
codetiger
2026-01-06 21:19:37 +05:30
parent 64020cf4dd
commit 3e78199709
11 changed files with 197 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-slam/configs/config.toml
+1 -4
View File
@@ -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
vastu-slam/examples/scenario_slam.rs
+3 -6
View File
@@ -280,12 +280,9 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
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<Pose2D> = vec![];
vastu-slam/src/config/README.md
+1 -1
View File
@@ -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();
vastu-slam/src/config/defaults.rs
-4
View File
@@ -16,10 +16,6 @@ pub fn enabled() -> bool {
true
}
pub fn lidar_offset_x() -> f32 {
-0.110
}
pub fn min_range() -> f32 {
0.15
}
vastu-slam/src/config/sensor.rs
+3 -12
View File
@@ -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,
}
vastu-slam/src/grid/README.md
+1 -1
View File
@@ -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
}
vastu-slam/src/grid/config.rs
+3 -4
View File
@@ -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,
vastu-slam/src/grid/lidar_update.rs
+5 -4
View File
@@ -43,8 +43,9 @@ pub fn update_from_lidar(
// Pre-allocate with typical capacity to avoid reallocations
let mut new_wall_coords: Vec<GridCoord> = 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 {
vastu-slam/src/io/mod.rs
+1 -1
View File
@@ -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};
vastu-slam/src/io/svg.rs
+173 -6
View File
@@ -82,6 +82,19 @@ pub struct Trajectory {
pub marker_indices: Vec<usize>,
}
/// 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<String>,
}
/// SVG visualization builder
pub struct SvgVisualizer {
config: SvgConfig,
@@ -95,6 +108,12 @@ pub struct SvgVisualizer {
odom_trajectory: Option<Trajectory>,
/// Title to display
title: Option<String>,
/// Debug markers (waypoints, frontiers, etc.)
debug_markers: Vec<DebugMarker>,
/// 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<WorldPoint>) -> 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<WorldPoint>) -> 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<DebugMarker>) -> 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, " </g>").unwrap();
// Render legend
@@ -337,6 +418,38 @@ impl SvgVisualizer {
writeln!(svg, " </g>").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#" <g id="debug-markers">"#).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#" <circle cx="{:.1}" cy="{:.1}" r="{:.1}" fill="{}" stroke="white" stroke-width="1" opacity="0.8"/>"#,
px, py, marker.radius, marker.color
).unwrap();
// Draw label if present
if let Some(ref label) = marker.label {
writeln!(
svg,
r##" <text x="{:.1}" y="{:.1}" font-family="sans-serif" font-size="10" fill="{}" text-anchor="middle" dy="-8">{}</text>"##,
px, py, marker.color, label
).unwrap();
}
}
writeln!(svg, " </g>").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##" <rect x="10" y="0" width="{:.0}" height="85" fill="white" stroke="#CCC" stroke-width="1" rx="4"/>"##,
svg_width - 20.0
r##" <rect x="10" y="0" width="{:.0}" height="{:.0}" fill="white" stroke="#CCC" stroke-width="1" rx="4"/>"##,
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##" <circle cx="35" cy="{:.0}" r="6" fill="#00AA00" stroke="white" stroke-width="1"/>"##,
entry_y
).unwrap();
writeln!(
svg,
r##" <text x="60" y="{:.0}" fill="#333">Frontier Targets</text>"##,
entry_y + 4.0
)
.unwrap();
entry_y += 20.0;
}
if self.has_waypoints {
// Orange circle for waypoints
writeln!(
svg,
r##" <circle cx="35" cy="{:.0}" r="3" fill="#FF8800" stroke="white" stroke-width="1"/>"##,
entry_y
).unwrap();
writeln!(
svg,
r##" <text x="60" y="{:.0}" fill="#333">Path Waypoints</text>"##,
entry_y + 4.0
)
.unwrap();
}
// Map legend (right side)
let map_legend_x = svg_width - 150.0;
writeln!(
vastu-slam/tests/quality.rs
+6 -6
View File
@@ -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);
}