Merge remote-tracking branch 'origin/sl-webui/issue-482-behavior-tree'
# Conflicts: # jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml # jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py
This commit is contained in:
commit
d97fa5fab0
75
jetson/ros2_ws/src/saltybot_bringup/BEHAVIOR_TREE_README.md
Normal file
75
jetson/ros2_ws/src/saltybot_bringup/BEHAVIOR_TREE_README.md
Normal file
@ -0,0 +1,75 @@
|
||||
# SaltyBot Autonomous Behavior Tree Coordinator (Issue #482)
|
||||
|
||||
## Overview
|
||||
|
||||
Autonomous mode state machine with 5 states:
|
||||
```
|
||||
idle → patrol → investigate → interact → return → idle
|
||||
```
|
||||
|
||||
## States
|
||||
|
||||
### IDLE: Waiting/Charging
|
||||
- Robot stationary, waiting for activation
|
||||
- Timeout: 60 seconds
|
||||
- Transition: Moves to PATROL when activated
|
||||
|
||||
### PATROL: Autonomous Patrolling
|
||||
- Executes waypoint routes within geofence (#441)
|
||||
- Integrates curiosity (#470) for autonomous exploration
|
||||
- Monitors for person detection
|
||||
- Battery check: Returns to IDLE if <20%
|
||||
|
||||
### INVESTIGATE: Person Investigation
|
||||
- Approaches and tracks detected person (#212)
|
||||
- Fallback: Navigate to last known position
|
||||
- Transition: → INTERACT when person approached
|
||||
|
||||
### INTERACT: Social Interaction
|
||||
- Face recognition and greeting
|
||||
- Gesture detection and response (#454)
|
||||
- Timeout: 30 seconds
|
||||
- Transition: → RETURN
|
||||
|
||||
### RETURN: Return to Home/Dock
|
||||
- Navigates back to home pose
|
||||
- Nav2 recovery behaviors with retries
|
||||
- Transition: → IDLE when complete
|
||||
|
||||
## Blackboard Variables
|
||||
|
||||
```python
|
||||
battery_level: float # Battery percentage (0-100)
|
||||
person_detected: bool # Person detection state
|
||||
obstacle_state: str # 'clear' | 'near' | 'blocked'
|
||||
current_mode: str # State: idle/patrol/investigate/interact/return
|
||||
home_pose: PoseStamped # Home/dock location
|
||||
```
|
||||
|
||||
## Safety Features
|
||||
|
||||
- **E-Stop** (#459): Highest priority, halts operation immediately
|
||||
- **Battery protection**: Returns home if <20%
|
||||
- **Geofence** (#441): Keeps patrol within 5m boundary
|
||||
- **Obstacle avoidance**: LIDAR monitoring
|
||||
|
||||
## Integration with Features
|
||||
|
||||
- **Patrol** (#446): Waypoint routes
|
||||
- **Curiosity** (#470): Exploration during patrol
|
||||
- **Person Following**: Approach detected persons
|
||||
- **E-Stop** (#459): Emergency override
|
||||
- **Geofence** (#441): Boundary constraint
|
||||
|
||||
## Launch
|
||||
|
||||
```bash
|
||||
ros2 launch saltybot_bringup autonomous_mode.launch.py
|
||||
```
|
||||
|
||||
## Files
|
||||
|
||||
- `autonomous_coordinator.xml`: BehaviorTree definition
|
||||
- `launch/autonomous_mode.launch.py`: Full launch setup
|
||||
- `saltybot_bringup/bt_nodes.py`: Custom BT node plugins
|
||||
- `BEHAVIOR_TREE_README.md`: Documentation
|
||||
@ -1,457 +1,34 @@
|
||||
<?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 -->
|
||||
<Inverter><CheckEmergencyStop/></Inverter>
|
||||
<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"/>
|
||||
<Sequence name="IdleState">
|
||||
<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">
|
||||
<Sequence name="PatrolState">
|
||||
<SetBlackboardVariable var_name="current_mode" var_value="patrol"/>
|
||||
|
||||
<!-- Check battery before patrol -->
|
||||
<Sequence name="BatteryCheck">
|
||||
<Inverter>
|
||||
<CheckBatteryLow threshold="20"/>
|
||||
</Inverter>
|
||||
<CheckBatteryLevel threshold="20"/>
|
||||
<PatrolWithCuriosity/>
|
||||
</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">
|
||||
<Sequence name="InvestigateState">
|
||||
<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"/>
|
||||
<FollowDetectedPerson/>
|
||||
</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">
|
||||
<Sequence name="InteractState">
|
||||
<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 -->
|
||||
<RecognizeAndGreet/>
|
||||
<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">
|
||||
<Sequence name="ReturnState">
|
||||
<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 -->
|
||||
<NavigateToHome retry_count="3"/>
|
||||
<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>
|
||||
|
||||
@ -1,178 +1,19 @@
|
||||
"""
|
||||
autonomous_mode.launch.py — SaltyBot Autonomous Mode Launcher (Issue #482)
|
||||
Starts the behavior tree coordinator with all required subsystems.
|
||||
"""
|
||||
|
||||
"""Autonomous Mode Launcher (Issue #482)"""
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
||||
import os
|
||||
from launch.substitutions import PathJoinSubstitution
|
||||
|
||||
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'])
|
||||
|
||||
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',
|
||||
),
|
||||
|
||||
Node(package="saltybot_bt_executor", executable="behavior_tree_executor", name="autonomous_coordinator",
|
||||
parameters=[{"bt_xml_filename": PathJoinSubstitution([bt_xml_dir, "autonomous_coordinator.xml"])}]),
|
||||
Node(package="saltybot_battery_monitor", executable="battery_monitor_node", name="battery_monitor"),
|
||||
Node(package="saltybot_perception", executable="person_detector", name="person_detector"),
|
||||
Node(package="saltybot_lidar_avoidance", executable="obstacle_monitor", name="obstacle_monitor"),
|
||||
Node(package="saltybot_follower", executable="person_follower", name="person_follower"),
|
||||
Node(package="saltybot_curiosity", executable="curiosity_explorer", name="curiosity_explorer"),
|
||||
Node(package="saltybot_emergency_stop", executable="emergency_stop_monitor", name="emergency_stop_monitor"),
|
||||
])
|
||||
|
||||
@ -0,0 +1,63 @@
|
||||
"""Custom Behavior Tree Node Plugins (Issue #482)"""
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from py_trees import behaviour, common
|
||||
from geometry_msgs.msg import Twist
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from std_msgs.msg import Float32, Bool
|
||||
|
||||
class GetBatteryLevel(behaviour.Behaviour):
|
||||
def __init__(self, name, node: Node):
|
||||
super().__init__(name)
|
||||
self.node = node
|
||||
self.battery_level = 100.0
|
||||
self.subscription = node.create_subscription(BatteryState, '/battery_state', self._battery_callback, 10)
|
||||
def _battery_callback(self, msg):
|
||||
self.battery_level = msg.percentage * 100.0
|
||||
def update(self) -> common.Status:
|
||||
self.blackboard.set('battery_level', self.battery_level, overwrite=True)
|
||||
return common.Status.SUCCESS
|
||||
|
||||
class CheckBatteryLevel(behaviour.Behaviour):
|
||||
def __init__(self, name, node: Node, threshold: float = 50.0):
|
||||
super().__init__(name)
|
||||
self.node = node
|
||||
self.threshold = threshold
|
||||
self.battery_level = 100.0
|
||||
self.subscription = node.create_subscription(BatteryState, '/battery_state', self._battery_callback, 10)
|
||||
def _battery_callback(self, msg):
|
||||
self.battery_level = msg.percentage * 100.0
|
||||
def update(self) -> common.Status:
|
||||
return common.Status.SUCCESS if self.battery_level >= self.threshold else common.Status.FAILURE
|
||||
|
||||
class CheckPersonDetected(behaviour.Behaviour):
|
||||
def __init__(self, name, node: Node):
|
||||
super().__init__(name)
|
||||
self.node = node
|
||||
self.person_detected = False
|
||||
self.subscription = node.create_subscription(Bool, '/person_detection/detected', self._detection_callback, 10)
|
||||
def _detection_callback(self, msg):
|
||||
self.person_detected = msg.data
|
||||
def update(self) -> common.Status:
|
||||
self.blackboard.set('person_detected', self.person_detected, overwrite=True)
|
||||
return common.Status.SUCCESS if self.person_detected else common.Status.FAILURE
|
||||
|
||||
class StopRobot(behaviour.Behaviour):
|
||||
def __init__(self, name, node: Node):
|
||||
super().__init__(name)
|
||||
self.node = node
|
||||
self.cmd_vel_pub = node.create_publisher(Twist, '/cmd_vel', 10)
|
||||
def update(self) -> common.Status:
|
||||
msg = Twist()
|
||||
self.cmd_vel_pub.publish(msg)
|
||||
return common.Status.SUCCESS
|
||||
|
||||
class SetBlackboardVariable(behaviour.Behaviour):
|
||||
def __init__(self, name, node: Node, var_name: str, var_value):
|
||||
super().__init__(name)
|
||||
self.node = node
|
||||
self.var_name = var_name
|
||||
self.var_value = var_value
|
||||
def update(self) -> common.Status:
|
||||
self.blackboard.set(self.var_name, self.var_value, overwrite=True)
|
||||
return common.Status.SUCCESS
|
||||
@ -283,4 +283,32 @@
|
||||
<origin xyz="0 ${-sensor_arm_r} 0" rpy="0 ${cam_tilt} ${-pi/2}"/>
|
||||
</joint>
|
||||
|
||||
<!-- Pan Servo Link -->
|
||||
<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">
|
||||
<parent link="sensor_head_link"/>
|
||||
<child link="pan_servo"/>
|
||||
<origin xyz="${sensor_arm_r} 0 ${-0.020}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- Tilt Servo Link -->
|
||||
<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">
|
||||
<parent link="pan_servo"/>
|
||||
<child link="tilt_servo"/>
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user