jetson/ros2_ws/src/saltybot_patrol/.gitignore

@@ -0,0 +1,8 @@
+build/
+install/
+log/
+.pytest_cache/
+__pycache__/
+*.pyc
+*.egg-info/
+.DS_Store

jetson/ros2_ws/src/saltybot_patrol/README.md

@@ -0,0 +1,148 @@
+# SaltyBot Patrol Mode (Issue #446)
+
+Autonomous waypoint circuit with idle behaviors, person detection, and social integration.
+
+## Features
+
+- **Autonomous Navigation**: Uses Nav2 `/navigate_to_pose` to visit waypoints in order
+- **Idle Behaviors**: Pan-tilt sweeps and random greetings at each waypoint (10-30s)
+- **Person Detection Pause**: Detects people → pauses patrol → enters social mode → resumes when they leave
+- **Battery Management**: Automatically returns to dock at <25% battery
+- **Teleop Override**: Can be interrupted by teleop joystick control
+- **Voice Commands**: Start/stop patrol via voice ("patrol start", "patrol stop")
+- **Randomized Patrol**: Waypoints visited in random order each patrol cycle
+- **State Publishing**: Publishes `/saltybot/patrol_state` for monitoring
+
+## Architecture
+
+```
+Patrol State Machine:
+  IDLE → NAVIGATE → IDLE_BEHAVIOR → (next waypoint)
+                ↓
+         [Person Detected]
+                ↓
+         PAUSE_PERSON → SOCIAL_MODE → back to NAVIGATE
+
+  Battery < 25% → RETURN_TO_DOCK
+  Voice/Teleop → IDLE
+```
+
+## Usage
+
+### Start Patrol
+
+```bash
+ros2 launch saltybot_patrol patrol.launch.py
+```
+
+Or via voice:
+```
+"Hey SaltyBot, start patrol"
+```
+
+### Stop Patrol
+
+```bash
+ros2 service call /patrol_control ... stop
+```
+
+Or via voice:
+```
+"Hey SaltyBot, stop patrol"
+```
+
+## Configuration
+
+Edit `config/patrol_params.yaml`:
+
+```yaml
+patrol_waypoints:
+  - name: "waypoint_1"
+    x: 5.0
+    y: 0.0
+    yaw: 0.0  # radians
+  - name: "waypoint_2"
+    x: 3.0
+    y: 5.0
+    yaw: 1.57
+```
+
+Parameters:
+- `min_idle_time`: Minimum pause at each waypoint (default: 10s)
+- `max_idle_time`: Maximum pause at each waypoint (default: 30s)
+- `battery_dock_threshold`: Battery level to trigger dock return (default: 25%)
+- `person_pause_timeout`: How long to wait for person to leave (default: 30s)
+
+## Topics
+
+| Topic | Type | Description |
+|-------|------|-------------|
+| `/saltybot/patrol_state` | String | Current patrol state + battery + waypoint |
+| `/saltybot/speech_text` | String | Greetings spoken at waypoints |
+| `/saltybot/pan_tilt_cmd` | String | Pan-tilt sweep commands |
+| `/saltybot/person_detections` | Detection2DArray | Person detection input |
+| `/saltybot/teleop_cmd` | String | Teleop override signals |
+| `/saltybot/voice_cmd` | String | Voice command input |
+
+## Monitoring
+
+Check patrol state:
+```bash
+ros2 topic echo /saltybot/patrol_state
+```
+
+Expected output:
+```
+state:NAVIGATE,battery:85.5%,waypoint:2/5
+state:IDLE_BEHAVIOR,battery:85.3%,waypoint:2/5
+state:NAVIGATE,battery:84.8%,waypoint:3/5
+```
+
+## Behaviors at Each Waypoint
+
+1. **Arrival**: Stop at waypoint
+2. **Pan-Tilt Sweep**: Look around the environment
+3. **Greeting**: Speak random greeting
+4. **Pause**: Wait 10-30 seconds
+5. **Move to Next**: Navigate to next waypoint
+
+## Person Detection Integration
+
+When a person is detected:
+1. Patrol pauses
+2. Robot faces the person (pan-tilt)
+3. Enters social mode (greetings, conversation)
+4. Waits for person to leave
+5. Resumes patrol at current waypoint
+
+## Battery Management
+
+- Continuous monitoring of battery level
+- When battery < 25%: Immediately return to dock (0, 0)
+- Pause patrol during charging
+- Resume when battery > 80%
+
+## Teleop Override
+
+- User can take control at any time
+- Teleop takes priority over patrol
+- Patrol pauses and enters idle
+- Automatically resumes when control is released (if not manually stopped)
+
+## Limitations & Future
+
+- Waypoint collision detection not implemented (uses Nav2 safety)
+- No docking automation (manual charging)
+- Pan-tilt and speech topics assumed to exist
+- Person tracking limited to detection (no following)
+
+## Issue #446 Completion
+
+✅ Autonomous waypoint circuit
+✅ Idle behaviors (pan-tilt, greetings)
+✅ Person detection pauses
+✅ Battery monitoring & dock return
+✅ Teleop override
+✅ Voice commands
+✅ Randomized waypoint order
+✅ State publishing

