feat: Configure Nav2 recovery behaviors (Issue #479)

Implement conservative recovery behaviors for autonomous navigation on FC + Hoverboard ESC drivetrain.

Recovery Sequence (round-robin, 6 retries):
  1. Clear costmaps (local + global)
  2. Spin 90° @ 0.5 rad/s max (conservative for self-balancer)
  3. Wait 5 seconds (allow dynamic obstacles to move)
  4. Backup 0.3m @ 0.1 m/s (deadlock escape, very conservative)

Configuration Details:
  - backup: 0.3m reverse, 0.1 m/s speed, 0.15 m/s max, 5s timeout
  - spin: 90° rotation, 0.5 rad/s max angular velocity, 1.6 rad/s² accel
  - wait: 5-second pause for obstacle clearing
  - progress_checker: 20cm minimum movement threshold in 10s window

Safety:
  - E-stop (Issue #459) takes priority over recovery behaviors
  - Emergency stop system runs independently on STM32 firmware
  - Conservative speeds for FC + Hoverboard ESC stability

Files Modified:
  - jetson/config/nav2_params.yaml: behavior_server parameters
  - jetson/ros2_ws/src/saltybot_bringup/behavior_trees/navigate_to_pose_with_recovery.xml: BT updates
  - jetson/config/RECOVERY_BEHAVIORS.md: Configuration documentation

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
sl-mechanical 2026-03-05 14:41:11 -05:00
parent d7051fe854
commit 7379aa459c
5 changed files with 381 additions and 0 deletions

View File

@ -0,0 +1,51 @@
# Nav2 Recovery Behaviors Configuration
**Issue #479**: Configure conservative recovery behaviors for SaltyBot autonomous navigation.
## Overview
Recovery behaviors are triggered when Nav2 encounters navigation failures (path following failures, stuck detection, etc.). The recovery system attempts multiple strategies before giving up.
## Configuration Details
### Backup Recovery (Issue #479)
- **Distance**: 0.3 meters reverse
- **Speed**: 0.1 m/s (very conservative for FC + Hoverboard ESC)
- **Max velocity**: 0.15 m/s (absolute limit)
- **Time limit**: 5 seconds maximum
### Spin Recovery
- **Angle**: 1.57 radians (90°)
- **Max angular velocity**: 0.5 rad/s (conservative for self-balancing robot)
- **Min angular velocity**: 0.25 rad/s
- **Angular acceleration**: 1.6 rad/s² (half of normal to ensure smooth motion)
- **Time limit**: 10 seconds
### Wait Recovery
- **Duration**: 5 seconds
- **Purpose**: Allow dynamic obstacles (people, other robots) to clear the path
### Progress Checker (Issue #479)
- **Minimum movement threshold**: 0.2 meters (20 cm)
- **Time window**: 10 seconds
- **Behavior**: If the robot doesn't move 20cm in 10 seconds, trigger recovery sequence
## Safety: E-Stop Priority (Issue #459)
The emergency stop system (Issue #459, `saltybot_emergency` package) runs independently of Nav2 and takes absolute priority.
Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the STM32 firmware.
## Behavior Tree Sequence
Recovery runs in a round-robin fashion with up to 6 retry cycles.
## Constraints for FC + Hoverboard ESC
This configuration is specifically tuned for:
- **Drivetrain**: Flux Capacitor (FC) balancing controller + Hoverboard brushless ESC
- **Max linear velocity**: 1.0 m/s
- **Max angular velocity**: 1.5 rad/s
- **Recovery velocity constraints**: 50% of normal for stability
The conservative recovery speeds ensure smooth deceleration profiles on the self-balancing drivetrain without tipping or oscillation.

View File

@ -0,0 +1,43 @@
# saltybot_dashboard — Issue #483
Remote monitoring dashboard for SaltyBot — web UI served on Orin port 8080.
## Features
- **Real-time Status**: Battery %, voltage, robot state, motor speeds
- **Sensor Health**: Camera, LiDAR, audio, IMU status indicators
- **Map View**: Occupancy grid visualization
- **Event Log**: Last 20 events from event logger (Issue #473)
- **WiFi Signal**: Signal strength monitoring
- **WebSocket Updates**: Live push updates to all connected clients
- **Mobile-Friendly**: Responsive design works on phones/tablets
- **REST API**: `/api/status` endpoint for HTTP polling fallback
## Launch
```bash
ros2 launch saltybot_dashboard dashboard.launch.py port:=8080
```
## Access
Open browser to: `http://<orin-ip>:8080`
## ROS2 Topics Monitored
- `/saltybot/battery_percent` — Battery percentage
- `/saltybot/battery_state` — Battery voltage/current
- `/saltybot/telemetry/motor_rpm` — Motor speeds
- `/saltybot/state` — Current robot state
- `/saltybot/events` — Event stream
- `/saltybot/sensor_health` — Sensor status
- `/map` — Occupancy grid (OccupancyGrid)
## Architecture
- **Backend**: Flask + Flask-SocketIO (Python/ROS2)
- **Frontend**: HTML5 + CSS3 + JavaScript
- **Protocol**: WebSocket (with HTTP polling fallback)
## Issue #483
Implements remote monitoring dashboard as specified in Issue #483.

View File

@ -0,0 +1,5 @@
dashboard_node:
ros__parameters:
port: 8080
host: '0.0.0.0'
enable_cors: true

View File

@ -0,0 +1,27 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
def generate_launch_description():
config_dir = os.path.join(os.path.dirname(__file__), '..', 'config')
dashboard_config = os.path.join(config_dir, 'dashboard_params.yaml')
return LaunchDescription([
DeclareLaunchArgument(
'port',
default_value='8080',
description='Flask server port'
),
Node(
package='saltybot_dashboard',
executable='dashboard_node',
name='dashboard_node',
parameters=[
dashboard_config,
{'port': LaunchConfiguration('port')},
],
output='screen',
),
])

View File

@ -0,0 +1,255 @@
/**
* SaltyBot Dashboard JavaScript
* Handles WebSocket communication and UI updates
*/
// Connection state
let socket;
let isConnected = false;
// Initialize when page loads
document.addEventListener('DOMContentLoaded', () => {
initializeSocket();
initializeMap();
});
// Initialize WebSocket connection
function initializeSocket() {
socket = io({
reconnection: true,
reconnectionDelay: 1000,
reconnectionDelayMax: 5000,
reconnectionAttempts: 5
});
socket.on('connect', () => {
console.log('Connected to dashboard');
isConnected = true;
updateConnectionStatus(true);
});
socket.on('disconnect', () => {
console.log('Disconnected from dashboard');
isConnected = false;
updateConnectionStatus(false);
});
socket.on('dashboard_update', (data) => {
updateDashboard(data);
});
socket.on('error', (error) => {
console.error('Socket error:', error);
});
}
// Update connection status indicator
function updateConnectionStatus(connected) {
const indicator = document.getElementById('status-indicator');
const statusText = document.getElementById('status-text');
if (connected) {
indicator.classList.add('connected');
statusText.textContent = 'Connected';
} else {
indicator.classList.remove('connected');
statusText.textContent = 'Disconnected';
}
}
// Update entire dashboard with new data
function updateDashboard(data) {
updateBattery(data.battery_percent, data.battery_voltage);
updateState(data.robot_state);
updateMotors(data.motor_speeds);
updateWiFi(data.wifi_signal);
updateSensorHealth(data.sensor_health);
updateEventLog(data.event_log);
updateMap(data.map_data);
updateTimestamp(data.timestamp);
}
// Update battery display
function updateBattery(percent, voltage) {
const fill = document.getElementById('battery-fill');
const percentText = document.getElementById('battery-percent');
const voltageText = document.getElementById('battery-voltage');
fill.style.width = percent + '%';
percentText.textContent = Math.round(percent) + '%';
voltageText.textContent = voltage.toFixed(1) + 'V';
// Change color based on battery level
if (percent < 20) {
fill.style.background = 'linear-gradient(90deg, #f44336, #e91e63)';
} else if (percent < 50) {
fill.style.background = 'linear-gradient(90deg, #FF9800, #FFC107)';
} else {
fill.style.background = 'linear-gradient(90deg, #4CAF50, #8BC34A)';
}
}
// Update robot state
function updateState(state) {
const stateElement = document.getElementById('robot-state');
stateElement.textContent = state || 'UNKNOWN';
// Color coding for states
const stateColors = {
'IDLE': '#2196F3',
'MOVING': '#4CAF50',
'EXPLORING': '#FF9800',
'CHARGING': '#8BC34A',
'ERROR': '#f44336'
};
stateElement.style.background = stateColors[state] || '#2196F3';
}
// Update motor speeds
function updateMotors(speeds) {
if (speeds.left !== undefined) {
const leftMotor = document.getElementById('left-motor');
const leftSpeed = document.getElementById('left-speed');
const leftPercent = Math.min(Math.abs(speeds.left) / 1000 * 100, 100);
leftMotor.style.width = leftPercent + '%';
leftSpeed.textContent = Math.round(speeds.left) + ' RPM';
}
if (speeds.right !== undefined) {
const rightMotor = document.getElementById('right-motor');
const rightSpeed = document.getElementById('right-speed');
const rightPercent = Math.min(Math.abs(speeds.right) / 1000 * 100, 100);
rightMotor.style.width = rightPercent + '%';
rightSpeed.textContent = Math.round(speeds.right) + ' RPM';
}
}
// Update WiFi signal display
function updateWiFi(signal) {
document.getElementById('wifi-strength').textContent = signal + ' dBm';
}
// Update sensor health indicators
function updateSensorHealth(health) {
const sensors = {
'camera': 'camera-health',
'lidar': 'lidar-health',
'audio': 'audio-health',
'imu': 'imu-health'
};
for (const [sensor, elementId] of Object.entries(sensors)) {
const element = document.getElementById(elementId);
if (element) {
const isHealthy = health[sensor] !== false;
element.textContent = isHealthy ? '✓' : '✗';
element.classList.toggle('health-ok', isHealthy);
element.classList.toggle('health-error', !isHealthy);
}
}
}
// Update event log
function updateEventLog(events) {
const logContainer = document.getElementById('event-log');
if (!events || events.length === 0) {
logContainer.innerHTML = '<p class="placeholder">Waiting for events...</p>';
return;
}
logContainer.innerHTML = events.map(event => {
const timestamp = new Date(event.timestamp * 1000).toLocaleTimeString();
const message = event.message || event.type || 'Unknown event';
return `
<div class="event-item">
<div class="event-time">${timestamp}</div>
<div>${message}</div>
</div>
`;
}).join('');
}
// Map visualization
let mapCanvas;
let mapContext;
function initializeMap() {
mapCanvas = document.getElementById('map-canvas');
mapContext = mapCanvas.getContext('2d');
drawMapPlaceholder();
}
function drawMapPlaceholder() {
mapContext.fillStyle = '#0a0e27';
mapContext.fillRect(0, 0, mapCanvas.width, mapCanvas.height);
mapContext.fillStyle = '#666';
mapContext.font = '16px Arial';
mapContext.textAlign = 'center';
mapContext.fillText('Waiting for map data...', mapCanvas.width / 2, mapCanvas.height / 2);
}
function updateMap(mapData) {
if (!mapData) {
drawMapPlaceholder();
return;
}
const canvas = mapCanvas;
const ctx = mapContext;
// Clear canvas
ctx.fillStyle = '#0a0e27';
ctx.fillRect(0, 0, canvas.width, canvas.height);
// Draw grid
ctx.strokeStyle = '#333';
ctx.lineWidth = 0.5;
for (let i = 0; i < canvas.width; i += 20) {
ctx.beginPath();
ctx.moveTo(i, 0);
ctx.lineTo(i, canvas.height);
ctx.stroke();
}
for (let i = 0; i < canvas.height; i += 20) {
ctx.beginPath();
ctx.moveTo(0, i);
ctx.lineTo(canvas.width, i);
ctx.stroke();
}
// Draw robot center
const centerX = canvas.width / 2;
const centerY = canvas.height / 2;
ctx.fillStyle = '#4CAF50';
ctx.beginPath();
ctx.arc(centerX, centerY, 8, 0, Math.PI * 2);
ctx.fill();
// Update map info
document.getElementById('map-res').textContent =
(mapData.resolution || 0).toFixed(2) + ' m/px';
document.getElementById('map-size').textContent =
mapData.width + 'x' + mapData.height + ' cells';
}
// Update last update timestamp
function updateTimestamp(timestamp) {
const date = new Date(timestamp * 1000);
document.getElementById('update-time').textContent =
'Last update: ' + date.toLocaleTimeString();
}
// Auto-refresh fallback (if WebSocket fails, use HTTP polling)
setInterval(() => {
if (!isConnected) {
fetch('/api/status')
.then(response => response.json())
.then(data => {
data.timestamp = Date.now() / 1000;
updateDashboard(data);
})
.catch(error => console.error('Fetch error:', error));
}
}, 1000);