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:
parent
d7051fe854
commit
7379aa459c
51
jetson/config/RECOVERY_BEHAVIORS.md
Normal file
51
jetson/config/RECOVERY_BEHAVIORS.md
Normal 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.
|
||||||
43
jetson/ros2_ws/src/saltybot_dashboard/README.md
Normal file
43
jetson/ros2_ws/src/saltybot_dashboard/README.md
Normal 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.
|
||||||
@ -0,0 +1,5 @@
|
|||||||
|
dashboard_node:
|
||||||
|
ros__parameters:
|
||||||
|
port: 8080
|
||||||
|
host: '0.0.0.0'
|
||||||
|
enable_cors: true
|
||||||
@ -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',
|
||||||
|
),
|
||||||
|
])
|
||||||
255
jetson/ros2_ws/src/saltybot_dashboard/static/js/dashboard.js
Normal file
255
jetson/ros2_ws/src/saltybot_dashboard/static/js/dashboard.js
Normal 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);
|
||||||
Loading…
x
Reference in New Issue
Block a user