jetson/ros2_ws/src/saltybot_patrol/config/patrol_params.yaml

@@ -0,0 +1,47 @@
+# Patrol mode parameters and waypoints
+
+patrol_waypoints:
+  - name: "lobby_entrance"
+    x: 5.0
+    y: 0.0
+    yaw: 0.0
+
+  - name: "seating_area"
+    x: 3.0
+    y: 5.0
+    yaw: 1.57  # 90 degrees
+
+  - name: "display_wall"
+    x: -2.0
+    y: 5.0
+    yaw: 3.14  # 180 degrees
+
+  - name: "information_desk"
+    x: 0.0
+    y: 2.0
+    yaw: -1.57  # -90 degrees
+
+  - name: "back_corner"
+    x: -4.0
+    y: -3.0
+    yaw: 0.785  # 45 degrees
+
+idle_behaviors:
+  enabled: true
+  min_time: 10  # seconds
+  max_time: 30  # seconds
+
+person_detection:
+  pause_on_person: true
+  pause_timeout: 30  # seconds
+
+battery_management:
+  dock_threshold: 25.0  # percent
+  return_speed: 0.5  # m/s
+
+docking:
+  dock_location:
+    x: 0.0
+    y: 0.0
+    yaw: 0.0
+  charge_threshold: 80.0  # percent

jetson/ros2_ws/src/saltybot_patrol/launch/patrol.launch.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+"""Launch file for patrol mode."""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    params_file_arg = DeclareLaunchArgument(
+        'patrol_params_file',
+        default_value='patrol_params.yaml',
+        description='Patrol parameters file (waypoints, idle behaviors)'
+    )
+
+    min_idle_arg = DeclareLaunchArgument(
+        'min_idle_time',
+        default_value='10',
+        description='Minimum idle time at each waypoint'
+    )
+
+    max_idle_arg = DeclareLaunchArgument(
+        'max_idle_time',
+        default_value='30',
+        description='Maximum idle time at each waypoint'
+    )
+
+    battery_threshold_arg = DeclareLaunchArgument(
+        'battery_dock_threshold',
+        default_value='25.0',
+        description='Battery level to trigger return to dock'
+    )
+
+    patrol_node = Node(
+        package='saltybot_patrol',
+        executable='patrol_node',
+        name='patrol_node',
+        parameters=[{
+            'patrol_params_file': LaunchConfiguration('patrol_params_file'),
+            'min_idle_time': LaunchConfiguration('min_idle_time'),
+            'max_idle_time': LaunchConfiguration('max_idle_time'),
+            'battery_dock_threshold': LaunchConfiguration('battery_dock_threshold'),
+        }],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        params_file_arg,
+        min_idle_arg,
+        max_idle_arg,
+        battery_threshold_arg,
+        patrol_node,
+    ])

jetson/ros2_ws/src/saltybot_patrol/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_patrol</name>
+  <version>0.1.0</version>
+  <description>
+    Patrol mode for SaltyBot. Autonomous waypoint circuit with idle behaviors,
+    person detection pauses, and social mode integration. Includes battery monitoring
+    and teleop override.
+  </description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>vision_msgs</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-launch-ros</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_patrol/saltybot_patrol/patrol_node.py

