sl-mechanical
fd742f6890
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>
179 lines
9.6 KiB
Python
179 lines
9.6 KiB
Python
"""
|
|
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',
|
|
),
|
|
|
|
])
|