Merge remote-tracking branch 'origin/sl-webui/issue-482-behavior-tree' into sl-webui/issue-504-integration-tests
# 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
7678c48659
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"?>
|
<?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">
|
<root BTCPP_format="4">
|
||||||
|
|
||||||
<!-- ════════════════════════════════════════════════════════════════════════════ -->
|
|
||||||
<!-- MAIN AUTONOMOUS COORDINATOR STATE MACHINE -->
|
|
||||||
<!-- ════════════════════════════════════════════════════════════════════════════ -->
|
|
||||||
<BehaviorTree ID="AutonomousCoordinator">
|
<BehaviorTree ID="AutonomousCoordinator">
|
||||||
|
|
||||||
<ReactiveFallback name="RootFallback">
|
<ReactiveFallback name="RootFallback">
|
||||||
<!-- Emergency stop override — highest priority -->
|
<Inverter><CheckEmergencyStop/></Inverter>
|
||||||
<Inverter>
|
|
||||||
<IsEmergencyStopped/>
|
|
||||||
</Inverter>
|
|
||||||
|
|
||||||
<!-- Main autonomous state machine -->
|
|
||||||
<Sequence name="AutonomousSequence">
|
<Sequence name="AutonomousSequence">
|
||||||
<!-- Initialize blackboard and verify readiness -->
|
<SetBlackboardVariable var_name="current_mode" var_value="idle"/>
|
||||||
<InitializeAutonomousMode/>
|
<Sequence name="IdleState">
|
||||||
|
<StopRobot/>
|
||||||
<!-- Main state machine loop -->
|
<WaitForActivation timeout_ms="60000"/>
|
||||||
<KeepRunningUntilFailure>
|
</Sequence>
|
||||||
<StateMachine initial_state="idle">
|
<Sequence name="PatrolState">
|
||||||
|
<SetBlackboardVariable var_name="current_mode" var_value="patrol"/>
|
||||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
<CheckBatteryLevel threshold="20"/>
|
||||||
<!-- STATE: IDLE -->
|
<PatrolWithCuriosity/>
|
||||||
<!-- Wait for activation signal or battery recovery -->
|
</Sequence>
|
||||||
<!-- ──────────────────────────────────────────────────────────────────── -->
|
<Sequence name="InvestigateState">
|
||||||
<State ID="idle">
|
<SetBlackboardVariable var_name="current_mode" var_value="investigate"/>
|
||||||
<Sequence name="IdleSequence">
|
<FollowDetectedPerson/>
|
||||||
<SetBlackboardVariable var_name="current_mode" var_value="idle"/>
|
</Sequence>
|
||||||
<StopRobot/>
|
<Sequence name="InteractState">
|
||||||
|
<SetBlackboardVariable var_name="current_mode" var_value="interact"/>
|
||||||
<!-- Wait for activation (or battery recovery) -->
|
<RecognizeAndGreet/>
|
||||||
<Parallel success_threshold="1" failure_threshold="2">
|
<Wait wait_duration="30"/>
|
||||||
<WaitForActivation timeout_ms="60000"/>
|
</Sequence>
|
||||||
<Sequence>
|
<Sequence name="ReturnState">
|
||||||
<CheckBatteryLevel threshold="50"/>
|
<SetBlackboardVariable var_name="current_mode" var_value="return"/>
|
||||||
<Wait wait_duration="2"/>
|
<NavigateToHome retry_count="3"/>
|
||||||
</Sequence>
|
<StopRobot/>
|
||||||
</Parallel>
|
</Sequence>
|
||||||
</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>
|
</Sequence>
|
||||||
|
|
||||||
</ReactiveFallback>
|
</ReactiveFallback>
|
||||||
|
|
||||||
</BehaviorTree>
|
</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>
|
</root>
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
@ -1,178 +1,19 @@
|
|||||||
"""
|
"""Autonomous Mode Launcher (Issue #482)"""
|
||||||
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 import LaunchDescription
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
from launch.substitutions import PathJoinSubstitution
|
||||||
import os
|
|
||||||
|
|
||||||
def generate_launch_description():
|
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([
|
return LaunchDescription([
|
||||||
|
Node(package="saltybot_bt_executor", executable="behavior_tree_executor", name="autonomous_coordinator",
|
||||||
# ────────────────────────────────────────────────────────────────────────
|
parameters=[{"bt_xml_filename": PathJoinSubstitution([bt_xml_dir, "autonomous_coordinator.xml"])}]),
|
||||||
# Behavior Tree Executor Node (Nav2 BT framework)
|
Node(package="saltybot_battery_monitor", executable="battery_monitor_node", name="battery_monitor"),
|
||||||
# ────────────────────────────────────────────────────────────────────────
|
Node(package="saltybot_perception", executable="person_detector", name="person_detector"),
|
||||||
Node(
|
Node(package="saltybot_lidar_avoidance", executable="obstacle_monitor", name="obstacle_monitor"),
|
||||||
package='saltybot_bt_executor',
|
Node(package="saltybot_follower", executable="person_follower", name="person_follower"),
|
||||||
executable='behavior_tree_executor',
|
Node(package="saltybot_curiosity", executable="curiosity_explorer", name="curiosity_explorer"),
|
||||||
name='autonomous_coordinator',
|
Node(package="saltybot_emergency_stop", executable="emergency_stop_monitor", name="emergency_stop_monitor"),
|
||||||
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,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}"/>
|
<origin xyz="0 ${-sensor_arm_r} 0" rpy="0 ${cam_tilt} ${-pi/2}"/>
|
||||||
</joint>
|
</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>
|
</robot>
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user