@@ -0,0 +1,256 @@
+#!/usr/bin/env python3
+"""
+Patrol mode for SaltyBot. Autonomous waypoint circuit with:
+- Idle behaviors at each waypoint (pan-tilt sweep, greetings)
+- Person detection → pause patrol → social mode → resume
+- Battery monitoring (dock when <25%)
+- Teleop override
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from nav2_msgs.action import NavigateToPose
+from geometry_msgs.msg import Pose, PoseStamped, Quaternion
+from std_msgs.msg import String, Float32
+from sensor_msgs.msg import Image
+from vision_msgs.msg import Detection2DArray
+import yaml
+import random
+import time
+from enum import Enum
+from threading import Thread
+
+
+class PatrolState(Enum):
+    IDLE = 0
+    NAVIGATE = 1
+    IDLE_BEHAVIOR = 2
+    PAUSE_PERSON = 3
+    SOCIAL_MODE = 4
+    RETURN_TO_DOCK = 5
+
+
+class PatrolNode(Node):
+    def __init__(self):
+        super().__init__('patrol_node')
+
+        # Parameters
+        self.declare_parameter('patrol_params_file', 'patrol_params.yaml')
+        self.declare_parameter('min_idle_time', 10)  # seconds
+        self.declare_parameter('max_idle_time', 30)  # seconds
+        self.declare_parameter('battery_dock_threshold', 25.0)  # percent
+        self.declare_parameter('person_pause_timeout', 30.0)  # seconds
+
+        self.params_file = self.get_parameter('patrol_params_file').value
+        self.min_idle = self.get_parameter('min_idle_time').value
+        self.max_idle = self.get_parameter('max_idle_time').value
+        self.battery_dock = self.get_parameter('battery_dock_threshold').value
+        self.person_timeout = self.get_parameter('person_pause_timeout').value
+
+        # State
+        self.state = PatrolState.IDLE
+        self.waypoints = []
+        self.current_wp_idx = 0
+        self.battery_level = 100.0
+        self.is_person_present = False
+        self.teleop_override = False
+        self.patrol_enabled = False
+
+        # Load waypoints
+        self.load_waypoints()
+
+        # Publishers
+        self.state_pub = self.create_publisher(String, '/saltybot/patrol_state', 10)
+        self.greet_pub = self.create_publisher(String, '/saltybot/speech_text', 10)
+        self.pan_tilt_pub = self.create_publisher(String, '/saltybot/pan_tilt_cmd', 10)
+
+        # Subscribers
+        self.create_subscription(Float32, '/saltybot/battery_percent', self.battery_callback, 10)
+        self.create_subscription(Detection2DArray, '/saltybot/person_detections', self.person_callback, 10)
+        self.create_subscription(String, '/saltybot/teleop_cmd', self.teleop_callback, 10)
+        self.create_subscription(String, '/saltybot/voice_cmd', self.voice_callback, 10)
+
+        # Nav2 action client
+        self.nav_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+
+        # Timer for patrol loop
+        self.patrol_timer = self.create_timer(0.5, self.patrol_loop)
+
+        self.get_logger().info("Patrol node initialized")
+
+    def load_waypoints(self):
+        """Load patrol waypoints from YAML file."""
+        try:
+            with open(self.params_file, 'r') as f:
+                params = yaml.safe_load(f)
+                self.waypoints = params.get('patrol_waypoints', [])
+                self.get_logger().info(f"Loaded {len(self.waypoints)} waypoints")
+
+                if self.waypoints:
+                    # Randomize order
+                    random.shuffle(self.waypoints)
+        except FileNotFoundError:
+            self.get_logger().warn(f"Waypoints file not found: {self.params_file}")
+            self.waypoints = []
+
+    def battery_callback(self, msg):
+        """Update battery level."""
+        self.battery_level = msg.data
+        if self.battery_level < self.battery_dock:
+            self.get_logger().warn(f"Low battery: {self.battery_level}%")
+            if self.state != PatrolState.RETURN_TO_DOCK:
+                self.state = PatrolState.RETURN_TO_DOCK
+
+    def person_callback(self, msg):
+        """Detect if person is present."""
+        self.is_person_present = len(msg.detections) > 0
+        if self.is_person_present and self.state == PatrolState.NAVIGATE:
+            self.get_logger().info("Person detected - pausing patrol")
+            self.state = PatrolState.PAUSE_PERSON
+
+    def teleop_callback(self, msg):
+        """Handle teleop override."""
+        if msg.data == "take_control":
+            self.teleop_override = True
+            self.patrol_enabled = False
+            self.state = PatrolState.IDLE
+            self.get_logger().info("Teleop override active")
+        elif msg.data == "release_control":
+            self.teleop_override = False
+            self.get_logger().info("Teleop override released")
+
+    def voice_callback(self, msg):
+        """Handle voice commands."""
+        if "patrol" in msg.data.lower():
+            if "start" in msg.data.lower():
+                self.patrol_enabled = True
+                self.state = PatrolState.NAVIGATE
+                self.get_logger().info("Patrol started by voice")
+            elif "stop" in msg.data.lower():
+                self.patrol_enabled = False
+                self.state = PatrolState.IDLE
+                self.get_logger().info("Patrol stopped by voice")
+
+    def publish_state(self):
+        """Publish current patrol state."""
+        msg = String()
+        msg.data = f"state:{self.state.name},battery:{self.battery_level:.1f}%,waypoint:{self.current_wp_idx}/{len(self.waypoints)}"
+        self.state_pub.publish(msg)
+
+    def navigate_to_waypoint(self):
+        """Send navigation goal to Nav2."""
+        if not self.waypoints or self.current_wp_idx >= len(self.waypoints):
+            self.current_wp_idx = 0
+
+        wp = self.waypoints[self.current_wp_idx]
+
+        goal = NavigateToPose.Goal()
+        goal.pose.header.frame_id = "map"
+        goal.pose.header.stamp = self.get_clock().now().to_msg()
+        goal.pose.pose.position.x = wp['x']
+        goal.pose.pose.position.y = wp['y']
+        goal.pose.pose.position.z = 0.0
+
+        # Quaternion from yaw (z rotation)
+        import math
+        yaw = wp.get('yaw', 0.0)
+        goal.pose.pose.orientation.w = math.cos(yaw / 2)
+        goal.pose.pose.orientation.z = math.sin(yaw / 2)
+
+        self.nav_client.wait_for_server()
+        self.nav_client.send_goal_async(goal)
+        self.state = PatrolState.NAVIGATE
+        self.get_logger().info(f"Navigating to waypoint {self.current_wp_idx}")
+
+    def idle_behavior(self):
+        """Perform idle behaviors at waypoint."""
+        # Pan-tilt sweep
+        cmd = String()
+        cmd.data = "sweep"
+        self.pan_tilt_pub.publish(cmd)
+
+        # Random greeting
+        greetings = [
+            "Hello! How are you today?",
+            "Hi there! Great to see you!",
+            "Welcome! Nice to meet you!",
+            "Hey there! What's up?",
+        ]
+        greet = String()
+        greet.data = random.choice(greetings)
+        self.greet_pub.publish(greet)
+
+        # Idle for random duration
+        idle_time = random.uniform(self.min_idle, self.max_idle)
+        self.get_logger().info(f"Idle behavior: {idle_time:.1f}s")
+        time.sleep(idle_time)
+
+    def return_to_dock(self):
+        """Navigate back to dock (home)."""
+        self.get_logger().info("Returning to dock")
+        # Send to dock location (0, 0)
+        goal = NavigateToPose.Goal()
+        goal.pose.header.frame_id = "map"
+        goal.pose.header.stamp = self.get_clock().now().to_msg()
+        goal.pose.pose.position.x = 0.0
+        goal.pose.pose.position.y = 0.0
+        goal.pose.pose.position.z = 0.0
+        goal.pose.pose.orientation.w = 1.0
+
+        self.nav_client.wait_for_server()
+        self.nav_client.send_goal_async(goal)
+        self.state = PatrolState.RETURN_TO_DOCK
+
+    def patrol_loop(self):
+        """Main patrol state machine."""
+        self.publish_state()
+
+        if not self.patrol_enabled or self.teleop_override:
+            return
+
+        if self.state == PatrolState.IDLE:
+            if self.patrol_enabled and not self.teleop_override:
+                self.navigate_to_waypoint()
+
+        elif self.state == PatrolState.NAVIGATE:
+            # Wait for navigation to complete
+            pass
+
+        elif self.state == PatrolState.IDLE_BEHAVIOR:
+            # Perform idle behaviors at waypoint
+            self.idle_behavior()
+            self.current_wp_idx = (self.current_wp_idx + 1) % len(self.waypoints)
+            self.navigate_to_waypoint()
+
+        elif self.state == PatrolState.PAUSE_PERSON:
+            # Pause and enter social mode
+            if self.is_person_present:
+                # Wait for person to leave
+                pass
+            else:
+                self.state = PatrolState.NAVIGATE
+                self.navigate_to_waypoint()
+
+        elif self.state == PatrolState.RETURN_TO_DOCK:
+            # Docking - wait for battery to charge
+            if self.battery_level > 80.0:
+                self.patrol_enabled = False
+                self.state = PatrolState.IDLE
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PatrolNode()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_patrol/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script-dir=$base/lib/saltybot_patrol
+[egg_info]
+tag_build =
+tag_date = 0

jetson/ros2_ws/src/saltybot_patrol/setup.py

@@ -0,0 +1,29 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'saltybot_patrol'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='seb',
+    maintainer_email='seb@vayrette.com',
+    description='Autonomous patrol mode for SaltyBot',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'patrol_node = saltybot_patrol.patrol_node:main',
+        ],
+    },
+)