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: - backup: 0.3m reverse, 0.1 m/s speed, 5s timeout - spin: 90° rotation, 0.5 rad/s max angular velocity - 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
fd742f6890
100
jetson/config/RECOVERY_BEHAVIORS.md
Normal file
100
jetson/config/RECOVERY_BEHAVIORS.md
Normal file
@ -0,0 +1,100 @@
|
||||
# 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:
|
||||
|
||||
1. **Clear costmaps** — Reset perception obstacles
|
||||
2. **Spin recovery** — In-place 90° rotation to detect surroundings
|
||||
3. **Wait recovery** — 5-second pause for dynamic obstacles to move
|
||||
4. **Backup recovery** — Reverse 0.3m to escape deadlocks
|
||||
|
||||
## 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:
|
||||
|
||||
- **Trigger**: Obstacles within 0.3m (configurable)
|
||||
- **Action**: Immediate zero velocity command, overrides all recovery behaviors
|
||||
- **Integration**: Publishes directly to `/cmd_vel`, pre-empting Nav2 output
|
||||
|
||||
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:
|
||||
|
||||
```
|
||||
NavigateRecovery (6 retries)
|
||||
├── Normal navigation (compute path → follow path)
|
||||
└── Recovery fallback:
|
||||
├── Clear costmaps
|
||||
├── Spin 90°
|
||||
├── Wait 5s
|
||||
└── Back up 0.3m @ 0.1 m/s
|
||||
```
|
||||
|
||||
Each cycle, the next recovery action is attempted (round-robin). If navigation succeeds, recovery stops. If all retries exhaust, goal fails.
|
||||
|
||||
## 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.
|
||||
|
||||
## Configuration Files
|
||||
|
||||
- **nav2_params.yaml**: behavior_server section with spin, backup, wait parameters
|
||||
- **navigate_to_pose_with_recovery.xml**: Behavior tree defining recovery sequence
|
||||
- **emergency.launch.py**: E-stop system (independent, higher priority)
|
||||
|
||||
## Testing
|
||||
|
||||
To test recovery behaviors:
|
||||
|
||||
```bash
|
||||
# Run Nav2 with recovery enabled
|
||||
ros2 launch saltybot_bringup nav2.launch.py
|
||||
|
||||
# Trigger recovery by creating obstacles in the path
|
||||
# or setting unreachable goals
|
||||
|
||||
# Monitor recovery with:
|
||||
ros2 topic echo /cmd_vel # Check velocity commands during recovery
|
||||
ros2 topic echo /diagnostics # Monitor Nav2 status
|
||||
```
|
||||
|
||||
## References
|
||||
|
||||
- Issue #479: Nav2 recovery behaviors
|
||||
- Issue #459: Emergency stop cascade (E-stop takes priority)
|
||||
- Nav2 recovery behavior documentation: https://docs.nav2.org/
|
||||
@ -103,8 +103,8 @@ controller_server:
|
||||
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
required_movement_radius: 0.2 # Issue #479: Minimum 20cm movement to detect progress
|
||||
movement_time_allowance: 10.0 # 10 second window to detect movement, then attempt recovery
|
||||
|
||||
general_goal_checker:
|
||||
stateful: true
|
||||
@ -180,6 +180,8 @@ smoother_server:
|
||||
do_refinement: true
|
||||
|
||||
# ── Behavior Server (recovery behaviors) ────────────────────────────────────
|
||||
# Issue #479: Recovery behaviors with conservative speed/distance limits for FC+Hoverboard
|
||||
# ESC priority: E-stop (Issue #459) takes priority over any recovery behavior.
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
@ -188,25 +190,40 @@ behavior_server:
|
||||
local_footprint_topic: local_costmap/published_footprint
|
||||
global_footprint_topic: global_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
behavior_plugins: ["spin", "backup", "wait"]
|
||||
|
||||
# Spin recovery: In-place 90° rotation to escape local deadlock
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
spin_dist: 1.5708 # 90 degrees (π/2 radians)
|
||||
max_rotational_vel: 0.5 # Conservative: 0.5 rad/s for self-balancer stability
|
||||
min_rotational_vel: 0.25 # Minimum angular velocity
|
||||
rotational_acc_lim: 1.6 # Half of max (3.2) for smooth accel on self-balancer
|
||||
time_allowance: 10.0 # max 10 seconds to complete 90° rotation
|
||||
|
||||
# Backup recovery: Reverse 0.3m at 0.1 m/s to escape obstacles
|
||||
# Conservative for FC (Flux Capacitor) + Hoverboard ESC drivetrain
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
backup_dist: 0.3 # Reverse 0.3 meters (safe distance for deadlock escape)
|
||||
backup_speed: 0.1 # Very conservative: 0.1 m/s reverse
|
||||
max_backup_vel: 0.15 # Absolute max reverse velocity
|
||||
min_backup_vel: 0.05 # Minimum backup velocity threshold
|
||||
time_allowance: 5.0 # max 5 seconds (0.3m at 0.1m/s = 3s normal)
|
||||
|
||||
# Wait recovery: Pause 5 seconds to let obstacles move away
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
wait_duration: 5.0 # 5 second pause
|
||||
|
||||
local_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
max_rotational_vel: 0.5 # Conservative: self-balancer stability limit
|
||||
min_rotational_vel: 0.25
|
||||
rotational_acc_lim: 1.6 # Half of main limit for recovery behaviors
|
||||
|
||||
# ── Waypoint Follower ────────────────────────────────────────────────────────
|
||||
waypoint_follower:
|
||||
|
||||
@ -0,0 +1,457 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
autonomous_coordinator.xml — SaltyBot Autonomous Mode Behavior Tree (Issue #482)
|
||||
|
||||
Main state machine for autonomous operation:
|
||||
idle → patrol → investigate → interact → return
|
||||
|
||||
Blackboard Variables:
|
||||
- battery_level (float, 0-100%)
|
||||
- person_detected (bool)
|
||||
- obstacle_state (string: clear/near/blocked)
|
||||
- current_mode (string: idle/patrol/investigate/interact/return)
|
||||
- home_pose (geometry_msgs/PoseStamped)
|
||||
|
||||
Integrations:
|
||||
- Patrol mode (#446)
|
||||
- Curiosity behavior (#470)
|
||||
- Person following
|
||||
- Emergency stop cascade (#459)
|
||||
- Geofence constraints (#441)
|
||||
|
||||
Fallback: Return to home pose on failure
|
||||
-->
|
||||
<root BTCPP_format="4">
|
||||
|
||||
<!-- ════════════════════════════════════════════════════════════════════════════ -->
|
||||
<!-- MAIN AUTONOMOUS COORDINATOR STATE MACHINE -->
|
||||
<!-- ════════════════════════════════════════════════════════════════════════════ -->
|
||||
<BehaviorTree ID="AutonomousCoordinator">
|
||||
|
||||
<ReactiveFallback name="RootFallback">
|
||||
<!-- Emergency stop override — highest priority -->
|
||||
<Inverter>
|
||||
<IsEmergencyStopped/>
|
||||
</Inverter>
|
||||
|
||||
<!-- Main autonomous state machine -->
|
||||
<Sequence name="AutonomousSequence">
|
||||
<!-- Initialize blackboard and verify readiness -->
|
||||
<InitializeAutonomousMode/>
|
||||
|
||||
<!-- Main state machine loop -->
|
||||
<KeepRunningUntilFailure>
|
||||
<StateMachine initial_state="idle">
|
||||
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<!-- STATE: IDLE -->
|
||||
<!-- Wait for activation signal or battery recovery -->
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<State ID="idle">
|
||||
<Sequence name="IdleSequence">
|
||||
<SetBlackboardVariable var_name="current_mode" var_value="idle"/>
|
||||
<StopRobot/>
|
||||
|
||||
<!-- Wait for activation (or battery recovery) -->
|
||||
<Parallel success_threshold="1" failure_threshold="2">
|
||||
<WaitForActivation timeout_ms="60000"/>
|
||||
<Sequence>
|
||||
<CheckBatteryLevel threshold="50"/>
|
||||
<Wait wait_duration="2"/>
|
||||
</Sequence>
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
<OnSuccess next_state="patrol"/>
|
||||
</State>
|
||||
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<!-- STATE: PATROL -->
|
||||
<!-- Autonomous patrolling with geofence boundaries (#446) -->
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<State ID="patrol">
|
||||
<Sequence name="PatrolSequence">
|
||||
<SetBlackboardVariable var_name="current_mode" var_value="patrol"/>
|
||||
|
||||
<!-- Check battery before patrol -->
|
||||
<Sequence name="BatteryCheck">
|
||||
<Inverter>
|
||||
<CheckBatteryLow threshold="20"/>
|
||||
</Inverter>
|
||||
</Sequence>
|
||||
|
||||
<!-- Patrol loop with fallbacks -->
|
||||
<KeepRunningUntilFailure>
|
||||
<Fallback name="PatrolWithCuriosityAndDetection">
|
||||
|
||||
<!-- Priority 1: Person detected → investigate -->
|
||||
<Sequence name="PersonDetectionCheck">
|
||||
<CheckPersonDetected/>
|
||||
<SetBlackboardVariable var_name="person_detected" var_value="true"/>
|
||||
</Sequence>
|
||||
|
||||
<!-- Priority 2: Curiosity-driven exploration (#470) -->
|
||||
<Parallel success_threshold="1" failure_threshold="1">
|
||||
<CuriosityExploration max_distance="2.0"/>
|
||||
<Sequence name="ObstacleMonitoring">
|
||||
<Wait wait_duration="1"/>
|
||||
<CheckGeofenceBoundary/>
|
||||
</Sequence>
|
||||
</Parallel>
|
||||
|
||||
<!-- Priority 3: Standard patrol route -->
|
||||
<PatrolRoute geofence_constraint="true"/>
|
||||
|
||||
</Fallback>
|
||||
</KeepRunningUntilFailure>
|
||||
</Sequence>
|
||||
<OnSuccess next_state="idle"/>
|
||||
<OnFailure next_state="investigate"/>
|
||||
</State>
|
||||
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<!-- STATE: INVESTIGATE -->
|
||||
<!-- Investigate detected persons or anomalies -->
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<State ID="investigate">
|
||||
<Sequence name="InvestigateSequence">
|
||||
<SetBlackboardVariable var_name="current_mode" var_value="investigate"/>
|
||||
|
||||
<!-- Approach and track detected person -->
|
||||
<Fallback name="InvestigationBehavior">
|
||||
|
||||
<!-- Primary: Follow detected person if still visible -->
|
||||
<Sequence name="PersonFollowing">
|
||||
<CheckPersonDetected/>
|
||||
<PersonFollower follow_distance="1.5"/>
|
||||
</Sequence>
|
||||
|
||||
<!-- Fallback: Navigate to last known position -->
|
||||
<Sequence name="NavigateToLastSeen">
|
||||
<GetLastSeenPose pose="{investigation_target}"/>
|
||||
<NavigateToPose goal="{investigation_target}"/>
|
||||
</Sequence>
|
||||
|
||||
</Fallback>
|
||||
</Sequence>
|
||||
<OnSuccess next_state="interact"/>
|
||||
<OnFailure next_state="return"/>
|
||||
</State>
|
||||
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<!-- STATE: INTERACT -->
|
||||
<!-- Social interaction: face recognition, gestures, speech -->
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<State ID="interact">
|
||||
<Sequence name="InteractionSequence">
|
||||
<SetBlackboardVariable var_name="current_mode" var_value="interact"/>
|
||||
|
||||
<!-- Face recognition and greeting -->
|
||||
<Parallel success_threshold="1" failure_threshold="2">
|
||||
|
||||
<!-- Facial recognition and enrollment -->
|
||||
<Sequence name="FaceRecognition">
|
||||
<RecognizeFace/>
|
||||
<PublishGreeting/>
|
||||
</Sequence>
|
||||
|
||||
<!-- Gesture detection and response -->
|
||||
<Sequence name="GestureResponse">
|
||||
<DetectGesture/>
|
||||
<RespondToGesture/>
|
||||
</Sequence>
|
||||
|
||||
<!-- Timeout to prevent stuck interactions -->
|
||||
<Wait wait_duration="30"/>
|
||||
|
||||
</Parallel>
|
||||
|
||||
<!-- Optional: Offer assistance or just say goodbye -->
|
||||
<Fallback name="InteractionExit">
|
||||
<OfferAssistance/>
|
||||
<PublishFarewell/>
|
||||
</Fallback>
|
||||
|
||||
</Sequence>
|
||||
<OnSuccess next_state="return"/>
|
||||
<OnFailure next_state="return"/>
|
||||
</State>
|
||||
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<!-- STATE: RETURN -->
|
||||
<!-- Return to home pose for docking or recharge -->
|
||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
||||
<State ID="return">
|
||||
<Sequence name="ReturnSequence">
|
||||
<SetBlackboardVariable var_name="current_mode" var_value="return"/>
|
||||
|
||||
<!-- Navigate home with recovery behaviors -->
|
||||
<RecoveryNode number_of_retries="3" name="ReturnHomeRecovery">
|
||||
<NavigateToPose goal="{home_pose}"/>
|
||||
|
||||
<!-- Recovery: Clear costmaps and retry -->
|
||||
<Sequence name="ReturnRecovery">
|
||||
<ClearEntireCostmap
|
||||
name="ClearLocalCostmap"
|
||||
service_name="local_costmap/clear_entirely_global_costmap"/>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="2"/>
|
||||
</Sequence>
|
||||
</RecoveryNode>
|
||||
|
||||
<!-- Arrive home and stop -->
|
||||
<StopRobot/>
|
||||
<SetBlackboardVariable var_name="current_mode" var_value="idle"/>
|
||||
|
||||
</Sequence>
|
||||
<OnSuccess next_state="idle"/>
|
||||
<OnFailure next_state="idle"/>
|
||||
</State>
|
||||
|
||||
</StateMachine>
|
||||
</KeepRunningUntilFailure>
|
||||
|
||||
</Sequence>
|
||||
|
||||
</ReactiveFallback>
|
||||
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- ════════════════════════════════════════════════════════════════════════════ -->
|
||||
<!-- CUSTOM ACTION NODES (Python/C++ implementations) -->
|
||||
<!-- ════════════════════════════════════════════════════════════════════════════ -->
|
||||
|
||||
<!-- Emergency Stop Check: Monitor /saltybot/emergency_stop topic -->
|
||||
<BehaviorTree ID="IsEmergencyStopped">
|
||||
<Root>
|
||||
<CheckTopicValue topic="/saltybot/emergency_stop" variable="data" expected="true"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Initialize Autonomous Mode -->
|
||||
<BehaviorTree ID="InitializeAutonomousMode">
|
||||
<Root>
|
||||
<Sequence>
|
||||
<!-- Set home pose (from current position or param server) -->
|
||||
<GetHomepose pose="{home_pose}"/>
|
||||
<!-- Initialize battery monitor -->
|
||||
<StartBatteryMonitoring/>
|
||||
<!-- Reset person detection -->
|
||||
<SetBlackboardVariable var_name="person_detected" var_value="false"/>
|
||||
</Sequence>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Battery Check: Ensure minimum battery level -->
|
||||
<BehaviorTree ID="CheckBatteryLevel">
|
||||
<Root>
|
||||
<Parallel success_threshold="2" failure_threshold="1">
|
||||
<GetBatteryLevel battery="{battery_level}"/>
|
||||
<Sequence>
|
||||
<CheckCondition test="{battery_level} >= {threshold}"/>
|
||||
</Sequence>
|
||||
</Parallel>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Low Battery Alert -->
|
||||
<BehaviorTree ID="CheckBatteryLow">
|
||||
<Root>
|
||||
<Sequence>
|
||||
<GetBatteryLevel battery="{battery_level}"/>
|
||||
<CheckCondition test="{battery_level} < {threshold}"/>
|
||||
</Sequence>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Curiosity-Driven Exploration: Autonomous discovery with obstacle avoidance -->
|
||||
<BehaviorTree ID="CuriosityExploration">
|
||||
<Root>
|
||||
<Parallel success_threshold="1" failure_threshold="1">
|
||||
<!-- Generate exploration goal -->
|
||||
<Sequence>
|
||||
<GenerateExplorationGoal max_distance="{max_distance}" goal="{exploration_goal}"/>
|
||||
<NavigateToPose goal="{exploration_goal}"/>
|
||||
</Sequence>
|
||||
<!-- Safety monitor: Check for obstacles -->
|
||||
<Inverter>
|
||||
<Sequence>
|
||||
<Wait wait_duration="0.5"/>
|
||||
<CheckObstacle threshold="0.3"/>
|
||||
</Sequence>
|
||||
</Inverter>
|
||||
</Parallel>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Geofence Boundary Check: Ensure patrol stays within boundaries -->
|
||||
<BehaviorTree ID="CheckGeofenceBoundary">
|
||||
<Root>
|
||||
<CheckGeofence position="{current_pose}" inside="true"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Standard Patrol Route: Execute predefined waypoints -->
|
||||
<BehaviorTree ID="PatrolRoute">
|
||||
<Root>
|
||||
<Sequence>
|
||||
<GetNextWaypoint waypoint="{next_waypoint}" geofence_constraint="{geofence_constraint}"/>
|
||||
<NavigateToPose goal="{next_waypoint}"/>
|
||||
</Sequence>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Person Detection Check -->
|
||||
<BehaviorTree ID="CheckPersonDetected">
|
||||
<Root>
|
||||
<Sequence>
|
||||
<GetPersonDetection detected="{person_detected}"/>
|
||||
<CheckCondition test="{person_detected} == true"/>
|
||||
</Sequence>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Robot Stop Action -->
|
||||
<BehaviorTree ID="StopRobot">
|
||||
<Root>
|
||||
<PublishVelocity linear_x="0.0" linear_y="0.0" angular_z="0.0"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Wait for Activation Signal -->
|
||||
<BehaviorTree ID="WaitForActivation">
|
||||
<Root>
|
||||
<WaitForTopic topic="/saltybot/autonomous_mode/activate" timeout="{timeout_ms}"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Get Last Seen Pose -->
|
||||
<BehaviorTree ID="GetLastSeenPose">
|
||||
<Root>
|
||||
<GetBlackboardVariable var_name="last_person_pose" output="{pose}"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Face Recognition and Greeting -->
|
||||
<BehaviorTree ID="RecognizeFace">
|
||||
<Root>
|
||||
<ServiceCall service="/social/recognize_face" response="person_id"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Publish Greeting Message -->
|
||||
<BehaviorTree ID="PublishGreeting">
|
||||
<Root>
|
||||
<PublishMessage topic="/saltybot/greeting" message="Hello, friend!"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Detect Gesture -->
|
||||
<BehaviorTree ID="DetectGesture">
|
||||
<Root>
|
||||
<ServiceCall service="/gesture/detect_gesture" response="gesture_type"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Respond to Gesture -->
|
||||
<BehaviorTree ID="RespondToGesture">
|
||||
<Root>
|
||||
<Sequence>
|
||||
<CheckCondition test="{gesture_type} != null"/>
|
||||
<PublishMessage topic="/saltybot/gesture_response" message="I see you!"/>
|
||||
</Sequence>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Offer Assistance -->
|
||||
<BehaviorTree ID="OfferAssistance">
|
||||
<Root>
|
||||
<PublishMessage topic="/saltybot/social/query" message="Can I help you?"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Publish Farewell -->
|
||||
<BehaviorTree ID="PublishFarewell">
|
||||
<Root>
|
||||
<PublishMessage topic="/saltybot/farewell" message="Goodbye!"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Get Home Pose -->
|
||||
<BehaviorTree ID="GetHomepose">
|
||||
<Root>
|
||||
<Sequence>
|
||||
<!-- Try to get from parameter server first -->
|
||||
<Fallback>
|
||||
<ServiceCall service="/saltybot/get_home_pose" response="pose"/>
|
||||
<!-- Fallback: Use current position as home -->
|
||||
<GetCurrentPose pose="{pose}"/>
|
||||
</Fallback>
|
||||
</Sequence>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Start Battery Monitoring -->
|
||||
<BehaviorTree ID="StartBatteryMonitoring">
|
||||
<Root>
|
||||
<ServiceCall service="/battery/start_monitoring"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Get Battery Level -->
|
||||
<BehaviorTree ID="GetBatteryLevel">
|
||||
<Root>
|
||||
<GetTopicValue topic="/battery_state" field="percentage" output="{battery}"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Generate Exploration Goal (Curiosity) -->
|
||||
<BehaviorTree ID="GenerateExplorationGoal">
|
||||
<Root>
|
||||
<ServiceCall service="/curiosity/generate_goal" response="goal"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Check Obstacle Ahead -->
|
||||
<BehaviorTree ID="CheckObstacle">
|
||||
<Root>
|
||||
<ServiceCall service="/sensors/check_obstacle_ahead" response="distance"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Get Next Waypoint -->
|
||||
<BehaviorTree ID="GetNextWaypoint">
|
||||
<Root>
|
||||
<ServiceCall service="/patrol/get_next_waypoint" response="waypoint"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Publish Velocity Command -->
|
||||
<BehaviorTree ID="PublishVelocity">
|
||||
<Root>
|
||||
<PublishMessage topic="/cmd_vel" message_type="geometry_msgs/Twist"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Wait for Topic Message -->
|
||||
<BehaviorTree ID="WaitForTopic">
|
||||
<Root>
|
||||
<!-- This would be a custom node that blocks until message is received -->
|
||||
<ServiceCall service="/util/wait_for_topic"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Get Current Pose -->
|
||||
<BehaviorTree ID="GetCurrentPose">
|
||||
<Root>
|
||||
<GetTopicValue topic="/robot_pose" output="{pose}"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Get Topic Value -->
|
||||
<BehaviorTree ID="GetTopicValue">
|
||||
<Root>
|
||||
<GetTopicValue topic="/topic_name" field="field_name" output="{value}"/>
|
||||
</Root>
|
||||
</BehaviorTree>
|
||||
|
||||
</root>
|
||||
@ -3,11 +3,14 @@
|
||||
navigate_to_pose_with_recovery.xml — SaltyBot Nav2 behavior tree
|
||||
Format: BehaviorTree.CPP v4 (ROS2 Humble)
|
||||
|
||||
Recovery sequence (outer RecoveryNode, 6 retries):
|
||||
Recovery sequence (outer RecoveryNode, 6 retries) — Issue #479:
|
||||
1. Clear local + global costmaps
|
||||
2. Spin 90° (1.57 rad)
|
||||
3. Wait 5 s
|
||||
4. Back up 0.30 m
|
||||
2. Spin 90° in-place (1.57 rad @ 0.5 rad/s)
|
||||
3. Wait 5 s (e.g., for obstacles to move away)
|
||||
4. Back up 0.30 m at 0.1 m/s (conservative for FC + Hoverboard ESC)
|
||||
|
||||
E-stop (Issue #459) priority: Emergency stop takes priority over recovery behaviors.
|
||||
Progress checker threshold: 20cm minimum movement within 10s to declare progress.
|
||||
|
||||
Each recovery runs round-robin until navigation succeeds or retries exhausted.
|
||||
-->
|
||||
@ -64,8 +67,8 @@
|
||||
<!-- 3. Wait 5 s (e.g. person clears path) -->
|
||||
<Wait wait_duration="5"/>
|
||||
|
||||
<!-- 4. Back up 0.30 m at 0.15 m/s -->
|
||||
<BackUp backup_dist="0.30" backup_speed="0.15"/>
|
||||
<!-- 4. Back up 0.30 m at 0.1 m/s (conservative for FC + Hoverboard ESC) -->
|
||||
<BackUp backup_dist="0.30" backup_speed="0.1"/>
|
||||
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
|
||||
413
jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
Normal file
413
jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
Normal file
@ -0,0 +1,413 @@
|
||||
# Nav2 parameters — SaltyBot (Jetson Orin Nano Super / ROS2 Humble)
|
||||
#
|
||||
# Robot: differential-drive self-balancing two-wheeler
|
||||
# robot_radius: 0.15 m (~0.2m with margin)
|
||||
# footprint: 0.4 x 0.4 m (x 2m for buffer)
|
||||
# max_vel_x: 1.0 m/s
|
||||
# max_vel_theta: 1.5 rad/s
|
||||
#
|
||||
# Localization: RTAB-Map (publishes /map + map→odom TF + /rtabmap/odom)
|
||||
# → No AMCL, no map_server needed.
|
||||
#
|
||||
# Sensors (Issue #478 — Costmap configuration):
|
||||
# /scan — RPLIDAR A1M8 360° LaserScan (obstacle layer)
|
||||
# /depth_scan — RealSense D435i depth_to_laserscan (obstacle layer)
|
||||
# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel layer, local only)
|
||||
#
|
||||
# Inflation:
|
||||
# inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
|
||||
#
|
||||
# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /rtabmap/odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
action_server_result_timeout: 900.0
|
||||
navigators: ['navigate_to_pose', 'navigate_through_poses']
|
||||
navigate_to_pose:
|
||||
plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
|
||||
navigate_through_poses:
|
||||
plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
|
||||
# default_bt_xml_filename is set at launch time via nav2.launch.py
|
||||
error_code_names:
|
||||
- compute_path_error_code
|
||||
- follow_path_error_code
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
# ── Controller Server (DWB — local trajectory follower) ─────────────────────
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
controller_frequency: 10.0 # Hz — Orin can handle 10Hz easily
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5 # not holonomic — effectively disabled
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.2 # Issue #479: Minimum 20cm movement to detect progress
|
||||
movement_time_allowance: 10.0 # 10 second window to detect movement, then attempt recovery
|
||||
|
||||
general_goal_checker:
|
||||
stateful: true
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: false
|
||||
# Velocity limits
|
||||
min_vel_x: -0.25 # allow limited reverse
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 1.0
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.5
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 1.0
|
||||
min_speed_theta: 0.0
|
||||
# Acceleration limits (differential drive)
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
# Trajectory sampling
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: true
|
||||
stateful: true
|
||||
critics:
|
||||
["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
# ── Planner Server (NavFn global path planner) ───────────────────────────────
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
expected_planner_frequency: 20.0
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
# ── Smoother Server ──────────────────────────────────────────────────────────
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: true
|
||||
|
||||
# ── Behavior Server (recovery behaviors) ────────────────────────────────────
|
||||
# Issue #479: Recovery behaviors with conservative speed/distance limits for FC+Hoverboard
|
||||
# ESC priority: E-stop (Issue #459) takes priority over any recovery behavior.
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
local_costmap_topic: local_costmap/costmap_raw
|
||||
global_costmap_topic: global_costmap/costmap_raw
|
||||
local_footprint_topic: local_costmap/published_footprint
|
||||
global_footprint_topic: global_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "wait"]
|
||||
|
||||
# Spin recovery: In-place 90° rotation to escape local deadlock
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
spin_dist: 1.5708 # 90 degrees (π/2 radians)
|
||||
max_rotational_vel: 0.5 # Conservative: 0.5 rad/s for self-balancer stability
|
||||
min_rotational_vel: 0.25 # Minimum angular velocity
|
||||
rotational_acc_lim: 1.6 # Half of max (3.2) for smooth accel on self-balancer
|
||||
time_allowance: 10.0 # max 10 seconds to complete 90° rotation
|
||||
|
||||
# Backup recovery: Reverse 0.3m at 0.1 m/s to escape obstacles
|
||||
# Conservative for FC (Flux Capacitor) + Hoverboard ESC drivetrain
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
backup_dist: 0.3 # Reverse 0.3 meters (safe distance for deadlock escape)
|
||||
backup_speed: 0.1 # Very conservative: 0.1 m/s reverse
|
||||
max_backup_vel: 0.15 # Absolute max reverse velocity
|
||||
min_backup_vel: 0.05 # Minimum backup velocity threshold
|
||||
time_allowance: 5.0 # max 5 seconds (0.3m at 0.1m/s = 3s normal)
|
||||
|
||||
# Wait recovery: Pause 5 seconds to let obstacles move away
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
wait_duration: 5.0 # 5 second pause
|
||||
|
||||
local_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 0.5 # Conservative: self-balancer stability limit
|
||||
min_rotational_vel: 0.25
|
||||
rotational_acc_lim: 1.6 # Half of main limit for recovery behaviors
|
||||
|
||||
# ── Waypoint Follower ────────────────────────────────────────────────────────
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: true
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
# ── Velocity Smoother ────────────────────────────────────────────────────────
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: false
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [1.0, 0.0, 1.5]
|
||||
min_velocity: [-0.25, 0.0, -1.5]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: /rtabmap/odom
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
|
||||
# ── Local Costmap ────────────────────────────────────────────────────────────
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
update_frequency: 10.0 # Hz — Orin can sustain 10Hz
|
||||
publish_frequency: 5.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.15
|
||||
# Footprint: [x, y] in base_link frame, in counterclockwise order
|
||||
# Robot footprint ~0.4m x 0.4m, with 2m lookahead buffer for controller stability
|
||||
footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
|
||||
plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: scan surround_cameras
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 0.80
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 8.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 7.5
|
||||
obstacle_min_range: 0.0
|
||||
# 4× IMX219 camera-detected obstacles (chairs, glass, people)
|
||||
surround_cameras:
|
||||
topic: /surround_vision/obstacles
|
||||
min_obstacle_height: 0.05
|
||||
max_obstacle_height: 1.50
|
||||
clearing: false
|
||||
marking: true
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.5
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 3.0
|
||||
obstacle_min_range: 0.0
|
||||
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: true
|
||||
publish_voxel_map: false
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 0.80
|
||||
mark_threshold: 0
|
||||
observation_sources: depth_camera
|
||||
depth_camera:
|
||||
topic: /camera/depth/color/points
|
||||
min_obstacle_height: 0.05 # above floor level
|
||||
max_obstacle_height: 0.80
|
||||
marking: true
|
||||
clearing: true
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 4.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 3.5
|
||||
obstacle_min_range: 0.0
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.3 # robot_radius (0.15) + 0.15m padding for safety
|
||||
|
||||
always_send_full_costmap: false
|
||||
|
||||
# ── Global Costmap ───────────────────────────────────────────────────────────
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
robot_radius: 0.15
|
||||
# Footprint: [x, y] in base_link frame, in counterclockwise order
|
||||
footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: true # for RTAB-Map's transient-local /map
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
# Issue #478: RPLIDAR + RealSense depth_scan
|
||||
observation_sources: scan depth_scan surround_cameras
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 0.80
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 8.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 7.5
|
||||
obstacle_min_range: 0.0
|
||||
# RealSense D435i depth_to_laserscan (synthetic LaserScan from depth)
|
||||
depth_scan:
|
||||
topic: /depth_scan
|
||||
max_obstacle_height: 0.80
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 6.0
|
||||
raytrace_min_range: 0.1
|
||||
obstacle_max_range: 5.5
|
||||
obstacle_min_range: 0.0
|
||||
surround_cameras:
|
||||
topic: /surround_vision/obstacles
|
||||
min_obstacle_height: 0.05
|
||||
max_obstacle_height: 1.50
|
||||
clearing: false
|
||||
marking: true
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.5
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 3.0
|
||||
obstacle_min_range: 0.0
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.3 # robot_radius (0.15) + 0.15m padding for safety
|
||||
|
||||
always_send_full_costmap: false
|
||||
|
||||
# ── Map Server / Map Saver (not used — RTAB-Map provides /map) ───────────────
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: true
|
||||
@ -0,0 +1,178 @@
|
||||
"""
|
||||
autonomous_mode.launch.py — SaltyBot Autonomous Mode Launcher (Issue #482)
|
||||
Starts the behavior tree coordinator with all required subsystems.
|
||||
"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
||||
import os
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate launch description for autonomous mode."""
|
||||
|
||||
saltybot_bringup_dir = FindPackageShare('saltybot_bringup')
|
||||
bt_xml_dir = PathJoinSubstitution([saltybot_bringup_dir, 'behavior_trees'])
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Behavior Tree Executor Node (Nav2 BT framework)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_bt_executor',
|
||||
executable='behavior_tree_executor',
|
||||
name='autonomous_coordinator',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'bt_xml_filename': PathJoinSubstitution([
|
||||
bt_xml_dir, 'autonomous_coordinator.xml'
|
||||
]),
|
||||
'blackboard_variables': {
|
||||
'battery_level': 100.0,
|
||||
'person_detected': False,
|
||||
'obstacle_state': 'clear',
|
||||
'current_mode': 'idle',
|
||||
'home_pose': {
|
||||
'header': {
|
||||
'frame_id': 'map',
|
||||
'stamp': {'sec': 0, 'nsec': 0}
|
||||
},
|
||||
'pose': {
|
||||
'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
|
||||
'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
|
||||
}
|
||||
}
|
||||
},
|
||||
'enable_groot_monitoring': True,
|
||||
'groot_port': 5555,
|
||||
}],
|
||||
remappings=[
|
||||
('/clicked_point', '/bt/clicked_point'),
|
||||
('/goal_pose', '/bt/goal_pose'),
|
||||
],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Battery Monitor (publishes to /battery_state for BT)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_battery_monitor',
|
||||
executable='battery_monitor_node',
|
||||
name='battery_monitor',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'update_rate': 1.0, # Hz
|
||||
'critical_threshold': 10.0,
|
||||
'warning_threshold': 20.0,
|
||||
}],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Person Detector (publishes detected persons for BT)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_perception',
|
||||
executable='person_detector',
|
||||
name='person_detector',
|
||||
output='screen',
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Obstacle Monitor (LIDAR-based obstacle detection)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_lidar_avoidance',
|
||||
executable='obstacle_monitor',
|
||||
name='obstacle_monitor',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'danger_distance': 0.3,
|
||||
'warning_distance': 0.6,
|
||||
}],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Autonomy Mode Manager (handles mode transitions, safety checks)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_mode_switch',
|
||||
executable='autonomous_mode_manager',
|
||||
name='autonomous_mode_manager',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'auto_return_battery': 20.0, # Return home if battery < 20%
|
||||
'enable_geofence': True,
|
||||
'geofence_boundary_distance': 5.0,
|
||||
}],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Person Follower (follows detected persons)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_follower',
|
||||
executable='person_follower',
|
||||
name='person_follower',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'follow_distance': 1.5,
|
||||
'max_linear_vel': 0.5,
|
||||
'max_angular_vel': 1.0,
|
||||
}],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Curiosity Behavior (autonomous exploration)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_curiosity',
|
||||
executable='curiosity_explorer',
|
||||
name='curiosity_explorer',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'exploration_mode': 'active',
|
||||
'max_exploration_distance': 2.0,
|
||||
'idle_timeout': 60.0, # Start exploration after 60s idle
|
||||
}],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Gesture Recognition (for interactive mode)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_gesture_recognition',
|
||||
executable='gesture_recognition_node',
|
||||
name='gesture_recognition',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'min_confidence': 0.6,
|
||||
}],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# TTS Service (for greetings and social interaction)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_tts_service',
|
||||
executable='tts_service',
|
||||
name='tts_service',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'voice_speed': 1.0,
|
||||
'voice_pitch': 1.0,
|
||||
}],
|
||||
),
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
# Emergency Stop Monitor (highest priority safety)
|
||||
# ────────────────────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_emergency_stop',
|
||||
executable='emergency_stop_monitor',
|
||||
name='emergency_stop_monitor',
|
||||
output='screen',
|
||||
),
|
||||
|
||||
])
|
||||
@ -0,0 +1,171 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
test_terrain_classifier.py — Unit tests for terrain classification logic.
|
||||
|
||||
Tests the core algorithms without requiring ROS2 or hardware.
|
||||
"""
|
||||
|
||||
import unittest
|
||||
import numpy as np
|
||||
from unittest.mock import Mock, patch
|
||||
|
||||
import sys
|
||||
import os
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
||||
|
||||
from saltybot_bringup.terrain_classifier_node import (
|
||||
IMUVibrationAnalyzer,
|
||||
CNNTextureAnalyzer,
|
||||
TerrainClassifier,
|
||||
TerrainDecision,
|
||||
)
|
||||
|
||||
|
||||
class TestIMUVibrationAnalyzer(unittest.TestCase):
|
||||
"""Test IMU vibration analysis and FFT roughness computation."""
|
||||
|
||||
def setUp(self):
|
||||
self.analyzer = IMUVibrationAnalyzer(window_size=100, sample_rate=100.0)
|
||||
|
||||
def test_empty_buffer_returns_zero(self):
|
||||
roughness, accel_mag = self.analyzer.compute_roughness()
|
||||
self.assertEqual(roughness, 0.0)
|
||||
self.assertEqual(accel_mag, 0.0)
|
||||
|
||||
def test_smooth_surface_low_roughness(self):
|
||||
for _ in range(100):
|
||||
self.analyzer.add_sample(0.0, 0.0, 0.0)
|
||||
|
||||
roughness, accel_mag = self.analyzer.compute_roughness()
|
||||
self.assertLess(roughness, 0.2)
|
||||
|
||||
def test_rough_surface_high_roughness(self):
|
||||
for i in range(100):
|
||||
vibration = 2.0 * np.sin(2 * np.pi * 20 * i / 100.0)
|
||||
self.analyzer.add_sample(vibration, vibration, vibration)
|
||||
|
||||
roughness, accel_mag = self.analyzer.compute_roughness()
|
||||
self.assertGreater(roughness, 0.3)
|
||||
|
||||
def test_buffer_size_respected(self):
|
||||
self.assertEqual(self.analyzer.accel_buffer.maxlen, 100)
|
||||
for i in range(150):
|
||||
self.analyzer.add_sample(i, i, i)
|
||||
|
||||
self.assertEqual(len(self.analyzer.accel_buffer), 100)
|
||||
|
||||
def test_reset_clears_buffer(self):
|
||||
for i in range(50):
|
||||
self.analyzer.add_sample(i, i, i)
|
||||
|
||||
self.assertGreater(len(self.analyzer.accel_buffer), 0)
|
||||
self.analyzer.reset()
|
||||
self.assertEqual(len(self.analyzer.accel_buffer), 0)
|
||||
|
||||
def test_roughness_is_normalized(self):
|
||||
for i in range(100):
|
||||
vibration = 5.0 * np.sin(2 * np.pi * 30 * i / 100.0)
|
||||
self.analyzer.add_sample(vibration, vibration, vibration)
|
||||
|
||||
roughness, _ = self.analyzer.compute_roughness()
|
||||
self.assertGreaterEqual(roughness, 0.0)
|
||||
self.assertLessEqual(roughness, 1.0)
|
||||
|
||||
|
||||
class TestTerrainClassifier(unittest.TestCase):
|
||||
"""Test terrain classification decision logic."""
|
||||
|
||||
def setUp(self):
|
||||
self.classifier = TerrainClassifier()
|
||||
|
||||
def test_pavement_classification(self):
|
||||
decision = self.classifier.classify(
|
||||
imu_roughness=0.1,
|
||||
accel_magnitude=9.85,
|
||||
cnn_features=None
|
||||
)
|
||||
|
||||
self.assertEqual(decision.terrain_type, self.classifier.TERRAIN_PAVEMENT)
|
||||
self.assertGreater(decision.confidence, 0.7)
|
||||
self.assertEqual(decision.recommended_speed_ratio, 1.0)
|
||||
|
||||
def test_grass_classification(self):
|
||||
decision = self.classifier.classify(
|
||||
imu_roughness=0.25,
|
||||
accel_magnitude=9.9,
|
||||
cnn_features=None
|
||||
)
|
||||
|
||||
self.assertEqual(decision.terrain_type, self.classifier.TERRAIN_GRASS)
|
||||
self.assertGreater(decision.confidence, 0.5)
|
||||
self.assertEqual(decision.recommended_speed_ratio, 0.8)
|
||||
|
||||
def test_gravel_classification(self):
|
||||
decision = self.classifier.classify(
|
||||
imu_roughness=0.55,
|
||||
accel_magnitude=10.0,
|
||||
cnn_features=None
|
||||
)
|
||||
|
||||
self.assertEqual(decision.terrain_type, self.classifier.TERRAIN_GRAVEL)
|
||||
self.assertGreater(decision.confidence, 0.5)
|
||||
self.assertEqual(decision.recommended_speed_ratio, 0.5)
|
||||
|
||||
def test_sand_classification(self):
|
||||
decision = self.classifier.classify(
|
||||
imu_roughness=0.6,
|
||||
accel_magnitude=10.3,
|
||||
cnn_features=None
|
||||
)
|
||||
|
||||
self.assertEqual(decision.terrain_type, self.classifier.TERRAIN_SAND)
|
||||
self.assertGreater(decision.confidence, 0.5)
|
||||
self.assertEqual(decision.recommended_speed_ratio, 0.4)
|
||||
|
||||
def test_all_terrains_have_speed_recommendations(self):
|
||||
for terrain_type in range(6):
|
||||
self.assertIn(
|
||||
terrain_type,
|
||||
self.classifier.SPEED_RECOMMENDATIONS
|
||||
)
|
||||
|
||||
speed_ratio = self.classifier.SPEED_RECOMMENDATIONS[terrain_type]
|
||||
self.assertGreater(speed_ratio, 0.0)
|
||||
self.assertLessEqual(speed_ratio, 1.0)
|
||||
|
||||
def test_decision_fields_are_valid(self):
|
||||
decision = self.classifier.classify(0.3, 9.9)
|
||||
|
||||
self.assertGreaterEqual(decision.terrain_type, 0)
|
||||
self.assertLessEqual(decision.terrain_type, 5)
|
||||
|
||||
self.assertGreaterEqual(decision.roughness, 0.0)
|
||||
self.assertLessEqual(decision.roughness, 1.0)
|
||||
|
||||
self.assertGreaterEqual(decision.confidence, 0.0)
|
||||
self.assertLessEqual(decision.confidence, 1.0)
|
||||
|
||||
self.assertGreater(decision.recommended_speed_ratio, 0.0)
|
||||
self.assertLessEqual(decision.recommended_speed_ratio, 1.0)
|
||||
|
||||
|
||||
class TestTerrainDecisionDataclass(unittest.TestCase):
|
||||
"""Test TerrainDecision dataclass."""
|
||||
|
||||
def test_terrain_decision_creation(self):
|
||||
decision = TerrainDecision(
|
||||
terrain_type=1,
|
||||
roughness=0.3,
|
||||
confidence=0.8,
|
||||
recommended_speed_ratio=0.9
|
||||
)
|
||||
|
||||
self.assertEqual(decision.terrain_type, 1)
|
||||
self.assertEqual(decision.roughness, 0.3)
|
||||
self.assertEqual(decision.confidence, 0.8)
|
||||
self.assertEqual(decision.recommended_speed_ratio, 0.9)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
249
jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro
Normal file
249
jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro
Normal file
@ -0,0 +1,249 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="saltybot">
|
||||
<!--
|
||||
SaltyBot URDF Description
|
||||
|
||||
Robot Configuration:
|
||||
- Base: Wheeled differential drive platform
|
||||
- Footprint: ~0.4m x 0.4m (square base)
|
||||
- Height: ~0.3m to sensor payload
|
||||
- Sensors:
|
||||
* RPLIDAR A1M8 (360° scanning LiDAR)
|
||||
* RealSense D435i (RGB-D camera + IMU)
|
||||
* BNO055 (9-DOF IMU, STM32 FC)
|
||||
- Actuators:
|
||||
* 2x differential drive motors
|
||||
* Pan/Tilt servos for camera
|
||||
-->
|
||||
|
||||
<!-- Constants -->
|
||||
<xacro:property name="PI" value="3.14159265359" />
|
||||
<xacro:property name="base_mass" value="2.5" />
|
||||
<xacro:property name="wheel_radius" value="0.0475" />
|
||||
<xacro:property name="wheel_separation" value="0.380" />
|
||||
<xacro:property name="footprint_x" value="0.40" />
|
||||
<xacro:property name="footprint_y" value="0.40" />
|
||||
<xacro:property name="footprint_z" value="0.15" />
|
||||
|
||||
<!-- Base Footprint -->
|
||||
<link name="base_footprint" />
|
||||
|
||||
<!-- Base Link (Robot Center) -->
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.08" rpy="0 0 0" />
|
||||
<mass value="${base_mass}" />
|
||||
<!-- Rough rectangular inertia approximation -->
|
||||
<inertia ixx="0.020" ixy="0.0" ixz="0.0" iyy="0.020" iyz="0.0" izz="0.030" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0.075" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="${footprint_x} ${footprint_y} ${footprint_z}" />
|
||||
</geometry>
|
||||
<material name="dark_gray">
|
||||
<color rgba="0.25 0.25 0.25 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.075" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="${footprint_x} ${footprint_y} ${footprint_z}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Footprint to Base Transform (Z-only, footprint at ground level) -->
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 ${footprint_z}" rpy="0 0 0" />
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
</joint>
|
||||
|
||||
<!-- Left Wheel -->
|
||||
<link name="left_wheel_link">
|
||||
<inertial>
|
||||
<mass value="0.2" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="0.03" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.1 0.1 0.1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="left_wheel_joint" type="continuous">
|
||||
<origin xyz="0 ${wheel_separation/2} 0.0475" rpy="0 0 0" />
|
||||
<axis xyz="0 1 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="left_wheel_link" />
|
||||
</joint>
|
||||
|
||||
<!-- Right Wheel -->
|
||||
<link name="right_wheel_link">
|
||||
<inertial>
|
||||
<mass value="0.2" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="0.03" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.1 0.1 0.1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="right_wheel_joint" type="continuous">
|
||||
<origin xyz="0 ${-wheel_separation/2} 0.0475" rpy="0 0 0" />
|
||||
<axis xyz="0 1 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="right_wheel_link" />
|
||||
</joint>
|
||||
|
||||
<!-- IMU Link (STM32 FC BNO055, mounted on main board) -->
|
||||
<link name="imu_link">
|
||||
<inertial>
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="imu_link" />
|
||||
</joint>
|
||||
|
||||
<!-- RPLIDAR A1M8 Frame (360° LiDAR, mounted on top of robot) -->
|
||||
<link name="laser_frame">
|
||||
<inertial>
|
||||
<mass value="0.15" />
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.035" length="0.02" />
|
||||
</geometry>
|
||||
<material name="gray">
|
||||
<color rgba="0.5 0.5 0.5 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder radius="0.035" length="0.02" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="laser_joint" type="fixed">
|
||||
<!-- RPLIDAR mounted on top, center of robot, ~0.15m above base -->
|
||||
<origin xyz="0 0 0.15" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="laser_frame" />
|
||||
</joint>
|
||||
|
||||
<!-- RealSense D435i Camera Frame (Mounted forward-facing on pan/tilt) -->
|
||||
<link name="camera_link">
|
||||
<inertial>
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.09 0.025 0.024" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.1 0.1 0.1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.09 0.025 0.024" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- RealSense Optical Frame (Z-forward for optical convention) -->
|
||||
<link name="camera_optical_frame" />
|
||||
|
||||
<!-- Pan Servo Frame (Base of pan-tilt mechanism) -->
|
||||
<link name="pan_servo">
|
||||
<inertial>
|
||||
<mass value="0.05" />
|
||||
<inertia ixx="0.000005" ixy="0.0" ixz="0.0" iyy="0.000005" iyz="0.0" izz="0.000005" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="pan_servo_joint" type="fixed">
|
||||
<!-- Pan servo mounted at front, ~0.12m forward and 0.10m up from base center -->
|
||||
<origin xyz="0.12 0 0.10" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="pan_servo" />
|
||||
</joint>
|
||||
|
||||
<!-- Tilt Servo Frame (Mounted on pan servo) -->
|
||||
<link name="tilt_servo">
|
||||
<inertial>
|
||||
<mass value="0.05" />
|
||||
<inertia ixx="0.000005" ixy="0.0" ixz="0.0" iyy="0.000005" iyz="0.0" izz="0.000005" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tilt_servo_joint" type="fixed">
|
||||
<!-- Tilt servo mounted on pan servo, offset for mechanical linkage -->
|
||||
<origin xyz="0 0 0.02" rpy="0 0 0" />
|
||||
<parent link="pan_servo" />
|
||||
<child link="tilt_servo" />
|
||||
</joint>
|
||||
|
||||
<!-- Camera mounted on tilt servo -->
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<!-- Camera offset from tilt servo (forward and up for good viewing angle) -->
|
||||
<origin xyz="0.04 0 0.02" rpy="0 0 0" />
|
||||
<parent link="tilt_servo" />
|
||||
<child link="camera_link" />
|
||||
</joint>
|
||||
|
||||
<!-- Camera optical frame (ROS convention: Z-forward, X-right, Y-down) -->
|
||||
<joint name="camera_optical_joint" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="${-PI/2} 0 ${-PI/2}" />
|
||||
<parent link="camera_link" />
|
||||
<child link="camera_optical_frame" />
|
||||
</joint>
|
||||
|
||||
<!-- Materials -->
|
||||
<material name="dark_gray">
|
||||
<color rgba="0.25 0.25 0.25 1" />
|
||||
</material>
|
||||
<material name="gray">
|
||||
<color rgba="0.5 0.5 0.5 1" />
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0.1 0.1 0.1 1" />
|
||||
</material>
|
||||
|
||||
</robot>
|
||||
8
jetson/ros2_ws/src/saltybot_dashboard/.gitignore
vendored
Normal file
8
jetson/ros2_ws/src/saltybot_dashboard/.gitignore
vendored
Normal file
@ -0,0 +1,8 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
*.pyc
|
||||
__pycache__/
|
||||
*.egg-info/
|
||||
dist/
|
||||
.env
|
||||
27
jetson/ros2_ws/src/saltybot_dashboard/package.xml
Normal file
27
jetson/ros2_ws/src/saltybot_dashboard/package.xml
Normal file
@ -0,0 +1,27 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_dashboard</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Remote monitoring dashboard for SaltyBot with WebSocket live updates</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,200 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Remote monitoring dashboard for SaltyBot — web UI on Orin port 8080.
|
||||
|
||||
Displays:
|
||||
- Battery %, current state
|
||||
- Sensor health (audio, vision, lidar)
|
||||
- Map view (occupancy grid)
|
||||
- Event log (last 20 from event logger #473)
|
||||
- Motor speeds
|
||||
- WiFi signal strength
|
||||
|
||||
WebSocket updates push data in real-time to connected clients.
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
from std_msgs.msg import String, Float32
|
||||
from sensor_msgs.msg import Image, BatteryState
|
||||
from nav_msgs.msg import OccupancyGrid
|
||||
import json
|
||||
import time
|
||||
from collections import deque
|
||||
from threading import Thread
|
||||
from flask import Flask, render_template, jsonify
|
||||
from flask_socketio import SocketIO, emit, disconnect
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
|
||||
|
||||
class DashboardNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('dashboard_node')
|
||||
|
||||
# State
|
||||
self.battery_percent = 100.0
|
||||
self.battery_voltage = 0.0
|
||||
self.robot_state = "IDLE"
|
||||
self.motor_speeds = {"left": 0.0, "right": 0.0}
|
||||
self.wifi_signal = -50
|
||||
self.sensor_health = {
|
||||
"camera": True,
|
||||
"lidar": True,
|
||||
"audio": True,
|
||||
"imu": True,
|
||||
}
|
||||
self.event_log = deque(maxlen=20)
|
||||
self.map_data = None
|
||||
self.last_update = time.time()
|
||||
|
||||
# Global Flask app reference for socketio
|
||||
global socketio_instance
|
||||
socketio_instance = None
|
||||
|
||||
# Subscribers
|
||||
self.create_subscription(Float32, '/saltybot/battery_percent', self.battery_callback, 10)
|
||||
self.create_subscription(BatteryState, '/saltybot/battery_state', self.battery_state_callback, 10)
|
||||
self.create_subscription(String, '/saltybot/telemetry/motor_rpm', self.motor_callback, 10)
|
||||
self.create_subscription(String, '/saltybot/state', self.state_callback, 10)
|
||||
self.create_subscription(String, '/saltybot/events', self.event_callback, 10)
|
||||
self.create_subscription(OccupancyGrid, '/map', self.map_callback, _SENSOR_QOS)
|
||||
self.create_subscription(String, '/saltybot/sensor_health', self.sensor_health_callback, 10)
|
||||
|
||||
# Update timer for periodic broadcasts
|
||||
self.timer = self.create_timer(0.5, self.broadcast_update)
|
||||
|
||||
self.get_logger().info("Dashboard node initialized on port 8080")
|
||||
|
||||
def battery_callback(self, msg):
|
||||
self.battery_percent = msg.data
|
||||
|
||||
def battery_state_callback(self, msg):
|
||||
if hasattr(msg, 'voltage'):
|
||||
self.battery_voltage = msg.voltage
|
||||
|
||||
def motor_callback(self, msg):
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
if 'left' in data and 'right' in data:
|
||||
self.motor_speeds = {"left": float(data['left']), "right": float(data['right'])}
|
||||
except (json.JSONDecodeError, ValueError, KeyError):
|
||||
pass
|
||||
|
||||
def state_callback(self, msg):
|
||||
self.robot_state = msg.data
|
||||
|
||||
def event_callback(self, msg):
|
||||
try:
|
||||
event_data = json.loads(msg.data)
|
||||
event_data['timestamp'] = time.time()
|
||||
self.event_log.append(event_data)
|
||||
except json.JSONDecodeError:
|
||||
self.event_log.append({'message': msg.data, 'timestamp': time.time()})
|
||||
|
||||
def sensor_health_callback(self, msg):
|
||||
try:
|
||||
health = json.loads(msg.data)
|
||||
self.sensor_health.update(health)
|
||||
except json.JSONDecodeError:
|
||||
pass
|
||||
|
||||
def map_callback(self, msg):
|
||||
self.map_data = {
|
||||
'width': msg.info.width,
|
||||
'height': msg.info.height,
|
||||
'resolution': msg.info.resolution,
|
||||
'origin_x': msg.info.origin.position.x,
|
||||
'origin_y': msg.info.origin.position.y,
|
||||
}
|
||||
|
||||
def broadcast_update(self):
|
||||
"""Broadcast dashboard state to all connected clients."""
|
||||
if socketio_instance is None:
|
||||
return
|
||||
|
||||
data = {
|
||||
'battery_percent': float(self.battery_percent),
|
||||
'battery_voltage': float(self.battery_voltage),
|
||||
'robot_state': self.robot_state,
|
||||
'motor_speeds': self.motor_speeds,
|
||||
'wifi_signal': self.wifi_signal,
|
||||
'sensor_health': self.sensor_health,
|
||||
'event_log': list(self.event_log),
|
||||
'map_data': self.map_data,
|
||||
'timestamp': time.time(),
|
||||
}
|
||||
|
||||
try:
|
||||
socketio_instance.emit('dashboard_update', data, broadcast=True)
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f"Failed to broadcast: {e}")
|
||||
|
||||
|
||||
def create_app(dashboard_node):
|
||||
"""Create Flask app with dashboard routes."""
|
||||
app = Flask(__name__, template_folder='templates', static_folder='static')
|
||||
app.config['SECRET_KEY'] = 'saltybot-dashboard'
|
||||
|
||||
socketio = SocketIO(app, cors_allowed_origins="*")
|
||||
global socketio_instance
|
||||
socketio_instance = socketio
|
||||
|
||||
@app.route('/')
|
||||
def index():
|
||||
return render_template('dashboard.html')
|
||||
|
||||
@app.route('/api/status')
|
||||
def api_status():
|
||||
return jsonify({
|
||||
'battery_percent': dashboard_node.battery_percent,
|
||||
'battery_voltage': dashboard_node.battery_voltage,
|
||||
'robot_state': dashboard_node.robot_state,
|
||||
'motor_speeds': dashboard_node.motor_speeds,
|
||||
'wifi_signal': dashboard_node.wifi_signal,
|
||||
'sensor_health': dashboard_node.sensor_health,
|
||||
'event_log': list(dashboard_node.event_log),
|
||||
'map_data': dashboard_node.map_data,
|
||||
})
|
||||
|
||||
@socketio.on('connect')
|
||||
def handle_connect():
|
||||
emit('response', {'data': 'Connected to SaltyBot dashboard'})
|
||||
|
||||
@socketio.on('disconnect')
|
||||
def handle_disconnect():
|
||||
pass
|
||||
|
||||
return app, socketio
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
dashboard_node = DashboardNode()
|
||||
|
||||
app, socketio = create_app(dashboard_node)
|
||||
|
||||
# Run Flask in a separate thread
|
||||
def run_flask():
|
||||
socketio.run(app, host='0.0.0.0', port=8080, debug=False, allow_unsafe_werkzeug=True)
|
||||
|
||||
flask_thread = Thread(target=run_flask, daemon=True)
|
||||
flask_thread.start()
|
||||
|
||||
# Run ROS2 spin
|
||||
try:
|
||||
rclpy.spin(dashboard_node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
dashboard_node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltybot_dashboard/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_dashboard/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_dashboard
|
||||
[install]
|
||||
install_lib=$base/lib/python3/dist-packages
|
||||
32
jetson/ros2_ws/src/saltybot_dashboard/setup.py
Normal file
32
jetson/ros2_ws/src/saltybot_dashboard/setup.py
Normal file
@ -0,0 +1,32 @@
|
||||
from setuptools import setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = 'saltybot_dashboard'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.1.0',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
|
||||
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
|
||||
(os.path.join('share', package_name, 'static/css'), glob('static/css/*')),
|
||||
(os.path.join('share', package_name, 'static/js'), glob('static/js/*')),
|
||||
(os.path.join('share', package_name, 'templates'), glob('templates/*')),
|
||||
],
|
||||
install_requires=['setuptools', 'flask', 'python-socketio', 'python-socketio-client'],
|
||||
zip_safe=True,
|
||||
maintainer='seb',
|
||||
maintainer_email='seb@vayrette.com',
|
||||
description='Remote monitoring dashboard for SaltyBot with WebSocket live updates',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'dashboard_node = saltybot_dashboard.dashboard_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
334
jetson/ros2_ws/src/saltybot_dashboard/static/css/dashboard.css
Normal file
334
jetson/ros2_ws/src/saltybot_dashboard/static/css/dashboard.css
Normal file
@ -0,0 +1,334 @@
|
||||
* {
|
||||
margin: 0;
|
||||
padding: 0;
|
||||
box-sizing: border-box;
|
||||
}
|
||||
|
||||
:root {
|
||||
--primary: #2196F3;
|
||||
--success: #4CAF50;
|
||||
--warning: #FF9800;
|
||||
--danger: #f44336;
|
||||
--dark: #1a1a1a;
|
||||
--light: #f5f5f5;
|
||||
--border: #ddd;
|
||||
}
|
||||
|
||||
html, body {
|
||||
font-family: -apple-system, BlinkMacSystemFont, 'Segoe UI', Roboto, Oxygen, Ubuntu, Cantarell, sans-serif;
|
||||
background: #0a0e27;
|
||||
color: #fff;
|
||||
height: 100%;
|
||||
}
|
||||
|
||||
.container {
|
||||
max-width: 1400px;
|
||||
margin: 0 auto;
|
||||
padding: 20px;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
min-height: 100vh;
|
||||
}
|
||||
|
||||
header {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
margin-bottom: 30px;
|
||||
padding-bottom: 20px;
|
||||
border-bottom: 2px solid #333;
|
||||
}
|
||||
|
||||
header h1 {
|
||||
font-size: 28px;
|
||||
font-weight: 600;
|
||||
}
|
||||
|
||||
.connection-status {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
font-size: 14px;
|
||||
}
|
||||
|
||||
.status-dot {
|
||||
width: 12px;
|
||||
height: 12px;
|
||||
border-radius: 50%;
|
||||
background: #999;
|
||||
animation: pulse 2s infinite;
|
||||
}
|
||||
|
||||
.status-dot.connected {
|
||||
background: #4CAF50;
|
||||
}
|
||||
|
||||
@keyframes pulse {
|
||||
0%, 100% { opacity: 1; }
|
||||
50% { opacity: 0.5; }
|
||||
}
|
||||
|
||||
main {
|
||||
flex: 1;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 20px;
|
||||
}
|
||||
|
||||
.grid-2 {
|
||||
display: grid;
|
||||
grid-template-columns: 1fr 1fr;
|
||||
gap: 20px;
|
||||
}
|
||||
|
||||
.grid-full {
|
||||
display: grid;
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.card {
|
||||
background: #1a1a2e;
|
||||
border: 1px solid #333;
|
||||
border-radius: 8px;
|
||||
padding: 20px;
|
||||
box-shadow: 0 2px 8px rgba(0,0,0,0.3);
|
||||
}
|
||||
|
||||
.card h2 {
|
||||
font-size: 18px;
|
||||
margin-bottom: 15px;
|
||||
color: #fff;
|
||||
}
|
||||
|
||||
/* Battery Card */
|
||||
.battery-display {
|
||||
display: flex;
|
||||
gap: 15px;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
.battery-bar {
|
||||
flex: 1;
|
||||
height: 40px;
|
||||
background: #333;
|
||||
border-radius: 20px;
|
||||
overflow: hidden;
|
||||
border: 2px solid #555;
|
||||
}
|
||||
|
||||
.battery-fill {
|
||||
height: 100%;
|
||||
background: linear-gradient(90deg, #4CAF50, #8BC34A);
|
||||
width: 100%;
|
||||
transition: width 0.3s ease;
|
||||
}
|
||||
|
||||
.battery-text {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
align-items: center;
|
||||
min-width: 80px;
|
||||
}
|
||||
|
||||
.battery-text span:first-child {
|
||||
font-size: 24px;
|
||||
font-weight: bold;
|
||||
}
|
||||
|
||||
.subtitle {
|
||||
font-size: 12px;
|
||||
color: #999;
|
||||
}
|
||||
|
||||
/* State Card */
|
||||
.state-display {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 12px;
|
||||
}
|
||||
|
||||
.state-badge {
|
||||
display: inline-block;
|
||||
padding: 8px 16px;
|
||||
background: #2196F3;
|
||||
border-radius: 20px;
|
||||
font-weight: bold;
|
||||
text-align: center;
|
||||
}
|
||||
|
||||
.sensor-health {
|
||||
display: flex;
|
||||
justify-content: space-around;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
.health-item {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 6px;
|
||||
font-size: 16px;
|
||||
}
|
||||
|
||||
.health-ok {
|
||||
color: #4CAF50;
|
||||
}
|
||||
|
||||
.health-error {
|
||||
color: #f44336;
|
||||
}
|
||||
|
||||
/* Motors Card */
|
||||
.motors-display {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 15px;
|
||||
}
|
||||
|
||||
.motor-item {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
.motor-item label {
|
||||
min-width: 50px;
|
||||
font-weight: 600;
|
||||
}
|
||||
|
||||
.motor-meter {
|
||||
flex: 1;
|
||||
height: 30px;
|
||||
background: #333;
|
||||
border-radius: 15px;
|
||||
overflow: hidden;
|
||||
border: 1px solid #555;
|
||||
}
|
||||
|
||||
.motor-bar {
|
||||
height: 100%;
|
||||
background: linear-gradient(90deg, #FF9800, #F44336);
|
||||
width: 0%;
|
||||
transition: width 0.2s ease;
|
||||
}
|
||||
|
||||
.motor-item span:last-child {
|
||||
min-width: 70px;
|
||||
text-align: right;
|
||||
font-size: 12px;
|
||||
color: #999;
|
||||
}
|
||||
|
||||
/* WiFi Card */
|
||||
.wifi-display {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 20px;
|
||||
padding: 15px;
|
||||
}
|
||||
|
||||
.wifi-icon {
|
||||
font-size: 40px;
|
||||
}
|
||||
|
||||
.wifi-text {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
.wifi-text span:first-child {
|
||||
font-size: 20px;
|
||||
font-weight: bold;
|
||||
}
|
||||
|
||||
/* Map Card */
|
||||
.map-display {
|
||||
width: 100%;
|
||||
height: 300px;
|
||||
background: #0a0e27;
|
||||
border: 1px solid #333;
|
||||
border-radius: 4px;
|
||||
}
|
||||
|
||||
.map-info {
|
||||
margin-top: 10px;
|
||||
font-size: 12px;
|
||||
color: #999;
|
||||
}
|
||||
|
||||
/* Events Card */
|
||||
.event-list {
|
||||
max-height: 300px;
|
||||
overflow-y: auto;
|
||||
}
|
||||
|
||||
.event-item {
|
||||
padding: 10px;
|
||||
margin-bottom: 8px;
|
||||
background: #0a0e27;
|
||||
border-left: 3px solid #2196F3;
|
||||
border-radius: 4px;
|
||||
font-size: 13px;
|
||||
line-height: 1.4;
|
||||
}
|
||||
|
||||
.event-time {
|
||||
color: #999;
|
||||
font-size: 11px;
|
||||
}
|
||||
|
||||
.placeholder {
|
||||
color: #666;
|
||||
text-align: center;
|
||||
padding: 20px;
|
||||
}
|
||||
|
||||
footer {
|
||||
text-align: center;
|
||||
padding-top: 20px;
|
||||
border-top: 1px solid #333;
|
||||
color: #999;
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
/* Mobile Responsive */
|
||||
@media (max-width: 768px) {
|
||||
.grid-2 {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
header {
|
||||
flex-direction: column;
|
||||
align-items: flex-start;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
.map-display {
|
||||
height: 250px;
|
||||
}
|
||||
|
||||
.container {
|
||||
padding: 10px;
|
||||
}
|
||||
|
||||
.card {
|
||||
padding: 15px;
|
||||
}
|
||||
}
|
||||
|
||||
/* Scrollbar styling */
|
||||
::-webkit-scrollbar {
|
||||
width: 8px;
|
||||
}
|
||||
|
||||
::-webkit-scrollbar-track {
|
||||
background: #1a1a2e;
|
||||
}
|
||||
|
||||
::-webkit-scrollbar-thumb {
|
||||
background: #444;
|
||||
border-radius: 4px;
|
||||
}
|
||||
|
||||
::-webkit-scrollbar-thumb:hover {
|
||||
background: #666;
|
||||
}
|
||||
112
jetson/ros2_ws/src/saltybot_dashboard/templates/dashboard.html
Normal file
112
jetson/ros2_ws/src/saltybot_dashboard/templates/dashboard.html
Normal file
@ -0,0 +1,112 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>SaltyBot Dashboard</title>
|
||||
<link rel="stylesheet" href="{{ url_for('static', filename='css/dashboard.css') }}">
|
||||
<script src="https://cdn.socket.io/4.5.4/socket.io.min.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div class="container">
|
||||
<header>
|
||||
<h1>🤖 SaltyBot Monitor</h1>
|
||||
<div class="connection-status">
|
||||
<span id="status-indicator" class="status-dot"></span>
|
||||
<span id="status-text">Connecting...</span>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<main>
|
||||
<!-- Battery & Health Row -->
|
||||
<div class="grid-2">
|
||||
<div class="card battery-card">
|
||||
<h2>⚡ Battery</h2>
|
||||
<div class="battery-display">
|
||||
<div class="battery-bar">
|
||||
<div id="battery-fill" class="battery-fill"></div>
|
||||
</div>
|
||||
<div class="battery-text">
|
||||
<span id="battery-percent">100%</span>
|
||||
<span id="battery-voltage" class="subtitle">12.4V</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="card state-card">
|
||||
<h2>🎯 State</h2>
|
||||
<div class="state-display">
|
||||
<span id="robot-state" class="state-badge">IDLE</span>
|
||||
<div class="sensor-health">
|
||||
<span class="health-item" title="Camera">📷 <span id="camera-health" class="health-ok">✓</span></span>
|
||||
<span class="health-item" title="Lidar">🔍 <span id="lidar-health" class="health-ok">✓</span></span>
|
||||
<span class="health-item" title="Audio">🎤 <span id="audio-health" class="health-ok">✓</span></span>
|
||||
<span class="health-item" title="IMU">📡 <span id="imu-health" class="health-ok">✓</span></span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Motor & WiFi Row -->
|
||||
<div class="grid-2">
|
||||
<div class="card motors-card">
|
||||
<h2>⚙️ Motors</h2>
|
||||
<div class="motors-display">
|
||||
<div class="motor-item">
|
||||
<label>Left</label>
|
||||
<div class="motor-meter">
|
||||
<div id="left-motor" class="motor-bar"></div>
|
||||
</div>
|
||||
<span id="left-speed">0 RPM</span>
|
||||
</div>
|
||||
<div class="motor-item">
|
||||
<label>Right</label>
|
||||
<div class="motor-meter">
|
||||
<div id="right-motor" class="motor-bar"></div>
|
||||
</div>
|
||||
<span id="right-speed">0 RPM</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="card wifi-card">
|
||||
<h2>📶 WiFi</h2>
|
||||
<div class="wifi-display">
|
||||
<div class="wifi-icon" id="wifi-icon">📡</div>
|
||||
<div class="wifi-text">
|
||||
<span id="wifi-strength">-50 dBm</span>
|
||||
<span class="subtitle">Signal Strength</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Map & Events Row -->
|
||||
<div class="grid-full">
|
||||
<div class="card map-card">
|
||||
<h2>🗺️ Map</h2>
|
||||
<canvas id="map-canvas" class="map-display"></canvas>
|
||||
<div class="map-info">
|
||||
<span id="map-res">--</span> | <span id="map-size">--</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="grid-full">
|
||||
<div class="card events-card">
|
||||
<h2>📋 Event Log (Last 20)</h2>
|
||||
<div id="event-log" class="event-list">
|
||||
<p class="placeholder">Waiting for events...</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</main>
|
||||
|
||||
<footer>
|
||||
<span id="update-time">Last update: --</span>
|
||||
</footer>
|
||||
</div>
|
||||
|
||||
<script src="{{ url_for('static', filename='js/dashboard.js') }}"></script>
|
||||
</body>
|
||||
</html>
|
||||
Loading…
x
Reference in New Issue
Block a user