jetson/ros2_ws/src/saltybot_bringup/BEHAVIOR_TREE_README.md

@@ -1,75 +0,0 @@
-# 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

jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml

@@ -1,34 +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">
-      <Inverter><CheckEmergencyStop/></Inverter>
+      <!-- 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"/>
-        <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>
-        <Sequence name="PatrolState">
+                </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"/>
-          <CheckBatteryLevel threshold="20"/>
+
-          <PatrolWithCuriosity/>
+                <!-- Check battery before patrol -->
+                <Sequence name="BatteryCheck">
+                  <Inverter>
+                    <CheckBatteryLow threshold="20"/>
+                  </Inverter>
                 </Sequence>
-        <Sequence name="InvestigateState">
+
+                <!-- 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"/>
-          <FollowDetectedPerson/>
+
+                <!-- 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>
-        <Sequence name="InteractState">
+
+                  <!-- 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"/>
-          <RecognizeAndGreet/>
+
+                <!-- 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>
-        <Sequence name="ReturnState">
+              <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"/>
-          <NavigateToHome retry_count="3"/>
+
+                <!-- 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} &lt; {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>

jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py

@@ -1,19 +1,178 @@
-"""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_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-from launch.substitutions import PathJoinSubstitution
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+import os
 
 def generate_launch_description():
-    saltybot_bringup_dir = FindPackageShare("saltybot_bringup")
+    """Generate launch description for autonomous mode."""
-    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([
-        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"),
+        # Behavior Tree Executor Node (Nav2 BT framework)
-        Node(package="saltybot_perception", executable="person_detector", name="person_detector"),
+        # ────────────────────────────────────────────────────────────────────────
-        Node(package="saltybot_lidar_avoidance", executable="obstacle_monitor", name="obstacle_monitor"),
+        Node(
-        Node(package="saltybot_follower", executable="person_follower", name="person_follower"),
+            package='saltybot_bt_executor',
-        Node(package="saltybot_curiosity", executable="curiosity_explorer", name="curiosity_explorer"),
+            executable='behavior_tree_executor',
-        Node(package="saltybot_emergency_stop", executable="emergency_stop_monitor", name="emergency_stop_monitor"),
+            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',
+        ),
+
     ])

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/bt_nodes.py

@@ -1,63 +0,0 @@
-"""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

jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro

@@ -283,32 +283,4 @@
     <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>