Replace JSON/Postcard with Protobuf-only wire format
Migrate all three components to use Protocol Buffers for consistent, fast binary serialization with cross-language support: SangamIO (Rust): - Add prost/prost-build dependencies, remove postcard/serde_json - Create proto/sangamio.proto with Message, SensorGroup, Command types - Rewrite wire.rs with Serializer for protobuf encoding - Update tcp_publisher.rs and tcp_receiver.rs for new wire format - Replace hardware.json with sangamio.toml configuration DhruvaSLAM (Rust): - Add prost/prost-build, keep postcard for internal bag format - Create proto/dhruva.proto with SLAM output message types - Copy sangamio.proto for client deserialization - Rewrite sangam_client.rs for protobuf parsing - Rewrite tcp_publisher.rs for protobuf broadcast - Remove WireFormat configuration throughout Drishti (Python): - Generate Python protobuf bindings (sangamio_pb2.py, dhruva_pb2.py) - Update telemetry_thread.py for protobuf parsing from SangamIO - Update odometry_thread.py for protobuf parsing from DhruvaSLAM - Fix send_command to handle dict-style action format Wire protocol: 4-byte big-endian length prefix + protobuf binary payload
This commit is contained in:
codetiger
@@ -11,15 +11,19 @@ ctrlc = "3.4"
|
||||
env_logger = "0.11"
|
||||
kiddo = "4.2" # K-d tree for nearest neighbor queries
|
||||
log = "0.4"
|
||||
postcard = { version = "1.0", features = ["alloc"] }
|
||||
postcard = { version = "1.0", features = ["alloc"] } # For bag file format (internal)
|
||||
prost = "0.13"
|
||||
serde = { version = "1.0", features = ["derive"] }
|
||||
serde_json = "1.0"
|
||||
thiserror = "2.0"
|
||||
toml = "0.8"
|
||||
|
||||
[build-dependencies]
|
||||
prost-build = "0.13"
|
||||
|
||||
[dev-dependencies]
|
||||
approx = "0.5"
|
||||
criterion = { version = "0.5", features = ["html_reports"] }
|
||||
serde_json = "1.0" # For tests only
|
||||
tempfile = "3.14"
|
||||
|
||||
[[bin]]
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
//! Build script for compiling protobuf definitions
|
||||
|
||||
fn main() -> Result<(), Box<dyn std::error::Error>> {
|
||||
// Compile proto files
|
||||
prost_build::compile_protos(&["proto/sangamio.proto", "proto/dhruva.proto"], &["proto/"])?;
|
||||
|
||||
// Rerun if proto files change
|
||||
println!("cargo:rerun-if-changed=proto/sangamio.proto");
|
||||
println!("cargo:rerun-if-changed=proto/dhruva.proto");
|
||||
|
||||
Ok(())
|
||||
}
|
||||
@@ -0,0 +1,252 @@
|
||||
Performance Analysis: dhruva-slam TCP & Memory Optimization
|
||||
|
||||
Executive Summary
|
||||
|
||||
The dhruva-slam codebase is generally well-architected with sensible memory management. However, there are several optimization opportunities, particularly in hot paths that execute at 500Hz (sensor
|
||||
data) and 5Hz (lidar processing).
|
||||
|
||||
---
|
||||
1. TCP Packet Reading (sangam_client.rs)
|
||||
|
||||
Current Implementation (Good):
|
||||
- Uses a reusable 64KB buffer that grows lazily
|
||||
- Length-prefixed framing avoids dynamic allocation for each message
|
||||
- Buffer resizes only when message exceeds current capacity
|
||||
|
||||
Bottleneck: JSON deserialization allocates heavily on every message.
|
||||
|
||||
| Issue | Location | Frequency | Impact |
|
||||
|-----------------------------------------|-------------|-----------|--------|
|
||||
| HashMap<String, SensorValue> allocation | Line 107 | 500 Hz | High |
|
||||
| String allocations for topic/group_id | Lines 95-97 | 500 Hz | Medium |
|
||||
| Vec<LidarPoint> for each scan | Line 129 | 5 Hz | Medium |
|
||||
|
||||
Recommendations:
|
||||
|
||||
// 1. Consider switching to Postcard binary format (already supported!)
|
||||
// In WireFormat::Postcard mode, serialization is 3-5x faster
|
||||
|
||||
// 2. For JSON, use serde_json::from_slice with borrowed strings
|
||||
#[derive(Deserialize)]
|
||||
pub struct Message<'a> {
|
||||
#[serde(borrow)]
|
||||
pub topic: &'a str, // Zero-copy from buffer
|
||||
pub payload: Payload<'a>,
|
||||
}
|
||||
|
||||
// 3. Pre-size lidar Vec based on expected scan size (~540 points)
|
||||
const EXPECTED_LIDAR_POINTS: usize = 540;
|
||||
|
||||
---
|
||||
2. TCP Publisher Serialization (tcp_publisher.rs)
|
||||
|
||||
Bottleneck: serde_json::to_vec() allocates a new Vec<u8> for every broadcast (50+ Hz combined).
|
||||
|
||||
| Pattern | Line | Frequency | Allocation |
|
||||
|-------------------------------|----------|-----------------|---------------------------|
|
||||
| serde_json::to_vec(msg) | 242, 254 | 50+ Hz | ~60-500 bytes per message |
|
||||
| Vec::new() for failed_indices | 279 | Every broadcast | Small |
|
||||
|
||||
Recommendations:
|
||||
|
||||
// 1. Add a reusable serialization buffer to OdometryPublisher
|
||||
pub struct OdometryPublisher {
|
||||
clients: Arc<Mutex<Vec<Client>>>,
|
||||
running: Arc<AtomicBool>,
|
||||
listener_handle: Option<thread::JoinHandle<()>>,
|
||||
// NEW: Reusable buffer for serialization
|
||||
serialize_buffer: RefCell<Vec<u8>>,
|
||||
}
|
||||
|
||||
// 2. Use serde_json::to_writer to serialize directly into buffer
|
||||
fn broadcast_odometry(&self, msg: &OdometryMessage) {
|
||||
let mut buf = self.serialize_buffer.borrow_mut();
|
||||
buf.clear();
|
||||
if let Err(e) = serde_json::to_writer(&mut *buf, msg) {
|
||||
log::error!("Serialization failed: {}", e);
|
||||
return;
|
||||
}
|
||||
self.broadcast_bytes(&buf);
|
||||
}
|
||||
|
||||
// 3. Pre-allocate failed_indices with small capacity
|
||||
let mut failed_indices: SmallVec<[usize; 4]> = SmallVec::new();
|
||||
|
||||
---
|
||||
3. Lidar Scan Conversion (scan.rs:59-98)
|
||||
|
||||
Bottleneck: from_lidar_scan() creates a full copy to sort.
|
||||
|
||||
// Line 73: Clones entire scan just to sort
|
||||
let mut sorted: Vec<_> = scan.to_vec(); // ALLOCATION: 540 × 12 bytes = 6.5KB
|
||||
sorted.sort_by(...);
|
||||
|
||||
Recommendations:
|
||||
|
||||
// Option A: Sort in-place if SangamIO can provide mutable reference
|
||||
pub fn from_lidar_scan_mut(scan: &mut LidarScan) -> Self {
|
||||
scan.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap_or(std::cmp::Ordering::Equal));
|
||||
// ... use sorted data directly
|
||||
}
|
||||
|
||||
// Option B: Check if already sorted (often is from hardware)
|
||||
pub fn from_lidar_scan(scan: &LidarScan) -> Self {
|
||||
let is_sorted = scan.windows(2).all(|w| w[0].0 <= w[1].0);
|
||||
|
||||
if is_sorted {
|
||||
// Use directly, no clone needed
|
||||
Self::from_sorted_lidar(scan)
|
||||
} else {
|
||||
// Only clone when necessary
|
||||
let mut sorted = scan.to_vec();
|
||||
sorted.sort_by(...);
|
||||
Self::from_sorted_lidar(&sorted)
|
||||
}
|
||||
}
|
||||
|
||||
// Option C: Use argsort indices (avoids moving data)
|
||||
let mut indices: Vec<usize> = (0..scan.len()).collect();
|
||||
indices.sort_by(|&a, &b| scan[a].0.partial_cmp(&scan[b].0).unwrap_or(...));
|
||||
|
||||
---
|
||||
4. Point Cloud Transform (scan.rs:298-316)
|
||||
|
||||
Current: Creates new Vec<Point2D> for each transform.
|
||||
|
||||
pub fn transform(&self, pose: &Pose2D) -> PointCloud2D {
|
||||
let transformed_points: Vec<Point2D> = self.points
|
||||
.iter()
|
||||
.map(|p| { ... })
|
||||
.collect(); // ALLOCATION: ~200 points × 8 bytes
|
||||
...
|
||||
}
|
||||
|
||||
Good News: transform_mut() already exists at line 319! Prefer this when possible.
|
||||
|
||||
Recommendation: Audit call sites to use transform_mut() where the original isn't needed.
|
||||
|
||||
---
|
||||
5. Map Serialization (slam_messages.rs:204-215)
|
||||
|
||||
Bottleneck: Double allocation - Vec<u8> + Base64 string.
|
||||
|
||||
// Line 204: First allocation
|
||||
let mut cells_u8 = Vec::with_capacity(width * height); // 40KB for 200×200
|
||||
|
||||
// Line 215: Second allocation (Base64 expands by ~33%)
|
||||
let cells = BASE64.encode(&cells_u8); // ~53KB string
|
||||
|
||||
Recommendations:
|
||||
|
||||
// 1. Encode directly to pre-sized String
|
||||
let encoded_len = base64::encoded_len(width * height, base64::Config::STANDARD);
|
||||
let mut cells = String::with_capacity(encoded_len);
|
||||
|
||||
// Use base64::write::EncoderStringWriter to encode chunks
|
||||
let mut encoder = base64::write::EncoderStringWriter::new(&mut cells, &BASE64);
|
||||
for y in 0..height {
|
||||
for x in 0..width {
|
||||
encoder.write_all(&[Self::log_odds_to_u8(grid.get_log_odds(x, y), config)])?;
|
||||
}
|
||||
}
|
||||
encoder.finish();
|
||||
|
||||
// 2. Alternative: Stream encoding with iterator
|
||||
use base64::engine::general_purpose::STANDARD;
|
||||
let cells = STANDARD.encode(
|
||||
(0..height).flat_map(|y| (0..width).map(move |x| {
|
||||
Self::log_odds_to_u8(grid.get_log_odds(x, y), config)
|
||||
}))
|
||||
);
|
||||
|
||||
---
|
||||
6. Diagnostics Cloning (tcp_publisher.rs:212, 236)
|
||||
|
||||
// Line 212
|
||||
let msg = OdometryMessage::Diagnostics(diagnostics.clone());
|
||||
|
||||
// Line 236
|
||||
let msg = SlamMessage::Diagnostics(diagnostics.clone());
|
||||
|
||||
Impact: Low (1 Hz publishing, ~20 bytes each).
|
||||
|
||||
Recommendation: Accept by value if caller can transfer ownership:
|
||||
|
||||
pub fn publish_diagnostics(&self, diagnostics: OdometryDiagnostics) {
|
||||
let msg = OdometryMessage::Diagnostics(diagnostics); // No clone
|
||||
self.broadcast_odometry(&msg);
|
||||
}
|
||||
|
||||
---
|
||||
7. Downsampler Early Returns (downsampler.rs:59, 64)
|
||||
|
||||
if input_count == 0 {
|
||||
return scan.clone(); // ALLOCATION but rare
|
||||
}
|
||||
if input_count <= self.config.target_points {
|
||||
return scan.clone(); // ALLOCATION: common case!
|
||||
}
|
||||
|
||||
Issue: Cloning entire scan when no downsampling needed.
|
||||
|
||||
Recommendations:
|
||||
|
||||
// Option A: Return Cow<LaserScan>
|
||||
use std::borrow::Cow;
|
||||
|
||||
pub fn apply<'a>(&self, scan: &'a LaserScan) -> Cow<'a, LaserScan> {
|
||||
if input_count <= self.config.target_points {
|
||||
Cow::Borrowed(scan)
|
||||
} else {
|
||||
Cow::Owned(self.downsample(scan))
|
||||
}
|
||||
}
|
||||
|
||||
// Option B: Return enum
|
||||
pub enum DownsampleResult<'a> {
|
||||
Unchanged(&'a LaserScan),
|
||||
Downsampled(LaserScan),
|
||||
}
|
||||
|
||||
---
|
||||
8. Quick Wins Summary
|
||||
|
||||
| Optimization | Effort | Impact | Files |
|
||||
|-------------------------------|--------|------------|-----------------------------------|
|
||||
| Use Postcard wire format | Low | High | sangam_client.rs, SangamIO config |
|
||||
| Reusable serialization buffer | Medium | Medium | tcp_publisher.rs |
|
||||
| Check if lidar already sorted | Low | Medium | scan.rs |
|
||||
| Use transform_mut() | Low | Low-Medium | Call sites |
|
||||
| Avoid downsampler clone | Medium | Medium | downsampler.rs |
|
||||
| Pre-allocate failed_indices | Low | Low | tcp_publisher.rs |
|
||||
| Stream base64 encoding | Medium | Low (1Hz) | slam_messages.rs |
|
||||
|
||||
---
|
||||
9. Profiling Recommendations
|
||||
|
||||
Before implementing, I recommend adding benchmark targets:
|
||||
|
||||
# Add to Cargo.toml
|
||||
[[bench]]
|
||||
name = "tcp_throughput"
|
||||
harness = false
|
||||
|
||||
[[bench]]
|
||||
name = "scan_processing"
|
||||
harness = false
|
||||
|
||||
Key metrics to measure:
|
||||
1. TCP message receive rate (target: sustain 500 Hz)
|
||||
2. Lidar scan conversion time (target: <1ms for 540 points)
|
||||
3. Map serialization time (target: <10ms for 200×200 grid)
|
||||
4. Memory high-water mark during operation
|
||||
|
||||
---
|
||||
10. Architecture-Level Optimization
|
||||
|
||||
For maximum performance on the embedded target, consider:
|
||||
|
||||
1. Zero-copy message passing using a ring buffer between TCP receiver and processing threads
|
||||
2. SIMD for point transforms using packed_simd or wide crates
|
||||
3. Pre-allocated object pools for frequently allocated structs like PointCloud2D
|
||||
|
||||
@@ -0,0 +1,120 @@
|
||||
syntax = "proto3";
|
||||
package dhruva;
|
||||
|
||||
// Top-level message wrapper for DhruvaSLAM TCP output (port 5557)
|
||||
message DhruvaMessage {
|
||||
string topic = 1;
|
||||
oneof payload {
|
||||
OdometryPose odometry_pose = 2;
|
||||
OdometryDiagnostics odometry_diagnostics = 3;
|
||||
SlamStatus slam_status = 4;
|
||||
SlamMap slam_map = 5;
|
||||
SlamScan slam_scan = 6;
|
||||
SlamDiagnostics slam_diagnostics = 7;
|
||||
}
|
||||
}
|
||||
|
||||
// Odometry pose output (50Hz) - topic: "odometry/pose"
|
||||
message OdometryPose {
|
||||
float x = 1; // meters
|
||||
float y = 2; // meters
|
||||
float theta = 3; // radians
|
||||
uint64 timestamp_us = 4;
|
||||
}
|
||||
|
||||
// Odometry diagnostics (1Hz) - topic: "odometry/diagnostics"
|
||||
message OdometryDiagnostics {
|
||||
float drift_rate = 1; // rad/s estimated drift
|
||||
float tick_rate_left = 2; // ticks/s
|
||||
float tick_rate_right = 3; // ticks/s
|
||||
float distance_traveled = 4; // meters total
|
||||
float gyro_bias = 5; // rad/s calibrated bias
|
||||
}
|
||||
|
||||
// SLAM status (5Hz) - topic: "slam/status"
|
||||
message SlamStatus {
|
||||
string mode = 1; // "Mapping", "Localization", "Idle"
|
||||
uint64 num_scans = 2;
|
||||
uint32 num_keyframes = 3;
|
||||
uint32 num_submaps = 4;
|
||||
uint32 num_finished_submaps = 5;
|
||||
float match_score = 6; // 0.0 - 1.0
|
||||
bool is_lost = 7;
|
||||
uint64 memory_usage_bytes = 8;
|
||||
}
|
||||
|
||||
// SLAM occupancy grid map (1Hz) - topic: "slam/map"
|
||||
message SlamMap {
|
||||
float resolution = 1; // meters per cell
|
||||
uint32 width = 2; // cells
|
||||
uint32 height = 3; // cells
|
||||
float origin_x = 4; // world X of cell (0,0)
|
||||
float origin_y = 5; // world Y of cell (0,0)
|
||||
bytes cells = 6; // occupancy grid: 0=unknown, 1-127=free, 128-255=occupied
|
||||
uint64 timestamp_us = 7;
|
||||
}
|
||||
|
||||
// SLAM scan in global frame (5Hz) - topic: "slam/scan"
|
||||
message SlamScan {
|
||||
repeated Point2D points = 1;
|
||||
Pose2D pose = 2;
|
||||
uint64 timestamp_us = 3;
|
||||
}
|
||||
|
||||
// Basic geometry types
|
||||
message Point2D {
|
||||
float x = 1;
|
||||
float y = 2;
|
||||
}
|
||||
|
||||
message Pose2D {
|
||||
float x = 1;
|
||||
float y = 2;
|
||||
float theta = 3;
|
||||
}
|
||||
|
||||
// SLAM diagnostics (5Hz) - topic: "slam/diagnostics"
|
||||
message SlamDiagnostics {
|
||||
uint64 timestamp_us = 1;
|
||||
TimingBreakdown timing = 2;
|
||||
ScanMatchStats scan_match = 3;
|
||||
MappingStats mapping = 4;
|
||||
LoopClosureStats loop_closure = 5;
|
||||
}
|
||||
|
||||
message TimingBreakdown {
|
||||
uint64 total_us = 1;
|
||||
uint64 preprocessing_us = 2;
|
||||
uint64 scan_matching_us = 3;
|
||||
uint64 map_update_us = 4;
|
||||
uint64 particle_filter_us = 5;
|
||||
uint64 keyframe_check_us = 6;
|
||||
float avg_total_us = 7;
|
||||
}
|
||||
|
||||
message ScanMatchStats {
|
||||
string method = 1; // "icp", "correlative", "hybrid"
|
||||
uint32 iterations = 2;
|
||||
float score = 3; // 0.0 - 1.0
|
||||
float mse = 4; // mean squared error
|
||||
bool converged = 5;
|
||||
uint32 correspondences = 6;
|
||||
float inlier_ratio = 7;
|
||||
}
|
||||
|
||||
message MappingStats {
|
||||
uint32 cells_updated = 1;
|
||||
uint64 map_size_bytes = 2;
|
||||
uint32 occupied_cells = 3;
|
||||
uint32 free_cells = 4;
|
||||
optional uint64 active_submap_id = 5;
|
||||
uint32 num_submaps = 6;
|
||||
}
|
||||
|
||||
message LoopClosureStats {
|
||||
uint32 candidates_evaluated = 1;
|
||||
uint32 closures_accepted = 2;
|
||||
float last_closure_score = 3;
|
||||
uint32 pose_graph_nodes = 4;
|
||||
uint32 pose_graph_edges = 5;
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
syntax = "proto3";
|
||||
package sangamio;
|
||||
|
||||
// Top-level message wrapper for all SangamIO TCP communication
|
||||
message Message {
|
||||
string topic = 1;
|
||||
oneof payload {
|
||||
SensorGroup sensor_group = 2;
|
||||
Command command = 3;
|
||||
}
|
||||
}
|
||||
|
||||
// Sensor data from robot (outbound @ 500Hz for telemetry, 5Hz for lidar)
|
||||
message SensorGroup {
|
||||
string group_id = 1;
|
||||
uint64 timestamp_us = 2;
|
||||
map<string, SensorValue> values = 3;
|
||||
}
|
||||
|
||||
// Tagged union for sensor values - matches Rust SensorValue enum
|
||||
message SensorValue {
|
||||
oneof value {
|
||||
bool bool_val = 1;
|
||||
uint32 u32_val = 2;
|
||||
uint64 u64_val = 3;
|
||||
int32 i32_val = 4;
|
||||
int64 i64_val = 5;
|
||||
float f32_val = 6;
|
||||
double f64_val = 7;
|
||||
string string_val = 8;
|
||||
bytes bytes_val = 9;
|
||||
Vector3 vector3_val = 10;
|
||||
PointCloud2D pointcloud_val = 11;
|
||||
}
|
||||
}
|
||||
|
||||
message Vector3 {
|
||||
float x = 1;
|
||||
float y = 2;
|
||||
float z = 3;
|
||||
}
|
||||
|
||||
// LiDAR point cloud data
|
||||
message PointCloud2D {
|
||||
repeated LidarPoint points = 1;
|
||||
}
|
||||
|
||||
message LidarPoint {
|
||||
float angle_rad = 1;
|
||||
float distance_m = 2;
|
||||
uint32 quality = 3;
|
||||
}
|
||||
|
||||
// Commands to robot (inbound)
|
||||
message Command {
|
||||
oneof command {
|
||||
ComponentControl component_control = 1;
|
||||
ProtocolSync protocol_sync = 2;
|
||||
Shutdown shutdown = 3;
|
||||
}
|
||||
}
|
||||
|
||||
// Empty messages for simple commands
|
||||
message ProtocolSync {}
|
||||
message Shutdown {}
|
||||
|
||||
// Component control command (motors, actuators, sensors)
|
||||
message ComponentControl {
|
||||
string id = 1; // "drive", "vacuum", "lidar", "main_brush", "side_brush", etc.
|
||||
ComponentAction action = 2;
|
||||
}
|
||||
|
||||
message ComponentAction {
|
||||
ActionType type = 1;
|
||||
map<string, SensorValue> config = 2; // Optional configuration parameters
|
||||
|
||||
enum ActionType {
|
||||
ENABLE = 0;
|
||||
DISABLE = 1;
|
||||
RESET = 2;
|
||||
CONFIGURE = 3;
|
||||
}
|
||||
}
|
||||
@@ -19,7 +19,7 @@ use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use dhruva_slam::io::bag::{BagRecorder, EncoderTicks, SensorStatusMsg};
|
||||
use dhruva_slam::{SangamClient, Timestamped, WireFormat};
|
||||
use dhruva_slam::{SangamClient, Timestamped};
|
||||
|
||||
fn main() {
|
||||
env_logger::init();
|
||||
@@ -44,14 +44,12 @@ struct Config {
|
||||
sangam_address: String,
|
||||
output_path: String,
|
||||
duration_secs: Option<u64>,
|
||||
wire_format: WireFormat,
|
||||
}
|
||||
|
||||
fn parse_args(args: &[String]) -> Result<Config, String> {
|
||||
let mut sangam_address = None;
|
||||
let mut output_path = None;
|
||||
let mut duration_secs = None;
|
||||
let mut wire_format = WireFormat::Json;
|
||||
|
||||
let mut i = 1;
|
||||
while i < args.len() {
|
||||
@@ -81,22 +79,6 @@ fn parse_args(args: &[String]) -> Result<Config, String> {
|
||||
.map_err(|_| format!("Invalid duration: {}", args[i]))?,
|
||||
);
|
||||
}
|
||||
"--format" | "-f" => {
|
||||
i += 1;
|
||||
if i >= args.len() {
|
||||
return Err("Missing value for --format".to_string());
|
||||
}
|
||||
wire_format = match args[i].to_lowercase().as_str() {
|
||||
"json" => WireFormat::Json,
|
||||
"postcard" => WireFormat::Postcard,
|
||||
_ => {
|
||||
return Err(format!(
|
||||
"Invalid format: {} (use json or postcard)",
|
||||
args[i]
|
||||
));
|
||||
}
|
||||
};
|
||||
}
|
||||
"--help" | "-h" => {
|
||||
return Err("Help requested".to_string());
|
||||
}
|
||||
@@ -114,7 +96,6 @@ fn parse_args(args: &[String]) -> Result<Config, String> {
|
||||
sangam_address,
|
||||
output_path,
|
||||
duration_secs,
|
||||
wire_format,
|
||||
})
|
||||
}
|
||||
|
||||
@@ -129,7 +110,6 @@ OPTIONS:
|
||||
-s, --sangam <ADDRESS> SangamIO address (e.g., 192.168.68.101:5555)
|
||||
-o, --output <PATH> Output bag file path
|
||||
-d, --duration <SECS> Recording duration in seconds (default: until Ctrl-C)
|
||||
-f, --format <FORMAT> Wire format: json or postcard (default: json)
|
||||
-h, --help Show this help message
|
||||
|
||||
EXAMPLES:
|
||||
@@ -152,8 +132,8 @@ fn run(config: Config) -> Result<(), Box<dyn std::error::Error>> {
|
||||
})?;
|
||||
|
||||
println!("Connecting to SangamIO at {}...", config.sangam_address);
|
||||
let mut client = SangamClient::connect_with_format(&config.sangam_address, config.wire_format)?;
|
||||
println!("Connected!");
|
||||
let mut client = SangamClient::connect(&config.sangam_address)?;
|
||||
println!("Connected! (wire format: Protobuf)");
|
||||
|
||||
println!("Creating bag file: {}", config.output_path);
|
||||
let mut recorder = BagRecorder::create(&config.output_path)?;
|
||||
@@ -184,11 +164,7 @@ fn run(config: Config) -> Result<(), Box<dyn std::error::Error>> {
|
||||
// Receive message with timeout
|
||||
match client.recv() {
|
||||
Ok(msg) => {
|
||||
let timestamp_us = match &msg.payload {
|
||||
dhruva_slam::io::sangam_client::Payload::SensorGroup {
|
||||
timestamp_us, ..
|
||||
} => *timestamp_us,
|
||||
};
|
||||
let timestamp_us = msg.timestamp_us();
|
||||
|
||||
// Check if it's sensor status or lidar
|
||||
if let Some((left, right)) = msg.encoder_ticks() {
|
||||
|
||||
@@ -266,6 +266,7 @@ fn main() {
|
||||
|
||||
log::info!("dhruva-slam-node starting...");
|
||||
log::info!(" SangamIO: {}", config.source.sangam_address);
|
||||
log::info!(" Wire format: Protobuf");
|
||||
log::info!(" Publish port: {}", config.output.bind_port);
|
||||
log::info!(" Output rate: {} Hz", config.output.odometry_rate_hz);
|
||||
log::info!(" SLAM enabled: {}", config.slam.enabled);
|
||||
@@ -307,7 +308,7 @@ fn run_main_loop(
|
||||
config.source.sangam_address
|
||||
);
|
||||
let mut client = SangamClient::connect(&config.source.sangam_address)?;
|
||||
log::info!("Connected to SangamIO");
|
||||
log::info!("Connected to SangamIO (format: Protobuf)");
|
||||
|
||||
// Start publisher
|
||||
let publisher = OdometryPublisher::new(config.output.bind_port)?;
|
||||
|
||||
@@ -8,7 +8,7 @@ use std::path::Path;
|
||||
|
||||
use super::player::{BagPlayer, PlayerError};
|
||||
use super::types::BagMessage;
|
||||
use crate::io::sangam_client::{ClientError, Message, Payload, SensorValue};
|
||||
use crate::io::sangam_client::{ClientError, Message, SensorValue};
|
||||
|
||||
/// A SangamClient replacement that reads from a bag file.
|
||||
///
|
||||
@@ -105,42 +105,53 @@ impl SimulatedClient {
|
||||
match bag_msg {
|
||||
BagMessage::SensorStatus(status) => {
|
||||
let mut values = HashMap::new();
|
||||
// U16 values are stored as U32 in protobuf
|
||||
values.insert(
|
||||
"wheel_left".to_string(),
|
||||
SensorValue::U16(status.encoder.left),
|
||||
SensorValue::U32(status.encoder.left as u32),
|
||||
);
|
||||
values.insert(
|
||||
"wheel_right".to_string(),
|
||||
SensorValue::U16(status.encoder.right),
|
||||
SensorValue::U32(status.encoder.right as u32),
|
||||
);
|
||||
// I16 values are stored as I32 in protobuf
|
||||
values.insert(
|
||||
"gyro_x".to_string(),
|
||||
SensorValue::I32(status.gyro_raw[0] as i32),
|
||||
);
|
||||
values.insert(
|
||||
"gyro_y".to_string(),
|
||||
SensorValue::I32(status.gyro_raw[1] as i32),
|
||||
);
|
||||
values.insert(
|
||||
"gyro_z".to_string(),
|
||||
SensorValue::I32(status.gyro_raw[2] as i32),
|
||||
);
|
||||
values.insert(
|
||||
"accel_x".to_string(),
|
||||
SensorValue::I32(status.accel_raw[0] as i32),
|
||||
);
|
||||
values.insert(
|
||||
"accel_y".to_string(),
|
||||
SensorValue::I32(status.accel_raw[1] as i32),
|
||||
);
|
||||
values.insert(
|
||||
"accel_z".to_string(),
|
||||
SensorValue::I32(status.accel_raw[2] as i32),
|
||||
);
|
||||
values.insert("gyro_x".to_string(), SensorValue::I16(status.gyro_raw[0]));
|
||||
values.insert("gyro_y".to_string(), SensorValue::I16(status.gyro_raw[1]));
|
||||
values.insert("gyro_z".to_string(), SensorValue::I16(status.gyro_raw[2]));
|
||||
values.insert("accel_x".to_string(), SensorValue::I16(status.accel_raw[0]));
|
||||
values.insert("accel_y".to_string(), SensorValue::I16(status.accel_raw[1]));
|
||||
values.insert("accel_z".to_string(), SensorValue::I16(status.accel_raw[2]));
|
||||
|
||||
Message {
|
||||
topic: "sensors/sensor_status".to_string(),
|
||||
payload: Payload::SensorGroup {
|
||||
group_id: "sensor_status".to_string(),
|
||||
timestamp_us: status.timestamp_us,
|
||||
values,
|
||||
},
|
||||
}
|
||||
Message::new(
|
||||
"sensors/sensor_status",
|
||||
"sensor_status",
|
||||
status.timestamp_us,
|
||||
values,
|
||||
)
|
||||
}
|
||||
BagMessage::Lidar(scan) => {
|
||||
let mut values = HashMap::new();
|
||||
values.insert("scan".to_string(), SensorValue::PointCloud2D(scan.data));
|
||||
|
||||
Message {
|
||||
topic: "sensors/lidar".to_string(),
|
||||
payload: Payload::SensorGroup {
|
||||
group_id: "lidar".to_string(),
|
||||
timestamp_us: scan.timestamp_us,
|
||||
values,
|
||||
},
|
||||
}
|
||||
Message::new("sensors/lidar", "lidar", scan.timestamp_us, values)
|
||||
}
|
||||
BagMessage::Odometry(pose) => {
|
||||
// Odometry not directly in SangamIO format, but useful for ground truth
|
||||
@@ -149,14 +160,7 @@ impl SimulatedClient {
|
||||
values.insert("y".to_string(), SensorValue::F32(pose.data.y));
|
||||
values.insert("theta".to_string(), SensorValue::F32(pose.data.theta));
|
||||
|
||||
Message {
|
||||
topic: "sensors/odometry".to_string(),
|
||||
payload: Payload::SensorGroup {
|
||||
group_id: "odometry".to_string(),
|
||||
timestamp_us: pose.timestamp_us,
|
||||
values,
|
||||
},
|
||||
}
|
||||
Message::new("sensors/odometry", "odometry", pose.timestamp_us, values)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -13,4 +13,4 @@ pub mod sangam_client;
|
||||
pub mod streaming;
|
||||
|
||||
// Re-export common types
|
||||
pub use sangam_client::{LidarPoint, LidarScan, SangamClient, WireFormat};
|
||||
pub use sangam_client::{LidarPoint, LidarScan, SangamClient};
|
||||
|
||||
+138
-176
@@ -1,28 +1,23 @@
|
||||
//! SangamIO TCP client for receiving sensor data.
|
||||
//!
|
||||
//! Connects to SangamIO daemon and receives sensor messages.
|
||||
//! Supports both JSON and Postcard wire formats.
|
||||
//! Connects to SangamIO daemon and receives sensor messages using Protobuf.
|
||||
//!
|
||||
//! # Wire Protocol
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌──────────────────┬──────────────────────────┐
|
||||
//! │ Length (4 bytes) │ Payload (variable) │
|
||||
//! │ Big-endian u32 │ JSON or Postcard binary │
|
||||
//! │ Big-endian u32 │ Protobuf binary │
|
||||
//! └──────────────────┴──────────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! # Example
|
||||
//!
|
||||
//! ```ignore
|
||||
//! use dhruva_slam::sangam_client::{SangamClient, WireFormat};
|
||||
//! use dhruva_slam::sangam_client::SangamClient;
|
||||
//! use std::time::Duration;
|
||||
//!
|
||||
//! // Connect with JSON format (default for SangamIO)
|
||||
//! let mut client = SangamClient::connect_with_format(
|
||||
//! "192.168.68.101:5555",
|
||||
//! WireFormat::Json
|
||||
//! )?;
|
||||
//! let mut client = SangamClient::connect("192.168.68.101:5555")?;
|
||||
//! client.set_timeout(Some(Duration::from_secs(5)))?;
|
||||
//!
|
||||
//! loop {
|
||||
@@ -34,13 +29,20 @@
|
||||
//! ```
|
||||
|
||||
use crate::core::types::Timestamped;
|
||||
use serde::Deserialize;
|
||||
use prost::Message as ProstMessage;
|
||||
use std::collections::HashMap;
|
||||
use std::io::Read;
|
||||
use std::net::TcpStream;
|
||||
use std::time::Duration;
|
||||
use thiserror::Error;
|
||||
|
||||
// Include generated protobuf types
|
||||
pub mod proto {
|
||||
pub mod sangamio {
|
||||
include!(concat!(env!("OUT_DIR"), "/sangamio.rs"));
|
||||
}
|
||||
}
|
||||
|
||||
/// Client errors
|
||||
#[derive(Error, Debug)]
|
||||
pub enum ClientError {
|
||||
@@ -57,67 +59,20 @@ pub enum ClientError {
|
||||
BufferTooSmall(usize),
|
||||
}
|
||||
|
||||
impl From<postcard::Error> for ClientError {
|
||||
fn from(e: postcard::Error) -> Self {
|
||||
ClientError::Deserialize(e.to_string())
|
||||
}
|
||||
}
|
||||
|
||||
impl From<serde_json::Error> for ClientError {
|
||||
fn from(e: serde_json::Error) -> Self {
|
||||
impl From<prost::DecodeError> for ClientError {
|
||||
fn from(e: prost::DecodeError) -> Self {
|
||||
ClientError::Deserialize(e.to_string())
|
||||
}
|
||||
}
|
||||
|
||||
pub type Result<T> = std::result::Result<T, ClientError>;
|
||||
|
||||
/// Supported wire formats for SangamIO communication.
|
||||
///
|
||||
/// Must match the `wire_format` setting in SangamIO's hardware.json.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub enum WireFormat {
|
||||
/// JSON format - human-readable, default for SangamIO
|
||||
///
|
||||
/// Pros: Easy to debug, widely supported
|
||||
/// Cons: Larger messages, slower serialization
|
||||
#[default]
|
||||
Json,
|
||||
|
||||
/// Postcard binary format - compact and fast
|
||||
///
|
||||
/// Pros: Small messages, fast serialization
|
||||
/// Cons: Binary format, harder to debug
|
||||
Postcard,
|
||||
}
|
||||
|
||||
/// SangamIO message (mirrors sangam-io/src/streaming/messages.rs)
|
||||
#[derive(Debug, Deserialize)]
|
||||
pub struct Message {
|
||||
pub topic: String,
|
||||
pub payload: Payload,
|
||||
}
|
||||
|
||||
/// Message payload (only SensorGroup for now)
|
||||
#[derive(Debug, Deserialize)]
|
||||
#[serde(tag = "type")]
|
||||
pub enum Payload {
|
||||
SensorGroup {
|
||||
group_id: String,
|
||||
timestamp_us: u64,
|
||||
values: HashMap<String, SensorValue>,
|
||||
},
|
||||
}
|
||||
|
||||
/// Sensor value types (subset of sangam-io types we need)
|
||||
#[derive(Debug, Clone, Deserialize)]
|
||||
/// Sensor value types (converted from protobuf)
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum SensorValue {
|
||||
Bool(bool),
|
||||
U8(u8),
|
||||
U16(u16),
|
||||
U32(u32),
|
||||
U64(u64),
|
||||
I8(i8),
|
||||
I16(i16),
|
||||
I32(i32),
|
||||
I64(i64),
|
||||
F32(f32),
|
||||
@@ -135,39 +90,64 @@ pub type LidarPoint = (f32, f32, u8);
|
||||
/// A complete LiDAR scan as a vector of points.
|
||||
pub type LidarScan = Vec<LidarPoint>;
|
||||
|
||||
impl Payload {
|
||||
/// Get group_id from payload.
|
||||
#[inline]
|
||||
fn group_id(&self) -> &str {
|
||||
let Payload::SensorGroup { group_id, .. } = self;
|
||||
group_id
|
||||
}
|
||||
|
||||
/// Get timestamp from payload.
|
||||
#[inline]
|
||||
fn timestamp_us(&self) -> u64 {
|
||||
let Payload::SensorGroup { timestamp_us, .. } = self;
|
||||
*timestamp_us
|
||||
}
|
||||
|
||||
/// Get values from payload.
|
||||
#[inline]
|
||||
fn values(&self) -> &HashMap<String, SensorValue> {
|
||||
let Payload::SensorGroup { values, .. } = self;
|
||||
values
|
||||
}
|
||||
/// Parsed message from SangamIO
|
||||
#[derive(Debug)]
|
||||
pub struct Message {
|
||||
topic: String,
|
||||
group_id: String,
|
||||
timestamp_us: u64,
|
||||
values: HashMap<String, SensorValue>,
|
||||
}
|
||||
|
||||
impl Message {
|
||||
/// Create a new message (used by SimulatedClient)
|
||||
pub fn new(
|
||||
topic: &str,
|
||||
group_id: &str,
|
||||
timestamp_us: u64,
|
||||
values: HashMap<String, SensorValue>,
|
||||
) -> Self {
|
||||
Self {
|
||||
topic: topic.to_string(),
|
||||
group_id: group_id.to_string(),
|
||||
timestamp_us,
|
||||
values,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create from protobuf message
|
||||
fn from_proto(msg: proto::sangamio::Message) -> Result<Self> {
|
||||
match msg.payload {
|
||||
Some(proto::sangamio::message::Payload::SensorGroup(sg)) => {
|
||||
let values: HashMap<String, SensorValue> = sg
|
||||
.values
|
||||
.into_iter()
|
||||
.map(|(k, v)| (k, SensorValue::from_proto(v)))
|
||||
.collect();
|
||||
|
||||
Ok(Self {
|
||||
topic: msg.topic,
|
||||
group_id: sg.group_id,
|
||||
timestamp_us: sg.timestamp_us,
|
||||
values,
|
||||
})
|
||||
}
|
||||
Some(proto::sangamio::message::Payload::Command(_)) => Err(ClientError::Deserialize(
|
||||
"Received command message from server".to_string(),
|
||||
)),
|
||||
None => Err(ClientError::Deserialize("Empty payload".to_string())),
|
||||
}
|
||||
}
|
||||
|
||||
/// Extract lidar point cloud if this is a lidar message.
|
||||
///
|
||||
/// Returns timestamped reference to LidarScan (Vec of (angle_rad, distance_m, quality)).
|
||||
pub fn as_lidar(&self) -> Option<Timestamped<&LidarScan>> {
|
||||
if self.payload.group_id() != "lidar" {
|
||||
if self.group_id != "lidar" {
|
||||
return None;
|
||||
}
|
||||
if let Some(SensorValue::PointCloud2D(points)) = self.payload.values().get("scan") {
|
||||
Some(Timestamped::new(points, self.payload.timestamp_us()))
|
||||
if let Some(SensorValue::PointCloud2D(points)) = self.values.get("scan") {
|
||||
Some(Timestamped::new(points, self.timestamp_us))
|
||||
} else {
|
||||
None
|
||||
}
|
||||
@@ -175,18 +155,17 @@ impl Message {
|
||||
|
||||
/// Extract wheel encoder ticks (left, right) if this is a sensor_status message.
|
||||
///
|
||||
/// Returns raw U16 tick values.
|
||||
/// Returns raw U16 tick values (stored as U32 in protobuf).
|
||||
pub fn encoder_ticks(&self) -> Option<(u16, u16)> {
|
||||
if self.payload.group_id() != "sensor_status" {
|
||||
if self.group_id != "sensor_status" {
|
||||
return None;
|
||||
}
|
||||
let values = self.payload.values();
|
||||
let left = match values.get("wheel_left") {
|
||||
Some(SensorValue::U16(v)) => *v,
|
||||
let left = match self.values.get("wheel_left") {
|
||||
Some(SensorValue::U32(v)) => *v as u16,
|
||||
_ => return None,
|
||||
};
|
||||
let right = match values.get("wheel_right") {
|
||||
Some(SensorValue::U16(v)) => *v,
|
||||
let right = match self.values.get("wheel_right") {
|
||||
Some(SensorValue::U32(v)) => *v as u16,
|
||||
_ => return None,
|
||||
};
|
||||
Some((left, right))
|
||||
@@ -197,13 +176,13 @@ impl Message {
|
||||
/// **Note**: This returns `gyro_z` which is actually the Roll axis on CRL-200S.
|
||||
/// For yaw (heading), use [`gyro_yaw_raw`] instead.
|
||||
///
|
||||
/// Returns raw I16 value from IMU. Needs calibration/conversion to rad/s.
|
||||
/// Returns raw I16 value from IMU (stored as I32 in protobuf).
|
||||
pub fn gyro_z_raw(&self) -> Option<i16> {
|
||||
if self.payload.group_id() != "sensor_status" {
|
||||
if self.group_id != "sensor_status" {
|
||||
return None;
|
||||
}
|
||||
if let Some(SensorValue::I16(v)) = self.payload.values().get("gyro_z") {
|
||||
Some(*v)
|
||||
if let Some(SensorValue::I32(v)) = self.values.get("gyro_z") {
|
||||
Some(*v as i16)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
@@ -218,12 +197,12 @@ impl Message {
|
||||
///
|
||||
/// Returns raw I16 value in 0.1 deg/s units.
|
||||
pub fn gyro_yaw_raw(&self) -> Option<i16> {
|
||||
if self.payload.group_id() != "sensor_status" {
|
||||
if self.group_id != "sensor_status" {
|
||||
return None;
|
||||
}
|
||||
// Yaw rate is in gyro_x on CRL-200S
|
||||
if let Some(SensorValue::I16(v)) = self.payload.values().get("gyro_x") {
|
||||
Some(*v)
|
||||
if let Some(SensorValue::I32(v)) = self.values.get("gyro_x") {
|
||||
Some(*v as i16)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
@@ -231,22 +210,21 @@ impl Message {
|
||||
|
||||
/// Extract full gyroscope raw reading [x, y, z].
|
||||
///
|
||||
/// Returns raw I16 values from IMU. Needs calibration/conversion to rad/s.
|
||||
/// Returns raw I16 values from IMU.
|
||||
pub fn gyro_raw(&self) -> Option<[i16; 3]> {
|
||||
if self.payload.group_id() != "sensor_status" {
|
||||
if self.group_id != "sensor_status" {
|
||||
return None;
|
||||
}
|
||||
let values = self.payload.values();
|
||||
let x = match values.get("gyro_x") {
|
||||
Some(SensorValue::I16(v)) => *v,
|
||||
let x = match self.values.get("gyro_x") {
|
||||
Some(SensorValue::I32(v)) => *v as i16,
|
||||
_ => return None,
|
||||
};
|
||||
let y = match values.get("gyro_y") {
|
||||
Some(SensorValue::I16(v)) => *v,
|
||||
let y = match self.values.get("gyro_y") {
|
||||
Some(SensorValue::I32(v)) => *v as i16,
|
||||
_ => return None,
|
||||
};
|
||||
let z = match values.get("gyro_z") {
|
||||
Some(SensorValue::I16(v)) => *v,
|
||||
let z = match self.values.get("gyro_z") {
|
||||
Some(SensorValue::I32(v)) => *v as i16,
|
||||
_ => return None,
|
||||
};
|
||||
Some([x, y, z])
|
||||
@@ -254,43 +232,77 @@ impl Message {
|
||||
|
||||
/// Extract accelerometer raw reading [x, y, z].
|
||||
///
|
||||
/// Returns raw I16 values from IMU. Needs calibration/conversion to m/s².
|
||||
/// Returns raw I16 values from IMU.
|
||||
pub fn accel_raw(&self) -> Option<[i16; 3]> {
|
||||
if self.payload.group_id() != "sensor_status" {
|
||||
if self.group_id != "sensor_status" {
|
||||
return None;
|
||||
}
|
||||
let values = self.payload.values();
|
||||
let x = match values.get("accel_x") {
|
||||
Some(SensorValue::I16(v)) => *v,
|
||||
let x = match self.values.get("accel_x") {
|
||||
Some(SensorValue::I32(v)) => *v as i16,
|
||||
_ => return None,
|
||||
};
|
||||
let y = match values.get("accel_y") {
|
||||
Some(SensorValue::I16(v)) => *v,
|
||||
let y = match self.values.get("accel_y") {
|
||||
Some(SensorValue::I32(v)) => *v as i16,
|
||||
_ => return None,
|
||||
};
|
||||
let z = match values.get("accel_z") {
|
||||
Some(SensorValue::I16(v)) => *v,
|
||||
let z = match self.values.get("accel_z") {
|
||||
Some(SensorValue::I32(v)) => *v as i16,
|
||||
_ => return None,
|
||||
};
|
||||
Some([x, y, z])
|
||||
}
|
||||
|
||||
/// Get the topic of this message (e.g., "sensors/sensor_status").
|
||||
#[inline]
|
||||
pub fn topic(&self) -> &str {
|
||||
&self.topic
|
||||
}
|
||||
|
||||
/// Get the group_id of this message.
|
||||
#[inline]
|
||||
pub fn group_id(&self) -> &str {
|
||||
self.payload.group_id()
|
||||
&self.group_id
|
||||
}
|
||||
|
||||
/// Get timestamp in microseconds since epoch.
|
||||
#[inline]
|
||||
pub fn timestamp_us(&self) -> u64 {
|
||||
self.payload.timestamp_us()
|
||||
self.timestamp_us
|
||||
}
|
||||
|
||||
/// Get raw access to sensor values.
|
||||
#[inline]
|
||||
pub fn values(&self) -> &HashMap<String, SensorValue> {
|
||||
self.payload.values()
|
||||
&self.values
|
||||
}
|
||||
}
|
||||
|
||||
impl SensorValue {
|
||||
/// Convert from protobuf sensor value
|
||||
fn from_proto(value: proto::sangamio::SensorValue) -> Self {
|
||||
match value.value {
|
||||
Some(proto::sangamio::sensor_value::Value::BoolVal(v)) => SensorValue::Bool(v),
|
||||
Some(proto::sangamio::sensor_value::Value::U32Val(v)) => SensorValue::U32(v),
|
||||
Some(proto::sangamio::sensor_value::Value::U64Val(v)) => SensorValue::U64(v),
|
||||
Some(proto::sangamio::sensor_value::Value::I32Val(v)) => SensorValue::I32(v),
|
||||
Some(proto::sangamio::sensor_value::Value::I64Val(v)) => SensorValue::I64(v),
|
||||
Some(proto::sangamio::sensor_value::Value::F32Val(v)) => SensorValue::F32(v),
|
||||
Some(proto::sangamio::sensor_value::Value::F64Val(v)) => SensorValue::F64(v),
|
||||
Some(proto::sangamio::sensor_value::Value::StringVal(v)) => SensorValue::String(v),
|
||||
Some(proto::sangamio::sensor_value::Value::BytesVal(v)) => SensorValue::Bytes(v),
|
||||
Some(proto::sangamio::sensor_value::Value::Vector3Val(v)) => {
|
||||
SensorValue::Vector3([v.x, v.y, v.z])
|
||||
}
|
||||
Some(proto::sangamio::sensor_value::Value::PointcloudVal(v)) => {
|
||||
SensorValue::PointCloud2D(
|
||||
v.points
|
||||
.into_iter()
|
||||
.map(|p| (p.angle_rad, p.distance_m, p.quality as u8))
|
||||
.collect(),
|
||||
)
|
||||
}
|
||||
None => SensorValue::Bool(false), // Default fallback
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -302,65 +314,27 @@ const MAX_BUFFER_SIZE: usize = 1024 * 1024;
|
||||
|
||||
/// TCP client for SangamIO daemon.
|
||||
///
|
||||
/// Supports both JSON and Postcard wire formats. The format must match
|
||||
/// the `wire_format` setting in SangamIO's hardware.json configuration.
|
||||
/// Uses Protobuf wire format for efficient binary communication.
|
||||
pub struct SangamClient {
|
||||
stream: TcpStream,
|
||||
buffer: Vec<u8>,
|
||||
format: WireFormat,
|
||||
}
|
||||
|
||||
impl SangamClient {
|
||||
/// Connect to SangamIO daemon using JSON wire format (default).
|
||||
///
|
||||
/// This is a convenience method equivalent to:
|
||||
/// ```ignore
|
||||
/// SangamClient::connect_with_format(addr, WireFormat::Json)
|
||||
/// ```
|
||||
/// Connect to SangamIO daemon.
|
||||
///
|
||||
/// # Example
|
||||
/// ```ignore
|
||||
/// let client = SangamClient::connect("192.168.68.101:5555")?;
|
||||
/// ```
|
||||
pub fn connect(addr: &str) -> Result<Self> {
|
||||
Self::connect_with_format(addr, WireFormat::Json)
|
||||
}
|
||||
|
||||
/// Connect to SangamIO daemon with specified wire format.
|
||||
///
|
||||
/// The format must match the `wire_format` setting in SangamIO's
|
||||
/// hardware.json configuration file.
|
||||
///
|
||||
/// # Example
|
||||
/// ```ignore
|
||||
/// use dhruva_slam::sangam_client::{SangamClient, WireFormat};
|
||||
///
|
||||
/// // For hardware.json with "wire_format": "json"
|
||||
/// let client = SangamClient::connect_with_format(
|
||||
/// "192.168.68.101:5555",
|
||||
/// WireFormat::Json
|
||||
/// )?;
|
||||
///
|
||||
/// // For hardware.json with "wire_format": "postcard"
|
||||
/// let client = SangamClient::connect_with_format(
|
||||
/// "192.168.68.101:5555",
|
||||
/// WireFormat::Postcard
|
||||
/// )?;
|
||||
/// ```
|
||||
pub fn connect_with_format(addr: &str, format: WireFormat) -> Result<Self> {
|
||||
let stream = TcpStream::connect(addr)?;
|
||||
Ok(Self {
|
||||
stream,
|
||||
buffer: vec![0u8; DEFAULT_BUFFER_SIZE],
|
||||
format,
|
||||
})
|
||||
}
|
||||
|
||||
/// Get the current wire format.
|
||||
pub fn wire_format(&self) -> WireFormat {
|
||||
self.format
|
||||
}
|
||||
|
||||
/// Set read timeout for the connection.
|
||||
///
|
||||
/// Pass `None` to disable timeout (blocking reads).
|
||||
@@ -379,14 +353,6 @@ impl SangamClient {
|
||||
Ok(self.stream.peer_addr()?)
|
||||
}
|
||||
|
||||
/// Deserialize message bytes using the configured wire format.
|
||||
fn deserialize(&self, bytes: &[u8]) -> Result<Message> {
|
||||
match self.format {
|
||||
WireFormat::Json => Ok(serde_json::from_slice(bytes)?),
|
||||
WireFormat::Postcard => Ok(postcard::from_bytes(bytes)?),
|
||||
}
|
||||
}
|
||||
|
||||
/// Receive the next message (blocking).
|
||||
///
|
||||
/// Returns deserialized message from SangamIO.
|
||||
@@ -407,8 +373,9 @@ impl SangamClient {
|
||||
// Read payload
|
||||
self.stream.read_exact(&mut self.buffer[..len])?;
|
||||
|
||||
// Deserialize using configured format
|
||||
self.deserialize(&self.buffer[..len])
|
||||
// Decode protobuf message
|
||||
let proto_msg = proto::sangamio::Message::decode(&self.buffer[..len])?;
|
||||
Message::from_proto(proto_msg)
|
||||
}
|
||||
|
||||
/// Receive with timeout, returns None on timeout.
|
||||
@@ -449,13 +416,8 @@ mod tests {
|
||||
fn test_sensor_value_sizes() {
|
||||
// Ensure SensorValue variants compile and have expected structure
|
||||
let _bool = SensorValue::Bool(true);
|
||||
let _u16 = SensorValue::U16(1000);
|
||||
let _u32 = SensorValue::U32(1000);
|
||||
let _vec3 = SensorValue::Vector3([1.0, 2.0, 3.0]);
|
||||
let _points = SensorValue::PointCloud2D(vec![(0.0, 1.0, 100)]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_wire_format_default() {
|
||||
assert_eq!(WireFormat::default(), WireFormat::Json);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,13 +1,13 @@
|
||||
//! TCP publisher for streaming odometry data to visualization clients.
|
||||
//!
|
||||
//! Broadcasts pose and diagnostics messages to connected TCP clients using
|
||||
//! length-prefixed JSON framing (compatible with SangamIO protocol).
|
||||
//! length-prefixed Protobuf framing.
|
||||
//!
|
||||
//! # Wire Protocol
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌──────────────────┬──────────────────────────┐
|
||||
//! │ Length (4 bytes) │ JSON Payload (variable) │
|
||||
//! │ Length (4 bytes) │ Protobuf Payload │
|
||||
//! │ Big-endian u32 │ │
|
||||
//! └──────────────────┴──────────────────────────┘
|
||||
//! ```
|
||||
@@ -28,6 +28,7 @@
|
||||
use crate::algorithms::mapping::OccupancyGrid;
|
||||
use crate::core::types::{PointCloud2D, Pose2D};
|
||||
use crate::engine::slam::SlamStatus;
|
||||
use prost::Message;
|
||||
use serde::Serialize;
|
||||
use std::io::Write;
|
||||
use std::net::{TcpListener, TcpStream};
|
||||
@@ -37,9 +38,14 @@ use std::thread;
|
||||
use std::time::Duration;
|
||||
use thiserror::Error;
|
||||
|
||||
use super::slam_messages::{
|
||||
SlamDiagnosticsMessage, SlamMapMessage, SlamScanMessage, SlamStatusMessage,
|
||||
};
|
||||
use super::slam_messages::SlamDiagnosticsMessage;
|
||||
|
||||
// Include generated protobuf types
|
||||
pub mod proto {
|
||||
pub mod dhruva {
|
||||
include!(concat!(env!("OUT_DIR"), "/dhruva.rs"));
|
||||
}
|
||||
}
|
||||
|
||||
/// Publisher errors
|
||||
#[derive(Error, Debug)]
|
||||
@@ -48,12 +54,18 @@ pub enum PublisherError {
|
||||
Io(#[from] std::io::Error),
|
||||
|
||||
#[error("Serialization error: {0}")]
|
||||
Serialization(#[from] serde_json::Error),
|
||||
Serialization(String),
|
||||
}
|
||||
|
||||
impl From<prost::EncodeError> for PublisherError {
|
||||
fn from(e: prost::EncodeError) -> Self {
|
||||
PublisherError::Serialization(e.to_string())
|
||||
}
|
||||
}
|
||||
|
||||
pub type Result<T> = std::result::Result<T, PublisherError>;
|
||||
|
||||
/// Odometry pose message payload
|
||||
/// Odometry pose message payload (for internal use)
|
||||
#[derive(Debug, Clone, Serialize)]
|
||||
pub struct OdometryPose {
|
||||
pub x: f32,
|
||||
@@ -99,18 +111,18 @@ pub enum OdometryMessage {
|
||||
Diagnostics(OdometryDiagnostics),
|
||||
}
|
||||
|
||||
/// Message types for SLAM streaming
|
||||
/// Message types for SLAM streaming (for serde compatibility in tests)
|
||||
#[derive(Debug, Clone, Serialize)]
|
||||
#[serde(tag = "topic", content = "payload")]
|
||||
pub enum SlamMessage {
|
||||
#[serde(rename = "slam/status")]
|
||||
Status(SlamStatusMessage),
|
||||
Status(super::slam_messages::SlamStatusMessage),
|
||||
|
||||
#[serde(rename = "slam/map")]
|
||||
Map(SlamMapMessage),
|
||||
Map(super::slam_messages::SlamMapMessage),
|
||||
|
||||
#[serde(rename = "slam/scan")]
|
||||
Scan(SlamScanMessage),
|
||||
Scan(super::slam_messages::SlamScanMessage),
|
||||
|
||||
#[serde(rename = "slam/diagnostics")]
|
||||
Diagnostics(SlamDiagnosticsMessage),
|
||||
@@ -203,62 +215,204 @@ impl OdometryPublisher {
|
||||
|
||||
/// Publish a pose message to all connected clients.
|
||||
pub fn publish_pose(&self, pose: &Pose2D, timestamp_us: u64) {
|
||||
let msg = OdometryMessage::Pose(OdometryPose::from_pose(pose, timestamp_us));
|
||||
self.broadcast_odometry(&msg);
|
||||
let msg = proto::dhruva::DhruvaMessage {
|
||||
topic: "odometry/pose".to_string(),
|
||||
payload: Some(proto::dhruva::dhruva_message::Payload::OdometryPose(
|
||||
proto::dhruva::OdometryPose {
|
||||
x: pose.x,
|
||||
y: pose.y,
|
||||
theta: pose.theta,
|
||||
timestamp_us,
|
||||
},
|
||||
)),
|
||||
};
|
||||
self.broadcast_proto(&msg);
|
||||
}
|
||||
|
||||
/// Publish diagnostics to all connected clients.
|
||||
pub fn publish_diagnostics(&self, diagnostics: &OdometryDiagnostics) {
|
||||
let msg = OdometryMessage::Diagnostics(diagnostics.clone());
|
||||
self.broadcast_odometry(&msg);
|
||||
let msg = proto::dhruva::DhruvaMessage {
|
||||
topic: "odometry/diagnostics".to_string(),
|
||||
payload: Some(proto::dhruva::dhruva_message::Payload::OdometryDiagnostics(
|
||||
proto::dhruva::OdometryDiagnostics {
|
||||
drift_rate: diagnostics.drift_rate,
|
||||
tick_rate_left: diagnostics.tick_rate_left,
|
||||
tick_rate_right: diagnostics.tick_rate_right,
|
||||
distance_traveled: diagnostics.distance_traveled,
|
||||
gyro_bias: diagnostics.gyro_bias,
|
||||
},
|
||||
)),
|
||||
};
|
||||
self.broadcast_proto(&msg);
|
||||
}
|
||||
|
||||
/// Publish SLAM status to all connected clients.
|
||||
pub fn publish_slam_status(&self, status: &SlamStatus) {
|
||||
let msg = SlamMessage::Status(SlamStatusMessage::from_status(status));
|
||||
self.broadcast_slam(&msg);
|
||||
let mode = match status.mode {
|
||||
crate::engine::slam::SlamMode::Mapping => "Mapping",
|
||||
crate::engine::slam::SlamMode::Localization => "Localization",
|
||||
crate::engine::slam::SlamMode::Idle => "Idle",
|
||||
};
|
||||
|
||||
let msg = proto::dhruva::DhruvaMessage {
|
||||
topic: "slam/status".to_string(),
|
||||
payload: Some(proto::dhruva::dhruva_message::Payload::SlamStatus(
|
||||
proto::dhruva::SlamStatus {
|
||||
mode: mode.to_string(),
|
||||
num_scans: status.num_scans,
|
||||
num_keyframes: status.num_keyframes as u32,
|
||||
num_submaps: status.num_submaps as u32,
|
||||
num_finished_submaps: status.num_finished_submaps as u32,
|
||||
match_score: status.last_match_score,
|
||||
is_lost: status.is_lost,
|
||||
memory_usage_bytes: status.memory_usage as u64,
|
||||
},
|
||||
)),
|
||||
};
|
||||
self.broadcast_proto(&msg);
|
||||
}
|
||||
|
||||
/// Publish SLAM map to all connected clients.
|
||||
pub fn publish_slam_map(&self, grid: &OccupancyGrid, timestamp_us: u64) {
|
||||
let msg = SlamMessage::Map(SlamMapMessage::from_grid(grid, timestamp_us));
|
||||
self.broadcast_slam(&msg);
|
||||
let (width, height) = grid.dimensions();
|
||||
let (origin_x, origin_y) = grid.origin();
|
||||
let config = grid.config();
|
||||
|
||||
// Convert log-odds to u8 encoding
|
||||
let mut cells_u8 = Vec::with_capacity(width * height);
|
||||
for y in 0..height {
|
||||
for x in 0..width {
|
||||
let log_odds = grid.get_log_odds(x, y);
|
||||
let cell_value = Self::log_odds_to_u8(log_odds, config);
|
||||
cells_u8.push(cell_value);
|
||||
}
|
||||
}
|
||||
|
||||
let msg = proto::dhruva::DhruvaMessage {
|
||||
topic: "slam/map".to_string(),
|
||||
payload: Some(proto::dhruva::dhruva_message::Payload::SlamMap(
|
||||
proto::dhruva::SlamMap {
|
||||
resolution: config.resolution,
|
||||
width: width as u32,
|
||||
height: height as u32,
|
||||
origin_x,
|
||||
origin_y,
|
||||
cells: cells_u8,
|
||||
timestamp_us,
|
||||
},
|
||||
)),
|
||||
};
|
||||
self.broadcast_proto(&msg);
|
||||
}
|
||||
|
||||
/// Convert log-odds value to u8 cell encoding.
|
||||
fn log_odds_to_u8(
|
||||
log_odds: f32,
|
||||
config: &crate::algorithms::mapping::OccupancyGridConfig,
|
||||
) -> u8 {
|
||||
if log_odds.abs() < 0.01 {
|
||||
0
|
||||
} else if log_odds <= config.free_threshold {
|
||||
let confidence = (-log_odds / config.log_odds_min.abs()).clamp(0.0, 1.0);
|
||||
(1.0 + confidence * 126.0) as u8
|
||||
} else if log_odds >= config.occupied_threshold {
|
||||
let confidence = (log_odds / config.log_odds_max).clamp(0.0, 1.0);
|
||||
(128.0 + confidence * 127.0) as u8
|
||||
} else {
|
||||
0
|
||||
}
|
||||
}
|
||||
|
||||
/// Publish SLAM scan to all connected clients.
|
||||
pub fn publish_slam_scan(&self, scan: &PointCloud2D, pose: &Pose2D, timestamp_us: u64) {
|
||||
let msg = SlamMessage::Scan(SlamScanMessage::from_scan(scan, pose, timestamp_us));
|
||||
self.broadcast_slam(&msg);
|
||||
let cos_theta = pose.theta.cos();
|
||||
let sin_theta = pose.theta.sin();
|
||||
|
||||
// Transform points to global frame
|
||||
let points: Vec<proto::dhruva::Point2D> = scan
|
||||
.points
|
||||
.iter()
|
||||
.map(|p| {
|
||||
let global_x = pose.x + p.x * cos_theta - p.y * sin_theta;
|
||||
let global_y = pose.y + p.x * sin_theta + p.y * cos_theta;
|
||||
proto::dhruva::Point2D {
|
||||
x: global_x,
|
||||
y: global_y,
|
||||
}
|
||||
})
|
||||
.collect();
|
||||
|
||||
let msg = proto::dhruva::DhruvaMessage {
|
||||
topic: "slam/scan".to_string(),
|
||||
payload: Some(proto::dhruva::dhruva_message::Payload::SlamScan(
|
||||
proto::dhruva::SlamScan {
|
||||
points,
|
||||
pose: Some(proto::dhruva::Pose2D {
|
||||
x: pose.x,
|
||||
y: pose.y,
|
||||
theta: pose.theta,
|
||||
}),
|
||||
timestamp_us,
|
||||
},
|
||||
)),
|
||||
};
|
||||
self.broadcast_proto(&msg);
|
||||
}
|
||||
|
||||
/// Publish SLAM diagnostics to all connected clients.
|
||||
pub fn publish_slam_diagnostics(&self, diagnostics: &SlamDiagnosticsMessage) {
|
||||
let msg = SlamMessage::Diagnostics(diagnostics.clone());
|
||||
self.broadcast_slam(&msg);
|
||||
let msg = proto::dhruva::DhruvaMessage {
|
||||
topic: "slam/diagnostics".to_string(),
|
||||
payload: Some(proto::dhruva::dhruva_message::Payload::SlamDiagnostics(
|
||||
proto::dhruva::SlamDiagnostics {
|
||||
timestamp_us: diagnostics.timestamp_us,
|
||||
timing: Some(proto::dhruva::TimingBreakdown {
|
||||
total_us: diagnostics.timing.total_us,
|
||||
preprocessing_us: diagnostics.timing.preprocessing_us,
|
||||
scan_matching_us: diagnostics.timing.scan_matching_us,
|
||||
map_update_us: diagnostics.timing.map_update_us,
|
||||
particle_filter_us: diagnostics.timing.particle_filter_us,
|
||||
keyframe_check_us: diagnostics.timing.keyframe_check_us,
|
||||
avg_total_us: diagnostics.timing.avg_total_us,
|
||||
}),
|
||||
scan_match: Some(proto::dhruva::ScanMatchStats {
|
||||
method: diagnostics.scan_match.method.clone(),
|
||||
iterations: diagnostics.scan_match.iterations,
|
||||
score: diagnostics.scan_match.score,
|
||||
mse: diagnostics.scan_match.mse,
|
||||
converged: diagnostics.scan_match.converged,
|
||||
correspondences: diagnostics.scan_match.correspondences as u32,
|
||||
inlier_ratio: diagnostics.scan_match.inlier_ratio,
|
||||
}),
|
||||
mapping: Some(proto::dhruva::MappingStats {
|
||||
cells_updated: diagnostics.mapping.cells_updated,
|
||||
map_size_bytes: diagnostics.mapping.map_size_bytes as u64,
|
||||
occupied_cells: diagnostics.mapping.occupied_cells,
|
||||
free_cells: diagnostics.mapping.free_cells,
|
||||
active_submap_id: diagnostics.mapping.active_submap_id,
|
||||
num_submaps: diagnostics.mapping.num_submaps as u32,
|
||||
}),
|
||||
loop_closure: Some(proto::dhruva::LoopClosureStats {
|
||||
candidates_evaluated: diagnostics.loop_closure.candidates_evaluated,
|
||||
closures_accepted: diagnostics.loop_closure.closures_accepted,
|
||||
last_closure_score: diagnostics.loop_closure.last_closure_score,
|
||||
pose_graph_nodes: diagnostics.loop_closure.pose_graph_nodes as u32,
|
||||
pose_graph_edges: diagnostics.loop_closure.pose_graph_edges as u32,
|
||||
}),
|
||||
},
|
||||
)),
|
||||
};
|
||||
self.broadcast_proto(&msg);
|
||||
}
|
||||
|
||||
/// Broadcast an odometry message to all connected clients.
|
||||
fn broadcast_odometry(&self, msg: &OdometryMessage) {
|
||||
let bytes = match serde_json::to_vec(msg) {
|
||||
Ok(b) => b,
|
||||
Err(e) => {
|
||||
log::error!("Failed to serialize message: {}", e);
|
||||
return;
|
||||
}
|
||||
};
|
||||
self.broadcast_bytes(&bytes);
|
||||
}
|
||||
|
||||
/// Broadcast a SLAM message to all connected clients.
|
||||
fn broadcast_slam(&self, msg: &SlamMessage) {
|
||||
let bytes = match serde_json::to_vec(msg) {
|
||||
Ok(b) => b,
|
||||
Err(e) => {
|
||||
log::error!("Failed to serialize message: {}", e);
|
||||
return;
|
||||
}
|
||||
};
|
||||
self.broadcast_bytes(&bytes);
|
||||
/// Broadcast a protobuf message to all connected clients.
|
||||
fn broadcast_proto(&self, msg: &proto::dhruva::DhruvaMessage) {
|
||||
let mut buf = Vec::with_capacity(msg.encoded_len());
|
||||
if let Err(e) = msg.encode(&mut buf) {
|
||||
log::error!("Failed to encode protobuf message: {}", e);
|
||||
return;
|
||||
}
|
||||
self.broadcast_bytes(&buf);
|
||||
}
|
||||
|
||||
/// Broadcast raw bytes to all connected clients.
|
||||
@@ -325,64 +479,19 @@ mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_odometry_pose_serialization() {
|
||||
let msg = OdometryMessage::Pose(OdometryPose {
|
||||
x: 1.234,
|
||||
y: 0.567,
|
||||
theta: 0.785,
|
||||
timestamp_us: 1234567890,
|
||||
});
|
||||
|
||||
let json = serde_json::to_string(&msg).unwrap();
|
||||
assert!(json.contains("odometry/pose"));
|
||||
assert!(json.contains("1.234"));
|
||||
fn test_odometry_pose_creation() {
|
||||
let pose = Pose2D::new(1.234, 0.567, 0.785);
|
||||
let odom_pose = OdometryPose::from_pose(&pose, 1234567890);
|
||||
assert_eq!(odom_pose.x, 1.234);
|
||||
assert_eq!(odom_pose.y, 0.567);
|
||||
assert_eq!(odom_pose.theta, 0.785);
|
||||
assert_eq!(odom_pose.timestamp_us, 1234567890);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_diagnostics_serialization() {
|
||||
let msg = OdometryMessage::Diagnostics(OdometryDiagnostics {
|
||||
drift_rate: 0.02,
|
||||
tick_rate_left: 450.0,
|
||||
tick_rate_right: 448.0,
|
||||
distance_traveled: 5.67,
|
||||
gyro_bias: 0.001,
|
||||
});
|
||||
|
||||
let json = serde_json::to_string(&msg).unwrap();
|
||||
assert!(json.contains("odometry/diagnostics"));
|
||||
assert!(json.contains("drift_rate"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_slam_status_serialization() {
|
||||
let msg = SlamMessage::Status(SlamStatusMessage {
|
||||
mode: "Mapping".to_string(),
|
||||
num_scans: 1234,
|
||||
num_keyframes: 45,
|
||||
num_submaps: 3,
|
||||
num_finished_submaps: 2,
|
||||
match_score: 0.78,
|
||||
is_lost: false,
|
||||
memory_usage_bytes: 5242880,
|
||||
});
|
||||
|
||||
let json = serde_json::to_string(&msg).unwrap();
|
||||
assert!(json.contains("slam/status"));
|
||||
assert!(json.contains("\"mode\":\"Mapping\""));
|
||||
assert!(json.contains("\"num_scans\":1234"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_slam_scan_serialization() {
|
||||
let msg = SlamMessage::Scan(SlamScanMessage {
|
||||
points: vec![[1.0, 2.0], [3.0, 4.0]],
|
||||
pose: [1.234, 0.567, 0.785],
|
||||
timestamp_us: 1234567890,
|
||||
});
|
||||
|
||||
let json = serde_json::to_string(&msg).unwrap();
|
||||
assert!(json.contains("slam/scan"));
|
||||
assert!(json.contains("points"));
|
||||
assert!(json.contains("pose"));
|
||||
fn test_diagnostics_default() {
|
||||
let diag = OdometryDiagnostics::default();
|
||||
assert_eq!(diag.drift_rate, 0.0);
|
||||
assert_eq!(diag.distance_traveled, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
//! ## Phase 1: Foundation
|
||||
//! - Core types: Pose2D, Point2D, Timestamped<T>
|
||||
//! - Math primitives: angle normalization, pose composition
|
||||
//! - SangamIO client: TCP connection with JSON/Postcard wire format
|
||||
//! - SangamIO client: TCP connection with Protobuf wire format
|
||||
//!
|
||||
//! ## Phase 2: Odometry
|
||||
//! - New types: Twist2D, ImuReading, Covariance2D
|
||||
@@ -169,7 +169,7 @@ pub use engine::graph::{
|
||||
};
|
||||
|
||||
// I/O - SangamIO client
|
||||
pub use io::sangam_client::{LidarPoint, LidarScan, SangamClient, WireFormat};
|
||||
pub use io::sangam_client::{LidarPoint, LidarScan, SangamClient};
|
||||
|
||||
// I/O - Bag
|
||||
pub use io::bag::{BagHeader, BagInfo, BagMessage, BagPlayer, BagRecorder, SimulatedClient};
|
||||
|
||||
@@ -3,27 +3,26 @@
|
||||
//! These tests require a live SangamIO daemon running on the robot.
|
||||
//! Run with: cargo test -- --ignored
|
||||
//!
|
||||
//! Note: The wire format must match SangamIO's hardware.json configuration.
|
||||
//! Default is JSON (wire_format: "json").
|
||||
//! Note: Wire format is Protobuf (length-prefixed binary).
|
||||
|
||||
#[allow(unused_imports)]
|
||||
use dhruva_slam::{
|
||||
ComplementaryConfig, ComplementaryFilter, LaserScan, Pose2D, PreprocessorConfig, SangamClient,
|
||||
ScanPreprocessor, WheelOdometry, WheelOdometryConfig, WireFormat,
|
||||
ScanPreprocessor, WheelOdometry, WheelOdometryConfig,
|
||||
};
|
||||
use std::time::Duration;
|
||||
|
||||
/// Test connecting to SangamIO and receiving messages (JSON format).
|
||||
/// Test connecting to SangamIO and receiving messages (Protobuf format).
|
||||
///
|
||||
/// Run with: cargo test test_sangam_connection -- --ignored
|
||||
#[test]
|
||||
#[ignore]
|
||||
fn test_sangam_connection() {
|
||||
// Connect with JSON format (matches default SangamIO config)
|
||||
// Connect to SangamIO (Protobuf wire format)
|
||||
let mut client =
|
||||
SangamClient::connect("192.168.68.101:5555").expect("Failed to connect to SangamIO");
|
||||
|
||||
println!("Using wire format: {:?}", client.wire_format());
|
||||
println!("Connected to SangamIO (Protobuf format)");
|
||||
|
||||
client
|
||||
.set_timeout(Some(Duration::from_secs(5)))
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
# Auto-generated protobuf bindings for Drishti
|
||||
#
|
||||
# These files are generated from the proto definitions in:
|
||||
# - sangam-io/proto/sangamio.proto
|
||||
# - dhruva-slam/proto/dhruva.proto
|
||||
#
|
||||
# To regenerate:
|
||||
# cd drishti/proto
|
||||
# protoc --python_out=. -I../../sangam-io/proto ../../sangam-io/proto/sangamio.proto
|
||||
# protoc --python_out=. -I../../dhruva-slam/proto ../../dhruva-slam/proto/dhruva.proto
|
||||
|
||||
from . import sangamio_pb2
|
||||
from . import dhruva_pb2
|
||||
|
||||
__all__ = ['sangamio_pb2', 'dhruva_pb2']
|
||||
@@ -0,0 +1,60 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# NO CHECKED-IN PROTOBUF GENCODE
|
||||
# source: dhruva.proto
|
||||
# Protobuf Python Version: 6.32.0
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import runtime_version as _runtime_version
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
from google.protobuf.internal import builder as _builder
|
||||
_runtime_version.ValidateProtobufRuntimeVersion(
|
||||
_runtime_version.Domain.PUBLIC,
|
||||
6,
|
||||
32,
|
||||
0,
|
||||
'',
|
||||
'dhruva.proto'
|
||||
)
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x0c\x64hruva.proto\x12\x06\x64hruva\"\xc1\x02\n\rDhruvaMessage\x12\r\n\x05topic\x18\x01 \x01(\t\x12-\n\rodometry_pose\x18\x02 \x01(\x0b\x32\x14.dhruva.OdometryPoseH\x00\x12;\n\x14odometry_diagnostics\x18\x03 \x01(\x0b\x32\x1b.dhruva.OdometryDiagnosticsH\x00\x12)\n\x0bslam_status\x18\x04 \x01(\x0b\x32\x12.dhruva.SlamStatusH\x00\x12#\n\x08slam_map\x18\x05 \x01(\x0b\x32\x0f.dhruva.SlamMapH\x00\x12%\n\tslam_scan\x18\x06 \x01(\x0b\x32\x10.dhruva.SlamScanH\x00\x12\x33\n\x10slam_diagnostics\x18\x07 \x01(\x0b\x32\x17.dhruva.SlamDiagnosticsH\x00\x42\t\n\x07payload\"I\n\x0cOdometryPose\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\r\n\x05theta\x18\x03 \x01(\x02\x12\x14\n\x0ctimestamp_us\x18\x04 \x01(\x04\"\x88\x01\n\x13OdometryDiagnostics\x12\x12\n\ndrift_rate\x18\x01 \x01(\x02\x12\x16\n\x0etick_rate_left\x18\x02 \x01(\x02\x12\x17\n\x0ftick_rate_right\x18\x03 \x01(\x02\x12\x19\n\x11\x64istance_traveled\x18\x04 \x01(\x02\x12\x11\n\tgyro_bias\x18\x05 \x01(\x02\"\xb9\x01\n\nSlamStatus\x12\x0c\n\x04mode\x18\x01 \x01(\t\x12\x11\n\tnum_scans\x18\x02 \x01(\x04\x12\x15\n\rnum_keyframes\x18\x03 \x01(\r\x12\x13\n\x0bnum_submaps\x18\x04 \x01(\r\x12\x1c\n\x14num_finished_submaps\x18\x05 \x01(\r\x12\x13\n\x0bmatch_score\x18\x06 \x01(\x02\x12\x0f\n\x07is_lost\x18\x07 \x01(\x08\x12\x1a\n\x12memory_usage_bytes\x18\x08 \x01(\x04\"\x85\x01\n\x07SlamMap\x12\x12\n\nresolution\x18\x01 \x01(\x02\x12\r\n\x05width\x18\x02 \x01(\r\x12\x0e\n\x06height\x18\x03 \x01(\r\x12\x10\n\x08origin_x\x18\x04 \x01(\x02\x12\x10\n\x08origin_y\x18\x05 \x01(\x02\x12\r\n\x05\x63\x65lls\x18\x06 \x01(\x0c\x12\x14\n\x0ctimestamp_us\x18\x07 \x01(\x04\"_\n\x08SlamScan\x12\x1f\n\x06points\x18\x01 \x03(\x0b\x32\x0f.dhruva.Point2D\x12\x1c\n\x04pose\x18\x02 \x01(\x0b\x32\x0e.dhruva.Pose2D\x12\x14\n\x0ctimestamp_us\x18\x03 \x01(\x04\"\x1f\n\x07Point2D\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\"-\n\x06Pose2D\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\r\n\x05theta\x18\x03 \x01(\x02\"\xd3\x01\n\x0fSlamDiagnostics\x12\x14\n\x0ctimestamp_us\x18\x01 \x01(\x04\x12\'\n\x06timing\x18\x02 \x01(\x0b\x32\x17.dhruva.TimingBreakdown\x12*\n\nscan_match\x18\x03 \x01(\x0b\x32\x16.dhruva.ScanMatchStats\x12%\n\x07mapping\x18\x04 \x01(\x0b\x32\x14.dhruva.MappingStats\x12.\n\x0cloop_closure\x18\x05 \x01(\x0b\x32\x18.dhruva.LoopClosureStats\"\xbb\x01\n\x0fTimingBreakdown\x12\x10\n\x08total_us\x18\x01 \x01(\x04\x12\x18\n\x10preprocessing_us\x18\x02 \x01(\x04\x12\x18\n\x10scan_matching_us\x18\x03 \x01(\x04\x12\x15\n\rmap_update_us\x18\x04 \x01(\x04\x12\x1a\n\x12particle_filter_us\x18\x05 \x01(\x04\x12\x19\n\x11keyframe_check_us\x18\x06 \x01(\x04\x12\x14\n\x0c\x61vg_total_us\x18\x07 \x01(\x02\"\x92\x01\n\x0eScanMatchStats\x12\x0e\n\x06method\x18\x01 \x01(\t\x12\x12\n\niterations\x18\x02 \x01(\r\x12\r\n\x05score\x18\x03 \x01(\x02\x12\x0b\n\x03mse\x18\x04 \x01(\x02\x12\x11\n\tconverged\x18\x05 \x01(\x08\x12\x17\n\x0f\x63orrespondences\x18\x06 \x01(\r\x12\x14\n\x0cinlier_ratio\x18\x07 \x01(\x02\"\xb2\x01\n\x0cMappingStats\x12\x15\n\rcells_updated\x18\x01 \x01(\r\x12\x16\n\x0emap_size_bytes\x18\x02 \x01(\x04\x12\x16\n\x0eoccupied_cells\x18\x03 \x01(\r\x12\x12\n\nfree_cells\x18\x04 \x01(\r\x12\x1d\n\x10\x61\x63tive_submap_id\x18\x05 \x01(\x04H\x00\x88\x01\x01\x12\x13\n\x0bnum_submaps\x18\x06 \x01(\rB\x13\n\x11_active_submap_id\"\x9b\x01\n\x10LoopClosureStats\x12\x1c\n\x14\x63\x61ndidates_evaluated\x18\x01 \x01(\r\x12\x19\n\x11\x63losures_accepted\x18\x02 \x01(\r\x12\x1a\n\x12last_closure_score\x18\x03 \x01(\x02\x12\x18\n\x10pose_graph_nodes\x18\x04 \x01(\r\x12\x18\n\x10pose_graph_edges\x18\x05 \x01(\rb\x06proto3')
|
||||
|
||||
_globals = globals()
|
||||
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
|
||||
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'dhruva_pb2', _globals)
|
||||
if not _descriptor._USE_C_DESCRIPTORS:
|
||||
DESCRIPTOR._loaded_options = None
|
||||
_globals['_DHRUVAMESSAGE']._serialized_start=25
|
||||
_globals['_DHRUVAMESSAGE']._serialized_end=346
|
||||
_globals['_ODOMETRYPOSE']._serialized_start=348
|
||||
_globals['_ODOMETRYPOSE']._serialized_end=421
|
||||
_globals['_ODOMETRYDIAGNOSTICS']._serialized_start=424
|
||||
_globals['_ODOMETRYDIAGNOSTICS']._serialized_end=560
|
||||
_globals['_SLAMSTATUS']._serialized_start=563
|
||||
_globals['_SLAMSTATUS']._serialized_end=748
|
||||
_globals['_SLAMMAP']._serialized_start=751
|
||||
_globals['_SLAMMAP']._serialized_end=884
|
||||
_globals['_SLAMSCAN']._serialized_start=886
|
||||
_globals['_SLAMSCAN']._serialized_end=981
|
||||
_globals['_POINT2D']._serialized_start=983
|
||||
_globals['_POINT2D']._serialized_end=1014
|
||||
_globals['_POSE2D']._serialized_start=1016
|
||||
_globals['_POSE2D']._serialized_end=1061
|
||||
_globals['_SLAMDIAGNOSTICS']._serialized_start=1064
|
||||
_globals['_SLAMDIAGNOSTICS']._serialized_end=1275
|
||||
_globals['_TIMINGBREAKDOWN']._serialized_start=1278
|
||||
_globals['_TIMINGBREAKDOWN']._serialized_end=1465
|
||||
_globals['_SCANMATCHSTATS']._serialized_start=1468
|
||||
_globals['_SCANMATCHSTATS']._serialized_end=1614
|
||||
_globals['_MAPPINGSTATS']._serialized_start=1617
|
||||
_globals['_MAPPINGSTATS']._serialized_end=1795
|
||||
_globals['_LOOPCLOSURESTATS']._serialized_start=1798
|
||||
_globals['_LOOPCLOSURESTATS']._serialized_end=1953
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@@ -0,0 +1,66 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# NO CHECKED-IN PROTOBUF GENCODE
|
||||
# source: sangamio.proto
|
||||
# Protobuf Python Version: 6.32.0
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import runtime_version as _runtime_version
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
from google.protobuf.internal import builder as _builder
|
||||
_runtime_version.ValidateProtobufRuntimeVersion(
|
||||
_runtime_version.Domain.PUBLIC,
|
||||
6,
|
||||
32,
|
||||
0,
|
||||
'',
|
||||
'sangamio.proto'
|
||||
)
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x0esangamio.proto\x12\x08sangamio\"x\n\x07Message\x12\r\n\x05topic\x18\x01 \x01(\t\x12-\n\x0csensor_group\x18\x02 \x01(\x0b\x32\x15.sangamio.SensorGroupH\x00\x12$\n\x07\x63ommand\x18\x03 \x01(\x0b\x32\x11.sangamio.CommandH\x00\x42\t\n\x07payload\"\xae\x01\n\x0bSensorGroup\x12\x10\n\x08group_id\x18\x01 \x01(\t\x12\x14\n\x0ctimestamp_us\x18\x02 \x01(\x04\x12\x31\n\x06values\x18\x03 \x03(\x0b\x32!.sangamio.SensorGroup.ValuesEntry\x1a\x44\n\x0bValuesEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12$\n\x05value\x18\x02 \x01(\x0b\x32\x15.sangamio.SensorValue:\x02\x38\x01\"\xa3\x02\n\x0bSensorValue\x12\x12\n\x08\x62ool_val\x18\x01 \x01(\x08H\x00\x12\x11\n\x07u32_val\x18\x02 \x01(\rH\x00\x12\x11\n\x07u64_val\x18\x03 \x01(\x04H\x00\x12\x11\n\x07i32_val\x18\x04 \x01(\x05H\x00\x12\x11\n\x07i64_val\x18\x05 \x01(\x03H\x00\x12\x11\n\x07\x66\x33\x32_val\x18\x06 \x01(\x02H\x00\x12\x11\n\x07\x66\x36\x34_val\x18\x07 \x01(\x01H\x00\x12\x14\n\nstring_val\x18\x08 \x01(\tH\x00\x12\x13\n\tbytes_val\x18\t \x01(\x0cH\x00\x12(\n\x0bvector3_val\x18\n \x01(\x0b\x32\x11.sangamio.Vector3H\x00\x12\x30\n\x0epointcloud_val\x18\x0b \x01(\x0b\x32\x16.sangamio.PointCloud2DH\x00\x42\x07\n\x05value\"*\n\x07Vector3\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"4\n\x0cPointCloud2D\x12$\n\x06points\x18\x01 \x03(\x0b\x32\x14.sangamio.LidarPoint\"D\n\nLidarPoint\x12\x11\n\tangle_rad\x18\x01 \x01(\x02\x12\x12\n\ndistance_m\x18\x02 \x01(\x02\x12\x0f\n\x07quality\x18\x03 \x01(\r\"\xa6\x01\n\x07\x43ommand\x12\x37\n\x11\x63omponent_control\x18\x01 \x01(\x0b\x32\x1a.sangamio.ComponentControlH\x00\x12/\n\rprotocol_sync\x18\x02 \x01(\x0b\x32\x16.sangamio.ProtocolSyncH\x00\x12&\n\x08shutdown\x18\x03 \x01(\x0b\x32\x12.sangamio.ShutdownH\x00\x42\t\n\x07\x63ommand\"\x0e\n\x0cProtocolSync\"\n\n\x08Shutdown\"I\n\x10\x43omponentControl\x12\n\n\x02id\x18\x01 \x01(\t\x12)\n\x06\x61\x63tion\x18\x02 \x01(\x0b\x32\x19.sangamio.ComponentAction\"\x83\x02\n\x0f\x43omponentAction\x12\x32\n\x04type\x18\x01 \x01(\x0e\x32$.sangamio.ComponentAction.ActionType\x12\x35\n\x06\x63onfig\x18\x02 \x03(\x0b\x32%.sangamio.ComponentAction.ConfigEntry\x1a\x44\n\x0b\x43onfigEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12$\n\x05value\x18\x02 \x01(\x0b\x32\x15.sangamio.SensorValue:\x02\x38\x01\"?\n\nActionType\x12\n\n\x06\x45NABLE\x10\x00\x12\x0b\n\x07\x44ISABLE\x10\x01\x12\t\n\x05RESET\x10\x02\x12\r\n\tCONFIGURE\x10\x03\x62\x06proto3')
|
||||
|
||||
_globals = globals()
|
||||
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
|
||||
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'sangamio_pb2', _globals)
|
||||
if not _descriptor._USE_C_DESCRIPTORS:
|
||||
DESCRIPTOR._loaded_options = None
|
||||
_globals['_SENSORGROUP_VALUESENTRY']._loaded_options = None
|
||||
_globals['_SENSORGROUP_VALUESENTRY']._serialized_options = b'8\001'
|
||||
_globals['_COMPONENTACTION_CONFIGENTRY']._loaded_options = None
|
||||
_globals['_COMPONENTACTION_CONFIGENTRY']._serialized_options = b'8\001'
|
||||
_globals['_MESSAGE']._serialized_start=28
|
||||
_globals['_MESSAGE']._serialized_end=148
|
||||
_globals['_SENSORGROUP']._serialized_start=151
|
||||
_globals['_SENSORGROUP']._serialized_end=325
|
||||
_globals['_SENSORGROUP_VALUESENTRY']._serialized_start=257
|
||||
_globals['_SENSORGROUP_VALUESENTRY']._serialized_end=325
|
||||
_globals['_SENSORVALUE']._serialized_start=328
|
||||
_globals['_SENSORVALUE']._serialized_end=619
|
||||
_globals['_VECTOR3']._serialized_start=621
|
||||
_globals['_VECTOR3']._serialized_end=663
|
||||
_globals['_POINTCLOUD2D']._serialized_start=665
|
||||
_globals['_POINTCLOUD2D']._serialized_end=717
|
||||
_globals['_LIDARPOINT']._serialized_start=719
|
||||
_globals['_LIDARPOINT']._serialized_end=787
|
||||
_globals['_COMMAND']._serialized_start=790
|
||||
_globals['_COMMAND']._serialized_end=956
|
||||
_globals['_PROTOCOLSYNC']._serialized_start=958
|
||||
_globals['_PROTOCOLSYNC']._serialized_end=972
|
||||
_globals['_SHUTDOWN']._serialized_start=974
|
||||
_globals['_SHUTDOWN']._serialized_end=984
|
||||
_globals['_COMPONENTCONTROL']._serialized_start=986
|
||||
_globals['_COMPONENTCONTROL']._serialized_end=1059
|
||||
_globals['_COMPONENTACTION']._serialized_start=1062
|
||||
_globals['_COMPONENTACTION']._serialized_end=1321
|
||||
_globals['_COMPONENTACTION_CONFIGENTRY']._serialized_start=1188
|
||||
_globals['_COMPONENTACTION_CONFIGENTRY']._serialized_end=1256
|
||||
_globals['_COMPONENTACTION_ACTIONTYPE']._serialized_start=1258
|
||||
_globals['_COMPONENTACTION_ACTIONTYPE']._serialized_end=1321
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@@ -1,8 +1,8 @@
|
||||
# Drishti - Real-time Robot Status Visualization System
|
||||
# Python dependencies
|
||||
|
||||
# MessagePack serialization
|
||||
msgpack>=1.0.0
|
||||
# Protocol Buffers for wire format
|
||||
protobuf>=4.0
|
||||
|
||||
# UI Framework
|
||||
PyQt5>=5.15.0
|
||||
|
||||
@@ -1,14 +1,19 @@
|
||||
"""
|
||||
Odometry thread for receiving pose data from dhruva-slam-node daemon.
|
||||
Supports JSON wire format with odometry/pose and odometry/diagnostics topics.
|
||||
Supports Protobuf wire format with DhruvaMessage wrapper.
|
||||
"""
|
||||
|
||||
import socket
|
||||
import struct
|
||||
import json
|
||||
import logging
|
||||
import os
|
||||
from PyQt5.QtCore import QThread, pyqtSignal
|
||||
|
||||
# Import protobuf definitions
|
||||
import sys
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..'))
|
||||
from proto import dhruva_pb2
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@@ -31,14 +36,14 @@ class OdometryThread(QThread):
|
||||
# num_finished_submaps, match_score, is_lost, memory_usage_bytes}
|
||||
slam_status_received = pyqtSignal(dict)
|
||||
|
||||
# slam/map payload: {resolution, width, height, origin_x, origin_y, cells (base64), timestamp_us}
|
||||
# slam/map payload: {resolution, width, height, origin_x, origin_y, cells (bytes), timestamp_us}
|
||||
slam_map_received = pyqtSignal(dict)
|
||||
|
||||
# slam/scan payload: {points: [[x,y],...], pose: [x,y,theta], timestamp_us}
|
||||
slam_scan_received = pyqtSignal(dict)
|
||||
|
||||
# slam/diagnostics payload: {timestamp_us, timing: {...}, scan_match: {...},
|
||||
# particle_filter: {...}, mapping: {...}, loop_closure: {...}}
|
||||
# mapping: {...}, loop_closure: {...}}
|
||||
slam_diagnostics_received = pyqtSignal(dict)
|
||||
|
||||
def __init__(self, host: str, port: int = 5557, parent=None):
|
||||
@@ -59,7 +64,7 @@ class OdometryThread(QThread):
|
||||
self.socket.settimeout(5.0)
|
||||
self.socket.connect((self.host, self.port))
|
||||
self.connection_status.emit(True, f"Connected to SLAM at {self.host}:{self.port}")
|
||||
logger.info(f"Connected to dhruva-slam-node at {self.host}:{self.port}")
|
||||
logger.info(f"Connected to dhruva-slam-node at {self.host}:{self.port} (wire format: Protobuf)")
|
||||
|
||||
# Receive loop
|
||||
while self._running:
|
||||
@@ -103,8 +108,8 @@ class OdometryThread(QThread):
|
||||
pass
|
||||
self.wait()
|
||||
|
||||
def _read_message(self) -> dict:
|
||||
"""Read a length-prefixed JSON message."""
|
||||
def _read_message(self) -> dhruva_pb2.DhruvaMessage:
|
||||
"""Read a length-prefixed Protobuf message."""
|
||||
# Read 4-byte length prefix
|
||||
length_data = self._recv_exact(4)
|
||||
if not length_data:
|
||||
@@ -121,8 +126,10 @@ class OdometryThread(QThread):
|
||||
if not data:
|
||||
return None
|
||||
|
||||
# Parse JSON
|
||||
return json.loads(data.decode('utf-8'))
|
||||
# Parse Protobuf
|
||||
msg = dhruva_pb2.DhruvaMessage()
|
||||
msg.ParseFromString(data)
|
||||
return msg
|
||||
|
||||
def _recv_exact(self, n: int) -> bytes:
|
||||
"""Receive exactly n bytes."""
|
||||
@@ -134,45 +141,124 @@ class OdometryThread(QThread):
|
||||
data += chunk
|
||||
return data
|
||||
|
||||
def _process_message(self, message: dict):
|
||||
def _process_message(self, message: dhruva_pb2.DhruvaMessage):
|
||||
"""Process a received message and emit appropriate signal."""
|
||||
topic = message.get('topic', '')
|
||||
payload = message.get('payload', {})
|
||||
topic = message.topic
|
||||
which = message.WhichOneof('payload')
|
||||
|
||||
if topic == 'odometry/pose':
|
||||
# Emit pose signal
|
||||
if topic == 'odometry/pose' and which == 'odometry_pose':
|
||||
pose = message.odometry_pose
|
||||
payload = {
|
||||
'x': pose.x,
|
||||
'y': pose.y,
|
||||
'theta': pose.theta,
|
||||
'timestamp_us': pose.timestamp_us
|
||||
}
|
||||
self.pose_received.emit(payload)
|
||||
logger.debug(f"Pose: x={payload.get('x', 0):.3f} y={payload.get('y', 0):.3f} θ={payload.get('theta', 0):.3f}")
|
||||
logger.debug(f"Pose: x={pose.x:.3f} y={pose.y:.3f} θ={pose.theta:.3f}")
|
||||
|
||||
elif topic == 'odometry/diagnostics':
|
||||
# Emit diagnostics signal
|
||||
elif topic == 'odometry/diagnostics' and which == 'odometry_diagnostics':
|
||||
diag = message.odometry_diagnostics
|
||||
payload = {
|
||||
'drift_rate': diag.drift_rate,
|
||||
'tick_rate_left': diag.tick_rate_left,
|
||||
'tick_rate_right': diag.tick_rate_right,
|
||||
'distance_traveled': diag.distance_traveled,
|
||||
'gyro_bias': diag.gyro_bias
|
||||
}
|
||||
self.diagnostics_received.emit(payload)
|
||||
logger.debug(f"Diagnostics: distance={payload.get('distance_traveled', 0):.2f}m")
|
||||
logger.debug(f"Diagnostics: distance={diag.distance_traveled:.2f}m")
|
||||
|
||||
elif topic == 'slam/status':
|
||||
# Emit SLAM status signal
|
||||
elif topic == 'slam/status' and which == 'slam_status':
|
||||
status = message.slam_status
|
||||
payload = {
|
||||
'mode': status.mode,
|
||||
'num_scans': status.num_scans,
|
||||
'num_keyframes': status.num_keyframes,
|
||||
'num_submaps': status.num_submaps,
|
||||
'num_finished_submaps': status.num_finished_submaps,
|
||||
'match_score': status.match_score,
|
||||
'is_lost': status.is_lost,
|
||||
'memory_usage_bytes': status.memory_usage_bytes
|
||||
}
|
||||
self.slam_status_received.emit(payload)
|
||||
logger.debug(f"SLAM status: mode={payload.get('mode')} scans={payload.get('num_scans')}")
|
||||
logger.debug(f"SLAM status: mode={status.mode} scans={status.num_scans}")
|
||||
|
||||
elif topic == 'slam/map':
|
||||
# Emit SLAM map signal
|
||||
elif topic == 'slam/map' and which == 'slam_map':
|
||||
slam_map = message.slam_map
|
||||
payload = {
|
||||
'resolution': slam_map.resolution,
|
||||
'width': slam_map.width,
|
||||
'height': slam_map.height,
|
||||
'origin_x': slam_map.origin_x,
|
||||
'origin_y': slam_map.origin_y,
|
||||
'cells': slam_map.cells, # Raw bytes (no base64)
|
||||
'timestamp_us': slam_map.timestamp_us
|
||||
}
|
||||
self.slam_map_received.emit(payload)
|
||||
logger.debug(f"SLAM map: {payload.get('width')}x{payload.get('height')} @ {payload.get('resolution')}m")
|
||||
logger.debug(f"SLAM map: {slam_map.width}x{slam_map.height} @ {slam_map.resolution}m")
|
||||
|
||||
elif topic == 'slam/scan':
|
||||
# Emit SLAM scan signal
|
||||
elif topic == 'slam/scan' and which == 'slam_scan':
|
||||
scan = message.slam_scan
|
||||
# Convert points to list of [x, y]
|
||||
points = [[p.x, p.y] for p in scan.points]
|
||||
pose = scan.pose
|
||||
payload = {
|
||||
'points': points,
|
||||
'pose': [pose.x, pose.y, pose.theta] if pose else [0, 0, 0],
|
||||
'timestamp_us': scan.timestamp_us
|
||||
}
|
||||
self.slam_scan_received.emit(payload)
|
||||
logger.debug(f"SLAM scan: {len(payload.get('points', []))} points")
|
||||
logger.debug(f"SLAM scan: {len(points)} points")
|
||||
|
||||
elif topic == 'slam/diagnostics':
|
||||
# Emit SLAM diagnostics signal
|
||||
elif topic == 'slam/diagnostics' and which == 'slam_diagnostics':
|
||||
diag = message.slam_diagnostics
|
||||
timing = diag.timing
|
||||
scan_match = diag.scan_match
|
||||
mapping = diag.mapping
|
||||
loop_closure = diag.loop_closure
|
||||
|
||||
payload = {
|
||||
'timestamp_us': diag.timestamp_us,
|
||||
'timing': {
|
||||
'total_us': timing.total_us if timing else 0,
|
||||
'preprocessing_us': timing.preprocessing_us if timing else 0,
|
||||
'scan_matching_us': timing.scan_matching_us if timing else 0,
|
||||
'map_update_us': timing.map_update_us if timing else 0,
|
||||
'particle_filter_us': timing.particle_filter_us if timing else 0,
|
||||
'keyframe_check_us': timing.keyframe_check_us if timing else 0,
|
||||
'avg_total_us': timing.avg_total_us if timing else 0,
|
||||
},
|
||||
'scan_match': {
|
||||
'method': scan_match.method if scan_match else '',
|
||||
'iterations': scan_match.iterations if scan_match else 0,
|
||||
'score': scan_match.score if scan_match else 0,
|
||||
'mse': scan_match.mse if scan_match else 0,
|
||||
'converged': scan_match.converged if scan_match else False,
|
||||
'correspondences': scan_match.correspondences if scan_match else 0,
|
||||
'inlier_ratio': scan_match.inlier_ratio if scan_match else 0,
|
||||
},
|
||||
'mapping': {
|
||||
'cells_updated': mapping.cells_updated if mapping else 0,
|
||||
'map_size_bytes': mapping.map_size_bytes if mapping else 0,
|
||||
'occupied_cells': mapping.occupied_cells if mapping else 0,
|
||||
'free_cells': mapping.free_cells if mapping else 0,
|
||||
'active_submap_id': mapping.active_submap_id if mapping and mapping.HasField('active_submap_id') else None,
|
||||
'num_submaps': mapping.num_submaps if mapping else 0,
|
||||
},
|
||||
'loop_closure': {
|
||||
'candidates_evaluated': loop_closure.candidates_evaluated if loop_closure else 0,
|
||||
'closures_accepted': loop_closure.closures_accepted if loop_closure else 0,
|
||||
'last_closure_score': loop_closure.last_closure_score if loop_closure else 0,
|
||||
'pose_graph_nodes': loop_closure.pose_graph_nodes if loop_closure else 0,
|
||||
'pose_graph_edges': loop_closure.pose_graph_edges if loop_closure else 0,
|
||||
},
|
||||
}
|
||||
self.slam_diagnostics_received.emit(payload)
|
||||
timing = payload.get('timing', {})
|
||||
scan_match = payload.get('scan_match', {})
|
||||
logger.debug(
|
||||
f"SLAM diagnostics: total={timing.get('total_us', 0)}us "
|
||||
f"match={timing.get('scan_matching_us', 0)}us "
|
||||
f"score={scan_match.get('score', 0):.2f}"
|
||||
f"SLAM diagnostics: total={timing.total_us if timing else 0}us "
|
||||
f"match={timing.scan_matching_us if timing else 0}us "
|
||||
f"score={scan_match.score if scan_match else 0:.2f}"
|
||||
)
|
||||
|
||||
def send_reset_command(self) -> bool:
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
"""
|
||||
Telemetry thread for receiving sensor data from SangamIO daemon.
|
||||
Supports JSON wire format with Message/Payload structure.
|
||||
Supports Protobuf wire format with Message/SensorGroup structure.
|
||||
"""
|
||||
|
||||
import socket
|
||||
@@ -11,6 +11,11 @@ import os
|
||||
from datetime import datetime
|
||||
from PyQt5.QtCore import QThread, pyqtSignal
|
||||
|
||||
# Import protobuf definitions
|
||||
import sys
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..'))
|
||||
from proto import sangamio_pb2
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@@ -56,7 +61,7 @@ class TelemetryThread(QThread):
|
||||
self.socket.settimeout(5.0)
|
||||
self.socket.connect((self.host, self.port))
|
||||
self.connection_status.emit(True, f"Connected to {self.host}:{self.port}")
|
||||
logger.info(f"Connected to {self.host}:{self.port}")
|
||||
logger.info(f"Connected to {self.host}:{self.port} (wire format: Protobuf)")
|
||||
|
||||
# Receive loop
|
||||
while self._running:
|
||||
@@ -119,10 +124,7 @@ class TelemetryThread(QThread):
|
||||
{
|
||||
"type": "ComponentControl",
|
||||
"id": "drive" | "vacuum" | "lidar" | etc.,
|
||||
"action": {
|
||||
"type": "Enable" | "Disable" | "Reset" | "Configure",
|
||||
"config": { ... } # optional
|
||||
}
|
||||
"action": {"type": "Enable" | "Disable" | "Reset" | "Configure", "config": {...}}
|
||||
}
|
||||
"""
|
||||
if not self.socket:
|
||||
@@ -130,22 +132,42 @@ class TelemetryThread(QThread):
|
||||
return False
|
||||
|
||||
try:
|
||||
# Create message with topic and payload
|
||||
message = {
|
||||
"topic": "command",
|
||||
"payload": {
|
||||
"type": "Command",
|
||||
"command": command
|
||||
}
|
||||
}
|
||||
# Create protobuf message
|
||||
msg = sangamio_pb2.Message()
|
||||
msg.topic = "command"
|
||||
|
||||
# Encode as JSON
|
||||
data = json.dumps(message).encode('utf-8')
|
||||
cmd = msg.command
|
||||
ctrl = cmd.component_control
|
||||
ctrl.id = command.get('id', '')
|
||||
|
||||
# Create length-prefixed message
|
||||
# Parse action - can be a string or a dict with "type" key
|
||||
action_data = command.get('action', {})
|
||||
if isinstance(action_data, str):
|
||||
action_type = action_data.lower()
|
||||
config_data = {}
|
||||
else:
|
||||
action_type = action_data.get('type', 'enable').lower()
|
||||
config_data = action_data.get('config', {})
|
||||
|
||||
action = ctrl.action
|
||||
if action_type == 'enable':
|
||||
action.type = sangamio_pb2.ComponentAction.ENABLE
|
||||
elif action_type == 'disable':
|
||||
action.type = sangamio_pb2.ComponentAction.DISABLE
|
||||
elif action_type == 'reset':
|
||||
action.type = sangamio_pb2.ComponentAction.RESET
|
||||
elif action_type == 'configure':
|
||||
action.type = sangamio_pb2.ComponentAction.CONFIGURE
|
||||
|
||||
# Add config values if present
|
||||
for key, value in config_data.items():
|
||||
sensor_val = sangamio_pb2.SensorValue()
|
||||
self._set_sensor_value(sensor_val, value)
|
||||
action.config[key].CopyFrom(sensor_val)
|
||||
|
||||
# Serialize and send
|
||||
data = msg.SerializeToString()
|
||||
length = struct.pack('>I', len(data))
|
||||
|
||||
# Send through existing socket
|
||||
self.socket.sendall(length + data)
|
||||
logger.debug(f"Command sent: {command}")
|
||||
return True
|
||||
@@ -154,8 +176,45 @@ class TelemetryThread(QThread):
|
||||
logger.error(f"Failed to send command: {e}")
|
||||
return False
|
||||
|
||||
def _read_message(self) -> dict:
|
||||
"""Read a length-prefixed JSON message."""
|
||||
def _set_sensor_value(self, sensor_val: sangamio_pb2.SensorValue, value):
|
||||
"""Set the appropriate field in a SensorValue based on the value type."""
|
||||
if isinstance(value, dict):
|
||||
# Handle typed values like {"U8": 100} or {"F32": 0.5}
|
||||
if 'Bool' in value:
|
||||
sensor_val.bool_val = value['Bool']
|
||||
elif 'U8' in value:
|
||||
sensor_val.u32_val = value['U8']
|
||||
elif 'U16' in value:
|
||||
sensor_val.u32_val = value['U16']
|
||||
elif 'U32' in value:
|
||||
sensor_val.u32_val = value['U32']
|
||||
elif 'U64' in value:
|
||||
sensor_val.u64_val = value['U64']
|
||||
elif 'I8' in value:
|
||||
sensor_val.i32_val = value['I8']
|
||||
elif 'I16' in value:
|
||||
sensor_val.i32_val = value['I16']
|
||||
elif 'I32' in value:
|
||||
sensor_val.i32_val = value['I32']
|
||||
elif 'I64' in value:
|
||||
sensor_val.i64_val = value['I64']
|
||||
elif 'F32' in value:
|
||||
sensor_val.f32_val = value['F32']
|
||||
elif 'F64' in value:
|
||||
sensor_val.f64_val = value['F64']
|
||||
elif 'String' in value:
|
||||
sensor_val.string_val = value['String']
|
||||
elif isinstance(value, bool):
|
||||
sensor_val.bool_val = value
|
||||
elif isinstance(value, int):
|
||||
sensor_val.i32_val = value
|
||||
elif isinstance(value, float):
|
||||
sensor_val.f32_val = value
|
||||
elif isinstance(value, str):
|
||||
sensor_val.string_val = value
|
||||
|
||||
def _read_message(self) -> sangamio_pb2.Message:
|
||||
"""Read a length-prefixed Protobuf message."""
|
||||
# Read 4-byte length prefix
|
||||
length_data = self._recv_exact(4)
|
||||
if not length_data:
|
||||
@@ -172,8 +231,10 @@ class TelemetryThread(QThread):
|
||||
if not data:
|
||||
return None
|
||||
|
||||
# Parse JSON
|
||||
return json.loads(data.decode('utf-8'))
|
||||
# Parse Protobuf
|
||||
msg = sangamio_pb2.Message()
|
||||
msg.ParseFromString(data)
|
||||
return msg
|
||||
|
||||
def _recv_exact(self, n: int) -> bytes:
|
||||
"""Receive exactly n bytes."""
|
||||
@@ -185,30 +246,30 @@ class TelemetryThread(QThread):
|
||||
data += chunk
|
||||
return data
|
||||
|
||||
def _process_message(self, message: dict):
|
||||
def _process_message(self, message: sangamio_pb2.Message):
|
||||
"""Process a received message and emit signal."""
|
||||
topic = message.get('topic', '')
|
||||
payload = message.get('payload', {})
|
||||
topic = message.topic
|
||||
|
||||
# Handle sensor group messages
|
||||
if topic.startswith('sensors/'):
|
||||
group_id = payload.get('group_id', '')
|
||||
values = payload.get('values', {})
|
||||
if topic.startswith('sensors/') and message.HasField('sensor_group'):
|
||||
sg = message.sensor_group
|
||||
group_id = sg.group_id
|
||||
timestamp_us = sg.timestamp_us
|
||||
|
||||
# Convert sensor values from protobuf to dict
|
||||
sensor_data = {}
|
||||
for key, value in sg.values.items():
|
||||
sensor_data[key] = self._extract_value(value)
|
||||
|
||||
# Log raw_packet if enabled and present
|
||||
if self.raw_packet_file and 'raw_packet' in values:
|
||||
raw_packet = self._extract_value(values['raw_packet'])
|
||||
if self.raw_packet_file and 'raw_packet' in sensor_data:
|
||||
raw_packet = sensor_data['raw_packet']
|
||||
if raw_packet:
|
||||
self._log_raw_packet(raw_packet, payload.get('timestamp_us', 0))
|
||||
|
||||
# Convert sensor values from tagged format to simple values
|
||||
sensor_data = {}
|
||||
for key, value in values.items():
|
||||
sensor_data[key] = self._extract_value(value)
|
||||
self._log_raw_packet(raw_packet, timestamp_us)
|
||||
|
||||
# Add metadata
|
||||
sensor_data['_group_id'] = group_id
|
||||
sensor_data['_timestamp_us'] = payload.get('timestamp_us', 0)
|
||||
sensor_data['_timestamp_us'] = timestamp_us
|
||||
|
||||
# Log parsed sensor data if enabled
|
||||
if self.sensor_data_file:
|
||||
@@ -216,7 +277,7 @@ class TelemetryThread(QThread):
|
||||
|
||||
self.sensor_data_received.emit(sensor_data)
|
||||
|
||||
def _log_raw_packet(self, raw_packet: list, timestamp_us: int):
|
||||
def _log_raw_packet(self, raw_packet: bytes, timestamp_us: int):
|
||||
"""Log raw packet bytes to file.
|
||||
|
||||
Format: timestamp_us,hex_bytes
|
||||
@@ -237,7 +298,8 @@ class TelemetryThread(QThread):
|
||||
"""Log parsed sensor data as JSON to file."""
|
||||
try:
|
||||
# Write as JSON line (exclude raw_packet to keep it readable)
|
||||
data_to_log = {k: v for k, v in sensor_data.items() if k != 'raw_packet'}
|
||||
data_to_log = {k: v for k, v in sensor_data.items()
|
||||
if k != 'raw_packet' and not isinstance(v, bytes)}
|
||||
self.sensor_data_file.write(json.dumps(data_to_log) + '\n')
|
||||
self._sensor_log_count += 1
|
||||
|
||||
@@ -247,12 +309,36 @@ class TelemetryThread(QThread):
|
||||
except Exception as e:
|
||||
logger.error(f"Failed to log sensor data: {e}")
|
||||
|
||||
def _extract_value(self, tagged_value):
|
||||
"""Extract value from tagged SensorValue format.
|
||||
def _extract_value(self, sensor_value: sangamio_pb2.SensorValue):
|
||||
"""Extract value from protobuf SensorValue.
|
||||
|
||||
Format: {"Bool": true} or {"U16": 1234} etc.
|
||||
Returns the appropriate Python type based on which field is set.
|
||||
"""
|
||||
if isinstance(tagged_value, dict):
|
||||
for type_tag, value in tagged_value.items():
|
||||
return value
|
||||
return tagged_value
|
||||
which = sensor_value.WhichOneof('value')
|
||||
if which == 'bool_val':
|
||||
return sensor_value.bool_val
|
||||
elif which == 'u32_val':
|
||||
return sensor_value.u32_val
|
||||
elif which == 'u64_val':
|
||||
return sensor_value.u64_val
|
||||
elif which == 'i32_val':
|
||||
return sensor_value.i32_val
|
||||
elif which == 'i64_val':
|
||||
return sensor_value.i64_val
|
||||
elif which == 'f32_val':
|
||||
return sensor_value.f32_val
|
||||
elif which == 'f64_val':
|
||||
return sensor_value.f64_val
|
||||
elif which == 'string_val':
|
||||
return sensor_value.string_val
|
||||
elif which == 'bytes_val':
|
||||
return sensor_value.bytes_val
|
||||
elif which == 'vector3_val':
|
||||
v = sensor_value.vector3_val
|
||||
return [v.x, v.y, v.z]
|
||||
elif which == 'pointcloud_val':
|
||||
pc = sensor_value.pointcloud_val
|
||||
# Return as list of tuples: (angle_rad, distance_m, quality)
|
||||
return [(p.angle_rad, p.distance_m, p.quality) for p in pc.points]
|
||||
else:
|
||||
return None
|
||||
|
||||
@@ -14,8 +14,8 @@ protocol_debug = []
|
||||
[dependencies]
|
||||
# Serialization
|
||||
serde = { version = "1.0", features = ["derive"] }
|
||||
serde_json = "1.0"
|
||||
postcard = { version = "1.0", features = ["alloc"] }
|
||||
toml = "0.8"
|
||||
prost = "0.13"
|
||||
|
||||
# Serial port
|
||||
serialport = "4.2"
|
||||
@@ -32,3 +32,6 @@ thiserror = "1.0"
|
||||
|
||||
# Signal handling
|
||||
ctrlc = "3.4"
|
||||
|
||||
[build-dependencies]
|
||||
prost-build = "0.13"
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
//! Build script for compiling protobuf definitions
|
||||
|
||||
fn main() -> Result<(), Box<dyn std::error::Error>> {
|
||||
// Configure prost-build to suppress clippy warnings on generated code
|
||||
let mut config = prost_build::Config::new();
|
||||
config.type_attribute(".", "#[allow(clippy::enum_variant_names)]");
|
||||
|
||||
// Compile proto files
|
||||
config.compile_protos(&["proto/sangamio.proto"], &["proto/"])?;
|
||||
|
||||
// Rerun if proto files change
|
||||
println!("cargo:rerun-if-changed=proto/sangamio.proto");
|
||||
|
||||
Ok(())
|
||||
}
|
||||
@@ -1,17 +0,0 @@
|
||||
{
|
||||
"device": {
|
||||
"type": "crl200s",
|
||||
"name": "CRL-200S Vacuum Robot",
|
||||
|
||||
"hardware": {
|
||||
"gd32_port": "/dev/ttyS3",
|
||||
"lidar_port": "/dev/ttyS1",
|
||||
"heartbeat_interval_ms": 20
|
||||
}
|
||||
},
|
||||
|
||||
"network": {
|
||||
"bind_address": "0.0.0.0:5555",
|
||||
"wire_format": "json"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
syntax = "proto3";
|
||||
package sangamio;
|
||||
|
||||
// Top-level message wrapper for all SangamIO TCP communication
|
||||
message Message {
|
||||
string topic = 1;
|
||||
oneof payload {
|
||||
SensorGroup sensor_group = 2;
|
||||
Command command = 3;
|
||||
}
|
||||
}
|
||||
|
||||
// Sensor data from robot (outbound @ 500Hz for telemetry, 5Hz for lidar)
|
||||
message SensorGroup {
|
||||
string group_id = 1;
|
||||
uint64 timestamp_us = 2;
|
||||
map<string, SensorValue> values = 3;
|
||||
}
|
||||
|
||||
// Tagged union for sensor values - matches Rust SensorValue enum
|
||||
message SensorValue {
|
||||
oneof value {
|
||||
bool bool_val = 1;
|
||||
uint32 u32_val = 2;
|
||||
uint64 u64_val = 3;
|
||||
int32 i32_val = 4;
|
||||
int64 i64_val = 5;
|
||||
float f32_val = 6;
|
||||
double f64_val = 7;
|
||||
string string_val = 8;
|
||||
bytes bytes_val = 9;
|
||||
Vector3 vector3_val = 10;
|
||||
PointCloud2D pointcloud_val = 11;
|
||||
}
|
||||
}
|
||||
|
||||
message Vector3 {
|
||||
float x = 1;
|
||||
float y = 2;
|
||||
float z = 3;
|
||||
}
|
||||
|
||||
// LiDAR point cloud data
|
||||
message PointCloud2D {
|
||||
repeated LidarPoint points = 1;
|
||||
}
|
||||
|
||||
message LidarPoint {
|
||||
float angle_rad = 1;
|
||||
float distance_m = 2;
|
||||
uint32 quality = 3;
|
||||
}
|
||||
|
||||
// Commands to robot (inbound)
|
||||
message Command {
|
||||
oneof command {
|
||||
ComponentControl component_control = 1;
|
||||
ProtocolSync protocol_sync = 2;
|
||||
Shutdown shutdown = 3;
|
||||
}
|
||||
}
|
||||
|
||||
// Empty messages for simple commands
|
||||
message ProtocolSync {}
|
||||
message Shutdown {}
|
||||
|
||||
// Component control command (motors, actuators, sensors)
|
||||
message ComponentControl {
|
||||
string id = 1; // "drive", "vacuum", "lidar", "main_brush", "side_brush", etc.
|
||||
ComponentAction action = 2;
|
||||
}
|
||||
|
||||
message ComponentAction {
|
||||
ActionType type = 1;
|
||||
map<string, SensorValue> config = 2; // Optional configuration parameters
|
||||
|
||||
enum ActionType {
|
||||
ENABLE = 0;
|
||||
DISABLE = 1;
|
||||
RESET = 2;
|
||||
CONFIGURE = 3;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
# SangamIO Configuration
|
||||
# Hardware abstraction daemon for CRL-200S robot vacuum
|
||||
|
||||
[device]
|
||||
type = "crl200s"
|
||||
name = "CRL-200S Vacuum Robot"
|
||||
|
||||
[device.hardware]
|
||||
gd32_port = "/dev/ttyS3"
|
||||
lidar_port = "/dev/ttyS1"
|
||||
heartbeat_interval_ms = 20
|
||||
|
||||
[network]
|
||||
bind_address = "0.0.0.0:5555"
|
||||
+18
-48
@@ -1,31 +1,26 @@
|
||||
//! Configuration loading from JSON
|
||||
//! Configuration loading from TOML
|
||||
//!
|
||||
//! # Configuration File Format
|
||||
//!
|
||||
//! The configuration file is JSON-formatted with the following structure:
|
||||
//! The configuration file is TOML-formatted with the following structure:
|
||||
//!
|
||||
//! ```json
|
||||
//! {
|
||||
//! "device": {
|
||||
//! "type": "crl-200s",
|
||||
//! "name": "robot-01",
|
||||
//! "hardware": {
|
||||
//! "gd32_port": "/dev/ttyS3",
|
||||
//! "lidar_port": "/dev/ttyS1",
|
||||
//! "heartbeat_interval_ms": 20
|
||||
//! }
|
||||
//! },
|
||||
//! "network": {
|
||||
//! "bind_address": "0.0.0.0:5555",
|
||||
//! "wire_format": "json"
|
||||
//! }
|
||||
//! }
|
||||
//! ```toml
|
||||
//! [device]
|
||||
//! type = "crl200s"
|
||||
//! name = "CRL-200S Vacuum Robot"
|
||||
//!
|
||||
//! [device.hardware]
|
||||
//! gd32_port = "/dev/ttyS3"
|
||||
//! lidar_port = "/dev/ttyS1"
|
||||
//! heartbeat_interval_ms = 20
|
||||
//!
|
||||
//! [network]
|
||||
//! bind_address = "0.0.0.0:5555"
|
||||
//! ```
|
||||
//!
|
||||
//! See `hardware.json` for complete example.
|
||||
//! See `sangamio.toml` for complete example.
|
||||
|
||||
use crate::error::{Error, Result};
|
||||
use crate::streaming::WireFormat;
|
||||
use serde::Deserialize;
|
||||
use std::fs;
|
||||
use std::path::Path;
|
||||
@@ -99,31 +94,6 @@ pub struct NetworkConfig {
|
||||
///
|
||||
/// **Required**: Yes
|
||||
pub bind_address: String,
|
||||
|
||||
/// Wire format for message serialization
|
||||
///
|
||||
/// **Valid values**: "json" (default), "postcard"
|
||||
/// **Optional**: Defaults to "json" if not specified
|
||||
#[serde(default)]
|
||||
pub wire_format: WireFormatConfig,
|
||||
}
|
||||
|
||||
/// Wire format configuration (deserializable from string)
|
||||
#[derive(Debug, Clone, Deserialize, Default)]
|
||||
#[serde(rename_all = "lowercase")]
|
||||
pub enum WireFormatConfig {
|
||||
Postcard,
|
||||
#[default]
|
||||
Json,
|
||||
}
|
||||
|
||||
impl From<WireFormatConfig> for WireFormat {
|
||||
fn from(config: WireFormatConfig) -> Self {
|
||||
match config {
|
||||
WireFormatConfig::Postcard => WireFormat::Postcard,
|
||||
WireFormatConfig::Json => WireFormat::Json,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Root configuration
|
||||
@@ -134,12 +104,12 @@ pub struct Config {
|
||||
}
|
||||
|
||||
impl Config {
|
||||
/// Load configuration from JSON file
|
||||
/// Load configuration from TOML file
|
||||
pub fn load<P: AsRef<Path>>(path: P) -> Result<Self> {
|
||||
let content = fs::read_to_string(path)
|
||||
let content = fs::read_to_string(&path)
|
||||
.map_err(|e| Error::Config(format!("Failed to read config: {}", e)))?;
|
||||
|
||||
let config: Config = serde_json::from_str(&content)
|
||||
let config: Config = toml::from_str(&content)
|
||||
.map_err(|e| Error::Config(format!("Failed to parse config: {}", e)))?;
|
||||
|
||||
Ok(config)
|
||||
|
||||
@@ -103,7 +103,6 @@ impl SensorGroupData {
|
||||
|
||||
/// Actions that can be performed on components (sensors and actuators)
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
#[serde(tag = "type")]
|
||||
pub enum ComponentAction {
|
||||
/// Turn on/activate the component with optional config (e.g., mode)
|
||||
Enable {
|
||||
@@ -128,7 +127,6 @@ pub enum ComponentAction {
|
||||
|
||||
/// Commands to device
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
#[serde(tag = "type")]
|
||||
pub enum Command {
|
||||
// Unified Component Control
|
||||
/// Control any component (sensor or actuator) with standard actions
|
||||
|
||||
@@ -25,11 +25,8 @@
|
||||
//!
|
||||
//! ## Protocol Errors (Log and Continue)
|
||||
//!
|
||||
//! - **`Json`**: Message deserialization failed. Log the error and discard the
|
||||
//! malformed message. The connection remains usable for future messages.
|
||||
//!
|
||||
//! - **`Serialization`**: Failed to serialize outbound message. Log and skip
|
||||
//! this specific message.
|
||||
//! - **`Serialization`**: Message serialization/deserialization failed. Log the error
|
||||
//! and discard the malformed message. The connection remains usable for future messages.
|
||||
//!
|
||||
//! ## Configuration Errors (Fix and Restart)
|
||||
//!
|
||||
@@ -60,9 +57,6 @@ pub enum Error {
|
||||
#[error("I/O error: {0}")]
|
||||
Io(#[from] std::io::Error),
|
||||
|
||||
#[error("JSON error: {0}")]
|
||||
Json(#[from] serde_json::Error),
|
||||
|
||||
#[error("Thread panic")]
|
||||
ThreadPanic,
|
||||
|
||||
|
||||
@@ -9,7 +9,7 @@ mod streaming;
|
||||
use crate::config::Config;
|
||||
use crate::devices::create_device;
|
||||
use crate::error::Result;
|
||||
use crate::streaming::{create_serializer, TcpPublisher, TcpReceiver, WireFormat};
|
||||
use crate::streaming::{create_serializer, TcpPublisher, TcpReceiver};
|
||||
use std::env;
|
||||
use std::net::TcpListener;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
@@ -56,9 +56,7 @@ fn main() -> Result<()> {
|
||||
})
|
||||
.map_err(|e| error::Error::Other(format!("Error setting Ctrl-C handler: {}", e)))?;
|
||||
|
||||
// Get wire format from config
|
||||
let wire_format: WireFormat = config.network.wire_format.into();
|
||||
log::info!("Wire format: {:?}", wire_format);
|
||||
log::info!("Wire format: Protobuf");
|
||||
|
||||
// Start TCP listener
|
||||
let bind_addr = &config.network.bind_address;
|
||||
@@ -82,8 +80,8 @@ fn main() -> Result<()> {
|
||||
let driver_clone = Arc::clone(&driver);
|
||||
|
||||
// Create serializers for each thread
|
||||
let pub_serializer = create_serializer(wire_format);
|
||||
let recv_serializer = create_serializer(wire_format);
|
||||
let pub_serializer = create_serializer();
|
||||
let recv_serializer = create_serializer();
|
||||
|
||||
// Spawn publisher thread
|
||||
let pub_running = Arc::clone(&running);
|
||||
|
||||
@@ -1,40 +0,0 @@
|
||||
//! TCP protocol message types
|
||||
|
||||
use crate::core::types::{Command, SensorGroupData, SensorValue};
|
||||
use serde::{Deserialize, Serialize};
|
||||
use std::collections::HashMap;
|
||||
|
||||
/// Top-level message wrapper
|
||||
#[derive(Serialize, Deserialize, Debug, Clone)]
|
||||
pub struct Message {
|
||||
pub topic: String,
|
||||
pub payload: Payload,
|
||||
}
|
||||
|
||||
/// Message payload types
|
||||
#[derive(Serialize, Deserialize, Debug, Clone)]
|
||||
#[serde(tag = "type")]
|
||||
pub enum Payload {
|
||||
/// Sensor group data (outbound)
|
||||
SensorGroup {
|
||||
group_id: String,
|
||||
timestamp_us: u64,
|
||||
values: HashMap<String, SensorValue>,
|
||||
},
|
||||
/// Command from client (inbound)
|
||||
Command { command: Command },
|
||||
}
|
||||
|
||||
impl Message {
|
||||
/// Create a sensor group message
|
||||
pub fn sensor_group(data: &SensorGroupData) -> Self {
|
||||
Self {
|
||||
topic: format!("sensors/{}", data.group_id),
|
||||
payload: Payload::SensorGroup {
|
||||
group_id: data.group_id.clone(),
|
||||
timestamp_us: data.timestamp_us,
|
||||
values: data.values.clone(),
|
||||
},
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,10 +1,9 @@
|
||||
//! TCP streaming module for SangamIO
|
||||
|
||||
pub mod messages;
|
||||
pub mod tcp_publisher;
|
||||
pub mod tcp_receiver;
|
||||
pub mod wire;
|
||||
|
||||
pub use tcp_publisher::TcpPublisher;
|
||||
pub use tcp_receiver::TcpReceiver;
|
||||
pub use wire::{create_serializer, WireFormat};
|
||||
pub use wire::create_serializer;
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
|
||||
use crate::core::types::SensorGroupData;
|
||||
use crate::error::Result;
|
||||
use crate::streaming::messages::Message;
|
||||
use crate::streaming::wire::Serializer;
|
||||
use std::collections::HashMap;
|
||||
use std::io::Write;
|
||||
@@ -66,9 +65,8 @@ impl TcpPublisher {
|
||||
|
||||
let last = last_sent.get(group_id).copied().unwrap_or(0);
|
||||
if data.timestamp_us > last {
|
||||
// Create and send message
|
||||
let msg = Message::sensor_group(&data);
|
||||
if let Err(e) = self.send_message(&mut stream, &msg) {
|
||||
// Serialize and send message
|
||||
if let Err(e) = self.send_sensor_group(&mut stream, &data) {
|
||||
log::error!("Failed to send sensor data: {}", e);
|
||||
return Err(e);
|
||||
}
|
||||
@@ -96,9 +94,9 @@ impl TcpPublisher {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Send a message to the client
|
||||
fn send_message(&self, stream: &mut TcpStream, msg: &Message) -> Result<()> {
|
||||
let bytes = self.serializer.serialize(msg)?;
|
||||
/// Send a sensor group to the client
|
||||
fn send_sensor_group(&self, stream: &mut TcpStream, data: &SensorGroupData) -> Result<()> {
|
||||
let bytes = self.serializer.serialize_sensor_group(data)?;
|
||||
let len = (bytes.len() as u32).to_be_bytes();
|
||||
|
||||
stream.write_all(&len)?;
|
||||
|
||||
@@ -3,7 +3,6 @@
|
||||
use crate::core::driver::DeviceDriver;
|
||||
use crate::core::types::Command;
|
||||
use crate::error::{Error, Result};
|
||||
use crate::streaming::messages::{Message, Payload};
|
||||
use crate::streaming::wire::Serializer;
|
||||
use std::io::Read;
|
||||
use std::net::TcpStream;
|
||||
@@ -48,15 +47,15 @@ impl TcpReceiver {
|
||||
break;
|
||||
}
|
||||
|
||||
match self.read_message(&mut stream) {
|
||||
Ok(Some(msg)) => {
|
||||
log::info!("Received message: {:?}", msg.topic);
|
||||
if let Err(e) = self.handle_message(msg) {
|
||||
log::error!("Failed to handle message: {}", e);
|
||||
match self.read_command(&mut stream) {
|
||||
Ok(Some(cmd)) => {
|
||||
log::info!("Received command: {:?}", cmd);
|
||||
if let Err(e) = self.handle_command(cmd) {
|
||||
log::error!("Failed to handle command: {}", e);
|
||||
}
|
||||
}
|
||||
Ok(None) => {
|
||||
// Timeout, continue loop
|
||||
// Timeout or non-command message, continue loop
|
||||
}
|
||||
Err(e) => {
|
||||
// Check if it's a connection closed error
|
||||
@@ -78,8 +77,8 @@ impl TcpReceiver {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Read a message from the client
|
||||
fn read_message(&self, stream: &mut TcpStream) -> Result<Option<Message>> {
|
||||
/// Read a command from the client
|
||||
fn read_command(&self, stream: &mut TcpStream) -> Result<Option<Command>> {
|
||||
// Read length prefix
|
||||
let mut len_buf = [0u8; 4];
|
||||
match stream.read_exact(&mut len_buf) {
|
||||
@@ -109,34 +108,20 @@ impl TcpReceiver {
|
||||
let mut buf = vec![0u8; len];
|
||||
stream.read_exact(&mut buf)?;
|
||||
|
||||
// Deserialize
|
||||
let msg: Message = self.serializer.deserialize(&buf)?;
|
||||
Ok(Some(msg))
|
||||
}
|
||||
|
||||
/// Handle a received message
|
||||
fn handle_message(&self, msg: Message) -> Result<()> {
|
||||
match msg.payload {
|
||||
Payload::Command { command } => {
|
||||
log::debug!("Received command: {:?}", command);
|
||||
let result = self.handle_command(command);
|
||||
if result.is_ok() {
|
||||
log::trace!("Command executed successfully");
|
||||
} else {
|
||||
log::error!("Command execution failed: {:?}", result);
|
||||
}
|
||||
result
|
||||
}
|
||||
Payload::SensorGroup { .. } => {
|
||||
log::warn!("Received unexpected SensorGroup message from client");
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
// Deserialize command
|
||||
self.serializer.deserialize_command(&buf)
|
||||
}
|
||||
|
||||
/// Handle a command
|
||||
fn handle_command(&self, cmd: Command) -> Result<()> {
|
||||
log::debug!("Executing command: {:?}", cmd);
|
||||
let mut driver = self.driver.lock().map_err(|_| Error::ThreadPanic)?;
|
||||
driver.send_command(cmd)
|
||||
let result = driver.send_command(cmd);
|
||||
if result.is_ok() {
|
||||
log::trace!("Command executed successfully");
|
||||
} else {
|
||||
log::error!("Command execution failed: {:?}", result);
|
||||
}
|
||||
result
|
||||
}
|
||||
}
|
||||
|
||||
+187
-79
@@ -1,4 +1,4 @@
|
||||
//! Wire format serialization abstraction
|
||||
//! Wire format serialization using Protobuf
|
||||
//!
|
||||
//! # TCP Protocol Specification
|
||||
//!
|
||||
@@ -7,111 +7,219 @@
|
||||
//! ```text
|
||||
//! ┌──────────────────┬──────────────────────────┐
|
||||
//! │ Length (4 bytes) │ Payload (variable) │
|
||||
//! │ Big-endian u32 │ JSON or Postcard binary │
|
||||
//! │ Big-endian u32 │ Protobuf binary │
|
||||
//! └──────────────────┴──────────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! ## Framing
|
||||
//!
|
||||
//! - **Length field**: 4-byte big-endian unsigned integer
|
||||
//! - **Payload**: Serialized message in configured wire format
|
||||
//! - **Payload**: Protobuf-encoded message
|
||||
//! - **Maximum message size**: 1MB (1,048,576 bytes)
|
||||
//! - **Byte order**: Network byte order (big-endian) for length prefix
|
||||
//!
|
||||
//! ## Wire Formats
|
||||
//!
|
||||
//! Two wire formats are supported:
|
||||
//!
|
||||
//! ### JSON (Default)
|
||||
//! - **Pros**: Human-readable, easy to debug, widely supported
|
||||
//! - **Cons**: Larger message size, slower serialization
|
||||
//! - **Use case**: Development, debugging, cross-language clients
|
||||
//!
|
||||
//! ### Postcard (Binary)
|
||||
//! - **Pros**: Compact, fast serialization, type-safe
|
||||
//! - **Cons**: Binary format, requires schema knowledge
|
||||
//! - **Use case**: Production, high-frequency sensor streaming
|
||||
//!
|
||||
//! ## Message Flow
|
||||
//!
|
||||
//! **Sensor streaming (daemon → client):**
|
||||
//! 1. Sensor data updates in driver threads
|
||||
//! 2. Publisher checks if data changed (timestamp comparison)
|
||||
//! 3. Serialize to wire format
|
||||
//! 4. Send length prefix + payload
|
||||
//! 5. Client receives and deserializes
|
||||
//!
|
||||
//! **Command handling (client → daemon):**
|
||||
//! 1. Client serializes command
|
||||
//! 2. Send length prefix + payload
|
||||
//! 3. Daemon receives and deserializes
|
||||
//! 4. Dispatch to device driver
|
||||
//! 5. No response (fire-and-forget for low latency)
|
||||
//!
|
||||
//! ## Error Handling
|
||||
//!
|
||||
//! - **Malformed length**: Connection closed
|
||||
//! - **Oversized message**: Connection closed (security)
|
||||
//! - **Deserialization failure**: Message logged and discarded, connection remains open
|
||||
//! - **Serialization failure**: Message skipped, error logged
|
||||
//!
|
||||
//! ## Performance Characteristics
|
||||
//!
|
||||
//! - **Latency**: <1ms serialization time (typical)
|
||||
//! - **Throughput**: ~10,000 messages/sec (JSON), ~50,000 messages/sec (Postcard)
|
||||
//! - **Bandwidth**: Sensor stream @ 500Hz ≈ 200KB/s (JSON), ≈ 80KB/s (Postcard)
|
||||
//! - **Throughput**: ~50,000 messages/sec
|
||||
//! - **Bandwidth**: Sensor stream @ 500Hz ≈ 60KB/s
|
||||
|
||||
use crate::core::types::{Command, SensorGroupData};
|
||||
use crate::error::{Error, Result};
|
||||
use crate::streaming::messages::Message;
|
||||
use prost::Message;
|
||||
|
||||
/// Supported wire formats
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub enum WireFormat {
|
||||
/// Binary format using postcard - fast and compact
|
||||
Postcard,
|
||||
/// JSON format - human-readable for debugging
|
||||
#[default]
|
||||
Json,
|
||||
// Include generated protobuf types
|
||||
pub mod proto {
|
||||
include!(concat!(env!("OUT_DIR"), "/sangamio.rs"));
|
||||
}
|
||||
|
||||
/// Serializer that can handle both formats
|
||||
#[derive(Clone)]
|
||||
pub struct Serializer {
|
||||
format: WireFormat,
|
||||
}
|
||||
/// Serializer for protobuf wire format
|
||||
#[derive(Clone, Default)]
|
||||
pub struct Serializer;
|
||||
|
||||
impl Serializer {
|
||||
/// Create a new serializer for the given format
|
||||
pub fn new(format: WireFormat) -> Self {
|
||||
Self { format }
|
||||
/// Create a new serializer
|
||||
pub fn new() -> Self {
|
||||
Self
|
||||
}
|
||||
|
||||
/// Serialize a message to bytes
|
||||
pub fn serialize(&self, msg: &Message) -> Result<Vec<u8>> {
|
||||
match self.format {
|
||||
WireFormat::Postcard => {
|
||||
postcard::to_allocvec(msg).map_err(|e| Error::Serialization(e.to_string()))
|
||||
}
|
||||
WireFormat::Json => {
|
||||
serde_json::to_vec(msg).map_err(|e| Error::Serialization(e.to_string()))
|
||||
}
|
||||
}
|
||||
/// Serialize a sensor group message to bytes
|
||||
pub fn serialize_sensor_group(&self, data: &SensorGroupData) -> Result<Vec<u8>> {
|
||||
let msg = proto::Message::from_sensor_group(data);
|
||||
let mut buf = Vec::with_capacity(msg.encoded_len());
|
||||
msg.encode(&mut buf)
|
||||
.map_err(|e| Error::Serialization(e.to_string()))?;
|
||||
Ok(buf)
|
||||
}
|
||||
|
||||
/// Deserialize bytes to a message
|
||||
pub fn deserialize(&self, bytes: &[u8]) -> Result<Message> {
|
||||
match self.format {
|
||||
WireFormat::Postcard => {
|
||||
postcard::from_bytes(bytes).map_err(|e| Error::Serialization(e.to_string()))
|
||||
/// Deserialize bytes to a command
|
||||
pub fn deserialize_command(&self, bytes: &[u8]) -> Result<Option<Command>> {
|
||||
let msg = proto::Message::decode(bytes).map_err(|e| Error::Serialization(e.to_string()))?;
|
||||
|
||||
match msg.payload {
|
||||
Some(proto::message::Payload::Command(cmd)) => Ok(Some(Command::from_proto(cmd)?)),
|
||||
Some(proto::message::Payload::SensorGroup(_)) => {
|
||||
log::warn!("Received unexpected SensorGroup message from client");
|
||||
Ok(None)
|
||||
}
|
||||
WireFormat::Json => {
|
||||
serde_json::from_slice(bytes).map_err(|e| Error::Serialization(e.to_string()))
|
||||
None => {
|
||||
log::warn!("Received message with no payload");
|
||||
Ok(None)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a serializer for the given wire format
|
||||
pub fn create_serializer(format: WireFormat) -> Serializer {
|
||||
Serializer::new(format)
|
||||
/// Create a serializer
|
||||
pub fn create_serializer() -> Serializer {
|
||||
Serializer::new()
|
||||
}
|
||||
|
||||
// Conversion implementations for proto types
|
||||
impl proto::Message {
|
||||
/// Create a sensor group message from internal types
|
||||
pub fn from_sensor_group(data: &SensorGroupData) -> Self {
|
||||
use std::collections::HashMap;
|
||||
|
||||
let values: HashMap<String, proto::SensorValue> = data
|
||||
.values
|
||||
.iter()
|
||||
.map(|(k, v)| (k.clone(), proto::SensorValue::from(v.clone())))
|
||||
.collect();
|
||||
|
||||
Self {
|
||||
topic: format!("sensors/{}", data.group_id),
|
||||
payload: Some(proto::message::Payload::SensorGroup(proto::SensorGroup {
|
||||
group_id: data.group_id.clone(),
|
||||
timestamp_us: data.timestamp_us,
|
||||
values,
|
||||
})),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<crate::core::types::SensorValue> for proto::SensorValue {
|
||||
fn from(value: crate::core::types::SensorValue) -> Self {
|
||||
use crate::core::types::SensorValue as SV;
|
||||
|
||||
let value = match value {
|
||||
SV::Bool(v) => proto::sensor_value::Value::BoolVal(v),
|
||||
SV::U8(v) => proto::sensor_value::Value::U32Val(v as u32),
|
||||
SV::U16(v) => proto::sensor_value::Value::U32Val(v as u32),
|
||||
SV::U32(v) => proto::sensor_value::Value::U32Val(v),
|
||||
SV::U64(v) => proto::sensor_value::Value::U64Val(v),
|
||||
SV::I8(v) => proto::sensor_value::Value::I32Val(v as i32),
|
||||
SV::I16(v) => proto::sensor_value::Value::I32Val(v as i32),
|
||||
SV::I32(v) => proto::sensor_value::Value::I32Val(v),
|
||||
SV::I64(v) => proto::sensor_value::Value::I64Val(v),
|
||||
SV::F32(v) => proto::sensor_value::Value::F32Val(v),
|
||||
SV::F64(v) => proto::sensor_value::Value::F64Val(v),
|
||||
SV::String(v) => proto::sensor_value::Value::StringVal(v),
|
||||
SV::Bytes(v) => proto::sensor_value::Value::BytesVal(v),
|
||||
SV::Vector3(arr) => proto::sensor_value::Value::Vector3Val(proto::Vector3 {
|
||||
x: arr[0],
|
||||
y: arr[1],
|
||||
z: arr[2],
|
||||
}),
|
||||
SV::PointCloud2D(points) => {
|
||||
proto::sensor_value::Value::PointcloudVal(proto::PointCloud2D {
|
||||
points: points
|
||||
.into_iter()
|
||||
.map(|(angle, dist, quality)| proto::LidarPoint {
|
||||
angle_rad: angle,
|
||||
distance_m: dist,
|
||||
quality: quality as u32,
|
||||
})
|
||||
.collect(),
|
||||
})
|
||||
}
|
||||
};
|
||||
|
||||
proto::SensorValue { value: Some(value) }
|
||||
}
|
||||
}
|
||||
|
||||
impl Command {
|
||||
/// Convert from proto command
|
||||
pub fn from_proto(cmd: proto::Command) -> Result<Self> {
|
||||
use crate::core::types::ComponentAction;
|
||||
|
||||
match cmd.command {
|
||||
Some(proto::command::Command::ComponentControl(ctrl)) => {
|
||||
let action = match ctrl.action {
|
||||
Some(action) => {
|
||||
let config = if action.config.is_empty() {
|
||||
None
|
||||
} else {
|
||||
Some(
|
||||
action
|
||||
.config
|
||||
.into_iter()
|
||||
.map(|(k, v)| {
|
||||
(k, crate::core::types::SensorValue::from_proto(v))
|
||||
})
|
||||
.collect(),
|
||||
)
|
||||
};
|
||||
|
||||
match proto::component_action::ActionType::try_from(action.r#type) {
|
||||
Ok(proto::component_action::ActionType::Enable) => {
|
||||
ComponentAction::Enable { config }
|
||||
}
|
||||
Ok(proto::component_action::ActionType::Disable) => {
|
||||
ComponentAction::Disable { config }
|
||||
}
|
||||
Ok(proto::component_action::ActionType::Reset) => {
|
||||
ComponentAction::Reset { config }
|
||||
}
|
||||
Ok(proto::component_action::ActionType::Configure) => {
|
||||
ComponentAction::Configure {
|
||||
config: config.unwrap_or_default(),
|
||||
}
|
||||
}
|
||||
Err(_) => {
|
||||
return Err(Error::Serialization("Unknown action type".to_string()))
|
||||
}
|
||||
}
|
||||
}
|
||||
None => return Err(Error::Serialization("Missing action".to_string())),
|
||||
};
|
||||
|
||||
Ok(Command::ComponentControl {
|
||||
id: ctrl.id,
|
||||
action,
|
||||
})
|
||||
}
|
||||
Some(proto::command::Command::ProtocolSync(_)) => Ok(Command::ProtocolSync),
|
||||
Some(proto::command::Command::Shutdown(_)) => Ok(Command::Shutdown),
|
||||
None => Err(Error::Serialization("Missing command".to_string())),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl crate::core::types::SensorValue {
|
||||
/// Convert from proto sensor value
|
||||
pub fn from_proto(value: proto::SensorValue) -> Self {
|
||||
use crate::core::types::SensorValue as SV;
|
||||
|
||||
match value.value {
|
||||
Some(proto::sensor_value::Value::BoolVal(v)) => SV::Bool(v),
|
||||
Some(proto::sensor_value::Value::U32Val(v)) => SV::U32(v),
|
||||
Some(proto::sensor_value::Value::U64Val(v)) => SV::U64(v),
|
||||
Some(proto::sensor_value::Value::I32Val(v)) => SV::I32(v),
|
||||
Some(proto::sensor_value::Value::I64Val(v)) => SV::I64(v),
|
||||
Some(proto::sensor_value::Value::F32Val(v)) => SV::F32(v),
|
||||
Some(proto::sensor_value::Value::F64Val(v)) => SV::F64(v),
|
||||
Some(proto::sensor_value::Value::StringVal(v)) => SV::String(v),
|
||||
Some(proto::sensor_value::Value::BytesVal(v)) => SV::Bytes(v),
|
||||
Some(proto::sensor_value::Value::Vector3Val(v)) => SV::Vector3([v.x, v.y, v.z]),
|
||||
Some(proto::sensor_value::Value::PointcloudVal(v)) => SV::PointCloud2D(
|
||||
v.points
|
||||
.into_iter()
|
||||
.map(|p| (p.angle_rad, p.distance_m, p.quality as u8))
|
||||
.collect(),
|
||||
),
|
||||
None => SV::Bool(false), // Default fallback
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user