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