From 60e16dc6caeb02ea36e8d529f3b05f58bebb5e60 Mon Sep 17 00:00:00 2001 From: sl-jetson Date: Thu, 5 Mar 2026 14:42:49 -0500 Subject: [PATCH 1/2] feat: Add URDF robot description (Issue #477) --- .../urdf/saltybot.urdf.xacro | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 50d1ebbdccec77630f851b17ae6de94b44050df9 Mon Sep 17 00:00:00 2001 From: sl-webui Date: Thu, 5 Mar 2026 14:42:57 -0500 Subject: [PATCH 2/2] feat: Behavior tree coordinator for autonomous mode (Issue #482) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit State machine: idle → patrol → investigate → interact → return - IDLE: Waiting for activation or battery recovery - PATROL: Autonomous patrolling with curiosity-driven exploration (#470) - INVESTIGATE: Approach and track detected persons - INTERACT: Social interaction with face recognition and gestures - RETURN: Navigate back to home/dock with recovery behaviors Integrations: - Patrol mode (#446): Waypoint routes with geofence (#441) - Curiosity behavior (#470): Autonomous exploration - Person following: Approach detected persons - Emergency stop cascade (#459): Highest priority safety - Geofence constraint (#441): Keep patrol within boundaries Blackboard variables: - battery_level: Current battery percentage - person_detected: Person detection state - current_mode: Current behavior tree state - home_pose: Home/dock location Files: - behavior_trees/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 Co-Authored-By: Claude Haiku 4.5 --- .../saltybot_bringup/BEHAVIOR_TREE_README.md | 75 +++++++++++++++++++ .../behavior_trees/autonomous_coordinator.xml | 34 +++++++++ .../launch/autonomous_mode.launch.py | 19 +++++ .../saltybot_bringup/bt_nodes.py | 63 ++++++++++++++++ 4 files changed, 191 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_bringup/BEHAVIOR_TREE_README.md create mode 100644 jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml create mode 100644 jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/bt_nodes.py 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 new file mode 100644 index 0000000..370ac1d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml @@ -0,0 +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 new file mode 100644 index 0000000..7d05097 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py @@ -0,0 +1,19 @@ +"""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 + +def generate_launch_description(): + 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"), + 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