Update drishti to unified port architecture and comprehensive docs
SLAM mode changes: - Use single port (5557) for both streaming and commands - command-port now defaults to same as --port (unified architecture) - Add debug logging for failed proto messages (saves to /tmp/failed_proto.bin) - Update docstrings to reflect unified architecture New utilities: - Add debug_proto.py for capturing and analyzing DhruvaSLAM proto messages Documentation: - Complete README rewrite with dual-mode architecture - Document hybrid UDP/TCP protocol for sensor mode - Add comprehensive feature lists for both modes - Document all keyboard shortcuts including diagonal movement - Add sensor data table with types and descriptions - Document IMU processing (Mahony AHRS filter) - Add coordinate conventions (ROS REP-103) - Add troubleshooting sections for both modes - Document UI components and architecture
This commit is contained in:
codetiger
+358
-77
@@ -1,146 +1,427 @@
|
||||
# Drishti - Robot Diagnostic Tool
|
||||
|
||||
PyQt-based monitoring tool for testing sensor diagnosis and TCP protocol communication with SangamIO.
|
||||
PyQt5-based real-time visualization and control client for the CRL-200S robotic vacuum with two operating modes:
|
||||
|
||||
- **Sensor Mode**: Connect to SangamIO for raw sensor visualization (robot view)
|
||||
- **SLAM Mode**: Connect to DhruvaSLAM for map visualization and autonomous navigation
|
||||
|
||||
## Installation
|
||||
|
||||
```bash
|
||||
cd drishti
|
||||
python -m venv venv
|
||||
source venv/bin/activate # or venv\Scripts\activate on Windows
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
### Dependencies
|
||||
|
||||
- `protobuf>=4.0` - Protocol buffer messaging
|
||||
- `PyQt5>=5.15.0` - GUI framework
|
||||
- `pyqtgraph>=0.13.0` - High-performance plotting/visualization
|
||||
- `PyOpenGL>=3.1.6` - 3D graphics via OpenGL
|
||||
- `numpy>=1.21.0` - Numerical computing
|
||||
|
||||
## Usage
|
||||
|
||||
### GUI Application
|
||||
### Sensor Mode
|
||||
|
||||
Connect to SangamIO for raw sensor data visualization:
|
||||
|
||||
```bash
|
||||
python drishti_ui.py --robot 192.168.68.101
|
||||
python drishti_ui.py sensor --robot 192.168.68.101
|
||||
```
|
||||
|
||||
Options:
|
||||
- `--robot` - Robot IP address (default: 192.168.68.101)
|
||||
- `--port` - TCP port (default: 5555)
|
||||
- `--verbose` - Enable debug logging
|
||||
| Option | Default | Description |
|
||||
|--------|---------|-------------|
|
||||
| `--robot` | 192.168.68.101 | SangamIO hostname or IP |
|
||||
| `--port` | 5555 | SangamIO port (TCP+UDP) |
|
||||
| `--log-raw-packets` | disabled | Log raw GD32 status packets |
|
||||
| `--verbose` | disabled | Enable debug logging |
|
||||
|
||||
### Console Demo
|
||||
### SLAM Mode
|
||||
|
||||
Connect to DhruvaSLAM for SLAM visualization and control:
|
||||
|
||||
```bash
|
||||
python drishti.py --robot 192.168.68.101
|
||||
python drishti_ui.py slam --host localhost
|
||||
```
|
||||
|
||||
Options:
|
||||
| Option | Default | Description |
|
||||
|--------|---------|-------------|
|
||||
| `--host` | localhost | DhruvaSLAM hostname |
|
||||
| `--port` | 5557 | DhruvaSLAM port (TCP+UDP) |
|
||||
| `--command-port` | same as --port | Optional separate command port |
|
||||
| `--verbose` | disabled | Enable debug logging |
|
||||
|
||||
### Console Demo (Legacy)
|
||||
|
||||
```bash
|
||||
python drishti.py --robot 192.168.68.101 --verbose
|
||||
```
|
||||
|
||||
## Features
|
||||
|
||||
- Full-screen robot diagram with sensor overlays
|
||||
- Real-time telemetry monitoring (110Hz)
|
||||
- JSON protocol over TCP
|
||||
- Visual indicators for all 13 sensors
|
||||
- Drive control with arrow keys
|
||||
- Actuator control (vacuum, brushes, lidar)
|
||||
### Sensor Mode
|
||||
|
||||
**3D Visualization:**
|
||||
- Full-screen OpenGL wireframe robot model
|
||||
- Animated wheels (spin based on encoder data)
|
||||
- Animated lidar turret (spins when enabled)
|
||||
- Animated brushes (main and side, when spinning)
|
||||
- Animated vacuum fan (when active)
|
||||
- Bumper indicators (green normal, red triggered)
|
||||
- Cliff sensor indicators (4 pairs)
|
||||
- Battery bar with charging animation
|
||||
- IMU orientation overlay (roll/pitch/yaw)
|
||||
- Multiple camera presets (iso, top, front, side)
|
||||
- Auto-rotate camera when idle
|
||||
|
||||
**Hardware Control:**
|
||||
- Drive motor enable/disable with speed control
|
||||
- Lidar enable/disable with scan counter
|
||||
- Vacuum pump speed 0-100%
|
||||
- Main brush speed 0-100%
|
||||
- Side brush speed 0-100%
|
||||
- Emergency stop (immediate watchdog reset)
|
||||
|
||||
**Sensor Display:**
|
||||
- Battery level + charging state
|
||||
- Bumper state (left/right)
|
||||
- Cliff sensors (4 pairs)
|
||||
- Wheel encoder ticks
|
||||
- Lidar range and quality
|
||||
- IMU gyro/accelerometer data
|
||||
- Motor current, voltage status
|
||||
- Error codes and diagnostics
|
||||
|
||||
|
||||
|
||||
### SLAM Mode
|
||||
|
||||
**Mapping:**
|
||||
- Start/stop mapping sessions
|
||||
- Clear current map
|
||||
- Save maps to persistent storage
|
||||
- List all saved maps with metadata (size, creation time, area)
|
||||
- Enable/rename/delete saved maps
|
||||
|
||||
**Visualization:**
|
||||
- Real-time occupancy grid (10Hz update)
|
||||
- Robot pose display (x, y, heading)
|
||||
- Trajectory trail (2000-point history)
|
||||
- Current lidar scan overlay
|
||||
- Navigation path visualization
|
||||
- Goal marker display
|
||||
- Grid overlay with axis labels
|
||||
|
||||
**Navigation:**
|
||||
- Set goal via Ctrl+Click on map
|
||||
- Visualize planned path (waypoints)
|
||||
- Track navigation progress (distance remaining, ETA)
|
||||
- Cancel active navigation goal
|
||||
|
||||
**Camera Control:**
|
||||
- Auto-follow mode (centers on robot)
|
||||
- Manual pan (click+drag)
|
||||
- Manual zoom (mouse wheel)
|
||||
|
||||
## Keyboard Shortcuts
|
||||
|
||||
### Sensor Mode
|
||||
|
||||
| Key | Action |
|
||||
|-----|--------|
|
||||
| `M` | Toggle drive motor (enable/disable) |
|
||||
| `L` | Toggle lidar |
|
||||
| `V` | Toggle vacuum |
|
||||
| `V` | Toggle vacuum pump |
|
||||
| `B` | Toggle main brush |
|
||||
| `S` | Toggle side brush |
|
||||
| `↑` | Drive forward (when motor enabled) |
|
||||
| `↑` | Drive forward |
|
||||
| `↓` | Drive backward |
|
||||
| `←` | Turn left |
|
||||
| `→` | Turn right |
|
||||
| `Space` | Emergency stop |
|
||||
| `Esc` | Quit |
|
||||
| `↑+←` | Drive forward while turning left |
|
||||
| `↑+→` | Drive forward while turning right |
|
||||
| `↓+←` | Drive backward while turning left |
|
||||
| `↓+→` | Drive backward while turning right |
|
||||
| `Space` | Emergency stop (immediate reset) |
|
||||
| `Esc` | Quit application |
|
||||
|
||||
## TCP Protocol
|
||||
### SLAM Mode
|
||||
|
||||
Connects to SangamIO on port 5555 for telemetry stream.
|
||||
| Key/Action | Description |
|
||||
|------------|-------------|
|
||||
| `Ctrl+Click` | Set navigation goal on map |
|
||||
| Mouse wheel | Zoom map in/out |
|
||||
| Click+drag | Pan map view |
|
||||
| `Esc` | Quit application |
|
||||
|
||||
### Message Format
|
||||
## Architecture
|
||||
|
||||
### Directory Structure
|
||||
|
||||
Length-prefixed JSON:
|
||||
```
|
||||
[4-byte length (big-endian)][JSON payload]
|
||||
drishti/
|
||||
├── drishti_ui.py # Entry point (dual-mode launcher)
|
||||
├── drishti.py # Legacy console client
|
||||
├── debug_proto.py # Proto message debug tool
|
||||
├── requirements.txt # Python dependencies
|
||||
├── proto/ # Protocol buffer definitions
|
||||
│ ├── sangamio_pb2.py # SangamIO protobuf (generated)
|
||||
│ └── dhruva_pb2.py # DhruvaSLAM protobuf (generated)
|
||||
└── ui/
|
||||
├── main_window.py # Sensor mode main window
|
||||
├── slam_main_window.py # SLAM mode main window
|
||||
├── threads/
|
||||
│ ├── telemetry_thread.py # SangamIO data (TCP+UDP)
|
||||
│ ├── slam_thread.py # DhruvaSLAM data (TCP)
|
||||
│ └── odometry_thread.py # Legacy SLAM odometry
|
||||
├── widgets/
|
||||
│ ├── robot_3d_view.py # 3D wireframe visualization
|
||||
│ ├── robot_3d_geometry.py # 3D geometry primitives
|
||||
│ ├── lidar_scan_overlay.py # Polar lidar display
|
||||
│ ├── slam_view.py # Map + trajectory view
|
||||
│ ├── control_panel.py # Sensor mode controls
|
||||
│ ├── slam_control_panel.py # SLAM mode controls
|
||||
│ ├── navigation_panel.py # Navigation status
|
||||
│ ├── diagnostics_panel.py # Timing diagnostics
|
||||
│ ├── compact_actuator.py # Actuator controls
|
||||
│ └── collapsible_group.py # Collapsible sections
|
||||
└── processors/
|
||||
└── imu_processor.py # Mahony AHRS filter
|
||||
```
|
||||
|
||||
### Receiving Sensor Data
|
||||
### Thread Model
|
||||
|
||||
```json
|
||||
{
|
||||
"topic": "sensors/sensor_status",
|
||||
"payload": {
|
||||
"type": "SensorGroup",
|
||||
"group_id": "sensor_status",
|
||||
"timestamp_us": 1700000000000,
|
||||
"values": {
|
||||
"is_charging": {"Bool": false},
|
||||
"wheel_left": {"U16": 12345},
|
||||
"bumper_left": {"Bool": false}
|
||||
}
|
||||
}
|
||||
}
|
||||
**Sensor Mode (TelemetryThread):**
|
||||
1. Connects via TCP (port 5555) for registration
|
||||
2. Receives UDP unicast sensor data (port 5555)
|
||||
3. TCP connection kept alive for sending commands
|
||||
4. Emits: `sensor_data_received(dict)`, `connection_status(bool, str)`
|
||||
|
||||
**SLAM Mode (SlamThread):**
|
||||
1. Connects via TCP (port 5557, unified architecture)
|
||||
2. Two separate connections: streaming (receive-only) and commands (request/response)
|
||||
3. Emits: `robot_status_received`, `sensor_status_received`, `current_map_received`, `map_list_received`, `navigation_status_received`
|
||||
|
||||
## Network Protocol
|
||||
|
||||
### Sensor Mode (SangamIO)
|
||||
|
||||
Uses hybrid UDP/TCP architecture on same port (5555):
|
||||
- **UDP**: Sensor streaming (low latency, no head-of-line blocking)
|
||||
- **TCP**: Commands (reliable delivery)
|
||||
|
||||
**Client Registration:**
|
||||
1. Client connects TCP to port 5555
|
||||
2. Server records client IP address
|
||||
3. UDP packets sent to client on same port
|
||||
4. When TCP disconnects, UDP streaming stops
|
||||
|
||||
**Message Format:** Length-prefixed Protobuf
|
||||
```
|
||||
[4-byte length (big-endian)][Protobuf payload]
|
||||
```
|
||||
|
||||
### Sending Commands
|
||||
**UDP Topics (Outbound @ 110Hz):**
|
||||
- `sensors/sensor_status`: All 13 sensors
|
||||
- `sensors/device_version`: Version info (one-time)
|
||||
- `sensors/lidar`: LidarScan @ 5Hz
|
||||
|
||||
All commands use the `ComponentControl` pattern:
|
||||
**TCP Commands (Inbound):**
|
||||
- `ComponentControl`: Enable/disable/configure components
|
||||
- `ProtocolSync`: Synchronization
|
||||
- `Shutdown`: Graceful shutdown
|
||||
|
||||
```json
|
||||
{
|
||||
"topic": "command",
|
||||
"payload": {
|
||||
"type": "Command",
|
||||
"command": {
|
||||
"type": "ComponentControl",
|
||||
"id": "drive",
|
||||
"action": {
|
||||
"type": "Configure",
|
||||
"config": {
|
||||
"linear": {"F32": 0.2},
|
||||
"angular": {"F32": 0.0}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
**Sensor Data Available:**
|
||||
| Sensor | Type | Description |
|
||||
|--------|------|-------------|
|
||||
| `is_charging` | bool | Charging state |
|
||||
| `battery_level` | u8 | Battery percentage (0-100) |
|
||||
| `bumper_left` | bool | Left bumper triggered |
|
||||
| `bumper_right` | bool | Right bumper triggered |
|
||||
| `cliff_left` | bool | Left cliff detected |
|
||||
| `cliff_center` | bool | Center cliff detected |
|
||||
| `cliff_right` | bool | Right cliff detected |
|
||||
| `wheel_left` | u32 | Left encoder ticks |
|
||||
| `wheel_right` | u32 | Right encoder ticks |
|
||||
| `lidar_speed` | u16 | Lidar RPM (0 when disabled) |
|
||||
| `gyro_x/y/z` | i16 | Gyroscope (0.1°/s units) |
|
||||
| `tilt_x/y/z` | i16 | Accelerometer (mG units) |
|
||||
|
||||
**Command Examples:**
|
||||
|
||||
Enable drive motor:
|
||||
```python
|
||||
cmd = sangamio_pb2.Command()
|
||||
cmd.component_control.id = "drive"
|
||||
cmd.component_control.action.type = ComponentAction.ENABLE
|
||||
```
|
||||
|
||||
### Command Examples
|
||||
|
||||
**Enable drive motor:**
|
||||
```json
|
||||
{"type": "ComponentControl", "id": "drive", "action": {"type": "Enable"}}
|
||||
Set velocity:
|
||||
```python
|
||||
cmd.component_control.id = "drive"
|
||||
cmd.component_control.action.type = ComponentAction.CONFIGURE
|
||||
cmd.component_control.action.config["linear"].f32_val = 0.2
|
||||
cmd.component_control.action.config["angular"].f32_val = 0.0
|
||||
```
|
||||
|
||||
**Set velocity:**
|
||||
```json
|
||||
{"type": "ComponentControl", "id": "drive", "action": {"type": "Configure", "config": {"linear": {"F32": 0.5}, "angular": {"F32": 0.0}}}}
|
||||
### SLAM Mode (DhruvaSLAM)
|
||||
|
||||
Uses unified port architecture (default: 5557) for both streaming and commands.
|
||||
|
||||
**Protocol:** Length-prefixed Protobuf (see `dhruva-slam/proto/dhruva.proto`)
|
||||
|
||||
**DhruvaStream (Inbound):**
|
||||
| Message | Rate | Content |
|
||||
|---------|------|---------|
|
||||
| `robot_status` | 10Hz | Pose, state, battery, mapping progress |
|
||||
| `sensor_status` | 10Hz | Lidar, encoders, IMU |
|
||||
| `current_map` | 1Hz | Occupancy grid |
|
||||
| `map_list` | on change | Saved maps |
|
||||
| `navigation_status` | 5Hz | Path, goal, progress |
|
||||
|
||||
**DhruvaCommand (Outbound):**
|
||||
| Command | Description |
|
||||
|---------|-------------|
|
||||
| `StartMapping(map_name)` | Begin SLAM session |
|
||||
| `StopMapping(save)` | End session, optionally save |
|
||||
| `ClearMap()` | Discard current map |
|
||||
| `EnableMap(map_id)` | Load saved map |
|
||||
| `RenameMap(map_id, new_name)` | Rename saved map |
|
||||
| `DeleteMap(map_id)` | Delete saved map |
|
||||
| `SetGoal(x, y, theta)` | Set navigation goal |
|
||||
| `CancelGoal()` | Stop navigation |
|
||||
| `EmergencyStop()` | Halt all motion |
|
||||
|
||||
**Occupancy Grid Encoding:**
|
||||
| Value | Meaning | Display Color |
|
||||
|-------|---------|---------------|
|
||||
| 0 | Free space | Light gray |
|
||||
| 100 | Occupied | White |
|
||||
| 255 | Unknown | Dark gray |
|
||||
|
||||
## IMU Processing
|
||||
|
||||
The `imu_processor.py` implements a **Mahony AHRS filter** with quaternion-based orientation estimation.
|
||||
|
||||
**Calibration:**
|
||||
- Collects bias samples for ~3 seconds (330 samples @ 110Hz)
|
||||
- Computes average gyro bias to subtract from measurements
|
||||
- Returns zeros during calibration phase
|
||||
|
||||
**Filter Parameters:**
|
||||
- `KP = 5.0`: Proportional gain for fast settling
|
||||
- `KI = 0.1`: Integral gain for bias correction
|
||||
|
||||
**Output:** Roll, pitch, yaw in degrees (ROS REP-103 convention)
|
||||
|
||||
## Coordinate Conventions
|
||||
|
||||
All coordinates use **ROS REP-103** standard:
|
||||
|
||||
```
|
||||
+X (forward)
|
||||
↑
|
||||
|
|
||||
←------+------→ +Y (left)
|
||||
|
|
||||
+Z (up, out of page)
|
||||
|
||||
Angles: 0 rad = +X direction, increases counter-clockwise
|
||||
```
|
||||
|
||||
**Enable lidar:**
|
||||
```json
|
||||
{"type": "ComponentControl", "id": "lidar", "action": {"type": "Enable"}}
|
||||
- **Roll**: Rotation around X (left/right tilt)
|
||||
- **Pitch**: Rotation around Y (nose up/down)
|
||||
- **Yaw**: Rotation around Z (heading, CCW positive)
|
||||
|
||||
## Debug Utilities
|
||||
|
||||
### debug_proto.py
|
||||
|
||||
Capture and analyze DhruvaSLAM proto messages:
|
||||
|
||||
```bash
|
||||
python debug_proto.py
|
||||
```
|
||||
|
||||
**Set vacuum to 75%:**
|
||||
```json
|
||||
{"type": "ComponentControl", "id": "vacuum", "action": {"type": "Configure", "config": {"speed": {"U8": 75}}}}
|
||||
```
|
||||
- Connects to localhost:5557
|
||||
- Sends StartMapping command
|
||||
- Analyzes received proto messages
|
||||
- Saves raw bytes to `/tmp/proto_message.bin`
|
||||
- Useful for debugging protocol issues
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
### Sensor Mode
|
||||
|
||||
**No data received:**
|
||||
- Verify SangamIO is running on robot
|
||||
- Check network connectivity: `ping 192.168.68.101`
|
||||
- Verify port 5555 is accessible
|
||||
- Verify SangamIO is running: `ssh root@vacuum "ps aux | grep sangam"`
|
||||
- Check network: `ping 192.168.68.101`
|
||||
- Verify port 5555 is accessible (both TCP and UDP)
|
||||
- Check firewall allows UDP on port 5555
|
||||
|
||||
**Connection refused:**
|
||||
- Robot may not be running SangamIO
|
||||
- Check with: `ssh root@vacuum "ps aux | grep sangam"`
|
||||
- AuxCtrl may still be running (rename it first)
|
||||
|
||||
**Drive not responding:**
|
||||
- Make sure motor is enabled first (press `M`)
|
||||
- Check status bar for "Drive ENABLED"
|
||||
- Verify no error codes displayed
|
||||
|
||||
**3D view not rendering:**
|
||||
- Ensure PyOpenGL is installed
|
||||
- Check OpenGL drivers are up to date
|
||||
|
||||
### SLAM Mode
|
||||
|
||||
**No map data:**
|
||||
- Verify DhruvaSLAM is running: `ps aux | grep dhruva`
|
||||
- Check connection status in UI status bar
|
||||
- Start mapping session using control panel
|
||||
|
||||
**Parse errors:**
|
||||
- Failed proto messages saved to `/tmp/failed_proto.bin`
|
||||
- Use `debug_proto.py` to analyze protocol issues
|
||||
|
||||
**Navigation not working:**
|
||||
- Ensure a map is loaded (Enable Map)
|
||||
- Robot must be localized first
|
||||
- Check localization confidence indicator
|
||||
|
||||
**Map not updating:**
|
||||
- Verify mapping is active (state = "Mapping")
|
||||
- Check trajectory is being drawn
|
||||
- Large maps may have slower update rate
|
||||
|
||||
## UI Components
|
||||
|
||||
### Control Panel (Sensor Mode)
|
||||
|
||||
Collapsible sections:
|
||||
- **Motion Control**: Drive motor enable + speed slider
|
||||
- **Cleaning Systems**: Vacuum, main brush, side brush
|
||||
- **Sensors**: Lidar toggle, cliff calibration
|
||||
- **System**: LED control, dustbox status
|
||||
- **Calibration**: IMU, compass calibration
|
||||
- **Power Management**: Voltage displays
|
||||
|
||||
### SLAM Control Panel
|
||||
|
||||
- **Status Display**: State, active map, pose, confidence
|
||||
- **Mapping Controls**: Start/Stop/Clear/Emergency Stop
|
||||
- **Map Management**: Enable/Rename/Delete
|
||||
- **Map List**: Scrollable list with metadata
|
||||
|
||||
### Navigation Panel
|
||||
|
||||
- Current state (Idle, Planning, Navigating, Goal Reached)
|
||||
- Target description
|
||||
- Distance remaining + ETA
|
||||
- Progress bar
|
||||
- Waypoint counter
|
||||
- Cancel Navigation button
|
||||
|
||||
@@ -0,0 +1,188 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Debug script to capture and analyze DhruvaSLAM proto messages."""
|
||||
|
||||
import socket
|
||||
import struct
|
||||
import sys
|
||||
sys.path.insert(0, 'proto')
|
||||
from proto import dhruva_pb2
|
||||
|
||||
HOST = 'localhost'
|
||||
PORT = 5557
|
||||
|
||||
def main():
|
||||
print(f"Connecting to {HOST}:{PORT}...")
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
sock.settimeout(30.0)
|
||||
sock.connect((HOST, PORT))
|
||||
print("Connected!")
|
||||
|
||||
# Send StartMapping command to trigger data flow
|
||||
print("Sending StartMapping command...")
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = "debug-001"
|
||||
cmd.start_mapping.map_name = "DebugMap"
|
||||
payload = cmd.SerializeToString()
|
||||
length = struct.pack('>I', len(payload))
|
||||
sock.sendall(length + payload)
|
||||
print(f"Sent {len(payload)} byte command")
|
||||
|
||||
# Read response first
|
||||
print("Waiting for response...")
|
||||
try:
|
||||
resp_len_data = sock.recv(4)
|
||||
if resp_len_data:
|
||||
resp_len = struct.unpack('>I', resp_len_data)[0]
|
||||
resp_data = b''
|
||||
while len(resp_data) < resp_len:
|
||||
chunk = sock.recv(resp_len - len(resp_data))
|
||||
if not chunk:
|
||||
break
|
||||
resp_data += chunk
|
||||
resp = dhruva_pb2.DhruvaResponse()
|
||||
resp.ParseFromString(resp_data)
|
||||
print(f"Response: success={resp.success}, error={resp.error_message}")
|
||||
except Exception as e:
|
||||
print(f"Response read error: {e}")
|
||||
|
||||
print("Waiting for stream data...")
|
||||
|
||||
try:
|
||||
while True:
|
||||
# Read length prefix
|
||||
length_data = b''
|
||||
while len(length_data) < 4:
|
||||
chunk = sock.recv(4 - len(length_data))
|
||||
if not chunk:
|
||||
print("Connection closed")
|
||||
return
|
||||
length_data += chunk
|
||||
|
||||
length = struct.unpack('>I', length_data)[0]
|
||||
print(f"\nMessage length: {length} bytes")
|
||||
|
||||
if length > 10 * 1024 * 1024:
|
||||
print(f"ERROR: Message too large!")
|
||||
return
|
||||
|
||||
# Read message body
|
||||
data = b''
|
||||
while len(data) < length:
|
||||
chunk = sock.recv(min(65536, length - len(data)))
|
||||
if not chunk:
|
||||
print("Connection closed mid-message")
|
||||
return
|
||||
data += chunk
|
||||
|
||||
print(f"Received {len(data)} bytes")
|
||||
print(f"First 100 bytes: {data[:100].hex()}")
|
||||
print(f"Last 100 bytes: {data[-100:].hex()}")
|
||||
|
||||
# Save to file
|
||||
with open('/tmp/proto_message.bin', 'wb') as f:
|
||||
f.write(data)
|
||||
print("Saved to /tmp/proto_message.bin")
|
||||
|
||||
# Try to parse
|
||||
try:
|
||||
msg = dhruva_pb2.DhruvaStream()
|
||||
msg.ParseFromString(data)
|
||||
which = msg.WhichOneof('data')
|
||||
print(f"SUCCESS! Message type: {which}")
|
||||
if which == 'current_map':
|
||||
cm = msg.current_map
|
||||
print(f" map_id: {cm.map_id}")
|
||||
print(f" size: {cm.width}x{cm.height}")
|
||||
print(f" cells len: {len(cm.cells)}")
|
||||
except Exception as e:
|
||||
print(f"PARSE ERROR: {e}")
|
||||
|
||||
# Try to find where the error is
|
||||
print("\nAnalyzing message structure...")
|
||||
analyze_proto_bytes(data)
|
||||
|
||||
# Only process first message
|
||||
break
|
||||
|
||||
finally:
|
||||
sock.close()
|
||||
|
||||
def decode_varint(data, start):
|
||||
"""Decode a varint from data starting at start."""
|
||||
result = 0
|
||||
shift = 0
|
||||
pos = start
|
||||
while pos < len(data):
|
||||
byte = data[pos]
|
||||
result |= (byte & 0x7f) << shift
|
||||
pos += 1
|
||||
if not (byte & 0x80):
|
||||
return result, pos
|
||||
shift += 7
|
||||
return None, pos
|
||||
|
||||
def analyze_proto_bytes(data):
|
||||
"""Analyze proto wire format."""
|
||||
pos = 0
|
||||
|
||||
# Parse outer DhruvaStream
|
||||
print(f"[{pos}] First byte: {data[pos]:02x}")
|
||||
|
||||
# Field 1 = timestamp_us (varint)
|
||||
if data[pos] == 0x08:
|
||||
pos += 1
|
||||
ts, pos = decode_varint(data, pos)
|
||||
print(f" timestamp_us = {ts}")
|
||||
|
||||
# Field 12 = current_map (length-delimited)
|
||||
if pos < len(data) and data[pos] == 0x62:
|
||||
pos += 1
|
||||
cm_len, pos = decode_varint(data, pos)
|
||||
print(f" current_map length = {cm_len}")
|
||||
cm_start = pos
|
||||
cm_end = pos + cm_len
|
||||
|
||||
# Parse CurrentMap fields
|
||||
print(f"\n Parsing CurrentMap ({cm_len} bytes):")
|
||||
while pos < cm_end and pos < len(data):
|
||||
if pos >= len(data):
|
||||
break
|
||||
tag = data[pos]
|
||||
field = tag >> 3
|
||||
wire = tag & 7
|
||||
pos += 1
|
||||
|
||||
if wire == 0: # varint
|
||||
val, pos = decode_varint(data, pos)
|
||||
print(f" field {field}: varint = {val}")
|
||||
elif wire == 2: # length-delimited
|
||||
length, pos = decode_varint(data, pos)
|
||||
if pos + length <= len(data):
|
||||
val = data[pos:pos+length]
|
||||
if length < 50:
|
||||
try:
|
||||
print(f" field {field}: len={length}, str='{val.decode()}'")
|
||||
except:
|
||||
print(f" field {field}: len={length}, bytes={val[:20].hex()}...")
|
||||
else:
|
||||
print(f" field {field}: len={length}, bytes (large)")
|
||||
pos += length
|
||||
else:
|
||||
print(f" field {field}: len={length} TRUNCATED!")
|
||||
break
|
||||
elif wire == 5: # 32-bit
|
||||
if pos + 4 <= len(data):
|
||||
val = struct.unpack('<f', data[pos:pos+4])[0]
|
||||
print(f" field {field}: float = {val}")
|
||||
pos += 4
|
||||
else:
|
||||
print(f" field {field}: 32-bit TRUNCATED!")
|
||||
break
|
||||
else:
|
||||
print(f" field {field}: UNKNOWN wire type {wire}!")
|
||||
break
|
||||
|
||||
print(f"\n Parsed up to byte {pos} of {len(data)}")
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -130,13 +130,13 @@ def main():
|
||||
"--port",
|
||||
type=int,
|
||||
default=5557,
|
||||
help="DhruvaSLAM stream port (default: 5557)"
|
||||
help="DhruvaSLAM port for TCP commands+maps and UDP status (default: 5557)"
|
||||
)
|
||||
slam_parser.add_argument(
|
||||
"--command-port",
|
||||
type=int,
|
||||
default=5558,
|
||||
help="DhruvaSLAM command port (default: 5558)"
|
||||
default=None,
|
||||
help="DhruvaSLAM command port (default: same as --port)"
|
||||
)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
@@ -28,12 +28,13 @@ logger = logging.getLogger(__name__)
|
||||
class SlamMainWindow(QMainWindow):
|
||||
"""Main application window for SLAM mode."""
|
||||
|
||||
def __init__(self, slam_host="localhost", slam_port=5557, command_port=5558):
|
||||
def __init__(self, slam_host="localhost", slam_port=5557, command_port=None):
|
||||
super().__init__()
|
||||
|
||||
self.slam_host = slam_host
|
||||
self.slam_port = slam_port
|
||||
self.command_port = command_port
|
||||
# Command port defaults to same as slam_port (unified architecture)
|
||||
self.command_port = command_port if command_port is not None else slam_port
|
||||
self.slam_thread = None
|
||||
|
||||
# Initialize UI
|
||||
|
||||
@@ -1,16 +1,20 @@
|
||||
"""
|
||||
SLAM thread for receiving data from DhruvaSLAM daemon.
|
||||
|
||||
Uses the new DhruvaStream protocol (port 5557) for continuous data:
|
||||
Uses DhruvaStream protocol for continuous data (via TCP):
|
||||
- RobotStatus: pose, state, battery, mapping progress
|
||||
- SensorStatus: lidar, encoders, IMU
|
||||
- CurrentMap: occupancy grid
|
||||
- MapList: saved maps
|
||||
- NavigationStatus: path planning (future)
|
||||
- NavigationStatus: path planning
|
||||
|
||||
And DhruvaCommand/Response protocol (port 5558) for commands:
|
||||
And DhruvaCommand/Response protocol for commands (via same TCP port):
|
||||
- StartMapping, StopMapping, ClearMap
|
||||
- RenameMap, DeleteMap, EnableMap
|
||||
- RenameMap, DeleteMap, EnableMap, SetGoal, CancelGoal
|
||||
|
||||
Note: Both streams and commands use the same port (5557) following the
|
||||
unified DhruvaSLAM architecture. Two separate TCP connections are used:
|
||||
one for streaming data, one for command/response.
|
||||
"""
|
||||
|
||||
import socket
|
||||
@@ -70,11 +74,12 @@ class SlamThread(QThread):
|
||||
# Payload: {request_id, success, error_message, data: {...}}
|
||||
command_response_received = pyqtSignal(dict)
|
||||
|
||||
def __init__(self, host: str, port: int = 5557, command_port: int = 5558, parent=None):
|
||||
def __init__(self, host: str, port: int = 5557, command_port: int = None, parent=None):
|
||||
super().__init__(parent)
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.command_port = command_port
|
||||
# Command port defaults to same as stream port (unified architecture)
|
||||
self.command_port = command_port if command_port is not None else port
|
||||
self._running = True
|
||||
self.stream_socket = None
|
||||
self.command_socket = None
|
||||
@@ -163,7 +168,11 @@ class SlamThread(QThread):
|
||||
msg.ParseFromString(data)
|
||||
return msg
|
||||
except Exception as e:
|
||||
# Save problematic data for debugging
|
||||
with open('/tmp/failed_proto.bin', 'wb') as f:
|
||||
f.write(data)
|
||||
logger.error(f"Parse error: {e}, data len={len(data)}, first 50 bytes: {data[:50].hex()}")
|
||||
logger.error(f"Saved failed message to /tmp/failed_proto.bin")
|
||||
raise
|
||||
|
||||
def _recv_exact(self, sock: socket.socket, n: int) -> bytes:
|
||||
|
||||
Reference in New Issue
Block a user