Reorganize docs and add mock device guide in sangam-io
Move hardware-specific docs (COMMANDS.md, SENSORSTATUS.md) into src/devices/crl200s/ for better organization. Add comprehensive mock device user guide and update README with UDP streaming and simulation documentation.
This commit is contained in:
codetiger
+238
-64
@@ -1,21 +1,22 @@
|
||||
# SangamIO
|
||||
|
||||
Hardware abstraction daemon for the CRL-200S robotic vacuum platform, providing real-time sensor streaming and command control via TCP.
|
||||
Hardware abstraction daemon for the CRL-200S robotic vacuum platform, providing real-time sensor streaming via UDP and command control via TCP.
|
||||
|
||||
## Overview
|
||||
|
||||
SangamIO acts as a bridge between high-level clients (SLAM, visualization) and low-level hardware (GD32 motor controller, Delta-2D lidar). It runs as a standalone daemon on the robot, streaming sensor data at 110Hz (limited by 115200 baud serial) and accepting commands over TCP.
|
||||
SangamIO acts as a bridge between high-level clients (SLAM, visualization) and low-level hardware (GD32 motor controller, Delta-2D lidar). It runs as a standalone daemon on the robot, streaming sensor data at 110Hz via UDP and accepting commands over TCP.
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────┐
|
||||
│ Client Applications (SLAM/Drishti) │
|
||||
└─────────────┬───────────────┬───────────────────┘
|
||||
│ TCP 5555 │
|
||||
│ UDP 5555 │ TCP 5555
|
||||
│ (sensors) │ (commands)
|
||||
┌─────────────▼───────────────▼───────────────────┐
|
||||
│ SangamIO Daemon (Robot) │
|
||||
│ • Telemetry streaming @ 110Hz (sensors) │
|
||||
│ • Telemetry streaming @ 110Hz (UDP unicast) │
|
||||
│ • Lidar streaming @ 5Hz (360° scans) │
|
||||
│ • Command processing (motion/actuators) │
|
||||
│ • Command processing (TCP, reliable) │
|
||||
│ • Real-time control loop (20ms heartbeat) │
|
||||
└─────────────┬───────────────┬───────────────────┘
|
||||
│ /dev/ttyS3 │ /dev/ttyS1
|
||||
@@ -29,9 +30,9 @@ SangamIO acts as a bridge between high-level clients (SLAM, visualization) and l
|
||||
|
||||
| Property | Value |
|
||||
|----------|-------|
|
||||
| Language | Rust 2021 edition |
|
||||
| Language | Rust 2024 edition |
|
||||
| Version | 0.3.0 |
|
||||
| Target | ARM (armv7-unknown-linux-musleabihf) |
|
||||
| Target | ARM (armv7-unknown-linux-musleabihf) or host (simulation) |
|
||||
| Binary Size | ~350KB (statically linked) |
|
||||
| Memory Usage | <10MB RSS |
|
||||
| CPU Usage | <1% on Allwinner A33 |
|
||||
@@ -41,20 +42,21 @@ SangamIO acts as a bridge between high-level clients (SLAM, visualization) and l
|
||||
### Development (Host Machine)
|
||||
|
||||
```bash
|
||||
# Build
|
||||
cargo build --release
|
||||
# Build for real hardware
|
||||
cargo build --release --target armv7-unknown-linux-musleabihf
|
||||
|
||||
# Run with default config
|
||||
cargo run --release
|
||||
# Build for simulation (mock device)
|
||||
cargo build --release --features mock
|
||||
|
||||
# Run with custom config
|
||||
cargo run --release -- sangamio.toml
|
||||
# Run with configuration file
|
||||
cargo run --release --features mock -- mock.toml
|
||||
cargo run --release --features mock -- --config mock.toml
|
||||
|
||||
# Enable verbose logging
|
||||
RUST_LOG=debug cargo run --release
|
||||
RUST_LOG=debug cargo run --release --features mock
|
||||
|
||||
# Run tests
|
||||
cargo test
|
||||
cargo test --features mock
|
||||
```
|
||||
|
||||
### Production (ARM Robot)
|
||||
@@ -63,7 +65,7 @@ cargo test
|
||||
# Add ARM target (one-time)
|
||||
rustup target add armv7-unknown-linux-musleabihf
|
||||
|
||||
# Build for ARM
|
||||
# Build for ARM (real hardware)
|
||||
cargo build --release --target armv7-unknown-linux-musleabihf
|
||||
|
||||
# Strip debug symbols (reduces size ~40%)
|
||||
@@ -71,6 +73,13 @@ arm-linux-gnueabihf-strip \
|
||||
target/armv7-unknown-linux-musleabihf/release/sangam-io
|
||||
```
|
||||
|
||||
### Feature Flags
|
||||
|
||||
| Feature | Description |
|
||||
|---------|-------------|
|
||||
| (default) | Real hardware support only |
|
||||
| `mock` | Enable mock device simulation |
|
||||
|
||||
## Deployment
|
||||
|
||||
**SSH**: `root@vacuum` (see project docs for credentials)
|
||||
@@ -148,11 +157,18 @@ All transforms default to identity (no change) if not specified. The CRL-200S re
|
||||
| `imu_gyro` | AxisTransform3D | `[source_axis, sign]` | Remap + flip yaw |
|
||||
| `imu_accel` | AxisTransform3D | `[source_axis, sign]` | Identity (default)
|
||||
|
||||
## TCP Protocol
|
||||
## Network Protocol
|
||||
|
||||
SangamIO uses a hybrid UDP/TCP architecture for optimal performance:
|
||||
|
||||
| Channel | Protocol | Purpose | Why |
|
||||
|---------|----------|---------|-----|
|
||||
| Sensor data | UDP unicast | Low-latency streaming | No head-of-line blocking |
|
||||
| Commands | TCP | Reliable control | Guaranteed delivery |
|
||||
|
||||
### Message Format
|
||||
|
||||
All messages use length-prefixed framing with Protobuf payloads:
|
||||
Both UDP and TCP use length-prefixed Protobuf framing:
|
||||
|
||||
```
|
||||
┌──────────────────┬─────────────────────┐
|
||||
@@ -163,38 +179,70 @@ All messages use length-prefixed framing with Protobuf payloads:
|
||||
|
||||
See `proto/sangamio.proto` for the complete schema.
|
||||
|
||||
### Topics
|
||||
### Topics (UDP Streaming)
|
||||
|
||||
| Direction | Topic | Rate | Description |
|
||||
|-----------|-------|------|-------------|
|
||||
| Out | `sensors/sensor_status` | 110Hz | All sensor data |
|
||||
| Out | `sensors/device_version` | Once | Version info |
|
||||
| Out | `sensors/lidar` | 5Hz | 360° point cloud |
|
||||
| In | `command` | On-demand | Robot commands |
|
||||
| Topic | Rate | Size | Description |
|
||||
|-------|------|------|-------------|
|
||||
| `sensors/sensor_status` | 110Hz | ~150B | Encoders, IMU, bumpers, cliffs, battery |
|
||||
| `sensors/lidar` | 5Hz | ~2KB | 360° point cloud (angle, distance, quality) |
|
||||
| `sensors/device_version` | Once | ~50B | Firmware version info |
|
||||
|
||||
### Commands (TCP)
|
||||
|
||||
| Command | Description |
|
||||
|---------|-------------|
|
||||
| `ComponentControl::drive` | Set linear/angular velocity |
|
||||
| `ComponentControl::lidar` | Enable/disable lidar motor |
|
||||
| `ComponentControl::vacuum` | Control vacuum motor (0-100%) |
|
||||
| `ComponentControl::*_brush` | Control brushes (0-100%) |
|
||||
| `Shutdown` | Graceful daemon shutdown |
|
||||
|
||||
### Client Registration
|
||||
|
||||
UDP uses unicast (not broadcast). Clients are registered automatically:
|
||||
|
||||
1. Client connects TCP to port 5555
|
||||
2. Server records client IP address
|
||||
3. UDP packets are sent to client on same port
|
||||
4. When TCP disconnects, UDP streaming stops
|
||||
|
||||
### Python Client Example
|
||||
|
||||
```python
|
||||
import socket
|
||||
import struct
|
||||
import threading
|
||||
from proto import sangamio_pb2
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
sock.connect(('192.168.68.101', 5555))
|
||||
ROBOT_IP = '192.168.68.101'
|
||||
PORT = 5555
|
||||
|
||||
while True:
|
||||
# Read length prefix
|
||||
length = struct.unpack('>I', sock.recv(4))[0]
|
||||
# TCP for commands
|
||||
tcp_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
tcp_sock.connect((ROBOT_IP, PORT))
|
||||
|
||||
# Read Protobuf message
|
||||
data = sock.recv(length)
|
||||
msg = sangamio_pb2.Message()
|
||||
msg.ParseFromString(data)
|
||||
# UDP for sensor data (same port)
|
||||
udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
udp_sock.bind(('0.0.0.0', PORT))
|
||||
|
||||
if msg.topic == 'sensors/sensor_status':
|
||||
sg = msg.sensor_group
|
||||
charging = sg.values['is_charging'].bool_val
|
||||
print(f"Charging: {charging}")
|
||||
def receive_sensors():
|
||||
while True:
|
||||
data, addr = udp_sock.recvfrom(4096)
|
||||
length = struct.unpack('>I', data[:4])[0]
|
||||
msg = sangamio_pb2.Message()
|
||||
msg.ParseFromString(data[4:4+length])
|
||||
|
||||
if msg.topic == 'sensors/sensor_status':
|
||||
sg = msg.sensor_group
|
||||
print(f"Battery: {sg.values['battery_level'].u8_val}%")
|
||||
|
||||
# Start receiving in background
|
||||
threading.Thread(target=receive_sensors, daemon=True).start()
|
||||
|
||||
# Send commands via TCP
|
||||
def send_command(cmd):
|
||||
payload = cmd.SerializeToString()
|
||||
tcp_sock.send(struct.pack('>I', len(payload)) + payload)
|
||||
```
|
||||
|
||||
## Module Structure
|
||||
@@ -202,7 +250,7 @@ while True:
|
||||
```
|
||||
sangam-io/
|
||||
├── src/
|
||||
│ ├── main.rs # Entry point, TCP listener
|
||||
│ ├── main.rs # Entry point, TCP/UDP listener
|
||||
│ ├── config.rs # TOML configuration loading
|
||||
│ ├── error.rs # Error types
|
||||
│ │
|
||||
@@ -211,32 +259,48 @@ sangam-io/
|
||||
│ │ └── types.rs # SensorValue, Command types
|
||||
│ │
|
||||
│ ├── devices/
|
||||
│ │ ├── mod.rs # Device factory
|
||||
│ │ └── crl200s/
|
||||
│ │ ├── mod.rs # CRL200S orchestrator
|
||||
│ │ ├── constants.rs # Hardware constants
|
||||
│ │ ├── gd32/ # Motor controller driver
|
||||
│ │ │ ├── mod.rs # Driver core
|
||||
│ │ │ ├── commands.rs # Command handlers
|
||||
│ │ │ ├── heartbeat.rs # 20ms watchdog
|
||||
│ │ │ ├── reader.rs # Status parsing
|
||||
│ │ │ ├── packet.rs # Packet encoding
|
||||
│ │ │ ├── protocol.rs # Packet framing
|
||||
│ │ │ └── state.rs # Atomic state
|
||||
│ │ └── delta2d/ # Lidar driver
|
||||
│ │ ├── mod.rs # Driver core
|
||||
│ │ └── protocol.rs # Packet parsing
|
||||
│ │ ├── mod.rs # Device factory (crl200s, mock)
|
||||
│ │ ├── crl200s/ # Real hardware driver
|
||||
│ │ │ ├── mod.rs # CRL200S orchestrator
|
||||
│ │ │ ├── constants.rs # Hardware constants
|
||||
│ │ │ ├── COMMANDS.md # GD32 command reference
|
||||
│ │ │ ├── SENSORSTATUS.md # Sensor packet docs
|
||||
│ │ │ ├── gd32/ # Motor controller driver
|
||||
│ │ │ │ ├── mod.rs # Driver core
|
||||
│ │ │ │ ├── commands.rs # Command handlers
|
||||
│ │ │ │ ├── heartbeat.rs # 20ms watchdog
|
||||
│ │ │ │ ├── reader.rs # Status parsing
|
||||
│ │ │ │ ├── packet.rs # Packet encoding
|
||||
│ │ │ │ ├── protocol.rs # Packet framing
|
||||
│ │ │ │ └── state.rs # Atomic state
|
||||
│ │ │ └── delta2d/ # Lidar driver
|
||||
│ │ │ ├── mod.rs # Driver core
|
||||
│ │ │ └── protocol.rs # Packet parsing
|
||||
│ │ │
|
||||
│ │ └── mock/ # Simulation driver (--features mock)
|
||||
│ │ ├── mod.rs # MockDriver, simulation loop
|
||||
│ │ ├── config.rs # SimulationConfig structs
|
||||
│ │ ├── physics.rs # Differential drive kinematics
|
||||
│ │ ├── lidar_sim.rs # Ray-casting lidar
|
||||
│ │ ├── imu_sim.rs # IMU with noise
|
||||
│ │ ├── encoder_sim.rs # Encoder with slip
|
||||
│ │ ├── sensor_sim.rs # Bumpers, cliffs
|
||||
│ │ ├── map_loader.rs # PGM+YAML loader
|
||||
│ │ └── noise.rs # Noise generator
|
||||
│ │
|
||||
│ └── streaming/
|
||||
│ ├── mod.rs # Streaming module docs
|
||||
│ ├── wire.rs # Protobuf serialization
|
||||
│ ├── tcp_publisher.rs # Outbound streaming
|
||||
│ └── tcp_receiver.rs # Command handling
|
||||
│ ├── udp_publisher.rs # UDP sensor streaming
|
||||
│ └── tcp_receiver.rs # TCP command handling
|
||||
│
|
||||
├── proto/
|
||||
│ └── sangamio.proto # Protobuf schema
|
||||
├── sangamio.toml # Default configuration
|
||||
├── COMMANDS.md # GD32 command reference
|
||||
└── SENSORSTATUS.md # Sensor packet documentation
|
||||
├── maps/ # Example simulation maps
|
||||
│ ├── example.yaml # Map metadata
|
||||
│ └── example.pgm # Occupancy grid
|
||||
├── sangamio.toml # Hardware configuration
|
||||
└── mock.toml # Simulation configuration
|
||||
```
|
||||
|
||||
## Supported Components
|
||||
@@ -277,13 +341,24 @@ The `sensor_status` group includes:
|
||||
|
||||
## Thread Model
|
||||
|
||||
### Real Hardware (crl200s)
|
||||
|
||||
| Thread | Purpose | Timing |
|
||||
|--------|---------|--------|
|
||||
| Main | TCP listener | - |
|
||||
| Main | TCP listener, client accept | - |
|
||||
| GD32 Heartbeat | Safety watchdog | 20ms ±2ms |
|
||||
| GD32 Reader | Status parsing | Continuous |
|
||||
| GD32 Reader | Status parsing → channel | Continuous |
|
||||
| Lidar Reader | Scan accumulation | Continuous |
|
||||
| TCP Publisher | Per-client streaming | 1ms loop |
|
||||
| UDP Publisher | Sensor streaming | <1ms loop |
|
||||
| TCP Receiver | Per-client commands | On-demand |
|
||||
|
||||
### Simulation (mock)
|
||||
|
||||
| Thread | Purpose | Timing |
|
||||
|--------|---------|--------|
|
||||
| Main | TCP listener, client accept | - |
|
||||
| Simulation Loop | Physics + sensors | 9ms (110Hz) |
|
||||
| UDP Publisher | Sensor streaming | <1ms loop |
|
||||
| TCP Receiver | Per-client commands | On-demand |
|
||||
|
||||
## Hardware Constraints
|
||||
@@ -301,10 +376,109 @@ The `sensor_status` group includes:
|
||||
| Command latency | ~30ms |
|
||||
| Protobuf bandwidth | ~95KB/s |
|
||||
|
||||
## Mock Device Simulation
|
||||
|
||||
The mock device driver enables algorithm development and testing without physical hardware.
|
||||
|
||||
### Quick Start
|
||||
|
||||
```bash
|
||||
# Build with mock feature
|
||||
cargo build --release --features mock
|
||||
|
||||
# Run simulation
|
||||
cargo run --release --features mock -- mock.toml
|
||||
```
|
||||
|
||||
### Map Format (ROS Standard)
|
||||
|
||||
Maps use PGM + YAML format compatible with ROS Navigation:
|
||||
|
||||
**maps/example.yaml:**
|
||||
```yaml
|
||||
image: example.pgm # Grayscale occupancy grid
|
||||
resolution: 0.02 # meters per pixel
|
||||
origin: [-5.0, -5.0, 0.0] # [x, y, yaw] of bottom-left
|
||||
occupied_thresh: 0.65 # Darker = occupied
|
||||
cliff_mask: cliffs.pgm # Optional cliff layer
|
||||
```
|
||||
|
||||
**PGM pixel values:**
|
||||
- White (255) = Free space
|
||||
- Black (0) = Wall/obstacle
|
||||
- Gray (205) = Unknown
|
||||
|
||||
### Configuration
|
||||
|
||||
**mock.toml:**
|
||||
```toml
|
||||
[device]
|
||||
type = "mock"
|
||||
name = "Mock CRL-200S"
|
||||
|
||||
[device.simulation]
|
||||
map_file = "maps/example.yaml"
|
||||
start_x = 1.5 # meters
|
||||
start_y = 3.5 # meters
|
||||
start_theta = 0.0 # radians
|
||||
speed_factor = 1.0 # 2.0 = 2x speed
|
||||
random_seed = 42 # 0 = random each run
|
||||
|
||||
[device.simulation.robot]
|
||||
wheel_base = 0.233 # meters
|
||||
ticks_per_meter = 4464.0
|
||||
collision_mode = "stop" # "stop", "slide", "passthrough"
|
||||
|
||||
[device.simulation.lidar]
|
||||
num_rays = 360
|
||||
scan_rate_hz = 5.0
|
||||
max_range = 8.0
|
||||
|
||||
[device.simulation.lidar.noise]
|
||||
range_stddev = 0.005 # 5mm noise
|
||||
miss_rate = 0.01 # 1% invalid readings
|
||||
|
||||
[network]
|
||||
bind_address = "0.0.0.0:5555"
|
||||
```
|
||||
|
||||
### Simulated Components
|
||||
|
||||
| Component | Method | Configurability |
|
||||
|-----------|--------|-----------------|
|
||||
| Lidar | Ray-casting | Noise, miss rate, quality |
|
||||
| Encoders | Kinematics | Slip, quantization |
|
||||
| IMU | Physics-based | Per-axis noise, drift |
|
||||
| Bumpers | Collision detect | Zone angles, trigger distance |
|
||||
| Cliffs | Mask lookup | Sensor positions |
|
||||
| Battery | Fixed values | Voltage, level, charging |
|
||||
|
||||
### Speed Factor
|
||||
|
||||
Accelerate simulation for faster testing:
|
||||
|
||||
| Factor | sensor_status | lidar | Wall time for 1 min sim |
|
||||
|--------|---------------|-------|-------------------------|
|
||||
| 1.0 | 110 Hz | 5 Hz | 60 sec |
|
||||
| 2.0 | 220 Hz | 10 Hz | 30 sec |
|
||||
| 5.0 | 550 Hz | 25 Hz | 12 sec |
|
||||
|
||||
### Creating Maps
|
||||
|
||||
1. **GIMP/Photoshop**: Draw black (walls) on white (free space)
|
||||
2. **ROS map_server**: Save maps from SLAM runs
|
||||
3. **Cartographer**: Export as PGM
|
||||
4. Save as 8-bit grayscale PGM with YAML metadata
|
||||
|
||||
## Documentation
|
||||
|
||||
- [COMMANDS.md](COMMANDS.md) - GD32 command reference (27 commands)
|
||||
- [SENSORSTATUS.md](SENSORSTATUS.md) - Status packet byte layout (96 bytes)
|
||||
### CRL-200S Hardware
|
||||
- [src/devices/crl200s/COMMANDS.md](src/devices/crl200s/COMMANDS.md) - GD32 command reference (27 commands)
|
||||
- [src/devices/crl200s/SENSORSTATUS.md](src/devices/crl200s/SENSORSTATUS.md) - Status packet byte layout (96 bytes)
|
||||
|
||||
### Mock Device Simulation
|
||||
- [docs/mock-device-guide.md](docs/mock-device-guide.md) - Mock device user guide
|
||||
- [docs/proposal-mock-device.md](docs/proposal-mock-device.md) - Mock device design document
|
||||
|
||||
## Debugging
|
||||
|
||||
|
||||
@@ -0,0 +1,299 @@
|
||||
# Mock Device User Guide
|
||||
|
||||
This guide explains how to use SangamIO's mock device driver for simulation-based development and testing.
|
||||
|
||||
## Overview
|
||||
|
||||
The mock device driver provides a complete simulation of CRL-200S hardware, enabling:
|
||||
|
||||
- **Algorithm development** without physical robots
|
||||
- **Unit testing** with deterministic sensor data
|
||||
- **CI/CD integration** for automated testing
|
||||
- **Accelerated testing** with speed factor
|
||||
|
||||
## Quick Start
|
||||
|
||||
### 1. Build with Mock Feature
|
||||
|
||||
```bash
|
||||
cd sangam-io
|
||||
cargo build --release --features mock
|
||||
```
|
||||
|
||||
### 2. Create or Use an Existing Map
|
||||
|
||||
Use the example map or create your own:
|
||||
|
||||
```bash
|
||||
ls maps/
|
||||
# example.yaml example.pgm
|
||||
```
|
||||
|
||||
### 3. Run Simulation
|
||||
|
||||
```bash
|
||||
# Using default mock.toml
|
||||
cargo run --release --features mock -- mock.toml
|
||||
|
||||
# Or with explicit config flag
|
||||
cargo run --release --features mock -- --config mock.toml
|
||||
|
||||
# With debug logging
|
||||
RUST_LOG=debug cargo run --release --features mock -- mock.toml
|
||||
```
|
||||
|
||||
### 4. Connect Your Client
|
||||
|
||||
Connect to `localhost:5555` using the same protocol as real hardware:
|
||||
- **UDP** for sensor data (automatic when TCP connects)
|
||||
- **TCP** for commands
|
||||
|
||||
## Map Creation
|
||||
|
||||
### PGM + YAML Format
|
||||
|
||||
Maps use the ROS-standard format for maximum compatibility.
|
||||
|
||||
**Step 1: Create PGM Image**
|
||||
|
||||
Use any image editor (GIMP, Photoshop, or programmatically):
|
||||
|
||||
| Color | Value | Meaning |
|
||||
|-------|-------|---------|
|
||||
| White | 255 | Free space |
|
||||
| Black | 0 | Wall/obstacle |
|
||||
| Gray | 205 | Unknown |
|
||||
|
||||
Save as 8-bit grayscale PGM (P5 binary or P2 ASCII).
|
||||
|
||||
**Step 2: Create YAML Metadata**
|
||||
|
||||
```yaml
|
||||
# maps/my_map.yaml
|
||||
image: my_map.pgm # PGM filename (relative to YAML)
|
||||
resolution: 0.02 # meters per pixel
|
||||
origin: [-5.0, -5.0, 0.0] # [x, y, yaw] of bottom-left pixel
|
||||
occupied_thresh: 0.65 # Pixels darker than this are occupied
|
||||
|
||||
# Optional: cliff mask (same dimensions as main map)
|
||||
cliff_mask: my_map_cliffs.pgm
|
||||
```
|
||||
|
||||
**Step 3: Calculate Origin**
|
||||
|
||||
The origin is the world coordinate of the bottom-left pixel:
|
||||
|
||||
```
|
||||
origin_x = -width_pixels * resolution / 2
|
||||
origin_y = -height_pixels * resolution / 2
|
||||
```
|
||||
|
||||
For a 500x500 pixel map at 0.02 m/px:
|
||||
```yaml
|
||||
origin: [-5.0, -5.0, 0.0] # Centers the map
|
||||
```
|
||||
|
||||
### Using ROS Maps
|
||||
|
||||
Maps saved by ROS tools work directly:
|
||||
|
||||
```bash
|
||||
# From ROS map_server
|
||||
ros2 run nav2_map_server map_saver_cli -f my_map
|
||||
|
||||
# Produces my_map.pgm and my_map.yaml
|
||||
# Copy to sangam-io/maps/
|
||||
```
|
||||
|
||||
### Creating Cliff Masks
|
||||
|
||||
Cliff masks define areas where cliff sensors should trigger:
|
||||
|
||||
1. Create a PGM with same dimensions as main map
|
||||
2. Paint black (0) where cliffs exist
|
||||
3. Leave white (255) for safe floor
|
||||
4. Reference in YAML: `cliff_mask: my_map_cliffs.pgm`
|
||||
|
||||
## Configuration Reference
|
||||
|
||||
### Minimal Configuration
|
||||
|
||||
```toml
|
||||
[device]
|
||||
type = "mock"
|
||||
name = "Mock Robot"
|
||||
|
||||
[device.simulation]
|
||||
map_file = "maps/example.yaml"
|
||||
|
||||
[network]
|
||||
bind_address = "0.0.0.0:5555"
|
||||
```
|
||||
|
||||
### Full Configuration
|
||||
|
||||
See `mock.toml` for all available options. Key sections:
|
||||
|
||||
#### Simulation Control
|
||||
|
||||
```toml
|
||||
[device.simulation]
|
||||
map_file = "maps/example.yaml"
|
||||
start_x = 1.5 # Initial X position (meters)
|
||||
start_y = 3.5 # Initial Y position (meters)
|
||||
start_theta = 0.0 # Initial heading (radians, 0 = +X)
|
||||
speed_factor = 1.0 # Time multiplier (2.0 = 2x speed)
|
||||
random_seed = 42 # 0 = random, >0 = deterministic
|
||||
log_level = "standard" # "minimal", "standard", "verbose"
|
||||
```
|
||||
|
||||
#### Robot Parameters
|
||||
|
||||
```toml
|
||||
[device.simulation.robot]
|
||||
wheel_base = 0.233 # Distance between wheels (meters)
|
||||
ticks_per_meter = 4464.0 # Encoder resolution
|
||||
max_linear_speed = 0.3 # Speed limit (m/s)
|
||||
max_angular_speed = 1.0 # Turn rate limit (rad/s)
|
||||
robot_radius = 0.17 # Collision radius (meters)
|
||||
collision_mode = "stop" # "stop", "slide", "passthrough"
|
||||
```
|
||||
|
||||
#### Lidar Simulation
|
||||
|
||||
```toml
|
||||
[device.simulation.lidar]
|
||||
num_rays = 360 # Points per scan
|
||||
scan_rate_hz = 5.0 # Scans per second
|
||||
min_range = 0.15 # Minimum detection (meters)
|
||||
max_range = 8.0 # Maximum detection (meters)
|
||||
mounting_x = -0.110 # Lidar offset from center
|
||||
angle_offset = 0.2182 # Angular offset (radians)
|
||||
|
||||
[device.simulation.lidar.noise]
|
||||
range_stddev = 0.005 # Distance noise (meters)
|
||||
angle_stddev = 0.001 # Angular noise (radians)
|
||||
miss_rate = 0.01 # Invalid reading probability
|
||||
quality_base = 200 # Base quality (0-255)
|
||||
```
|
||||
|
||||
#### IMU Simulation
|
||||
|
||||
```toml
|
||||
[device.simulation.imu.gyro_noise]
|
||||
stddev = [5.0, 5.0, 10.0] # Per-axis noise
|
||||
bias = [0.0, 0.0, 0.0] # Constant bias
|
||||
drift_rate = 0.0 # Bias drift (units/sec)
|
||||
|
||||
[device.simulation.imu.accel_noise]
|
||||
stddev = [10.0, 10.0, 20.0]
|
||||
bias = [0.0, 0.0, 0.0]
|
||||
```
|
||||
|
||||
#### Encoder Simulation
|
||||
|
||||
```toml
|
||||
[device.simulation.encoder.noise]
|
||||
slip_stddev = 0.002 # Wheel slip noise (0-1)
|
||||
slip_bias = 0.0 # Systematic slip
|
||||
quantization_noise = true # ±0.5 tick jitter
|
||||
```
|
||||
|
||||
## Speed Factor Usage
|
||||
|
||||
The `speed_factor` setting accelerates simulation time:
|
||||
|
||||
| Factor | sensor_status | lidar | Use Case |
|
||||
|--------|---------------|-------|----------|
|
||||
| 1.0 | 110 Hz | 5 Hz | Real-time development |
|
||||
| 2.0 | 220 Hz | 10 Hz | Faster iteration |
|
||||
| 5.0 | 550 Hz | 25 Hz | Quick integration tests |
|
||||
| 10.0 | 1100 Hz | 50 Hz | Stress testing |
|
||||
|
||||
**Note:** Higher speeds increase CPU usage and may not be sustainable on all systems.
|
||||
|
||||
## Deterministic Testing
|
||||
|
||||
For reproducible tests, set a fixed random seed:
|
||||
|
||||
```toml
|
||||
[device.simulation]
|
||||
random_seed = 12345 # Same seed = same sensor noise sequence
|
||||
```
|
||||
|
||||
This ensures:
|
||||
- Identical lidar noise patterns
|
||||
- Same encoder slip variations
|
||||
- Reproducible IMU readings
|
||||
|
||||
## Collision Modes
|
||||
|
||||
Three collision handling modes are available:
|
||||
|
||||
| Mode | Behavior | Use Case |
|
||||
|------|----------|----------|
|
||||
| `stop` | Robot stops on collision | Realistic behavior |
|
||||
| `slide` | Robot slides along walls | Wall-following tests |
|
||||
| `passthrough` | No collision | Debugging, path planning |
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
### "Map file not found"
|
||||
|
||||
```
|
||||
Error: Failed to read map YAML: No such file or directory
|
||||
```
|
||||
|
||||
Check that `map_file` path is relative to where you run the command, or use absolute path.
|
||||
|
||||
### "Mock device not available"
|
||||
|
||||
```
|
||||
Error: Mock device not available: rebuild with --features mock
|
||||
```
|
||||
|
||||
The mock feature is not enabled. Rebuild:
|
||||
```bash
|
||||
cargo build --release --features mock
|
||||
```
|
||||
|
||||
### High CPU Usage
|
||||
|
||||
If CPU usage is high:
|
||||
1. Reduce `speed_factor`
|
||||
2. Check `log_level` (verbose logging is expensive)
|
||||
3. Ensure release build: `--release`
|
||||
|
||||
### No Lidar Data
|
||||
|
||||
Lidar must be explicitly enabled:
|
||||
```python
|
||||
# Send lidar enable command via TCP
|
||||
cmd = ComponentControl(id="lidar", action=Enable())
|
||||
```
|
||||
|
||||
### Client Not Receiving UDP
|
||||
|
||||
1. Ensure TCP connection is established first
|
||||
2. Check firewall allows UDP on port 5555
|
||||
3. Verify client is binding to correct port
|
||||
|
||||
## Integration with SLAM
|
||||
|
||||
The mock device produces identical sensor data format to real hardware:
|
||||
|
||||
```python
|
||||
# Same code works for both real and simulated robot
|
||||
client = SangamClient(host="localhost", port=5555)
|
||||
# or
|
||||
client = SangamClient(host="192.168.68.101", port=5555)
|
||||
```
|
||||
|
||||
## Example Workflow
|
||||
|
||||
1. **Develop algorithm** using mock device on laptop
|
||||
2. **Write unit tests** with deterministic random seed
|
||||
3. **Run CI tests** with mock device (no hardware needed)
|
||||
4. **Deploy to robot** - same client code works
|
||||
5. **Record real data** using bag files
|
||||
6. **Replay and compare** mock vs real performance
|
||||
@@ -1,879 +0,0 @@
|
||||
# Proposal: Mock Device Driver for SangamIO
|
||||
|
||||
## Overview
|
||||
|
||||
This proposal outlines the implementation of a **mock device driver** for SangamIO that simulates CRL-200S robot hardware using a pre-recorded map file. This enables algorithm development, testing, and debugging without physical robot hardware.
|
||||
|
||||
## Goals
|
||||
|
||||
1. Enable SLAM/navigation algorithm testing in simulation
|
||||
2. Use **ROS-standard PGM + YAML map format** for interoperability
|
||||
3. Match CRL-200S physical characteristics exactly
|
||||
4. Provide configurable noise for realistic sensor simulation
|
||||
5. Support deterministic/repeatable testing scenarios
|
||||
6. Full CRL-200S sensor parity from initial release
|
||||
|
||||
## Requirements Summary
|
||||
|
||||
Based on clarification discussions, the following decisions were made:
|
||||
|
||||
| Topic | Decision |
|
||||
|-------|----------|
|
||||
| **Collision handling** | Stop or slide based on config; slide friction configurable |
|
||||
| **Noise parameters** | Start with datasheet estimates; calibrate later based on SLAM performance |
|
||||
| **Actuator commands** | Log state changes only (no simulation effect) |
|
||||
| **Map source** | Manual creation (GIMP) or external sources; no DhruvaSLAM export initially |
|
||||
| **Cliff sensors** | Separate mask file (`map_cliffs.pgm`) |
|
||||
| **Time scaling** | Configurable speed factor (1x, 2x, 5x, 10x) |
|
||||
| **Bumper geometry** | Configurable trigger zones (angles + distances) |
|
||||
| **Logging level** | Standard: log velocity commands, collisions, sensor triggers |
|
||||
| **Button simulation** | Fixed state (never pressed) |
|
||||
| **Fast mode behavior** | Increase packet rate (220Hz at 2x, 550Hz at 5x) |
|
||||
| **Time source** | Wall-clock multiplied by speed_factor |
|
||||
| **MVP scope** | Full CRL-200S parity (all sensors from day one) |
|
||||
|
||||
## Map File Format (ROS Standard)
|
||||
|
||||
### PGM + YAML Format
|
||||
|
||||
Maps use the ROS-standard two-file format:
|
||||
|
||||
**YAML Metadata File** (`map.yaml`):
|
||||
```yaml
|
||||
image: map.pgm
|
||||
resolution: 0.02 # meters per pixel (matches dhruva-slam default)
|
||||
origin: [-10.0, -10.0, 0.0] # [x, y, yaw] of bottom-left pixel in world frame
|
||||
occupied_thresh: 0.65 # Pixel value > 65% = occupied
|
||||
free_thresh: 0.196 # Pixel value < 19.6% = free
|
||||
negate: 0 # 0 = white is free, 1 = white is occupied
|
||||
|
||||
# Optional: cliff mask file (same dimensions as main map)
|
||||
cliff_mask: map_cliffs.pgm # White = safe, Black = cliff
|
||||
```
|
||||
|
||||
**PGM Image File** (`map.pgm`):
|
||||
- Grayscale image (8-bit, 0-255)
|
||||
- **White (255)** = free space
|
||||
- **Black (0)** = occupied
|
||||
- **Gray (205)** = unknown
|
||||
|
||||
**Cliff Mask File** (`map_cliffs.pgm`) - Optional:
|
||||
- Grayscale image (same dimensions as main map)
|
||||
- **White (255)** = safe floor
|
||||
- **Black (0)** = cliff/drop-off
|
||||
- If not specified, cliffs only trigger at map boundary
|
||||
|
||||
### Map Compatibility
|
||||
|
||||
This format is compatible with:
|
||||
- ROS `map_server` / Nav2 `map_server`
|
||||
- Cartographer
|
||||
- SLAM Toolbox
|
||||
- gmapping
|
||||
- Any standard image editor (GIMP, Photoshop)
|
||||
|
||||
## Configuration
|
||||
|
||||
### Mock Configuration File (`mock.toml`)
|
||||
|
||||
This configuration mirrors CRL-200S as closely as possible:
|
||||
|
||||
```toml
|
||||
# Mock Device Configuration for SangamIO
|
||||
# Simulates CRL-200S hardware for algorithm testing
|
||||
|
||||
[device]
|
||||
type = "mock"
|
||||
name = "Mock CRL-200S"
|
||||
|
||||
# ============================================================================
|
||||
# Simulation Environment
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation]
|
||||
# Map file (ROS-standard PGM+YAML format)
|
||||
# Point to the .yaml file, PGM is referenced inside
|
||||
map_file = "maps/apartment.yaml"
|
||||
|
||||
# Initial robot pose in world coordinates (ROS REP-103)
|
||||
# X = forward, Y = left, Theta = CCW positive
|
||||
start_x = 0.0 # meters
|
||||
start_y = 0.0 # meters
|
||||
start_theta = 0.0 # radians (0 = facing +X direction)
|
||||
|
||||
# Simulation speed multiplier (1.0 = real-time)
|
||||
# Higher values increase packet rate proportionally:
|
||||
# 1.0 = 110Hz sensor_status, 5Hz lidar
|
||||
# 2.0 = 220Hz sensor_status, 10Hz lidar
|
||||
# 5.0 = 550Hz sensor_status, 25Hz lidar
|
||||
# 10.0 = 1100Hz sensor_status, 50Hz lidar
|
||||
speed_factor = 1.0
|
||||
|
||||
# Random seed for reproducible noise (0 = random seed each run)
|
||||
random_seed = 0
|
||||
|
||||
# Logging level: "minimal", "standard", "verbose"
|
||||
# - minimal: errors and warnings only
|
||||
# - standard: velocity commands, collisions, sensor triggers
|
||||
# - verbose: pose and sensor values at configurable interval
|
||||
log_level = "standard"
|
||||
|
||||
# ============================================================================
|
||||
# Robot Physical Parameters (CRL-200S Specifications)
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.robot]
|
||||
# Differential drive parameters
|
||||
wheel_base = 0.233 # meters (distance between wheel centers)
|
||||
wheel_radius = 0.033 # meters
|
||||
ticks_per_meter = 4464.0 # encoder ticks per meter of travel
|
||||
|
||||
# Velocity limits
|
||||
max_linear_speed = 0.3 # m/s
|
||||
max_angular_speed = 1.0 # rad/s
|
||||
|
||||
# Robot physical dimensions (for collision detection)
|
||||
robot_radius = 0.17 # meters (approximate circular footprint)
|
||||
|
||||
# Collision behavior: "stop", "slide", "passthrough"
|
||||
# - stop: robot velocity becomes zero on collision
|
||||
# - slide: robot slides along obstacle surface
|
||||
# - passthrough: ignore collisions (for debugging)
|
||||
collision_mode = "stop"
|
||||
|
||||
# Slide friction coefficient (only used when collision_mode = "slide")
|
||||
# 0.0 = frictionless sliding, 1.0 = high friction (slow slide)
|
||||
slide_friction = 0.3
|
||||
|
||||
# ============================================================================
|
||||
# Lidar Configuration (Delta-2D Specifications)
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.lidar]
|
||||
# Scan parameters
|
||||
num_rays = 360 # points per 360° scan
|
||||
scan_rate_hz = 5.0 # scans per second (Delta-2D native rate)
|
||||
|
||||
# Range limits
|
||||
min_range = 0.15 # meters (matches dhruva-slam preprocessing)
|
||||
max_range = 8.0 # meters (Delta-2D max range)
|
||||
|
||||
# Lidar mounting position (relative to robot center)
|
||||
# CRL-200S: lidar is mounted behind center
|
||||
mounting_x = -0.110 # meters (behind robot center)
|
||||
mounting_y = 0.0 # meters (centered on axis)
|
||||
optical_offset = 0.025 # meters (radial offset to laser origin)
|
||||
angle_offset = 0.2182 # radians (~12.5°, lidar's 0° vs robot forward)
|
||||
|
||||
# Noise configuration (initial estimates - calibrate based on SLAM performance)
|
||||
[device.simulation.lidar.noise]
|
||||
# Distance noise (Gaussian)
|
||||
range_stddev = 0.005 # meters (5mm standard deviation)
|
||||
range_bias = 0.0 # meters (systematic offset)
|
||||
|
||||
# Angular noise (Gaussian)
|
||||
angle_stddev = 0.001 # radians (~0.06°)
|
||||
|
||||
# Quality simulation
|
||||
quality_base = 200 # base quality value (0-255)
|
||||
quality_distance_decay = 10 # quality reduction per meter distance
|
||||
|
||||
# Miss rate (probability of invalid reading)
|
||||
miss_rate = 0.01 # 1% chance of invalid point
|
||||
|
||||
# ============================================================================
|
||||
# IMU Configuration (GD32 Specifications)
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.imu]
|
||||
update_rate_hz = 110.0 # matches GD32 status packet rate
|
||||
|
||||
# Gyroscope noise (raw i16 units, same as hardware)
|
||||
# Initial estimates based on typical MPU6050 - calibrate later
|
||||
[device.simulation.imu.gyro_noise]
|
||||
stddev = [5, 5, 10] # [x, y, z] standard deviation
|
||||
bias = [0, 0, 0] # [x, y, z] constant bias
|
||||
drift_rate = 0.0 # bias drift per second
|
||||
|
||||
# Accelerometer noise (raw i16 units)
|
||||
[device.simulation.imu.accel_noise]
|
||||
stddev = [10, 10, 20] # [x, y, z] standard deviation
|
||||
bias = [0, 0, 0] # [x, y, z] constant bias
|
||||
|
||||
# Tilt sensor noise (low-pass filtered gravity vector)
|
||||
[device.simulation.imu.tilt_noise]
|
||||
stddev = [5, 5, 5] # [x, y, z] standard deviation
|
||||
|
||||
# ============================================================================
|
||||
# Encoder Configuration
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.encoder]
|
||||
update_rate_hz = 110.0 # matches GD32 status packet rate
|
||||
|
||||
# Encoder noise (initial estimates - calibrate later)
|
||||
[device.simulation.encoder.noise]
|
||||
# Slip simulation (multiplicative noise on tick count)
|
||||
slip_stddev = 0.002 # 0.2% standard deviation
|
||||
slip_bias = 0.0 # systematic over/under counting
|
||||
|
||||
# Quantization (encoder resolution)
|
||||
quantization_noise = true # add ±0.5 tick quantization noise
|
||||
|
||||
# ============================================================================
|
||||
# Bumper Configuration (Configurable Zones)
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.bumpers]
|
||||
# Bumper trigger distance from robot edge
|
||||
trigger_distance = 0.01 # meters (1cm)
|
||||
|
||||
# Left bumper arc (angles relative to robot forward, CCW positive)
|
||||
[device.simulation.bumpers.left]
|
||||
start_angle = 0.3 # radians (~17°)
|
||||
end_angle = 1.57 # radians (90°, robot left side)
|
||||
|
||||
# Right bumper arc
|
||||
[device.simulation.bumpers.right]
|
||||
start_angle = -1.57 # radians (-90°, robot right side)
|
||||
end_angle = -0.3 # radians (~-17°)
|
||||
|
||||
# ============================================================================
|
||||
# Cliff Sensor Configuration
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.cliffs]
|
||||
# Cliff detection sources:
|
||||
# - Map boundary (always active)
|
||||
# - Cliff mask file (if specified in map YAML)
|
||||
enabled = true
|
||||
|
||||
# Cliff sensor positions (relative to robot center)
|
||||
# CRL-200S has 4 cliff sensors
|
||||
[[device.simulation.cliffs.sensors]]
|
||||
name = "left_side"
|
||||
x = 0.12 # meters forward from center
|
||||
y = 0.10 # meters left from center
|
||||
|
||||
[[device.simulation.cliffs.sensors]]
|
||||
name = "left_front"
|
||||
x = 0.15
|
||||
y = 0.05
|
||||
|
||||
[[device.simulation.cliffs.sensors]]
|
||||
name = "right_front"
|
||||
x = 0.15
|
||||
y = -0.05
|
||||
|
||||
[[device.simulation.cliffs.sensors]]
|
||||
name = "right_side"
|
||||
x = 0.12
|
||||
y = -0.10
|
||||
|
||||
# ============================================================================
|
||||
# Binary Sensors Configuration
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.sensors]
|
||||
# Battery simulation (fixed values)
|
||||
[device.simulation.sensors.battery]
|
||||
voltage = 14.8 # volts (between 13.5-15.5V range)
|
||||
level = 85 # percentage (0-100)
|
||||
is_charging = false
|
||||
|
||||
# Fixed sensor states
|
||||
dustbox_attached = true
|
||||
dock_connected = false
|
||||
|
||||
# Buttons - always fixed state (never pressed)
|
||||
start_button = 0
|
||||
dock_button = 0
|
||||
|
||||
# ============================================================================
|
||||
# Actuator Logging (no simulation effect)
|
||||
# ============================================================================
|
||||
|
||||
[device.simulation.actuators]
|
||||
# When actuator commands are received, log state changes
|
||||
# Supported actuators: vacuum, main_brush, side_brush, water_pump, led, lidar
|
||||
log_state_changes = true
|
||||
|
||||
# ============================================================================
|
||||
# Coordinate Frame Transforms (matches CRL-200S sangamio.toml)
|
||||
# ============================================================================
|
||||
|
||||
[device.hardware.frame_transforms]
|
||||
|
||||
# Lidar angle transform: output = scale * input + offset
|
||||
# CRL-200S: mounted backward (offset=π), clockwise angles (scale=-1)
|
||||
[device.hardware.frame_transforms.lidar]
|
||||
scale = -1.0
|
||||
offset = 3.14159265
|
||||
|
||||
# IMU gyroscope axis remap: [source_axis, sign]
|
||||
# CRL-200S: x=yaw, y=pitch, z=roll -> remap to ROS standard
|
||||
[device.hardware.frame_transforms.imu_gyro]
|
||||
x = [2, 1] # output_x (roll) = input_z * 1
|
||||
y = [1, 1] # output_y (pitch) = input_y * 1
|
||||
z = [0, -1] # output_z (yaw) = input_x * -1
|
||||
|
||||
# IMU accelerometer axis remap (identity for CRL-200S)
|
||||
[device.hardware.frame_transforms.imu_accel]
|
||||
x = [0, 1]
|
||||
y = [1, 1]
|
||||
z = [2, 1]
|
||||
|
||||
# ============================================================================
|
||||
# Network Configuration
|
||||
# ============================================================================
|
||||
|
||||
[network]
|
||||
bind_address = "0.0.0.0:5555"
|
||||
udp_streaming_port = 5556
|
||||
```
|
||||
|
||||
## Architecture
|
||||
|
||||
### Module Structure
|
||||
|
||||
```
|
||||
sangam-io/src/devices/
|
||||
├── mod.rs # Add "mock" to device factory
|
||||
└── mock/
|
||||
├── mod.rs # MockDriver struct + DeviceDriver impl
|
||||
├── config.rs # SimulationConfig deserialization
|
||||
├── map_loader.rs # PGM+YAML parser + cliff mask
|
||||
├── physics.rs # Differential drive kinematics + collision
|
||||
├── noise.rs # Configurable noise generator
|
||||
├── lidar_sim.rs # Ray-casting lidar simulation
|
||||
├── imu_sim.rs # IMU data generation with noise
|
||||
├── encoder_sim.rs # Encoder simulation with slip/noise
|
||||
└── sensor_sim.rs # Bumpers, cliffs, battery (binary sensors)
|
||||
```
|
||||
|
||||
### Component Diagram
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ MockDriver │
|
||||
├─────────────────────────────────────────────────────────────────────┤
|
||||
│ ┌─────────────────────────────────────────────────────────────────┐│
|
||||
│ │ Configuration (TOML) ││
|
||||
│ │ • Robot params (wheel_base, ticks_per_meter, limits) ││
|
||||
│ │ • Sensor noise configs (stddev, bias, drift) ││
|
||||
│ │ • Map file path + cliff mask + initial pose ││
|
||||
│ │ • Bumper zones, cliff sensor positions ││
|
||||
│ │ • Speed factor, collision mode, logging level ││
|
||||
│ └─────────────────────────────────────────────────────────────────┘│
|
||||
│ │ │
|
||||
│ ▼ │
|
||||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────────────────┐ │
|
||||
│ │ MapLoader │ │ PhysicsEngine│ │ Sensor Simulators │ │
|
||||
│ │ (PGM+YAML) │ │ │ │ │ │
|
||||
│ │ │ │ • pose │ │ • LidarSim (ray-cast) │ │
|
||||
│ │ • load() │──▶│ • velocity │──▶│ • ImuSim (gyro/accel) │ │
|
||||
│ │ • ray_cast() │ │ • collision │ │ • EncoderSim (ticks) │ │
|
||||
│ │ • is_cliff() │ │ • slide │ │ • BumperSim (zones) │ │
|
||||
│ │ • is_free() │ │ • update() │ │ • CliffSim (mask) │ │
|
||||
│ └──────────────┘ └──────────────┘ └──────────────────────────┘ │
|
||||
│ │ │ │ │
|
||||
│ ▼ ▼ ▼ │
|
||||
│ ┌─────────────────────────────────────────────────────────────────┐│
|
||||
│ │ Sensor Groups (identical to CRL-200S output) ││
|
||||
│ │ ││
|
||||
│ │ sensor_status (~110Hz × speed_factor): ││
|
||||
│ │ • wheel_left, wheel_right (U16 encoder ticks) ││
|
||||
│ │ • gyro_x/y/z, accel_x/y/z, tilt_x/y/z (I16 raw) ││
|
||||
│ │ • bumper_left, bumper_right (Bool) ││
|
||||
│ │ • cliff_left_side/front, cliff_right_front/side (Bool) ││
|
||||
│ │ • battery_voltage (F32), battery_level (U8), is_charging (Bool)││
|
||||
│ │ • dustbox_attached, is_dock_connected (Bool) ││
|
||||
│ │ • start_button, dock_button (U16) - always 0 ││
|
||||
│ │ ││
|
||||
│ │ lidar (5Hz × speed_factor): ││
|
||||
│ │ • scan: PointCloud2D [(angle_rad, distance_m, quality)] ││
|
||||
│ │ ││
|
||||
│ │ device_version (one-time): ││
|
||||
│ │ • version_string: "MockDevice v1.0" ││
|
||||
│ │ • version_code: 100 ││
|
||||
│ └─────────────────────────────────────────────────────────────────┘│
|
||||
└─────────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
### Simulation Loop
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ Simulation Thread (~110Hz × speed_factor) │
|
||||
├─────────────────────────────────────────────────────────────────────┤
|
||||
│ │
|
||||
│ Time source: wall_clock × speed_factor │
|
||||
│ At 2x speed: loop runs every ~4.5ms instead of ~9ms │
|
||||
│ │
|
||||
│ Every tick (110Hz × speed_factor): │
|
||||
│ │
|
||||
│ 1. Compute dt = (wall_time_delta × speed_factor) │
|
||||
│ │
|
||||
│ 2. Read velocity command (from atomic state, set by TCP) │
|
||||
│ └─ linear_vel (m/s), angular_vel (rad/s) │
|
||||
│ │
|
||||
│ 3. Update physics (differential drive kinematics) │
|
||||
│ ├─ Compute wheel velocities from (v, ω) │
|
||||
│ ├─ Integrate pose: x, y, theta │
|
||||
│ ├─ Check collision against map │
|
||||
│ │ ├─ "stop": zero velocity, log collision │
|
||||
│ │ ├─ "slide": project velocity along surface │
|
||||
│ │ └─ "passthrough": ignore collision │
|
||||
│ └─ Update encoder tick counters (with slip noise) │
|
||||
│ │
|
||||
│ 4. Generate sensor_status data: │
|
||||
│ ├─ Encoders: wheel_left, wheel_right (U16 + noise) │
|
||||
│ ├─ IMU: gyro_x/y/z (from ω + noise) │
|
||||
│ │ accel_x/y/z (gravity + centripetal + noise) │
|
||||
│ │ tilt_x/y/z (gravity vector + noise) │
|
||||
│ ├─ Bumpers: check configurable arc zones against map │
|
||||
│ ├─ Cliffs: check sensor positions against cliff mask │
|
||||
│ ├─ Battery: constant from config │
|
||||
│ └─ Buttons: always 0 │
|
||||
│ │
|
||||
│ 5. Publish to streaming channel │
|
||||
│ │
|
||||
│ 6. Log state changes (if log_level >= "standard"): │
|
||||
│ ├─ Velocity commands received │
|
||||
│ ├─ Collision events │
|
||||
│ ├─ Bumper/cliff triggers │
|
||||
│ └─ Actuator state changes │
|
||||
│ │
|
||||
│ Every (200ms / speed_factor) - lidar tick: │
|
||||
│ │
|
||||
│ 7. Generate lidar scan: │
|
||||
│ ├─ For each of 360 rays: │
|
||||
│ │ ├─ Transform ray origin by lidar mount offset │
|
||||
│ │ ├─ Ray-cast against occupancy grid │
|
||||
│ │ ├─ Add range noise (Gaussian) │
|
||||
│ │ ├─ Apply miss rate (random invalid points) │
|
||||
│ │ └─ Compute quality from distance │
|
||||
│ └─ Apply frame transform (scale, offset) │
|
||||
│ │
|
||||
│ 8. Update lidar sensor group │
|
||||
│ │
|
||||
└─────────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
## Implementation Details
|
||||
|
||||
### 1. Map Loader with Cliff Mask (`map_loader.rs`)
|
||||
|
||||
```rust
|
||||
use std::path::Path;
|
||||
use image::GrayImage;
|
||||
|
||||
/// Parsed map YAML metadata
|
||||
#[derive(Debug, Deserialize)]
|
||||
pub struct MapMetadata {
|
||||
pub image: String,
|
||||
pub resolution: f32,
|
||||
pub origin: [f32; 3],
|
||||
pub occupied_thresh: f32,
|
||||
pub free_thresh: f32,
|
||||
#[serde(default)]
|
||||
pub negate: u8,
|
||||
#[serde(default)]
|
||||
pub cliff_mask: Option<String>, // Optional cliff mask file
|
||||
}
|
||||
|
||||
/// Simulation map loaded from PGM+YAML
|
||||
pub struct SimulationMap {
|
||||
pixels: GrayImage,
|
||||
cliff_mask: Option<GrayImage>, // Optional cliff layer
|
||||
resolution: f32,
|
||||
origin: (f32, f32),
|
||||
occupied_thresh: u8,
|
||||
free_thresh: u8,
|
||||
}
|
||||
|
||||
impl SimulationMap {
|
||||
pub fn load<P: AsRef<Path>>(yaml_path: P) -> Result<Self> {
|
||||
let yaml_content = std::fs::read_to_string(&yaml_path)?;
|
||||
let metadata: MapMetadata = serde_yaml::from_str(&yaml_content)?;
|
||||
|
||||
let yaml_dir = yaml_path.as_ref().parent().unwrap_or(Path::new("."));
|
||||
let pgm_path = yaml_dir.join(&metadata.image);
|
||||
let img = image::open(&pgm_path)?.into_luma8();
|
||||
|
||||
// Load optional cliff mask
|
||||
let cliff_mask = if let Some(cliff_file) = &metadata.cliff_mask {
|
||||
let cliff_path = yaml_dir.join(cliff_file);
|
||||
Some(image::open(&cliff_path)?.into_luma8())
|
||||
} else {
|
||||
None
|
||||
};
|
||||
|
||||
let occupied_thresh = (metadata.occupied_thresh * 255.0) as u8;
|
||||
let free_thresh = ((1.0 - metadata.free_thresh) * 255.0) as u8;
|
||||
|
||||
Ok(Self {
|
||||
pixels: img,
|
||||
cliff_mask,
|
||||
resolution: metadata.resolution,
|
||||
origin: (metadata.origin[0], metadata.origin[1]),
|
||||
occupied_thresh,
|
||||
free_thresh,
|
||||
})
|
||||
}
|
||||
|
||||
/// Check if position is a cliff (from mask or map boundary)
|
||||
pub fn is_cliff(&self, x: f32, y: f32) -> bool {
|
||||
match self.world_to_pixel(x, y) {
|
||||
Some((px, py)) => {
|
||||
if let Some(mask) = &self.cliff_mask {
|
||||
let pixel = mask.get_pixel(px, py).0[0];
|
||||
pixel < 128 // Black = cliff
|
||||
} else {
|
||||
false // No mask, not a cliff
|
||||
}
|
||||
}
|
||||
None => true, // Out of bounds = cliff
|
||||
}
|
||||
}
|
||||
|
||||
// ... rest of implementation (is_occupied, is_free, ray_cast, world_to_pixel)
|
||||
}
|
||||
```
|
||||
|
||||
### 2. Physics Engine with Collision Modes (`physics.rs`)
|
||||
|
||||
```rust
|
||||
pub enum CollisionMode {
|
||||
Stop,
|
||||
Slide { friction: f32 },
|
||||
Passthrough,
|
||||
}
|
||||
|
||||
pub struct PhysicsState {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub theta: f32,
|
||||
pub linear_vel: f32,
|
||||
pub angular_vel: f32,
|
||||
pub left_ticks: u16,
|
||||
pub right_ticks: u16,
|
||||
collision_mode: CollisionMode,
|
||||
robot_radius: f32,
|
||||
}
|
||||
|
||||
impl PhysicsState {
|
||||
pub fn update(&mut self, dt: f32, map: &SimulationMap, config: &RobotConfig) -> bool {
|
||||
// Compute new position
|
||||
let new_x = self.x + self.linear_vel * self.theta.cos() * dt;
|
||||
let new_y = self.y + self.linear_vel * self.theta.sin() * dt;
|
||||
let new_theta = self.theta + self.angular_vel * dt;
|
||||
|
||||
// Check collision
|
||||
let would_collide = self.check_collision(new_x, new_y, map);
|
||||
|
||||
match &self.collision_mode {
|
||||
CollisionMode::Stop => {
|
||||
if would_collide {
|
||||
// Stop movement, log collision
|
||||
log::info!("Collision at ({:.3}, {:.3})", new_x, new_y);
|
||||
return true; // Collision occurred
|
||||
}
|
||||
self.x = new_x;
|
||||
self.y = new_y;
|
||||
}
|
||||
CollisionMode::Slide { friction } => {
|
||||
if would_collide {
|
||||
// Find collision normal and slide along surface
|
||||
let (slide_x, slide_y) = self.compute_slide_vector(
|
||||
new_x, new_y, *friction, map
|
||||
);
|
||||
self.x += slide_x * dt;
|
||||
self.y += slide_y * dt;
|
||||
log::info!("Sliding at ({:.3}, {:.3})", self.x, self.y);
|
||||
return true;
|
||||
}
|
||||
self.x = new_x;
|
||||
self.y = new_y;
|
||||
}
|
||||
CollisionMode::Passthrough => {
|
||||
self.x = new_x;
|
||||
self.y = new_y;
|
||||
}
|
||||
}
|
||||
|
||||
self.theta = normalize_angle(new_theta);
|
||||
false // No collision
|
||||
}
|
||||
|
||||
fn check_collision(&self, x: f32, y: f32, map: &SimulationMap) -> bool {
|
||||
// Check multiple points around robot circumference
|
||||
let num_checks = 8;
|
||||
for i in 0..num_checks {
|
||||
let angle = (i as f32 / num_checks as f32) * TAU;
|
||||
let check_x = x + self.robot_radius * angle.cos();
|
||||
let check_y = y + self.robot_radius * angle.sin();
|
||||
if map.is_occupied(check_x, check_y) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
false
|
||||
}
|
||||
|
||||
fn compute_slide_vector(&self, x: f32, y: f32, friction: f32, map: &SimulationMap) -> (f32, f32) {
|
||||
// Simplified: find surface normal and project velocity
|
||||
// Full implementation would use gradient of distance field
|
||||
let vel_mag = self.linear_vel * (1.0 - friction);
|
||||
|
||||
// Try perpendicular directions
|
||||
let perp1 = (self.theta + FRAC_PI_2).sin();
|
||||
let perp2 = (self.theta + FRAC_PI_2).cos();
|
||||
|
||||
// Return the direction that doesn't collide
|
||||
if !map.is_occupied(x + perp1 * 0.01, y + perp2 * 0.01) {
|
||||
(perp1 * vel_mag, perp2 * vel_mag)
|
||||
} else {
|
||||
(-perp1 * vel_mag, -perp2 * vel_mag)
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 3. Bumper Simulator with Configurable Zones (`sensor_sim.rs`)
|
||||
|
||||
```rust
|
||||
pub struct BumperZone {
|
||||
pub start_angle: f32, // radians, relative to robot forward
|
||||
pub end_angle: f32,
|
||||
}
|
||||
|
||||
pub struct BumperSimulator {
|
||||
left_zone: BumperZone,
|
||||
right_zone: BumperZone,
|
||||
trigger_distance: f32,
|
||||
}
|
||||
|
||||
impl BumperSimulator {
|
||||
pub fn check(&self, map: &SimulationMap, x: f32, y: f32, theta: f32, robot_radius: f32)
|
||||
-> (bool, bool)
|
||||
{
|
||||
let left = self.check_zone(&self.left_zone, map, x, y, theta, robot_radius);
|
||||
let right = self.check_zone(&self.right_zone, map, x, y, theta, robot_radius);
|
||||
(left, right)
|
||||
}
|
||||
|
||||
fn check_zone(&self, zone: &BumperZone, map: &SimulationMap,
|
||||
x: f32, y: f32, theta: f32, robot_radius: f32) -> bool
|
||||
{
|
||||
// Check multiple rays within the bumper arc
|
||||
let num_rays = 5;
|
||||
let angle_range = zone.end_angle - zone.start_angle;
|
||||
|
||||
for i in 0..num_rays {
|
||||
let local_angle = zone.start_angle + (i as f32 / (num_rays - 1) as f32) * angle_range;
|
||||
let world_angle = theta + local_angle;
|
||||
|
||||
// Ray from robot edge in this direction
|
||||
let start_x = x + robot_radius * world_angle.cos();
|
||||
let start_y = y + robot_radius * world_angle.sin();
|
||||
|
||||
let check_x = start_x + self.trigger_distance * world_angle.cos();
|
||||
let check_y = start_y + self.trigger_distance * world_angle.sin();
|
||||
|
||||
if map.is_occupied(check_x, check_y) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
false
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 4. Cliff Simulator (`sensor_sim.rs`)
|
||||
|
||||
```rust
|
||||
pub struct CliffSensor {
|
||||
pub name: String,
|
||||
pub x: f32, // position relative to robot center
|
||||
pub y: f32,
|
||||
}
|
||||
|
||||
pub struct CliffSimulator {
|
||||
sensors: Vec<CliffSensor>,
|
||||
}
|
||||
|
||||
impl CliffSimulator {
|
||||
pub fn check(&self, map: &SimulationMap, robot_x: f32, robot_y: f32, robot_theta: f32)
|
||||
-> HashMap<String, bool>
|
||||
{
|
||||
let mut results = HashMap::new();
|
||||
|
||||
for sensor in &self.sensors {
|
||||
// Transform sensor position to world frame
|
||||
let world_x = robot_x
|
||||
+ sensor.x * robot_theta.cos()
|
||||
- sensor.y * robot_theta.sin();
|
||||
let world_y = robot_y
|
||||
+ sensor.x * robot_theta.sin()
|
||||
+ sensor.y * robot_theta.cos();
|
||||
|
||||
let is_cliff = map.is_cliff(world_x, world_y);
|
||||
results.insert(sensor.name.clone(), is_cliff);
|
||||
}
|
||||
|
||||
results
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 5. Actuator Logger (`mod.rs`)
|
||||
|
||||
```rust
|
||||
pub struct ActuatorState {
|
||||
vacuum: bool,
|
||||
vacuum_speed: u8,
|
||||
main_brush: bool,
|
||||
main_brush_speed: u8,
|
||||
side_brush: bool,
|
||||
side_brush_speed: u8,
|
||||
water_pump: bool,
|
||||
water_pump_speed: u8,
|
||||
led_state: u8,
|
||||
lidar_enabled: bool,
|
||||
lidar_pwm: u8,
|
||||
}
|
||||
|
||||
impl ActuatorState {
|
||||
pub fn handle_command(&mut self, id: &str, action: &ComponentAction) {
|
||||
match (id, action) {
|
||||
("vacuum", ComponentAction::Enable { .. }) => {
|
||||
self.vacuum = true;
|
||||
log::info!("Actuator: vacuum ENABLED");
|
||||
}
|
||||
("vacuum", ComponentAction::Disable { .. }) => {
|
||||
self.vacuum = false;
|
||||
log::info!("Actuator: vacuum DISABLED");
|
||||
}
|
||||
("vacuum", ComponentAction::Configure { config }) => {
|
||||
if let Some(SensorValue::U8(speed)) = config.get("speed") {
|
||||
self.vacuum_speed = *speed;
|
||||
log::info!("Actuator: vacuum speed = {}%", speed);
|
||||
}
|
||||
}
|
||||
// ... similar for other actuators
|
||||
_ => {
|
||||
log::debug!("Actuator: ignoring command for '{}'", id);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Device Factory Integration
|
||||
|
||||
Update `src/devices/mod.rs`:
|
||||
|
||||
```rust
|
||||
pub mod crl200s;
|
||||
pub mod mock;
|
||||
|
||||
use crl200s::CRL200SDriver;
|
||||
use mock::MockDriver;
|
||||
|
||||
pub fn create_device(config: &Config) -> Result<Box<dyn DeviceDriver>> {
|
||||
match config.device.device_type.as_str() {
|
||||
"crl200s" => {
|
||||
let driver = CRL200SDriver::new(config.device.clone())?;
|
||||
Ok(Box::new(driver))
|
||||
}
|
||||
"mock" => {
|
||||
let driver = MockDriver::new(config)?;
|
||||
Ok(Box::new(driver))
|
||||
}
|
||||
_ => Err(Error::UnknownDevice(config.device.device_type.clone())),
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Usage Workflow
|
||||
|
||||
### 1. Create a Map
|
||||
|
||||
```bash
|
||||
# Create map in GIMP or other image editor
|
||||
# - White (255) = free space
|
||||
# - Black (0) = walls/obstacles
|
||||
# - Gray (205) = unknown
|
||||
|
||||
# Save as PGM (P5 binary or P2 ASCII)
|
||||
# Create YAML metadata file
|
||||
```
|
||||
|
||||
Example `maps/apartment.yaml`:
|
||||
```yaml
|
||||
image: apartment.pgm
|
||||
resolution: 0.02
|
||||
origin: [-5.0, -5.0, 0.0]
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
negate: 0
|
||||
cliff_mask: apartment_cliffs.pgm # Optional
|
||||
```
|
||||
|
||||
### 2. Create Mock Config
|
||||
|
||||
Copy the template from this proposal and customize:
|
||||
- Set `map_file` to your map YAML path
|
||||
- Adjust `start_x`, `start_y`, `start_theta` for initial pose
|
||||
- Set `speed_factor` for faster testing (if needed)
|
||||
- Tune noise parameters based on SLAM performance
|
||||
|
||||
### 3. Run Simulation
|
||||
|
||||
```bash
|
||||
# Terminal 1: Start mock SangamIO
|
||||
cd sangam-io
|
||||
cargo run --release -- --config mock.toml
|
||||
|
||||
# Terminal 2: Connect SLAM/client
|
||||
# (configure client to connect to 127.0.0.1:5555)
|
||||
```
|
||||
|
||||
## Dependencies
|
||||
|
||||
**New dependencies** (add to `Cargo.toml`):
|
||||
```toml
|
||||
[dependencies]
|
||||
image = "0.25" # PGM loading
|
||||
serde_yaml = "0.9" # YAML parsing
|
||||
rand = "0.8" # Noise generation
|
||||
rand_distr = "0.4" # Normal distribution
|
||||
```
|
||||
|
||||
## Testing Strategy
|
||||
|
||||
### Unit Tests
|
||||
- Map loader: PGM+YAML parsing, cliff mask loading
|
||||
- Physics: Differential drive kinematics, collision modes
|
||||
- Noise: Statistical properties (mean, stddev match config)
|
||||
- Bumpers: Zone detection accuracy
|
||||
- Cliffs: Mask lookup, boundary detection
|
||||
|
||||
### Integration Tests
|
||||
- **Protocol compatibility**: Mock sensor groups match CRL-200S format exactly
|
||||
- **Determinism**: Same `random_seed` produces identical sensor streams
|
||||
- **Speed factor**: Verify packet rates scale correctly (110Hz × factor)
|
||||
|
||||
### Manual Testing
|
||||
- Connect Drishti and verify all sensor displays work
|
||||
- Test collision modes (stop, slide, passthrough)
|
||||
- Verify bumper/cliff triggers at expected positions
|
||||
|
||||
## Future Enhancements
|
||||
|
||||
1. **DhruvaSLAM map export**: Add PGM+YAML export to DhruvaSLAM for record→replay workflow
|
||||
2. **Dynamic obstacles**: Moving obstacles in simulation
|
||||
3. **Multiple robots**: Simulate multi-robot scenarios
|
||||
4. **Sensor failures**: Inject failures for resilience testing
|
||||
5. **GUI visualization**: Optional debug visualization window
|
||||
6. **Virtual clock**: Support pause/step/resume for debugging
|
||||
@@ -1,8 +1,11 @@
|
||||
# GD32 Command Reference
|
||||
# CRL-200S GD32 Command Reference
|
||||
|
||||
> **Device-specific documentation for the CRL-200S robotic vacuum platform.**
|
||||
> This document describes the serial protocol between SangamIO and the GD32F103 motor controller MCU.
|
||||
|
||||
This document lists all known GD32 commands discovered from reverse-engineering the AuxCtrl binary, captured logs, and current SangamIO implementation.
|
||||
|
||||
**Source Reference:** `src/devices/crl200s/constants.rs`, `src/devices/crl200s/gd32/packet.rs`, `src/devices/crl200s/gd32/commands.rs`
|
||||
**Source Code:** [`constants.rs`](constants.rs), [`gd32/packet.rs`](gd32/packet.rs), [`gd32/commands.rs`](gd32/commands.rs)
|
||||
|
||||
## Command Format
|
||||
|
||||
@@ -14,7 +17,7 @@ All commands use the packet format: `[0xFA 0xFB] [LEN] [CMD] [PAYLOAD] [CRC_H] [
|
||||
- **PAYLOAD**: Variable length data
|
||||
- **CRC**: 16-bit big-endian word sum checksum (except 0x08)
|
||||
|
||||
### CRC Calculation (from `packet.rs`)
|
||||
### CRC Calculation (from [`gd32/packet.rs`](gd32/packet.rs))
|
||||
|
||||
```rust
|
||||
fn checksum(data: &[u8]) -> u16 {
|
||||
@@ -177,3 +180,10 @@ RobotCommand {
|
||||
|
||||
**Note:** The `Configure` action for lidar is ignored. PWM is controlled exclusively by `sangamio.toml`, not by upstream clients.
|
||||
|
||||
---
|
||||
|
||||
## Related Documentation
|
||||
|
||||
- [SENSORSTATUS.md](SENSORSTATUS.md) - Status packet byte layout (96 bytes)
|
||||
- [`gd32/reader.rs`](gd32/reader.rs) - Status packet parsing implementation
|
||||
- [`delta2d/protocol.rs`](delta2d/protocol.rs) - Lidar protocol (separate from GD32)
|
||||
@@ -1,6 +1,7 @@
|
||||
# GD32 Sensor Status Packet (0x15)
|
||||
# CRL-200S GD32 Sensor Status Packet (0x15)
|
||||
|
||||
This document details the 96-byte sensor status packet sent by the GD32 motor controller at approximately 110Hz (limited by 115200 baud serial).
|
||||
> **Device-specific documentation for the CRL-200S robotic vacuum platform.**
|
||||
> This document describes the 96-byte sensor status packet sent by the GD32 motor controller.
|
||||
|
||||
## Packet Structure
|
||||
|
||||
@@ -60,7 +61,7 @@ Frequency: ~110 Hz (every ~9ms, limited by 115200 baud)
|
||||
| 2 | 0x04 | Bumper Left | 1 = left bumper triggered |
|
||||
| 3-7 | - | Reserved | - |
|
||||
|
||||
**Code Reference:** `constants.rs:97-98`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) lines 97-98
|
||||
|
||||
### Byte 0x03: Cliff Flags
|
||||
|
||||
@@ -72,7 +73,7 @@ Frequency: ~110 Hz (every ~9ms, limited by 115200 baud)
|
||||
| 3 | 0x08 | Cliff Right Side | 1 = drop detected |
|
||||
| 4-7 | - | Reserved | - |
|
||||
|
||||
**Code Reference:** `constants.rs:99-102`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) lines 99-102
|
||||
|
||||
### Byte 0x04: Dustbox Flags
|
||||
|
||||
@@ -84,7 +85,7 @@ Frequency: ~110 Hz (every ~9ms, limited by 115200 baud)
|
||||
|
||||
**Note:** This byte may also indicate box type (0x00 = normal dustbox, 0x04 = 2-in-1 mop box) based on MITM observations.
|
||||
|
||||
**Code Reference:** `constants.rs:103`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) line 103
|
||||
|
||||
### Byte 0x07: Charging Flags
|
||||
|
||||
@@ -94,7 +95,7 @@ Frequency: ~110 Hz (every ~9ms, limited by 115200 baud)
|
||||
| 1 | 0x02 | Charging | 1 = actively charging |
|
||||
| 2-7 | - | Reserved | - |
|
||||
|
||||
**Code Reference:** `constants.rs:95-96`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) lines 95-96
|
||||
|
||||
### Byte 0x08: Battery Voltage
|
||||
|
||||
@@ -109,25 +110,25 @@ BATTERY_VOLTAGE_MAX = 15.5V (100%)
|
||||
percentage = (voltage - 13.5) / (15.5 - 13.5) * 100
|
||||
```
|
||||
|
||||
**Code Reference:** `constants.rs:91-92`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) lines 91-92
|
||||
|
||||
### Bytes 0x10-0x11: Left Wheel Encoder
|
||||
|
||||
- **Type:** u16 little-endian
|
||||
- **Description:** Encoder tick count for left wheel
|
||||
- **Implementation:** Only lower 2 bytes are read by `reader.rs`
|
||||
- **Implementation:** Only lower 2 bytes are read by [`gd32/reader.rs`](gd32/reader.rs)
|
||||
- **Note:** Bytes 0x12-0x17 may contain upper bytes of a larger counter, but are currently unused
|
||||
|
||||
**Code Reference:** `constants.rs:67`, `reader.rs`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) line 67
|
||||
|
||||
### Bytes 0x18-0x19: Right Wheel Encoder
|
||||
|
||||
- **Type:** u16 little-endian
|
||||
- **Description:** Encoder tick count for right wheel
|
||||
- **Implementation:** Only lower 2 bytes are read by `reader.rs`
|
||||
- **Implementation:** Only lower 2 bytes are read by [`gd32/reader.rs`](gd32/reader.rs)
|
||||
- **Note:** Bytes 0x1A-0x1F may contain upper bytes of a larger counter, but are currently unused
|
||||
|
||||
**Code Reference:** `constants.rs:68`, `reader.rs`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) line 68
|
||||
|
||||
### IMU Data (0x28-0x33)
|
||||
|
||||
@@ -163,7 +164,7 @@ z = [0, -1] # output_z (Yaw) = input_x * -1 (sign flip)
|
||||
- `gyro_y` = Pitch rate (Y axis rotation, nose up/down)
|
||||
- `gyro_z` = Yaw rate (Z axis rotation, CCW positive)
|
||||
|
||||
**Code Reference:** `constants.rs:73-88`, `reader.rs:313-323`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) lines 73-88, [`gd32/reader.rs`](gd32/reader.rs) lines 313-323
|
||||
|
||||
### LP-Filtered Tilt Vector (0x34-0x39)
|
||||
|
||||
@@ -175,7 +176,7 @@ Low-pass filtered gravity vector for tilt correction. Useful for determining rob
|
||||
| 0x36-0x37 | 2 | Tilt Y | i16 LE - LP gravity vector Y |
|
||||
| 0x38-0x39 | 2 | Tilt Z | i16 LE - LP gravity vector Z |
|
||||
|
||||
**Code Reference:** `constants.rs:85-88`, `reader.rs:334-355`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) lines 85-88, [`gd32/reader.rs`](gd32/reader.rs) lines 334-355
|
||||
|
||||
### Bytes 0x3A-0x3B: Start Button
|
||||
|
||||
@@ -183,7 +184,7 @@ Low-pass filtered gravity vector for tilt correction. Useful for determining rob
|
||||
- **Description:** Start button press state
|
||||
- **Implementation:** Read as u16 from bytes 0x3A-0x3B
|
||||
|
||||
**Code Reference:** `constants.rs:70`, `reader.rs:223-229`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) line 70, [`gd32/reader.rs`](gd32/reader.rs) lines 223-229
|
||||
|
||||
### Bytes 0x3E-0x3F: Dock Button
|
||||
|
||||
@@ -191,7 +192,7 @@ Low-pass filtered gravity vector for tilt correction. Useful for determining rob
|
||||
- **Description:** Dock/home button press state
|
||||
- **Implementation:** Read as u16 from bytes 0x3E-0x3F
|
||||
|
||||
**Code Reference:** `constants.rs:71`, `reader.rs:230-236`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) line 71, [`gd32/reader.rs`](gd32/reader.rs) lines 230-236
|
||||
|
||||
### Byte 0x46: Water Tank Level
|
||||
|
||||
@@ -239,7 +240,7 @@ Legend:
|
||||
|
||||
The reader requires a minimum payload size of 80 bytes (`STATUS_PAYLOAD_MIN_SIZE`). Packets smaller than this are discarded.
|
||||
|
||||
**Code Reference:** `constants.rs:59`
|
||||
**Code Reference:** [`constants.rs`](constants.rs) line 59
|
||||
|
||||
### Data Endianness
|
||||
|
||||
@@ -300,17 +301,18 @@ These may contain additional sensor data, error codes, or internal state. Furthe
|
||||
|
||||
---
|
||||
|
||||
## Related Files
|
||||
## Related Documentation
|
||||
|
||||
- `src/devices/crl200s/constants.rs` - Offset and flag constants
|
||||
- `src/devices/crl200s/gd32/reader.rs` - Status packet parsing logic
|
||||
- `src/devices/crl200s/gd32/protocol.rs` - Packet framing and CRC
|
||||
- `COMMANDS.md` - GD32 command reference
|
||||
- [COMMANDS.md](COMMANDS.md) - GD32 command reference
|
||||
- [`constants.rs`](constants.rs) - Offset and flag constants
|
||||
- [`gd32/reader.rs`](gd32/reader.rs) - Status packet parsing logic
|
||||
- [`gd32/protocol.rs`](gd32/protocol.rs) - Packet framing and CRC
|
||||
|
||||
---
|
||||
|
||||
## Changelog
|
||||
|
||||
- **2024-12-15:** Moved to `src/devices/crl200s/` directory, updated relative paths
|
||||
- **2024-12-07:** Added coordinate frame transform documentation for IMU data (ROS REP-103 conversion)
|
||||
- **2024-11-30:** Fixed encoder and button sizes to match actual reader.rs implementation (u16, not u64/u32)
|
||||
- **2024-11-30:** Initial documentation based on reverse-engineered constants and MITM captures
|
||||
@@ -63,7 +63,7 @@ const MODE_SWITCH_DELAY_MS: u64 = 100;
|
||||
/// Interval between STM32 data requests in milliseconds.
|
||||
///
|
||||
/// The stock R2D firmware sends 0x0D requests approximately every 1.5 seconds,
|
||||
/// as observed in MITM captures (see COMMANDS.md "Request STM32 Data" entry).
|
||||
/// as observed in MITM captures (see [`../COMMANDS.md`](../COMMANDS.md) "Request STM32 Data" entry).
|
||||
/// This appears to be a keep-alive/diagnostic query.
|
||||
const STM32_REQUEST_INTERVAL_MS: u64 = 1500;
|
||||
|
||||
|
||||
@@ -1,7 +1,56 @@
|
||||
//! Mock device simulation configuration
|
||||
//!
|
||||
//! Defines all configuration structures for simulating CRL-200S hardware.
|
||||
//! All parameters have defaults matching the real hardware specifications.
|
||||
//! This module defines all configuration structures for the mock device driver.
|
||||
//! Every parameter has sensible defaults matching CRL-200S hardware specifications,
|
||||
//! allowing minimal configuration for basic usage while enabling deep customization
|
||||
//! for advanced testing scenarios.
|
||||
//!
|
||||
//! # Configuration Hierarchy
|
||||
//!
|
||||
//! ```text
|
||||
//! SimulationConfig
|
||||
//! ├── map_file, start_x/y/theta # Environment setup
|
||||
//! ├── speed_factor, random_seed # Simulation control
|
||||
//! ├── RobotConfig # Physical parameters
|
||||
//! │ ├── wheel_base, ticks_per_meter
|
||||
//! │ ├── max_linear/angular_speed
|
||||
//! │ └── collision_mode, robot_radius
|
||||
//! ├── LidarConfig # Lidar simulation
|
||||
//! │ ├── num_rays, scan_rate_hz
|
||||
//! │ ├── min/max_range, mounting offsets
|
||||
//! │ └── LidarNoiseConfig
|
||||
//! ├── ImuConfig # IMU simulation
|
||||
//! │ ├── GyroNoiseConfig
|
||||
//! │ ├── Noise3DConfig (accel)
|
||||
//! │ └── Noise3DConfig (tilt)
|
||||
//! ├── EncoderConfig # Encoder simulation
|
||||
//! │ └── EncoderNoiseConfig
|
||||
//! ├── BumpersConfig # Bumper zones
|
||||
//! ├── CliffsConfig # Cliff sensor positions
|
||||
//! ├── SensorsConfig # Battery, buttons, etc.
|
||||
//! └── ActuatorsConfig # Logging options
|
||||
//! ```
|
||||
//!
|
||||
//! # Default Values
|
||||
//!
|
||||
//! All defaults match CRL-200S hardware specifications:
|
||||
//!
|
||||
//! | Parameter | Default | Source |
|
||||
//! |-----------|---------|--------|
|
||||
//! | wheel_base | 0.233 m | Measured |
|
||||
//! | ticks_per_meter | 4464 | Calibrated |
|
||||
//! | max_linear_speed | 0.3 m/s | Hardware limit |
|
||||
//! | max_angular_speed | 1.0 rad/s | Hardware limit |
|
||||
//! | robot_radius | 0.17 m | Physical size |
|
||||
//! | lidar_num_rays | 360 | Delta-2D spec |
|
||||
//! | lidar_scan_rate | 5 Hz | Delta-2D spec |
|
||||
//! | lidar_max_range | 8 m | Delta-2D spec |
|
||||
//!
|
||||
//! # Noise Configuration
|
||||
//!
|
||||
//! Noise parameters are initial estimates and should be calibrated based on
|
||||
//! SLAM performance with real hardware data. Start with defaults, then adjust
|
||||
//! if simulated performance differs significantly from real-world results.
|
||||
|
||||
use serde::Deserialize;
|
||||
use std::f32::consts::PI;
|
||||
|
||||
@@ -1,7 +1,128 @@
|
||||
//! Mock device driver for simulation
|
||||
//! Mock device driver for hardware-free robot simulation
|
||||
//!
|
||||
//! Provides a simulated CRL-200S robot for algorithm testing without hardware.
|
||||
//! Uses PGM+YAML maps for environment simulation.
|
||||
//! This module provides a complete simulation of CRL-200S robot hardware,
|
||||
//! enabling SLAM and navigation algorithm development without physical robots.
|
||||
//!
|
||||
//! # Overview
|
||||
//!
|
||||
//! The mock driver simulates all sensors and actuators of the CRL-200S:
|
||||
//!
|
||||
//! | Component | Simulation Method |
|
||||
//! |-----------|-------------------|
|
||||
//! | Lidar (Delta-2D) | Ray-casting against occupancy grid |
|
||||
//! | Wheel encoders | Differential drive kinematics + slip noise |
|
||||
//! | IMU (gyro/accel) | Physics-based with configurable noise |
|
||||
//! | Bumpers | Collision detection against map |
|
||||
//! | Cliff sensors | Cliff mask lookup or boundary detection |
|
||||
//! | Battery/buttons | Fixed configurable states |
|
||||
//!
|
||||
//! # Map Format
|
||||
//!
|
||||
//! Uses ROS-standard PGM + YAML format for maximum compatibility:
|
||||
//!
|
||||
//! ```yaml
|
||||
//! # maps/example.yaml
|
||||
//! image: example.pgm # Grayscale occupancy grid
|
||||
//! resolution: 0.02 # meters per pixel
|
||||
//! origin: [-5.0, -5.0, 0.0] # [x, y, yaw] of bottom-left pixel
|
||||
//! occupied_thresh: 0.65 # Darker = occupied
|
||||
//! cliff_mask: cliffs.pgm # Optional cliff layer
|
||||
//! ```
|
||||
//!
|
||||
//! PGM pixel values:
|
||||
//! - **White (255)**: Free space
|
||||
//! - **Black (0)**: Occupied/wall
|
||||
//! - **Gray (205)**: Unknown
|
||||
//!
|
||||
//! # Configuration
|
||||
//!
|
||||
//! Enable the `mock` feature to use this driver:
|
||||
//!
|
||||
//! ```bash
|
||||
//! cargo build --features mock
|
||||
//! cargo run --features mock -- mock.toml
|
||||
//! ```
|
||||
//!
|
||||
//! Example configuration (`mock.toml`):
|
||||
//!
|
||||
//! ```toml
|
||||
//! [device]
|
||||
//! type = "mock"
|
||||
//! name = "Mock CRL-200S"
|
||||
//!
|
||||
//! [device.simulation]
|
||||
//! map_file = "maps/example.yaml"
|
||||
//! start_x = 1.5
|
||||
//! start_y = 3.5
|
||||
//! start_theta = 0.0
|
||||
//! speed_factor = 1.0 # 2.0 = 2x speed
|
||||
//! random_seed = 42 # 0 = random each run
|
||||
//!
|
||||
//! [network]
|
||||
//! bind_address = "0.0.0.0:5555"
|
||||
//! ```
|
||||
//!
|
||||
//! # Simulation Loop
|
||||
//!
|
||||
//! The simulation runs at 110Hz (matching CRL-200S hardware):
|
||||
//!
|
||||
//! ```text
|
||||
//! Every ~9ms:
|
||||
//! 1. Read velocity commands from shared state
|
||||
//! 2. Update physics (pose, collision detection)
|
||||
//! 3. Generate encoder ticks from wheel velocities
|
||||
//! 4. Generate IMU readings (gyro, accel, tilt)
|
||||
//! 5. Check bumpers and cliff sensors
|
||||
//! 6. Publish sensor_status via streaming channel
|
||||
//!
|
||||
//! Every ~200ms (5Hz):
|
||||
//! 7. Generate lidar scan via ray-casting
|
||||
//! 8. Update lidar sensor group
|
||||
//! ```
|
||||
//!
|
||||
//! # Speed Factor
|
||||
//!
|
||||
//! The `speed_factor` setting accelerates simulation time:
|
||||
//!
|
||||
//! | Factor | sensor_status | lidar | Use Case |
|
||||
//! |--------|---------------|-------|----------|
|
||||
//! | 1.0 | 110 Hz | 5 Hz | Real-time testing |
|
||||
//! | 2.0 | 220 Hz | 10 Hz | Faster algorithm iteration |
|
||||
//! | 5.0 | 550 Hz | 25 Hz | Quick integration tests |
|
||||
//!
|
||||
//! # Noise Models
|
||||
//!
|
||||
//! All sensors include configurable noise for realistic testing:
|
||||
//!
|
||||
//! - **Lidar**: Range stddev, angle stddev, miss rate, quality decay
|
||||
//! - **Encoders**: Wheel slip (multiplicative), quantization noise
|
||||
//! - **IMU**: Per-axis stddev, bias, gyro drift rate
|
||||
//!
|
||||
//! # Thread Model
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌─────────────────┐
|
||||
//! │ Main Thread │
|
||||
//! │ (initialize) │
|
||||
//! └────────┬────────┘
|
||||
//! │ spawns
|
||||
//! ▼
|
||||
//! ┌─────────────────┐ ┌─────────────────┐
|
||||
//! │ Simulation Loop │────▶│ Streaming Chan │
|
||||
//! │ (mock-sim) │ │ (sensor_status) │
|
||||
//! └─────────────────┘ └─────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! # Module Structure
|
||||
//!
|
||||
//! - [`config`]: Configuration structures for all simulation parameters
|
||||
//! - [`physics`]: Differential drive kinematics and collision handling
|
||||
//! - [`lidar_sim`]: Ray-casting lidar simulation
|
||||
//! - [`imu_sim`]: IMU data generation with noise
|
||||
//! - [`encoder_sim`]: Wheel encoder simulation with slip
|
||||
//! - [`sensor_sim`]: Bumper, cliff, and binary sensor simulation
|
||||
//! - [`map_loader`]: PGM + YAML map loading
|
||||
//! - [`noise`]: Configurable noise generator
|
||||
|
||||
pub mod config;
|
||||
mod encoder_sim;
|
||||
|
||||
@@ -1,7 +1,74 @@
|
||||
//! Streaming module for SangamIO
|
||||
//! Network streaming module for SangamIO
|
||||
//!
|
||||
//! - **UDP**: Unicast sensor data to registered clients (fire-and-forget)
|
||||
//! - **TCP**: Handles commands from clients (reliable, bidirectional)
|
||||
//! This module provides the communication layer between SangamIO and client
|
||||
//! applications (SLAM, visualization, etc.) using a hybrid UDP/TCP architecture.
|
||||
//!
|
||||
//! # Architecture Overview
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌───────────────────────────────────────────────────────────┐
|
||||
//! │ SangamIO Daemon │
|
||||
//! │ │
|
||||
//! │ ┌─────────────────┐ ┌─────────────────┐ │
|
||||
//! │ │ UDP Publisher │ │ TCP Receiver │ │
|
||||
//! │ │ (port 5555) │ │ (port 5555) │ │
|
||||
//! │ │ │ │ │ │
|
||||
//! │ │ • sensor_status│ │ • Commands │ │
|
||||
//! │ │ @ 110Hz │ │ • Client │ │
|
||||
//! │ │ • lidar @ 5Hz │ │ registration │ │
|
||||
//! │ └────────┬────────┘ └────────┬────────┘ │
|
||||
//! │ │ │ │
|
||||
//! └───────────────┼──────────────────────────┼────────────────┘
|
||||
//! │ UDP unicast │ TCP stream
|
||||
//! ▼ ▼
|
||||
//! ┌───────────────────────────────────────────────────────────┐
|
||||
//! │ Client Application │
|
||||
//! │ (receives sensor data via UDP, sends commands via TCP) │
|
||||
//! └───────────────────────────────────────────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! # Protocol Design
|
||||
//!
|
||||
//! ## Why UDP for Sensor Data?
|
||||
//!
|
||||
//! - **Lower latency**: No TCP handshake overhead, no head-of-line blocking
|
||||
//! - **Better for real-time**: Dropped packets are preferable to delayed ones
|
||||
//! - **Reduced buffering**: No kernel TCP buffer delays
|
||||
//! - **Fire-and-forget**: Server doesn't wait for ACKs
|
||||
//!
|
||||
//! ## Why TCP for Commands?
|
||||
//!
|
||||
//! - **Reliability**: Commands must not be lost or duplicated
|
||||
//! - **Ordering**: Commands must execute in order
|
||||
//! - **Acknowledgment**: Caller knows command was received
|
||||
//!
|
||||
//! # Wire Format
|
||||
//!
|
||||
//! Both UDP and TCP use the same length-prefixed Protobuf format for
|
||||
//! client compatibility:
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌──────────────────┬─────────────────────┐
|
||||
//! │ Length (4 bytes) │ Protobuf Payload │
|
||||
//! │ Big-endian u32 │ (variable size) │
|
||||
//! └──────────────────┴─────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! # Client Registration
|
||||
//!
|
||||
//! UDP streaming uses unicast (not broadcast) for security and efficiency.
|
||||
//! Clients must first connect via TCP to register their UDP endpoint:
|
||||
//!
|
||||
//! 1. Client connects to TCP port 5555
|
||||
//! 2. Server records client's IP address
|
||||
//! 3. UDP packets are sent to client's IP on same port
|
||||
//! 4. When TCP disconnects, UDP streaming to that client stops
|
||||
//!
|
||||
//! # Module Contents
|
||||
//!
|
||||
//! - [`UdpPublisher`]: Streams sensor data to registered clients
|
||||
//! - [`TcpReceiver`]: Handles incoming commands and client registration
|
||||
//! - [`wire`]: Protobuf serialization/deserialization
|
||||
|
||||
pub mod tcp_receiver;
|
||||
pub mod udp_publisher;
|
||||
|
||||
@@ -1,4 +1,57 @@
|
||||
//! TCP receiver thread - receives commands from client
|
||||
//! TCP command receiver for client control
|
||||
//!
|
||||
//! This module handles incoming commands from connected clients over TCP.
|
||||
//! Commands include robot motion control, actuator control, and component
|
||||
//! enable/disable operations.
|
||||
//!
|
||||
//! # Purpose
|
||||
//!
|
||||
//! TCP is used for commands (not UDP) because:
|
||||
//!
|
||||
//! - **Reliability**: Commands must not be lost (e.g., "stop motors")
|
||||
//! - **Ordering**: Commands must execute in sequence
|
||||
//! - **Acknowledgment**: Sender knows the command was received
|
||||
//! - **State sync**: TCP connection state tracks client presence
|
||||
//!
|
||||
//! # Wire Format
|
||||
//!
|
||||
//! Commands use length-prefixed Protobuf encoding:
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌──────────────────┬─────────────────────┐
|
||||
//! │ Length (4 bytes) │ Protobuf Command │
|
||||
//! │ Big-endian u32 │ (variable size) │
|
||||
//! └──────────────────┴─────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! # Command Types
|
||||
//!
|
||||
//! | Command | Description |
|
||||
//! |---------|-------------|
|
||||
//! | `ComponentControl::drive` | Set linear/angular velocity |
|
||||
//! | `ComponentControl::lidar` | Enable/disable lidar motor |
|
||||
//! | `ComponentControl::vacuum` | Control vacuum motor speed |
|
||||
//! | `ComponentControl::main_brush` | Control main brush speed |
|
||||
//! | `ComponentControl::side_brush` | Control side brush speed |
|
||||
//! | `ComponentControl::water_pump` | Control water pump speed |
|
||||
//! | `ComponentControl::led` | Set LED state |
|
||||
//! | `Shutdown` | Graceful daemon shutdown |
|
||||
//!
|
||||
//! # Connection Lifecycle
|
||||
//!
|
||||
//! ```text
|
||||
//! 1. Client connects to TCP port 5555
|
||||
//! 2. Server spawns TcpReceiver thread for this client
|
||||
//! 3. Client IP is registered for UDP streaming
|
||||
//! 4. Receiver loop processes commands until disconnect
|
||||
//! 5. On disconnect, UDP registration is cleared
|
||||
//! ```
|
||||
//!
|
||||
//! # Safety Features
|
||||
//!
|
||||
//! - **Read timeout**: 500ms timeout allows periodic shutdown flag checks
|
||||
//! - **Buffer limit**: Commands > 1MB are rejected (DoS protection)
|
||||
//! - **Graceful shutdown**: Handles both global and per-connection flags
|
||||
|
||||
use crate::core::driver::DeviceDriver;
|
||||
use crate::core::types::Command;
|
||||
|
||||
@@ -1,11 +1,71 @@
|
||||
//! UDP publisher thread - streams sensor data to registered clients
|
||||
//! UDP publisher for real-time sensor data streaming
|
||||
//!
|
||||
//! Supports two modes for different data rates:
|
||||
//! - **Streaming mode**: For high-rate sensors (GD32 @ 110Hz), consumes from channel
|
||||
//! - **Polling mode**: For low-rate sensors (lidar @ 5Hz), polls shared mutex
|
||||
//! This module implements the UDP unicast publisher that streams sensor data
|
||||
//! to registered clients. It's optimized for low-latency, high-frequency
|
||||
//! sensor delivery on embedded systems.
|
||||
//!
|
||||
//! Uses UDP unicast to registered clients only (not broadcast).
|
||||
//! Clients are registered when they connect via TCP for commands.
|
||||
//! # Design Principles
|
||||
//!
|
||||
//! ## Two Data Delivery Modes
|
||||
//!
|
||||
//! Different sensors have different update rates and delivery requirements:
|
||||
//!
|
||||
//! | Mode | Sensors | Rate | Mechanism |
|
||||
//! |------|---------|------|-----------|
|
||||
//! | Streaming | sensor_status (GD32) | 110Hz | Crossbeam channel |
|
||||
//! | Polling | lidar, device_version | 5Hz | Shared mutex |
|
||||
//!
|
||||
//! **Streaming mode** uses lock-free channels for high-rate sensors where
|
||||
//! every packet matters and we need to minimize latency.
|
||||
//!
|
||||
//! **Polling mode** uses shared mutexes for low-rate sensors where it's
|
||||
//! acceptable to occasionally miss an update if the publisher is busy.
|
||||
//!
|
||||
//! ## Unicast vs Broadcast
|
||||
//!
|
||||
//! UDP streaming uses **unicast** (not broadcast) because:
|
||||
//!
|
||||
//! - **Security**: Only authorized clients receive data
|
||||
//! - **Efficiency**: No processing overhead on non-listening hosts
|
||||
//! - **Firewall-friendly**: Works across subnets with proper routing
|
||||
//! - **Single client**: Robot typically has one SLAM client at a time
|
||||
//!
|
||||
//! # Client Registration
|
||||
//!
|
||||
//! Clients are automatically registered when they connect via TCP:
|
||||
//!
|
||||
//! ```text
|
||||
//! 1. Client connects TCP to port 5555
|
||||
//! 2. TcpReceiver extracts client IP from socket
|
||||
//! 3. Client IP is stored in UdpClientRegistry
|
||||
//! 4. UdpPublisher sends packets to registered IP
|
||||
//! 5. When TCP disconnects, registration is cleared
|
||||
//! ```
|
||||
//!
|
||||
//! # Wire Format
|
||||
//!
|
||||
//! Each UDP datagram uses the same format as TCP for client compatibility:
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌──────────────────┬──────────────────────┐
|
||||
//! │ Length (4 bytes) │ Protobuf SensorGroup │
|
||||
//! │ Big-endian u32 │ (~150 bytes typical) │
|
||||
//! └──────────────────┴──────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! # Performance Characteristics
|
||||
//!
|
||||
//! - **Buffer reuse**: Pre-allocated send buffer avoids allocation per packet
|
||||
//! - **Non-blocking**: Channel draining and mutex polling are non-blocking
|
||||
//! - **Rate limiting**: 500μs minimum sleep between packets to yield CPU
|
||||
//! - **Graceful degradation**: Packet loss is logged but doesn't block sender
|
||||
//!
|
||||
//! # Example Packet Rates
|
||||
//!
|
||||
//! At default settings:
|
||||
//! - sensor_status: ~110 packets/sec (~16.5 KB/s)
|
||||
//! - lidar: ~5 packets/sec (~10 KB/s with 360 points)
|
||||
//! - device_version: 1 packet (one-time)
|
||||
|
||||
use crate::core::types::{SensorGroupData, StreamReceiver};
|
||||
use crate::error::Result;
|
||||
|
||||
Reference in New Issue
Block a user