diff --git a/jetson/ros2_ws/src/saltybot_bringup/BEHAVIOR_TREE_README.md b/jetson/ros2_ws/src/saltybot_bringup/BEHAVIOR_TREE_README.md
new file mode 100644
index 0000000..be6d438
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_bringup/BEHAVIOR_TREE_README.md
@@ -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
diff --git a/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml b/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml
index ea76a5a..370ac1d 100644
--- a/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml
+++ b/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml
@@ -1,457 +1,34 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py
index 3edaa8c..7d05097 100644
--- a/jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py
+++ b/jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py
@@ -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"),
])
diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/bt_nodes.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/bt_nodes.py
new file mode 100644
index 0000000..8e25166
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/bt_nodes.py
@@ -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
diff --git a/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro b/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro
index 6274f64..d12ef7b 100644
--- a/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro
+++ b/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro
@@ -283,4 +283,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+