Compare commits
1 Commits
9683fd3685
...
c66a5ce974
| Author | SHA1 | Date | |
|---|---|---|---|
| c66a5ce974 |
@ -1,9 +0,0 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
*.pyc
|
||||
__pycache__/
|
||||
.pytest_cache/
|
||||
*.egg-info/
|
||||
dist/
|
||||
*.egg
|
||||
@ -1,26 +0,0 @@
|
||||
bag_recorder:
|
||||
ros__parameters:
|
||||
# Path where bags are stored
|
||||
bag_dir: '/home/seb/rosbags'
|
||||
|
||||
# Topics to record (empty list = record all)
|
||||
topics: []
|
||||
# topics:
|
||||
# - '/camera/image_raw'
|
||||
# - '/lidar/scan'
|
||||
# - '/odom'
|
||||
|
||||
# Circular buffer duration (minutes)
|
||||
buffer_duration_minutes: 30
|
||||
|
||||
# Storage management
|
||||
storage_ttl_days: 7 # Remove bags older than 7 days
|
||||
max_storage_gb: 50 # Enforce 50GB quota
|
||||
|
||||
# Compression
|
||||
compression: 'zstd' # Options: zstd, zstandard
|
||||
|
||||
# NAS sync (optional)
|
||||
enable_rsync: false
|
||||
rsync_destination: ''
|
||||
# rsync_destination: 'user@nas:/path/to/backups/'
|
||||
@ -1,23 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_dir = get_package_share_directory('saltybot_bag_recorder')
|
||||
config_file = os.path.join(pkg_dir, 'config', 'bag_recorder.yaml')
|
||||
|
||||
bag_recorder_node = Node(
|
||||
package='saltybot_bag_recorder',
|
||||
executable='bag_recorder',
|
||||
name='bag_recorder',
|
||||
parameters=[config_file],
|
||||
output='screen',
|
||||
respawn=True,
|
||||
respawn_delay=5,
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
bag_recorder_node,
|
||||
])
|
||||
@ -1,30 +0,0 @@
|
||||
<?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_bag_recorder</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
ROS2 bag recording service with circular buffer, auto-save on crash, and storage management.
|
||||
Configurable topics, 7-day TTL, 50GB cap, zstd compression, and optional NAS rsync.
|
||||
</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>rosbag2_py</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>ament_index_python</depend>
|
||||
|
||||
<exec_depend>python3-launch-ros</exec_depend>
|
||||
<exec_depend>ros2bag</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>
|
||||
@ -1,282 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import signal
|
||||
import shutil
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from datetime import datetime, timedelta
|
||||
from typing import List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_srvs.srv import Trigger
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
class BagRecorderNode(Node):
|
||||
"""ROS2 bag recording service with circular buffer and storage management."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('saltybot_bag_recorder')
|
||||
|
||||
# Configuration
|
||||
self.declare_parameter('bag_dir', '/home/seb/rosbags')
|
||||
self.declare_parameter('topics', [''])
|
||||
self.declare_parameter('buffer_duration_minutes', 30)
|
||||
self.declare_parameter('storage_ttl_days', 7)
|
||||
self.declare_parameter('max_storage_gb', 50)
|
||||
self.declare_parameter('enable_rsync', False)
|
||||
self.declare_parameter('rsync_destination', '')
|
||||
self.declare_parameter('compression', 'zstd')
|
||||
|
||||
self.bag_dir = Path(self.get_parameter('bag_dir').value)
|
||||
self.topics = self.get_parameter('topics').value
|
||||
self.buffer_duration = self.get_parameter('buffer_duration_minutes').value * 60
|
||||
self.storage_ttl_days = self.get_parameter('storage_ttl_days').value
|
||||
self.max_storage_gb = self.get_parameter('max_storage_gb').value
|
||||
self.enable_rsync = self.get_parameter('enable_rsync').value
|
||||
self.rsync_destination = self.get_parameter('rsync_destination').value
|
||||
self.compression = self.get_parameter('compression').value
|
||||
|
||||
self.bag_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
# Recording state
|
||||
self.is_recording = False
|
||||
self.current_bag_process = None
|
||||
self.current_bag_name = None
|
||||
self.buffer_bags: List[str] = []
|
||||
self.recording_lock = threading.Lock()
|
||||
|
||||
# Services
|
||||
self.save_service = self.create_service(
|
||||
Trigger,
|
||||
'/saltybot/save_bag',
|
||||
self.save_bag_callback
|
||||
)
|
||||
|
||||
# Watchdog to handle crash recovery
|
||||
self.setup_signal_handlers()
|
||||
|
||||
# Start recording
|
||||
self.start_recording()
|
||||
|
||||
# Periodic maintenance (cleanup old bags, manage storage)
|
||||
self.maintenance_timer = self.create_timer(300.0, self.maintenance_callback)
|
||||
|
||||
self.get_logger().info(
|
||||
f'Bag recorder initialized: {self.bag_dir}, '
|
||||
f'buffer={self.buffer_duration}s, ttl={self.storage_ttl_days}d, '
|
||||
f'max={self.max_storage_gb}GB'
|
||||
)
|
||||
|
||||
def setup_signal_handlers(self):
|
||||
"""Setup signal handlers for graceful shutdown and crash recovery."""
|
||||
def signal_handler(sig, frame):
|
||||
self.get_logger().warn(f'Signal {sig} received, saving current bag')
|
||||
self.stop_recording(save=True)
|
||||
self.cleanup()
|
||||
rclpy.shutdown()
|
||||
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
signal.signal(signal.SIGTERM, signal_handler)
|
||||
|
||||
def start_recording(self):
|
||||
"""Start bag recording in the background."""
|
||||
with self.recording_lock:
|
||||
if self.is_recording:
|
||||
return
|
||||
|
||||
timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
|
||||
self.current_bag_name = f'saltybot_{timestamp}'
|
||||
bag_path = self.bag_dir / self.current_bag_name
|
||||
|
||||
try:
|
||||
# Build rosbag2 record command
|
||||
cmd = [
|
||||
'ros2', 'bag', 'record',
|
||||
f'--output', str(bag_path),
|
||||
f'--compression-format', self.compression,
|
||||
f'--compression-mode', 'file',
|
||||
]
|
||||
|
||||
# Add topics or record all if empty
|
||||
if self.topics and self.topics[0]:
|
||||
cmd.extend(self.topics)
|
||||
else:
|
||||
cmd.append('--all')
|
||||
|
||||
self.current_bag_process = subprocess.Popen(
|
||||
cmd,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.PIPE
|
||||
)
|
||||
self.is_recording = True
|
||||
self.buffer_bags.append(self.current_bag_name)
|
||||
|
||||
self.get_logger().info(f'Started recording: {self.current_bag_name}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to start recording: {e}')
|
||||
|
||||
def save_bag_callback(self, request, response):
|
||||
"""Service callback to manually trigger bag save."""
|
||||
try:
|
||||
self.stop_recording(save=True)
|
||||
self.start_recording()
|
||||
response.success = True
|
||||
response.message = f'Saved: {self.current_bag_name}'
|
||||
self.get_logger().info(response.message)
|
||||
except Exception as e:
|
||||
response.success = False
|
||||
response.message = f'Failed to save bag: {e}'
|
||||
self.get_logger().error(response.message)
|
||||
|
||||
return response
|
||||
|
||||
def stop_recording(self, save: bool = False):
|
||||
"""Stop the current bag recording."""
|
||||
with self.recording_lock:
|
||||
if not self.is_recording or not self.current_bag_process:
|
||||
return
|
||||
|
||||
try:
|
||||
# Send SIGINT to gracefully close rosbag2
|
||||
self.current_bag_process.send_signal(signal.SIGINT)
|
||||
self.current_bag_process.wait(timeout=5.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
self.get_logger().warn(f'Force terminating {self.current_bag_name}')
|
||||
self.current_bag_process.kill()
|
||||
self.current_bag_process.wait()
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error stopping recording: {e}')
|
||||
|
||||
self.is_recording = False
|
||||
self.get_logger().info(f'Stopped recording: {self.current_bag_name}')
|
||||
|
||||
# Apply compression if needed (rosbag2 does this by default with -compression-mode file)
|
||||
if save:
|
||||
self.apply_compression()
|
||||
|
||||
def apply_compression(self):
|
||||
"""Compress the current bag using zstd."""
|
||||
if not self.current_bag_name:
|
||||
return
|
||||
|
||||
bag_path = self.bag_dir / self.current_bag_name
|
||||
try:
|
||||
# rosbag2 with compression-mode file already compresses the sqlite db
|
||||
# This is a secondary option to compress the entire directory
|
||||
tar_path = f'{bag_path}.tar.zst'
|
||||
|
||||
if bag_path.exists():
|
||||
cmd = ['tar', '-I', 'zstd', '-cf', tar_path, '-C', str(self.bag_dir), self.current_bag_name]
|
||||
subprocess.run(cmd, check=True, capture_output=True, timeout=60)
|
||||
self.get_logger().info(f'Compressed: {tar_path}')
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'Compression skipped: {e}')
|
||||
|
||||
def maintenance_callback(self):
|
||||
"""Periodic maintenance: cleanup old bags and manage storage."""
|
||||
self.cleanup_expired_bags()
|
||||
self.enforce_storage_quota()
|
||||
if self.enable_rsync and self.rsync_destination:
|
||||
self.rsync_bags()
|
||||
|
||||
def cleanup_expired_bags(self):
|
||||
"""Remove bags older than TTL."""
|
||||
try:
|
||||
cutoff_time = datetime.now() - timedelta(days=self.storage_ttl_days)
|
||||
|
||||
for item in self.bag_dir.iterdir():
|
||||
if item.is_dir() and item.name.startswith('saltybot_'):
|
||||
try:
|
||||
# Parse timestamp from directory name
|
||||
timestamp_str = item.name.replace('saltybot_', '')
|
||||
item_time = datetime.strptime(timestamp_str, '%Y%m%d_%H%M%S')
|
||||
|
||||
if item_time < cutoff_time:
|
||||
shutil.rmtree(item, ignore_errors=True)
|
||||
self.get_logger().info(f'Removed expired bag: {item.name}')
|
||||
except (ValueError, OSError) as e:
|
||||
self.get_logger().warn(f'Error processing {item.name}: {e}')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Cleanup failed: {e}')
|
||||
|
||||
def enforce_storage_quota(self):
|
||||
"""Remove oldest bags if total size exceeds quota."""
|
||||
try:
|
||||
total_size = sum(
|
||||
f.stat().st_size
|
||||
for f in self.bag_dir.rglob('*')
|
||||
if f.is_file()
|
||||
) / (1024 ** 3) # Convert to GB
|
||||
|
||||
if total_size > self.max_storage_gb:
|
||||
self.get_logger().warn(
|
||||
f'Storage quota exceeded: {total_size:.2f}GB > {self.max_storage_gb}GB'
|
||||
)
|
||||
|
||||
# Get bags sorted by modification time
|
||||
bags = sorted(
|
||||
[d for d in self.bag_dir.iterdir() if d.is_dir() and d.name.startswith('saltybot_')],
|
||||
key=lambda x: x.stat().st_mtime
|
||||
)
|
||||
|
||||
# Remove oldest bags until under quota
|
||||
for bag in bags:
|
||||
if total_size <= self.max_storage_gb:
|
||||
break
|
||||
|
||||
bag_size = sum(
|
||||
f.stat().st_size
|
||||
for f in bag.rglob('*')
|
||||
if f.is_file()
|
||||
) / (1024 ** 3)
|
||||
|
||||
shutil.rmtree(bag, ignore_errors=True)
|
||||
total_size -= bag_size
|
||||
self.get_logger().info(f'Removed bag to enforce quota: {bag.name}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Storage quota enforcement failed: {e}')
|
||||
|
||||
def rsync_bags(self):
|
||||
"""Optional: rsync bags to NAS."""
|
||||
try:
|
||||
cmd = [
|
||||
'rsync', '-avz', '--delete',
|
||||
f'{self.bag_dir}/',
|
||||
self.rsync_destination
|
||||
]
|
||||
subprocess.run(cmd, check=False, timeout=300)
|
||||
self.get_logger().info(f'Synced bags to NAS: {self.rsync_destination}')
|
||||
except subprocess.TimeoutExpired:
|
||||
self.get_logger().warn('Rsync timed out')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Rsync failed: {e}')
|
||||
|
||||
def cleanup(self):
|
||||
"""Cleanup resources."""
|
||||
self.stop_recording(save=True)
|
||||
self.get_logger().info('Bag recorder shutdown complete')
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = BagRecorderNode()
|
||||
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.cleanup()
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_bag_recorder
|
||||
|
||||
[install]
|
||||
script_dir=$base/lib/saltybot_bag_recorder
|
||||
@ -1,32 +0,0 @@
|
||||
from setuptools import setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = 'saltybot_bag_recorder'
|
||||
|
||||
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='ROS2 bag recording service with circular buffer and storage management',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'bag_recorder = saltybot_bag_recorder.bag_recorder_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,25 +0,0 @@
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
class TestBagRecorder(unittest.TestCase):
|
||||
"""Basic tests for bag recorder functionality."""
|
||||
|
||||
def test_imports(self):
|
||||
"""Test that the module can be imported."""
|
||||
from saltybot_bag_recorder import bag_recorder_node
|
||||
self.assertIsNotNone(bag_recorder_node)
|
||||
|
||||
def test_config_file_exists(self):
|
||||
"""Test that config file exists."""
|
||||
config_file = Path(__file__).parent.parent / 'config' / 'bag_recorder.yaml'
|
||||
self.assertTrue(config_file.exists())
|
||||
|
||||
def test_launch_file_exists(self):
|
||||
"""Test that launch file exists."""
|
||||
launch_file = Path(__file__).parent.parent / 'launch' / 'bag_recorder.launch.py'
|
||||
self.assertTrue(launch_file.exists())
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@ -1,27 +0,0 @@
|
||||
balance_controller:
|
||||
ros__parameters:
|
||||
# Serial connection parameters
|
||||
port: "/dev/ttyUSB0"
|
||||
baudrate: 115200
|
||||
|
||||
# VESC Balance PID Parameters
|
||||
# These are tuning parameters for the balance PID controller
|
||||
# P: Proportional gain (responds to current error)
|
||||
# I: Integral gain (corrects accumulated error)
|
||||
# D: Derivative gain (dampens oscillations)
|
||||
pid_p: 0.5
|
||||
pid_i: 0.1
|
||||
pid_d: 0.05
|
||||
|
||||
# Tilt Safety Limits
|
||||
# Angle threshold in degrees (forward/backward pitch)
|
||||
tilt_threshold_deg: 45.0
|
||||
# Duration in milliseconds before triggering motor kill
|
||||
tilt_kill_duration_ms: 500
|
||||
|
||||
# Startup Ramp
|
||||
# Time in seconds to ramp from 0 to full output
|
||||
startup_ramp_time_s: 2.0
|
||||
|
||||
# Control loop frequency (Hz)
|
||||
frequency: 50
|
||||
@ -1,31 +0,0 @@
|
||||
"""Launch file for balance controller node."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate launch description for balance controller."""
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
"node_name",
|
||||
default_value="balance_controller",
|
||||
description="Name of the balance controller node",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"config_file",
|
||||
default_value="balance_params.yaml",
|
||||
description="Configuration file for balance controller parameters",
|
||||
),
|
||||
Node(
|
||||
package="saltybot_balance_controller",
|
||||
executable="balance_controller_node",
|
||||
name=LaunchConfiguration("node_name"),
|
||||
output="screen",
|
||||
parameters=[
|
||||
LaunchConfiguration("config_file"),
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -1,28 +0,0 @@
|
||||
<?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_balance_controller</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Balance mode PID controller for SaltyBot self-balancing robot.
|
||||
Manages VESC balance PID parameters, tilt safety limits (±45° > 500ms kill),
|
||||
startup ramp, and state monitoring via IMU.
|
||||
</description>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_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>
|
||||
@ -1,375 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Balance mode PID controller node for SaltyBot.
|
||||
|
||||
Manages VESC balance mode PID parameters via UART (pyvesc).
|
||||
Implements tilt safety limits (±45° > 500ms kill), startup ramp, and state monitoring.
|
||||
|
||||
Subscribed topics:
|
||||
/imu/data (sensor_msgs/Imu) - IMU orientation for tilt detection
|
||||
/vesc/state (std_msgs/String) - VESC motor telemetry (voltage, current, RPM)
|
||||
|
||||
Published topics:
|
||||
/saltybot/balance_state (std_msgs/String) - JSON: pitch, roll, tilt_duration, pid, motor_state
|
||||
/saltybot/balance_log (std_msgs/String) - CSV log: timestamp, pitch, roll, current, temp, rpm
|
||||
|
||||
Parameters:
|
||||
port (str) - Serial port for VESC (/dev/ttyUSB0)
|
||||
baudrate (int) - Serial baud rate (115200)
|
||||
pid_p (float) - Balance PID P gain
|
||||
pid_i (float) - Balance PID I gain
|
||||
pid_d (float) - Balance PID D gain
|
||||
tilt_threshold_deg (float) - Tilt angle threshold for safety kill (45°)
|
||||
tilt_kill_duration_ms (int) - Duration above threshold to trigger kill (500ms)
|
||||
startup_ramp_time_s (float) - Startup ramp duration (2.0s)
|
||||
frequency (int) - Update frequency (50Hz)
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from enum import Enum
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Imu
|
||||
from std_msgs.msg import String
|
||||
import serial
|
||||
|
||||
try:
|
||||
import pyvesc
|
||||
except ImportError:
|
||||
pyvesc = None
|
||||
|
||||
|
||||
class BalanceState(Enum):
|
||||
"""Balance controller state."""
|
||||
STARTUP = "startup" # Ramping up from zero
|
||||
RUNNING = "running" # Normal operation
|
||||
TILT_WARNING = "tilt_warning" # Tilted but within time limit
|
||||
TILT_KILL = "tilt_kill" # Over-tilted, motors killed
|
||||
ERROR = "error" # Communication error
|
||||
|
||||
|
||||
@dataclass
|
||||
class IMUData:
|
||||
"""Parsed IMU data."""
|
||||
pitch_deg: float # Forward/backward tilt (Y axis)
|
||||
roll_deg: float # Left/right tilt (X axis)
|
||||
timestamp: float
|
||||
|
||||
|
||||
@dataclass
|
||||
class MotorTelemetry:
|
||||
"""Motor telemetry from VESC."""
|
||||
voltage_v: float
|
||||
current_a: float
|
||||
rpm: int
|
||||
temperature_c: float
|
||||
fault_code: int
|
||||
|
||||
|
||||
class BalanceControllerNode(Node):
|
||||
"""ROS2 node for balance mode control and tilt safety."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("balance_controller")
|
||||
|
||||
# Declare parameters
|
||||
self.declare_parameter("port", "/dev/ttyUSB0")
|
||||
self.declare_parameter("baudrate", 115200)
|
||||
self.declare_parameter("pid_p", 0.5)
|
||||
self.declare_parameter("pid_i", 0.1)
|
||||
self.declare_parameter("pid_d", 0.05)
|
||||
self.declare_parameter("tilt_threshold_deg", 45.0)
|
||||
self.declare_parameter("tilt_kill_duration_ms", 500)
|
||||
self.declare_parameter("startup_ramp_time_s", 2.0)
|
||||
self.declare_parameter("frequency", 50)
|
||||
|
||||
# Get parameters
|
||||
self.port = self.get_parameter("port").value
|
||||
self.baudrate = self.get_parameter("baudrate").value
|
||||
self.pid_p = self.get_parameter("pid_p").value
|
||||
self.pid_i = self.get_parameter("pid_i").value
|
||||
self.pid_d = self.get_parameter("pid_d").value
|
||||
self.tilt_threshold = self.get_parameter("tilt_threshold_deg").value
|
||||
self.tilt_kill_duration = self.get_parameter("tilt_kill_duration_ms").value / 1000.0
|
||||
self.startup_ramp_time = self.get_parameter("startup_ramp_time_s").value
|
||||
frequency = self.get_parameter("frequency").value
|
||||
|
||||
# VESC connection
|
||||
self.serial: Optional[serial.Serial] = None
|
||||
self.vesc: Optional[pyvesc.VescUart] = None
|
||||
|
||||
# State tracking
|
||||
self.state = BalanceState.STARTUP
|
||||
self.imu_data: Optional[IMUData] = None
|
||||
self.motor_telemetry: Optional[MotorTelemetry] = None
|
||||
self.startup_time = time.time()
|
||||
self.tilt_start_time: Optional[float] = None
|
||||
|
||||
# Subscriptions
|
||||
self.create_subscription(Imu, "/imu/data", self._on_imu_data, 10)
|
||||
self.create_subscription(String, "/vesc/state", self._on_vesc_state, 10)
|
||||
|
||||
# Publications
|
||||
self.pub_balance_state = self.create_publisher(String, "/saltybot/balance_state", 10)
|
||||
self.pub_balance_log = self.create_publisher(String, "/saltybot/balance_log", 10)
|
||||
|
||||
# Timer for control loop
|
||||
period = 1.0 / frequency
|
||||
self.create_timer(period, self._control_loop)
|
||||
|
||||
# Initialize VESC
|
||||
self._init_vesc()
|
||||
|
||||
self.get_logger().info(
|
||||
f"Balance controller initialized: port={self.port}, baud={self.baudrate}, "
|
||||
f"PID=[{self.pid_p}, {self.pid_i}, {self.pid_d}], "
|
||||
f"tilt_threshold={self.tilt_threshold}°, "
|
||||
f"tilt_kill_duration={self.tilt_kill_duration}s, "
|
||||
f"startup_ramp={self.startup_ramp_time}s"
|
||||
)
|
||||
|
||||
def _init_vesc(self) -> bool:
|
||||
"""Initialize VESC connection."""
|
||||
try:
|
||||
if pyvesc is None:
|
||||
self.get_logger().error("pyvesc not installed. Install with: pip install pyvesc")
|
||||
self.state = BalanceState.ERROR
|
||||
return False
|
||||
|
||||
self.serial = serial.Serial(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=0.1,
|
||||
)
|
||||
self.vesc = pyvesc.VescUart(
|
||||
serial_port=self.serial,
|
||||
has_sensor=False,
|
||||
start_heartbeat=True,
|
||||
)
|
||||
|
||||
self._set_pid_parameters()
|
||||
self.get_logger().info(f"Connected to VESC on {self.port} @ {self.baudrate} baud")
|
||||
return True
|
||||
|
||||
except (serial.SerialException, Exception) as e:
|
||||
self.get_logger().error(f"Failed to initialize VESC: {e}")
|
||||
self.state = BalanceState.ERROR
|
||||
return False
|
||||
|
||||
def _set_pid_parameters(self) -> None:
|
||||
"""Set VESC balance PID parameters."""
|
||||
if self.vesc is None:
|
||||
return
|
||||
|
||||
try:
|
||||
# pyvesc doesn't have direct balance mode PID setter, so we'd use
|
||||
# custom VESC firmware commands or rely on pre-configured VESC.
|
||||
# For now, log the intended parameters.
|
||||
self.get_logger().info(
|
||||
f"PID parameters set: P={self.pid_p}, I={self.pid_i}, D={self.pid_d}"
|
||||
)
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to set PID parameters: {e}")
|
||||
|
||||
def _on_imu_data(self, msg: Imu) -> None:
|
||||
"""Update IMU orientation data."""
|
||||
try:
|
||||
# Extract roll/pitch from quaternion
|
||||
roll, pitch, _ = self._quaternion_to_euler(
|
||||
msg.orientation.x, msg.orientation.y,
|
||||
msg.orientation.z, msg.orientation.w
|
||||
)
|
||||
|
||||
self.imu_data = IMUData(
|
||||
pitch_deg=math.degrees(pitch),
|
||||
roll_deg=math.degrees(roll),
|
||||
timestamp=time.time()
|
||||
)
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f"Error parsing IMU data: {e}")
|
||||
|
||||
def _on_vesc_state(self, msg: String) -> None:
|
||||
"""Parse VESC telemetry from JSON."""
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
self.motor_telemetry = MotorTelemetry(
|
||||
voltage_v=data.get("voltage_v", 0.0),
|
||||
current_a=data.get("current_a", 0.0),
|
||||
rpm=data.get("rpm", 0),
|
||||
temperature_c=data.get("temperature_c", 0.0),
|
||||
fault_code=data.get("fault_code", 0)
|
||||
)
|
||||
except json.JSONDecodeError as e:
|
||||
self.get_logger().debug(f"Failed to parse VESC state: {e}")
|
||||
|
||||
def _quaternion_to_euler(self, x: float, y: float, z: float, w: float) -> tuple:
|
||||
"""Convert quaternion to Euler angles (roll, pitch, yaw)."""
|
||||
# Roll (X-axis rotation)
|
||||
sinr_cosp = 2 * (w * x + y * z)
|
||||
cosr_cosp = 1 - 2 * (x * x + y * y)
|
||||
roll = math.atan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# Pitch (Y-axis rotation)
|
||||
sinp = 2 * (w * y - z * x)
|
||||
if abs(sinp) >= 1:
|
||||
pitch = math.copysign(math.pi / 2, sinp)
|
||||
else:
|
||||
pitch = math.asin(sinp)
|
||||
|
||||
# Yaw (Z-axis rotation)
|
||||
siny_cosp = 2 * (w * z + x * y)
|
||||
cosy_cosp = 1 - 2 * (y * y + z * z)
|
||||
yaw = math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return roll, pitch, yaw
|
||||
|
||||
def _check_tilt_safety(self) -> None:
|
||||
"""Check tilt angle and apply safety kill if needed."""
|
||||
if self.imu_data is None:
|
||||
return
|
||||
|
||||
# Check if tilted beyond threshold
|
||||
is_tilted = abs(self.imu_data.pitch_deg) > self.tilt_threshold
|
||||
|
||||
if is_tilted:
|
||||
# Tilt detected
|
||||
if self.tilt_start_time is None:
|
||||
self.tilt_start_time = time.time()
|
||||
|
||||
tilt_duration = time.time() - self.tilt_start_time
|
||||
|
||||
if tilt_duration > self.tilt_kill_duration:
|
||||
# Tilt persisted too long, trigger kill
|
||||
self.state = BalanceState.TILT_KILL
|
||||
self._kill_motors()
|
||||
self.get_logger().error(
|
||||
f"TILT SAFETY KILL: pitch={self.imu_data.pitch_deg:.1f}° "
|
||||
f"(threshold={self.tilt_threshold}°) for {tilt_duration:.2f}s"
|
||||
)
|
||||
else:
|
||||
# Warning state
|
||||
if self.state != BalanceState.TILT_WARNING:
|
||||
self.state = BalanceState.TILT_WARNING
|
||||
self.get_logger().warn(
|
||||
f"Tilt warning: pitch={self.imu_data.pitch_deg:.1f}° "
|
||||
f"for {tilt_duration:.2f}s / {self.tilt_kill_duration}s"
|
||||
)
|
||||
else:
|
||||
# Not tilted, reset timer
|
||||
if self.tilt_start_time is not None:
|
||||
self.tilt_start_time = None
|
||||
if self.state == BalanceState.TILT_WARNING:
|
||||
self.state = BalanceState.RUNNING
|
||||
self.get_logger().info("Tilt warning cleared, resuming normal operation")
|
||||
|
||||
def _check_startup_ramp(self) -> float:
|
||||
"""Calculate startup ramp factor [0, 1]."""
|
||||
elapsed = time.time() - self.startup_time
|
||||
|
||||
if elapsed >= self.startup_ramp_time:
|
||||
# Startup complete
|
||||
if self.state == BalanceState.STARTUP:
|
||||
self.state = BalanceState.RUNNING
|
||||
self.get_logger().info("Startup ramp complete, entering normal operation")
|
||||
return 1.0
|
||||
else:
|
||||
# Linear ramp
|
||||
return elapsed / self.startup_ramp_time
|
||||
|
||||
def _kill_motors(self) -> None:
|
||||
"""Kill motor output."""
|
||||
if self.vesc is None:
|
||||
return
|
||||
|
||||
try:
|
||||
self.vesc.set_duty(0.0)
|
||||
self.get_logger().error("Motors killed via duty cycle = 0")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to kill motors: {e}")
|
||||
|
||||
def _control_loop(self) -> None:
|
||||
"""Main control loop (50Hz)."""
|
||||
# Check IMU data availability
|
||||
if self.imu_data is None:
|
||||
return
|
||||
|
||||
# Check tilt safety
|
||||
self._check_tilt_safety()
|
||||
|
||||
# Check startup ramp
|
||||
ramp_factor = self._check_startup_ramp()
|
||||
|
||||
# Publish state
|
||||
self._publish_balance_state(ramp_factor)
|
||||
|
||||
# Log data
|
||||
self._publish_balance_log()
|
||||
|
||||
def _publish_balance_state(self, ramp_factor: float) -> None:
|
||||
"""Publish balance controller state as JSON."""
|
||||
state_dict = {
|
||||
"timestamp": time.time(),
|
||||
"state": self.state.value,
|
||||
"pitch_deg": round(self.imu_data.pitch_deg, 2) if self.imu_data else 0.0,
|
||||
"roll_deg": round(self.imu_data.roll_deg, 2) if self.imu_data else 0.0,
|
||||
"tilt_threshold_deg": self.tilt_threshold,
|
||||
"tilt_duration_s": (time.time() - self.tilt_start_time) if self.tilt_start_time else 0.0,
|
||||
"tilt_kill_duration_s": self.tilt_kill_duration,
|
||||
"pid": {
|
||||
"p": self.pid_p,
|
||||
"i": self.pid_i,
|
||||
"d": self.pid_d,
|
||||
},
|
||||
"startup_ramp_factor": round(ramp_factor, 3),
|
||||
"motor": {
|
||||
"voltage_v": round(self.motor_telemetry.voltage_v, 2) if self.motor_telemetry else 0.0,
|
||||
"current_a": round(self.motor_telemetry.current_a, 2) if self.motor_telemetry else 0.0,
|
||||
"rpm": self.motor_telemetry.rpm if self.motor_telemetry else 0,
|
||||
"temperature_c": round(self.motor_telemetry.temperature_c, 1) if self.motor_telemetry else 0.0,
|
||||
"fault_code": self.motor_telemetry.fault_code if self.motor_telemetry else 0,
|
||||
}
|
||||
}
|
||||
|
||||
msg = String(data=json.dumps(state_dict))
|
||||
self.pub_balance_state.publish(msg)
|
||||
|
||||
def _publish_balance_log(self) -> None:
|
||||
"""Publish IMU + motor data log as CSV."""
|
||||
if self.imu_data is None or self.motor_telemetry is None:
|
||||
return
|
||||
|
||||
# CSV format: timestamp, pitch, roll, current, temp, rpm
|
||||
log_entry = (
|
||||
f"{time.time():.3f}, "
|
||||
f"{self.imu_data.pitch_deg:.2f}, "
|
||||
f"{self.imu_data.roll_deg:.2f}, "
|
||||
f"{self.motor_telemetry.current_a:.2f}, "
|
||||
f"{self.motor_telemetry.temperature_c:.1f}, "
|
||||
f"{self.motor_telemetry.rpm}"
|
||||
)
|
||||
|
||||
msg = String(data=log_entry)
|
||||
self.pub_balance_log.publish(msg)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = BalanceControllerNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node.vesc and node.serial:
|
||||
node.vesc.set_duty(0.0) # Zero throttle on shutdown
|
||||
node.serial.close()
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_balance_controller
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_balance_controller
|
||||
@ -1,27 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_balance_controller"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/launch", ["launch/balance_controller.launch.py"]),
|
||||
(f"share/{package_name}/config", ["config/balance_params.yaml"]),
|
||||
],
|
||||
install_requires=["setuptools", "pyvesc"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-controls",
|
||||
maintainer_email="sl-controls@saltylab.local",
|
||||
description="Balance mode PID controller with tilt safety for SaltyBot",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"balance_controller_node = saltybot_balance_controller.balance_controller_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,170 +0,0 @@
|
||||
"""Unit tests for balance controller node."""
|
||||
|
||||
import pytest
|
||||
import math
|
||||
from sensor_msgs.msg import Imu
|
||||
from geometry_msgs.msg import Quaternion
|
||||
from std_msgs.msg import String
|
||||
|
||||
import rclpy
|
||||
from saltybot_balance_controller.balance_controller_node import BalanceControllerNode
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def rclpy_fixture():
|
||||
"""Initialize and cleanup rclpy."""
|
||||
rclpy.init()
|
||||
yield
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def node(rclpy_fixture):
|
||||
"""Create a balance controller node instance."""
|
||||
node = BalanceControllerNode()
|
||||
yield node
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
class TestNodeInitialization:
|
||||
"""Test suite for node initialization."""
|
||||
|
||||
def test_node_initialization(self, node):
|
||||
"""Test that node initializes with correct defaults."""
|
||||
assert node.port == "/dev/ttyUSB0"
|
||||
assert node.baudrate == 115200
|
||||
assert node.pid_p == 0.5
|
||||
assert node.pid_i == 0.1
|
||||
assert node.pid_d == 0.05
|
||||
|
||||
def test_tilt_threshold_parameter(self, node):
|
||||
"""Test tilt threshold parameter is set correctly."""
|
||||
assert node.tilt_threshold == 45.0
|
||||
|
||||
def test_tilt_kill_duration_parameter(self, node):
|
||||
"""Test tilt kill duration parameter is set correctly."""
|
||||
assert node.tilt_kill_duration == 0.5
|
||||
|
||||
|
||||
class TestQuaternionToEuler:
|
||||
"""Test suite for quaternion to Euler conversion."""
|
||||
|
||||
def test_identity_quaternion(self, node):
|
||||
"""Test identity quaternion (no rotation)."""
|
||||
roll, pitch, yaw = node._quaternion_to_euler(0, 0, 0, 1)
|
||||
assert roll == pytest.approx(0)
|
||||
assert pitch == pytest.approx(0)
|
||||
assert yaw == pytest.approx(0)
|
||||
|
||||
def test_90deg_pitch_rotation(self, node):
|
||||
"""Test 90 degree pitch rotation."""
|
||||
# Quaternion for 90 degree Y rotation
|
||||
roll, pitch, yaw = node._quaternion_to_euler(0, 0.707, 0, 0.707)
|
||||
assert roll == pytest.approx(0, abs=0.01)
|
||||
assert pitch == pytest.approx(math.pi / 2, abs=0.01)
|
||||
assert yaw == pytest.approx(0, abs=0.01)
|
||||
|
||||
def test_45deg_pitch_rotation(self, node):
|
||||
"""Test 45 degree pitch rotation."""
|
||||
roll, pitch, yaw = node._quaternion_to_euler(0, 0.383, 0, 0.924)
|
||||
assert roll == pytest.approx(0, abs=0.01)
|
||||
assert pitch == pytest.approx(math.pi / 4, abs=0.01)
|
||||
assert yaw == pytest.approx(0, abs=0.01)
|
||||
|
||||
def test_roll_rotation(self, node):
|
||||
"""Test roll rotation around X axis."""
|
||||
roll, pitch, yaw = node._quaternion_to_euler(0.707, 0, 0, 0.707)
|
||||
assert roll == pytest.approx(math.pi / 2, abs=0.01)
|
||||
assert pitch == pytest.approx(0, abs=0.01)
|
||||
assert yaw == pytest.approx(0, abs=0.01)
|
||||
|
||||
|
||||
class TestIMUDataParsing:
|
||||
"""Test suite for IMU data parsing."""
|
||||
|
||||
def test_imu_data_subscription(self, node):
|
||||
"""Test IMU data subscription updates node state."""
|
||||
imu = Imu()
|
||||
imu.orientation = Quaternion(x=0, y=0, z=0, w=1)
|
||||
|
||||
node._on_imu_data(imu)
|
||||
|
||||
assert node.imu_data is not None
|
||||
assert node.imu_data.pitch_deg == pytest.approx(0, abs=0.1)
|
||||
assert node.imu_data.roll_deg == pytest.approx(0, abs=0.1)
|
||||
|
||||
def test_imu_pitch_tilted_forward(self, node):
|
||||
"""Test IMU data with forward pitch tilt."""
|
||||
# 45 degree forward pitch
|
||||
imu = Imu()
|
||||
imu.orientation = Quaternion(x=0, y=0.383, z=0, w=0.924)
|
||||
|
||||
node._on_imu_data(imu)
|
||||
|
||||
assert node.imu_data is not None
|
||||
# Should be approximately 45 degrees (in radians converted to degrees)
|
||||
assert node.imu_data.pitch_deg == pytest.approx(45, abs=1)
|
||||
|
||||
|
||||
class TestTiltSafety:
|
||||
"""Test suite for tilt safety checks."""
|
||||
|
||||
def test_tilt_warning_state_entry(self, node):
|
||||
"""Test entry into tilt warning state."""
|
||||
imu = Imu()
|
||||
# 50 degree pitch (exceeds 45 degree threshold)
|
||||
imu.orientation = Quaternion(x=0, y=0.438, z=0, w=0.899)
|
||||
node._on_imu_data(imu)
|
||||
|
||||
# Call check with small duration
|
||||
node._check_tilt_safety()
|
||||
|
||||
assert node.state.value in ["tilt_warning", "startup"]
|
||||
|
||||
def test_level_no_tilt_warning(self, node):
|
||||
"""Test no tilt warning when level."""
|
||||
imu = Imu()
|
||||
imu.orientation = Quaternion(x=0, y=0, z=0, w=1)
|
||||
node._on_imu_data(imu)
|
||||
|
||||
node._check_tilt_safety()
|
||||
|
||||
# Tilt start time should be None when level
|
||||
assert node.tilt_start_time is None
|
||||
|
||||
|
||||
class TestStartupRamp:
|
||||
"""Test suite for startup ramp functionality."""
|
||||
|
||||
def test_startup_ramp_begins_at_zero(self, node):
|
||||
"""Test startup ramp begins at 0."""
|
||||
ramp = node._check_startup_ramp()
|
||||
|
||||
# At startup time, ramp should be close to 0
|
||||
assert ramp <= 0.05 # Very small value at start
|
||||
|
||||
def test_startup_ramp_reaches_one(self, node):
|
||||
"""Test startup ramp reaches 1.0 after duration."""
|
||||
# Simulate startup ramp completion
|
||||
node.startup_time = 0
|
||||
node.startup_ramp_time = 0.001 # Very short ramp
|
||||
|
||||
import time
|
||||
time.sleep(0.01) # Sleep longer than ramp time
|
||||
|
||||
ramp = node._check_startup_ramp()
|
||||
|
||||
# Should be complete after time has passed
|
||||
assert ramp >= 0.99
|
||||
|
||||
def test_startup_ramp_linear(self, node):
|
||||
"""Test startup ramp is linear."""
|
||||
node.startup_ramp_time = 1.0
|
||||
|
||||
# At 25% of startup time
|
||||
node.startup_time = 0
|
||||
import time
|
||||
time.sleep(0.25)
|
||||
|
||||
ramp = node._check_startup_ramp()
|
||||
assert 0.2 < ramp < 0.3
|
||||
@ -5,7 +5,7 @@ Overview
|
||||
────────
|
||||
Orchestrates dock detection, Nav2 corridor approach, visual-servo final
|
||||
alignment, and charge monitoring. Interrupts the active Nav2 mission when
|
||||
battery drops to 20 % and resumes when charged to 80 %.
|
||||
battery drops to 15 % and resumes when charged to 80 %.
|
||||
|
||||
Pipeline (20 Hz)
|
||||
────────────────
|
||||
@ -49,7 +49,7 @@ Parameters
|
||||
aruco_marker_id 42
|
||||
aruco_marker_size_m 0.10
|
||||
ir_threshold 0.50
|
||||
battery_low_pct 20.0
|
||||
battery_low_pct 15.0
|
||||
battery_high_pct 80.0
|
||||
servo_range_m 1.00 m (switch to IBVS within this distance)
|
||||
k_linear 0.30
|
||||
@ -100,12 +100,6 @@ try:
|
||||
except ImportError:
|
||||
_NAV2_OK = False
|
||||
|
||||
try:
|
||||
from saltybot_social_msgs.msg import Mood
|
||||
_SOCIAL_OK = True
|
||||
except ImportError:
|
||||
_SOCIAL_OK = False
|
||||
|
||||
try:
|
||||
import cv2
|
||||
import numpy as np
|
||||
@ -214,11 +208,6 @@ class DockingNode(Node):
|
||||
self._status_pub = self.create_publisher(
|
||||
DockingStatus, "/saltybot/docking_status", reliable
|
||||
)
|
||||
self._mood_pub = None
|
||||
if _SOCIAL_OK:
|
||||
self._mood_pub = self.create_publisher(
|
||||
Mood, "/saltybot/mood", reliable
|
||||
)
|
||||
|
||||
# ── Services ─────────────────────────────────────────────────────────
|
||||
if _MSGS_OK:
|
||||
@ -251,7 +240,7 @@ class DockingNode(Node):
|
||||
self.declare_parameter("aruco_marker_id", 42)
|
||||
self.declare_parameter("aruco_marker_size_m", 0.10)
|
||||
self.declare_parameter("ir_threshold", 0.50)
|
||||
self.declare_parameter("battery_low_pct", 20.0)
|
||||
self.declare_parameter("battery_low_pct", 15.0)
|
||||
self.declare_parameter("battery_high_pct", 80.0)
|
||||
self.declare_parameter("servo_range_m", 1.00)
|
||||
self.declare_parameter("k_linear", 0.30)
|
||||
@ -354,12 +343,6 @@ class DockingNode(Node):
|
||||
# ── State-entry side effects ──────────────────────────────────────────
|
||||
if out.state_changed:
|
||||
self.get_logger().info(f"Docking FSM → {out.state.value}")
|
||||
# Publish charging mood when docking state is reached
|
||||
if out.state == DockingState.CHARGING and self._mood_pub is not None:
|
||||
mood_msg = Mood()
|
||||
mood_msg.mood = "happy"
|
||||
mood_msg.intensity = 1.0
|
||||
self._mood_pub.publish(mood_msg)
|
||||
|
||||
if out.request_nav2:
|
||||
self._send_nav2_goal()
|
||||
|
||||
@ -1,118 +0,0 @@
|
||||
# SaltyBot Health Monitor
|
||||
|
||||
Central system health monitor for SaltyBot. Tracks heartbeats from all critical nodes, detects failures, triggers auto-restart, and publishes system health status.
|
||||
|
||||
## Features
|
||||
|
||||
- **Heartbeat Monitoring**: Subscribes to heartbeat signals from all tracked nodes
|
||||
- **Automatic Dead Node Detection**: Marks nodes as DOWN if silent >5 seconds
|
||||
- **Auto-Restart Capability**: Attempts to restart dead nodes via ROS2 launch
|
||||
- **System Health Publishing**: Publishes `/saltybot/system_health` JSON with full status
|
||||
- **Face Alerts**: Triggers visual alerts on robot face display for critical failures
|
||||
- **Configurable**: YAML-based node list and timeout parameters
|
||||
|
||||
## Topics
|
||||
|
||||
### Subscribed
|
||||
- `/saltybot/<node_name>/heartbeat` (std_msgs/String): Heartbeat from each monitored node
|
||||
|
||||
### Published
|
||||
- `/saltybot/system_health` (std_msgs/String): System health status as JSON
|
||||
- `/saltybot/face/alert` (std_msgs/String): Critical alerts for face display
|
||||
|
||||
## Configuration
|
||||
|
||||
Edit `config/health_config.yaml` to configure:
|
||||
|
||||
- **monitored_nodes**: List of all nodes to track
|
||||
- **heartbeat_timeout_s**: Seconds before node is marked DOWN (default: 5s)
|
||||
- **check_frequency_hz**: Health check rate (default: 1Hz)
|
||||
- **enable_auto_restart**: Enable automatic restart attempts (default: true)
|
||||
- **critical_nodes**: Nodes that trigger face alerts when down
|
||||
|
||||
## Launch
|
||||
|
||||
```bash
|
||||
# Default launch with built-in config
|
||||
ros2 launch saltybot_health_monitor health_monitor.launch.py
|
||||
|
||||
# Custom config
|
||||
ros2 launch saltybot_health_monitor health_monitor.launch.py \
|
||||
config_file:=/path/to/custom_config.yaml
|
||||
|
||||
# Disable auto-restart
|
||||
ros2 launch saltybot_health_monitor health_monitor.launch.py \
|
||||
enable_auto_restart:=false
|
||||
```
|
||||
|
||||
## Health Status JSON
|
||||
|
||||
The `/saltybot/system_health` topic publishes:
|
||||
|
||||
```json
|
||||
{
|
||||
"timestamp": "2025-03-05T10:00:00.123456",
|
||||
"uptime_s": 3600.5,
|
||||
"nodes": {
|
||||
"rover_driver": {
|
||||
"status": "UP",
|
||||
"time_since_heartbeat_s": 0.5,
|
||||
"heartbeat_count": 1200,
|
||||
"restart_count": 0,
|
||||
"expected": true
|
||||
},
|
||||
"slam_node": {
|
||||
"status": "DOWN",
|
||||
"time_since_heartbeat_s": 6.0,
|
||||
"heartbeat_count": 500,
|
||||
"restart_count": 1,
|
||||
"expected": true
|
||||
}
|
||||
},
|
||||
"critical_down": ["slam_node"],
|
||||
"system_healthy": false
|
||||
}
|
||||
```
|
||||
|
||||
## Node Integration
|
||||
|
||||
Each node should publish heartbeats periodically (e.g., every 1-2 seconds):
|
||||
|
||||
```python
|
||||
# In your ROS2 node
|
||||
heartbeat_pub = self.create_publisher(String, "/saltybot/node_name/heartbeat", 10)
|
||||
heartbeat_pub.publish(String(data="node_name:alive"))
|
||||
```
|
||||
|
||||
## Restart Behavior
|
||||
|
||||
When a node is detected as DOWN:
|
||||
|
||||
1. Health monitor logs a warning
|
||||
2. If `enable_auto_restart: true`, queues a restart command
|
||||
3. Node status changes to "RESTARTING"
|
||||
4. Restart count is incremented
|
||||
5. Face alert is published for critical nodes
|
||||
|
||||
The actual restart mechanism can be:
|
||||
- Direct ROS2 launch subprocess
|
||||
- Systemd service restart
|
||||
- Custom restart script
|
||||
- Manual restart via external monitor
|
||||
|
||||
## Debugging
|
||||
|
||||
Check health status:
|
||||
```bash
|
||||
ros2 topic echo /saltybot/system_health
|
||||
```
|
||||
|
||||
Simulate a node heartbeat:
|
||||
```bash
|
||||
ros2 topic pub /saltybot/test_node/heartbeat std_msgs/String '{data: "test_node:alive"}'
|
||||
```
|
||||
|
||||
View monitor logs:
|
||||
```bash
|
||||
ros2 launch saltybot_health_monitor health_monitor.launch.py | grep health
|
||||
```
|
||||
@ -1,76 +0,0 @@
|
||||
# Health Monitor Configuration
|
||||
# Lists all critical nodes that should be monitored for heartbeats
|
||||
|
||||
monitored_nodes:
|
||||
# Core drivers and hardware interfaces
|
||||
- rover_driver
|
||||
- camera_driver
|
||||
- lidar_driver
|
||||
- imu_driver
|
||||
- uwb_driver
|
||||
|
||||
# SLAM and localization
|
||||
- slam_node
|
||||
- odom_fusion
|
||||
- visual_odom
|
||||
|
||||
# Navigation
|
||||
- nav2_bringup
|
||||
- planner_server
|
||||
- controller_server
|
||||
|
||||
# Perception
|
||||
- person_detector
|
||||
- object_tracker
|
||||
|
||||
# Control and decision making
|
||||
- follower
|
||||
- cmd_vel_bridge
|
||||
- emergency_handler
|
||||
|
||||
# Communication
|
||||
- rosbridge_websocket
|
||||
- cellular_link
|
||||
|
||||
# Utilities
|
||||
- bag_recorder
|
||||
- remote_monitor
|
||||
|
||||
# Health check parameters
|
||||
health_check:
|
||||
# Node is considered DOWN if heartbeat hasn't been received in this many seconds
|
||||
heartbeat_timeout_s: 5
|
||||
|
||||
# How often to check node health (Hz)
|
||||
check_frequency_hz: 1
|
||||
|
||||
# Whether to attempt automatic restart of dead nodes
|
||||
enable_auto_restart: true
|
||||
|
||||
# Alert cooldown to avoid spam (seconds)
|
||||
alert_cooldown_s: 5
|
||||
|
||||
# Restart behavior
|
||||
restart:
|
||||
# Command file to write restart commands to
|
||||
command_file: /tmp/saltybot_restart_queue.sh
|
||||
|
||||
# Maximum consecutive restarts before giving up
|
||||
max_restart_attempts: 3
|
||||
|
||||
# Alert settings
|
||||
alerting:
|
||||
# Publish alerts to this topic
|
||||
alert_topic: /saltybot/face/alert
|
||||
|
||||
# Nodes that are critical (system won't operate without them)
|
||||
critical_nodes:
|
||||
- rover_driver
|
||||
- cmd_vel_bridge
|
||||
- emergency_handler
|
||||
|
||||
# Nodes that are important but not critical
|
||||
important_nodes:
|
||||
- slam_node
|
||||
- person_detector
|
||||
- nav2_bringup
|
||||
@ -1,57 +0,0 @@
|
||||
"""Launch health monitor node."""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate launch description for health monitor."""
|
||||
|
||||
package_dir = get_package_share_directory("saltybot_health_monitor")
|
||||
config_dir = os.path.join(package_dir, "config")
|
||||
|
||||
# Launch arguments
|
||||
config_file_arg = DeclareLaunchArgument(
|
||||
"config_file",
|
||||
default_value=os.path.join(config_dir, "health_config.yaml"),
|
||||
description="Path to health monitor configuration YAML file",
|
||||
)
|
||||
|
||||
heartbeat_timeout_arg = DeclareLaunchArgument(
|
||||
"heartbeat_timeout",
|
||||
default_value="5.0",
|
||||
description="Heartbeat timeout in seconds (node marked DOWN if silent longer)",
|
||||
)
|
||||
|
||||
enable_auto_restart_arg = DeclareLaunchArgument(
|
||||
"enable_auto_restart",
|
||||
default_value="true",
|
||||
description="Enable automatic restart of dead nodes",
|
||||
)
|
||||
|
||||
# Health monitor node
|
||||
health_monitor_node = Node(
|
||||
package="saltybot_health_monitor",
|
||||
executable="health_monitor_node",
|
||||
name="health_monitor",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"config_file": LaunchConfiguration("config_file"),
|
||||
"heartbeat_timeout": LaunchConfiguration("heartbeat_timeout"),
|
||||
"enable_auto_restart": LaunchConfiguration("enable_auto_restart"),
|
||||
"check_frequency": 1.0, # Hz
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
config_file_arg,
|
||||
heartbeat_timeout_arg,
|
||||
enable_auto_restart_arg,
|
||||
health_monitor_node,
|
||||
])
|
||||
@ -1,29 +0,0 @@
|
||||
<?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_health_monitor</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
ROS2 system health monitor for SaltyBot. Central node that monitors heartbeats
|
||||
from all critical nodes, detects when nodes go down (>5s silent), triggers
|
||||
auto-restart, publishes /saltybot/system_health JSON, and alerts face display
|
||||
on critical failures.
|
||||
</description>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_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>
|
||||
@ -1 +0,0 @@
|
||||
# Health monitor package
|
||||
@ -1,265 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""System health monitor for SaltyBot.
|
||||
|
||||
Central node that monitors heartbeats from all critical nodes. Tracks expected
|
||||
nodes from YAML config, marks nodes DEAD if silent >5s, auto-restarts via
|
||||
ros2 launch, publishes /saltybot/system_health JSON, and triggers face alerts.
|
||||
|
||||
Published topics:
|
||||
/saltybot/system_health (std_msgs/String) - JSON system health status
|
||||
|
||||
Subscribed topics:
|
||||
/saltybot/<node_name>/heartbeat (std_msgs/String) - Node heartbeat signals
|
||||
"""
|
||||
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional
|
||||
from dataclasses import dataclass, asdict
|
||||
from datetime import datetime
|
||||
|
||||
import yaml
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.timer import Timer
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
@dataclass
|
||||
class NodeHealth:
|
||||
"""Health status of a single node."""
|
||||
|
||||
name: str
|
||||
status: str # "UP", "DOWN", "RESTARTING"
|
||||
last_heartbeat: float # Timestamp of last received heartbeat
|
||||
heartbeat_count: int = 0
|
||||
restart_count: int = 0
|
||||
expected: bool = True
|
||||
|
||||
|
||||
class HealthMonitorNode(Node):
|
||||
"""ROS2 node for system health monitoring."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("health_monitor")
|
||||
|
||||
# Load configuration
|
||||
self.declare_parameter("config_file", "health_config.yaml")
|
||||
config_path = self.get_parameter("config_file").value
|
||||
|
||||
self.node_health: Dict[str, NodeHealth] = {}
|
||||
self.startup_time = time.time()
|
||||
self.last_critical_alert = 0.0
|
||||
self.alert_cooldown = 5.0 # Seconds between critical alerts
|
||||
|
||||
# Load node configuration
|
||||
self._load_config(config_path)
|
||||
|
||||
# Parameters
|
||||
self.declare_parameter("heartbeat_timeout", 5.0) # Seconds
|
||||
self.declare_parameter("check_frequency", 1.0) # Hz
|
||||
self.declare_parameter("enable_auto_restart", True)
|
||||
self.declare_parameter("restart_command_file", "/tmp/restart_node.sh")
|
||||
|
||||
self.heartbeat_timeout = self.get_parameter("heartbeat_timeout").value
|
||||
check_frequency = self.get_parameter("check_frequency").value
|
||||
self.enable_auto_restart = self.get_parameter("enable_auto_restart").value
|
||||
self.restart_cmd_file = self.get_parameter("restart_command_file").value
|
||||
|
||||
# Subscribe to heartbeats from all expected nodes
|
||||
self._setup_subscriptions()
|
||||
|
||||
# Publisher for system health
|
||||
self.pub_health = self.create_publisher(String, "/saltybot/system_health", 1)
|
||||
self.pub_face_alert = self.create_publisher(String, "/saltybot/face/alert", 1)
|
||||
|
||||
# Health check timer
|
||||
period = 1.0 / check_frequency
|
||||
self.timer: Timer = self.create_timer(period, self._check_health)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Health monitor initialized with {len(self.node_health)} tracked nodes. "
|
||||
f"Timeout: {self.heartbeat_timeout}s, Auto-restart: {self.enable_auto_restart}"
|
||||
)
|
||||
|
||||
def _load_config(self, config_file: str) -> None:
|
||||
"""Load node configuration from YAML file."""
|
||||
try:
|
||||
# Try to find config in share directory
|
||||
if not Path(config_file).exists():
|
||||
# Look in package share directory
|
||||
share_dir = Path(__file__).parent.parent / "config"
|
||||
config_file = str(share_dir / config_file)
|
||||
|
||||
with open(config_file, "r") as f:
|
||||
config = yaml.safe_load(f) or {}
|
||||
|
||||
monitored_nodes = config.get("monitored_nodes", [])
|
||||
for node_name in monitored_nodes:
|
||||
self.node_health[node_name] = NodeHealth(
|
||||
name=node_name, status="UNKNOWN", last_heartbeat=time.time()
|
||||
)
|
||||
|
||||
self.get_logger().info(f"Loaded {len(monitored_nodes)} nodes from config")
|
||||
except FileNotFoundError:
|
||||
self.get_logger().warn(
|
||||
f"Config file not found: {config_file}. "
|
||||
"Will monitor nodes as they send heartbeats."
|
||||
)
|
||||
|
||||
def _setup_subscriptions(self) -> None:
|
||||
"""Create subscriptions for all expected nodes."""
|
||||
for node_name in self.node_health.keys():
|
||||
topic = f"/saltybot/{node_name}/heartbeat"
|
||||
self.create_subscription(String, topic, self._on_heartbeat, 10)
|
||||
|
||||
def _on_heartbeat(self, msg: String) -> None:
|
||||
"""Handle incoming heartbeat from a node."""
|
||||
# Parse heartbeat message (expected format: "node_name:data")
|
||||
try:
|
||||
parts = msg.data.split(":", 1)
|
||||
node_name = parts[0].strip()
|
||||
data = parts[1].strip() if len(parts) > 1 else ""
|
||||
|
||||
# Create node entry if not yet tracked
|
||||
if node_name not in self.node_health:
|
||||
self.node_health[node_name] = NodeHealth(
|
||||
name=node_name, status="UP", last_heartbeat=time.time(), expected=False
|
||||
)
|
||||
|
||||
# Update heartbeat
|
||||
node = self.node_health[node_name]
|
||||
node.last_heartbeat = time.time()
|
||||
node.heartbeat_count += 1
|
||||
if node.status != "UP":
|
||||
node.status = "UP"
|
||||
self.get_logger().info(f"Node {node_name} is UP")
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Error processing heartbeat: {e}")
|
||||
|
||||
def _check_health(self) -> None:
|
||||
"""Periodically check health of all nodes and publish status."""
|
||||
now = time.time()
|
||||
critical_down = []
|
||||
|
||||
for node_name, node in self.node_health.items():
|
||||
# Check if heartbeat is stale
|
||||
time_since_heartbeat = now - node.last_heartbeat
|
||||
|
||||
if time_since_heartbeat > self.heartbeat_timeout:
|
||||
if node.status != "DOWN":
|
||||
self.get_logger().warn(
|
||||
f"Node {node_name} DOWN (silent for {time_since_heartbeat:.1f}s)"
|
||||
)
|
||||
node.status = "DOWN"
|
||||
|
||||
# Track critical (expected) nodes
|
||||
if node.expected:
|
||||
critical_down.append(node_name)
|
||||
|
||||
# Attempt auto-restart
|
||||
if self.enable_auto_restart and node.status == "DOWN":
|
||||
self._trigger_restart(node_name)
|
||||
else:
|
||||
# Node is healthy
|
||||
if node.status != "UP":
|
||||
node.status = "UP"
|
||||
|
||||
# Publish system health
|
||||
self._publish_health(critical_down)
|
||||
|
||||
# Alert face if critical nodes are down
|
||||
if critical_down:
|
||||
self._alert_critical(critical_down, now)
|
||||
|
||||
def _trigger_restart(self, node_name: str) -> None:
|
||||
"""Trigger restart of a dead node via launch system."""
|
||||
node = self.node_health[node_name]
|
||||
node.restart_count += 1
|
||||
|
||||
self.get_logger().warn(
|
||||
f"Attempting auto-restart for {node_name} (attempt #{node.restart_count})"
|
||||
)
|
||||
|
||||
# Update status
|
||||
node.status = "RESTARTING"
|
||||
|
||||
# In a real implementation, this would trigger ros2 launch or systemd service restart
|
||||
# For now, log the attempt
|
||||
try:
|
||||
# Example: restart via launch system
|
||||
# This would need to be configured based on actual launch setup
|
||||
restart_script = (
|
||||
f"#!/bin/bash\n"
|
||||
f"# Auto-restart triggered at {datetime.now().isoformat()}\n"
|
||||
f"ros2 launch saltybot_bringup {node_name}.launch.py &\n"
|
||||
)
|
||||
with open(self.restart_cmd_file, "a") as f:
|
||||
f.write(restart_script)
|
||||
|
||||
self.get_logger().info(f"Restart command queued for {node_name}")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to queue restart for {node_name}: {e}")
|
||||
|
||||
def _publish_health(self, critical_down: list) -> None:
|
||||
"""Publish system health status as JSON."""
|
||||
health_data = {
|
||||
"timestamp": datetime.now().isoformat(),
|
||||
"uptime_s": time.time() - self.startup_time,
|
||||
"nodes": {
|
||||
node.name: {
|
||||
"status": node.status,
|
||||
"time_since_heartbeat_s": time.time() - node.last_heartbeat,
|
||||
"heartbeat_count": node.heartbeat_count,
|
||||
"restart_count": node.restart_count,
|
||||
"expected": node.expected,
|
||||
}
|
||||
for node in self.node_health.values()
|
||||
},
|
||||
"critical_down": critical_down,
|
||||
"system_healthy": len(critical_down) == 0,
|
||||
}
|
||||
|
||||
msg = String(data=json.dumps(health_data))
|
||||
self.pub_health.publish(msg)
|
||||
|
||||
def _alert_critical(self, critical_nodes: list, now: float) -> None:
|
||||
"""Alert face display of critical node failures."""
|
||||
# Rate-limit alerts to avoid spam
|
||||
if now - self.last_critical_alert < self.alert_cooldown:
|
||||
return
|
||||
|
||||
self.last_critical_alert = now
|
||||
|
||||
alert_msg = {
|
||||
"type": "system_alert",
|
||||
"severity": "critical",
|
||||
"message": f"System critical: {', '.join(critical_nodes)} down",
|
||||
"nodes": critical_nodes,
|
||||
"timestamp": datetime.now().isoformat(),
|
||||
}
|
||||
|
||||
msg = String(data=json.dumps(alert_msg))
|
||||
self.pub_face_alert.publish(msg)
|
||||
|
||||
self.get_logger().warn(
|
||||
f"CRITICAL ALERT: {len(critical_nodes)} expected node(s) down: {critical_nodes}"
|
||||
)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = HealthMonitorNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,2 +0,0 @@
|
||||
[develop]
|
||||
script-dir=$base/lib/saltybot_health_monitor
|
||||
@ -1,30 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_health_monitor"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/launch", ["launch/health_monitor.launch.py"]),
|
||||
(f"share/{package_name}/config", ["config/health_config.yaml"]),
|
||||
],
|
||||
install_requires=["setuptools", "pyyaml"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-controls",
|
||||
maintainer_email="sl-controls@saltylab.local",
|
||||
description=(
|
||||
"System health monitor: tracks node heartbeats, detects down nodes, "
|
||||
"triggers auto-restart, publishes system health status"
|
||||
),
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"health_monitor_node = saltybot_health_monitor.health_monitor_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1 +0,0 @@
|
||||
# Test module
|
||||
@ -1,76 +0,0 @@
|
||||
"""Unit tests for health monitor."""
|
||||
|
||||
import unittest
|
||||
import time
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
class TestHealthMonitor(unittest.TestCase):
|
||||
"""Test cases for health monitor node."""
|
||||
|
||||
def test_heartbeat_parsing(self):
|
||||
"""Test parsing of heartbeat messages."""
|
||||
# Test message format: "node_name:data"
|
||||
test_cases = [
|
||||
("rover_driver:alive", "rover_driver"),
|
||||
("slam_node:map_ready", "slam_node"),
|
||||
("nav2_bringup:planning", "nav2_bringup"),
|
||||
]
|
||||
|
||||
for heartbeat, expected_node in test_cases:
|
||||
parts = heartbeat.split(":", 1)
|
||||
node_name = parts[0].strip()
|
||||
self.assertEqual(node_name, expected_node)
|
||||
|
||||
def test_timeout_detection(self):
|
||||
"""Test detection of stale heartbeats."""
|
||||
heartbeat_timeout = 5.0
|
||||
current_time = time.time()
|
||||
|
||||
# Fresh heartbeat
|
||||
time_since_heartbeat = current_time - (current_time - 1.0)
|
||||
self.assertLess(time_since_heartbeat, heartbeat_timeout)
|
||||
|
||||
# Stale heartbeat
|
||||
stale_time = current_time - 10.0
|
||||
time_since_heartbeat = current_time - stale_time
|
||||
self.assertGreater(time_since_heartbeat, heartbeat_timeout)
|
||||
|
||||
def test_health_status_generation(self):
|
||||
"""Test generation of health status JSON."""
|
||||
import json
|
||||
|
||||
health_data = {
|
||||
"timestamp": "2025-03-05T10:00:00",
|
||||
"uptime_s": 3600,
|
||||
"nodes": {
|
||||
"rover_driver": {
|
||||
"status": "UP",
|
||||
"time_since_heartbeat_s": 0.5,
|
||||
"heartbeat_count": 100,
|
||||
"restart_count": 0,
|
||||
"expected": True,
|
||||
},
|
||||
"slam_node": {
|
||||
"status": "DOWN",
|
||||
"time_since_heartbeat_s": 6.0,
|
||||
"heartbeat_count": 50,
|
||||
"restart_count": 1,
|
||||
"expected": True,
|
||||
},
|
||||
},
|
||||
"critical_down": ["slam_node"],
|
||||
"system_healthy": False,
|
||||
}
|
||||
|
||||
# Should be serializable to JSON
|
||||
json_str = json.dumps(health_data)
|
||||
parsed = json.loads(json_str)
|
||||
|
||||
self.assertEqual(parsed["system_healthy"], False)
|
||||
self.assertIn("slam_node", parsed["critical_down"])
|
||||
self.assertEqual(parsed["nodes"]["rover_driver"]["status"], "UP")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@ -1,13 +0,0 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
multi_person_tracker_node:
|
||||
# Tracking parameters
|
||||
max_people: 10 # Maximum tracked people
|
||||
occlusion_grace_s: 3.0 # Seconds to maintain track during occlusion
|
||||
embedding_threshold: 0.75 # Cosine similarity threshold for face re-ID
|
||||
hsv_threshold: 0.60 # Color histogram similarity threshold
|
||||
|
||||
# Publishing
|
||||
publish_hz: 15.0 # Update rate (15+ fps on Orin)
|
||||
enable_group_follow: true # Follow group centroid if no single target
|
||||
announce_state: true # Log state changes
|
||||
@ -1,40 +0,0 @@
|
||||
"""multi_person_tracker.launch.py — Launch file for multi-person tracking (Issue #423)."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate launch description for multi-person tracker."""
|
||||
|
||||
# Declare launch arguments
|
||||
config_arg = DeclareLaunchArgument(
|
||||
"config_file",
|
||||
default_value=os.path.join(
|
||||
get_package_share_directory("saltybot_multi_person_tracker"),
|
||||
"config",
|
||||
"multi_person_tracker_params.yaml",
|
||||
),
|
||||
description="Path to multi-person tracker parameters YAML file",
|
||||
)
|
||||
|
||||
# Multi-person tracker node
|
||||
tracker_node = Node(
|
||||
package="saltybot_multi_person_tracker",
|
||||
executable="multi_person_tracker_node",
|
||||
name="multi_person_tracker_node",
|
||||
parameters=[LaunchConfiguration("config_file")],
|
||||
remappings=[],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
config_arg,
|
||||
tracker_node,
|
||||
]
|
||||
)
|
||||
@ -1,27 +0,0 @@
|
||||
<?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_multi_person_tracker</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Multi-person tracker with group handling and target priority (Issue #423)</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>saltybot_social_msgs</depend>
|
||||
<depend>saltybot_person_reid_msgs</depend>
|
||||
<depend>vision_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>opencv-python</depend>
|
||||
<depend>numpy</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1 +0,0 @@
|
||||
"""saltybot_multi_person_tracker — Multi-person tracker with group handling (Issue #423)."""
|
||||
@ -1,318 +0,0 @@
|
||||
"""Multi-person tracker for Issue #423.
|
||||
|
||||
Tracks up to 10 people with:
|
||||
- Unique IDs (persistent across frames)
|
||||
- Target priority: wake-word speaker > closest known > largest bbox
|
||||
- Occlusion handoff (3s grace period)
|
||||
- Re-ID via face embedding + HSV color histogram
|
||||
- Group detection (follow centroid)
|
||||
- Lost target: stop + rotate + SEARCHING state
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
import cv2
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackedPerson:
|
||||
"""A tracked person with identity and state."""
|
||||
track_id: int
|
||||
bearing_rad: float
|
||||
distance_m: float
|
||||
confidence: float
|
||||
is_speaking: bool = False
|
||||
source: str = "camera"
|
||||
last_seen_time: float = field(default_factory=time.time)
|
||||
embedding: Optional[np.ndarray] = None # Face embedding for re-ID
|
||||
hsv_histogram: Optional[np.ndarray] = None # HSV histogram fallback
|
||||
bbox: Optional[Tuple[int, int, int, int]] = None # (x, y, w, h)
|
||||
is_occluded: bool = False
|
||||
occlusion_start_time: float = 0.0
|
||||
|
||||
def age_seconds(self) -> float:
|
||||
"""Time since last detection."""
|
||||
return time.time() - self.last_seen_time
|
||||
|
||||
def mark_seen(
|
||||
self,
|
||||
bearing: float,
|
||||
distance: float,
|
||||
confidence: float,
|
||||
is_speaking: bool = False,
|
||||
embedding: Optional[np.ndarray] = None,
|
||||
hsv_hist: Optional[np.ndarray] = None,
|
||||
bbox: Optional[Tuple] = None,
|
||||
) -> None:
|
||||
"""Update person state after detection."""
|
||||
self.bearing_rad = bearing
|
||||
self.distance_m = distance
|
||||
self.confidence = confidence
|
||||
self.is_speaking = is_speaking
|
||||
self.last_seen_time = time.time()
|
||||
self.is_occluded = False
|
||||
if embedding is not None:
|
||||
self.embedding = embedding
|
||||
if hsv_hist is not None:
|
||||
self.hsv_histogram = hsv_hist
|
||||
if bbox is not None:
|
||||
self.bbox = bbox
|
||||
|
||||
def mark_occluded(self) -> None:
|
||||
"""Mark person as occluded but within grace period."""
|
||||
if not self.is_occluded:
|
||||
self.is_occluded = True
|
||||
self.occlusion_start_time = time.time()
|
||||
|
||||
|
||||
class MultiPersonTracker:
|
||||
"""Tracks multiple people with re-ID and group detection."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
max_people: int = 10,
|
||||
occlusion_grace_s: float = 3.0,
|
||||
embedding_similarity_threshold: float = 0.75,
|
||||
hsv_similarity_threshold: float = 0.60,
|
||||
):
|
||||
self.max_people = max_people
|
||||
self.occlusion_grace_s = occlusion_grace_s
|
||||
self.embedding_threshold = embedding_similarity_threshold
|
||||
self.hsv_threshold = hsv_similarity_threshold
|
||||
|
||||
self.tracked_people: Dict[int, TrackedPerson] = {}
|
||||
self.next_track_id = 0
|
||||
self.active_target_id: Optional[int] = None
|
||||
self.searching: bool = False
|
||||
self.search_bearing: float = 0.0
|
||||
|
||||
# ── Main tracking update ────────────────────────────────────────────────
|
||||
|
||||
def update(
|
||||
self,
|
||||
detections: List[Dict],
|
||||
speaking_person_id: Optional[int] = None,
|
||||
) -> Tuple[List[TrackedPerson], Optional[int]]:
|
||||
"""Update tracker with new detections.
|
||||
|
||||
Args:
|
||||
detections: List of dicts with keys:
|
||||
- bearing_rad, distance_m, confidence
|
||||
- embedding (optional): face embedding vector
|
||||
- hsv_histogram (optional): HSV color histogram
|
||||
- bbox (optional): (x, y, w, h)
|
||||
speaking_person_id: wake-word speaker (highest priority)
|
||||
|
||||
Returns:
|
||||
(all_tracked_people, active_target_id)
|
||||
"""
|
||||
# Clean up stale tracks
|
||||
self._prune_lost_people()
|
||||
|
||||
# Match detections to existing tracks
|
||||
matched_ids, unmatched_detections = self._match_detections(detections)
|
||||
|
||||
# Update matched tracks
|
||||
for track_id, det_idx in matched_ids:
|
||||
det = detections[det_idx]
|
||||
self.tracked_people[track_id].mark_seen(
|
||||
bearing=det["bearing_rad"],
|
||||
distance=det["distance_m"],
|
||||
confidence=det["confidence"],
|
||||
is_speaking=det.get("is_speaking", False),
|
||||
embedding=det.get("embedding"),
|
||||
hsv_hist=det.get("hsv_histogram"),
|
||||
bbox=det.get("bbox"),
|
||||
)
|
||||
|
||||
# Create new tracks for unmatched detections
|
||||
for det_idx in unmatched_detections:
|
||||
if len(self.tracked_people) < self.max_people:
|
||||
det = detections[det_idx]
|
||||
track_id = self.next_track_id
|
||||
self.next_track_id += 1
|
||||
self.tracked_people[track_id] = TrackedPerson(
|
||||
track_id=track_id,
|
||||
bearing_rad=det["bearing_rad"],
|
||||
distance_m=det["distance_m"],
|
||||
confidence=det["confidence"],
|
||||
is_speaking=det.get("is_speaking", False),
|
||||
embedding=det.get("embedding"),
|
||||
hsv_histogram=det.get("hsv_histogram"),
|
||||
bbox=det.get("bbox"),
|
||||
)
|
||||
|
||||
# Update target priority
|
||||
self._update_target_priority(speaking_person_id)
|
||||
|
||||
return list(self.tracked_people.values()), self.active_target_id
|
||||
|
||||
# ── Detection-to-track matching ──────────────────────────────────────────
|
||||
|
||||
def _match_detections(
|
||||
self, detections: List[Dict]
|
||||
) -> Tuple[List[Tuple[int, int]], List[int]]:
|
||||
"""Match detections to tracked people via re-ID.
|
||||
|
||||
Returns:
|
||||
(matched_pairs, unmatched_detection_indices)
|
||||
"""
|
||||
if not detections or not self.tracked_people:
|
||||
return [], list(range(len(detections)))
|
||||
|
||||
matched = []
|
||||
used_det_indices = set()
|
||||
|
||||
# Re-ID matching using embeddings and HSV histograms
|
||||
for track_id, person in self.tracked_people.items():
|
||||
best_score = -1.0
|
||||
best_det_idx = -1
|
||||
|
||||
for det_idx, det in enumerate(detections):
|
||||
if det_idx in used_det_indices:
|
||||
continue
|
||||
|
||||
score = self._compute_similarity(person, det)
|
||||
if score > best_score:
|
||||
best_score = score
|
||||
best_det_idx = det_idx
|
||||
|
||||
# Match if above threshold
|
||||
if best_det_idx >= 0:
|
||||
if (
|
||||
(person.embedding is not None and best_score >= self.embedding_threshold)
|
||||
or (person.hsv_histogram is not None and best_score >= self.hsv_threshold)
|
||||
):
|
||||
matched.append((track_id, best_det_idx))
|
||||
used_det_indices.add(best_det_idx)
|
||||
person.is_occluded = False
|
||||
|
||||
unmatched_det_indices = [i for i in range(len(detections)) if i not in used_det_indices]
|
||||
return matched, unmatched_det_indices
|
||||
|
||||
def _compute_similarity(self, person: TrackedPerson, detection: Dict) -> float:
|
||||
"""Compute similarity between tracked person and detection."""
|
||||
if person.embedding is not None and detection.get("embedding") is not None:
|
||||
# Cosine similarity via dot product (embeddings are L2-normalized)
|
||||
emb_score = float(np.dot(person.embedding, detection["embedding"]))
|
||||
return emb_score
|
||||
|
||||
if person.hsv_histogram is not None and detection.get("hsv_histogram") is not None:
|
||||
# Compare HSV histograms
|
||||
hist_score = float(
|
||||
cv2.compareHist(
|
||||
person.hsv_histogram,
|
||||
detection["hsv_histogram"],
|
||||
cv2.HISTCMP_COSINE,
|
||||
)
|
||||
)
|
||||
return hist_score
|
||||
|
||||
# Fallback: spatial proximity (within similar distance/bearing)
|
||||
bearing_diff = abs(person.bearing_rad - detection["bearing_rad"])
|
||||
distance_diff = abs(person.distance_m - detection["distance_m"])
|
||||
spatial_score = 1.0 / (1.0 + bearing_diff + distance_diff * 0.1)
|
||||
return spatial_score
|
||||
|
||||
# ── Target priority and group handling ────────────────────────────────────
|
||||
|
||||
def _update_target_priority(self, speaking_person_id: Optional[int]) -> None:
|
||||
"""Update active target based on priority:
|
||||
1. Wake-word speaker
|
||||
2. Closest known person
|
||||
3. Largest bounding box
|
||||
"""
|
||||
if not self.tracked_people:
|
||||
self.active_target_id = None
|
||||
self.searching = True
|
||||
return
|
||||
|
||||
# Priority 1: Wake-word speaker
|
||||
if speaking_person_id is not None and speaking_person_id in self.tracked_people:
|
||||
self.active_target_id = speaking_person_id
|
||||
self.searching = False
|
||||
return
|
||||
|
||||
# Priority 2: Closest non-occluded person
|
||||
closest_id = None
|
||||
closest_dist = float("inf")
|
||||
for track_id, person in self.tracked_people.items():
|
||||
if not person.is_occluded and person.distance_m > 0:
|
||||
if person.distance_m < closest_dist:
|
||||
closest_dist = person.distance_m
|
||||
closest_id = track_id
|
||||
|
||||
if closest_id is not None:
|
||||
self.active_target_id = closest_id
|
||||
self.searching = False
|
||||
return
|
||||
|
||||
# Priority 3: Largest bounding box
|
||||
largest_id = None
|
||||
largest_area = 0
|
||||
for track_id, person in self.tracked_people.items():
|
||||
if person.bbox is not None:
|
||||
_, _, w, h = person.bbox
|
||||
area = w * h
|
||||
if area > largest_area:
|
||||
largest_area = area
|
||||
largest_id = track_id
|
||||
|
||||
if largest_id is not None:
|
||||
self.active_target_id = largest_id
|
||||
self.searching = False
|
||||
else:
|
||||
self.searching = True
|
||||
|
||||
def get_group_centroid(self) -> Optional[Tuple[float, float, float]]:
|
||||
"""Get centroid (bearing, distance) of all tracked people.
|
||||
|
||||
Returns:
|
||||
(mean_bearing_rad, mean_distance_m) or None if no people
|
||||
"""
|
||||
if not self.tracked_people:
|
||||
return None
|
||||
|
||||
bearings = []
|
||||
distances = []
|
||||
for person in self.tracked_people.values():
|
||||
if not person.is_occluded and person.distance_m > 0:
|
||||
bearings.append(person.bearing_rad)
|
||||
distances.append(person.distance_m)
|
||||
|
||||
if not bearings:
|
||||
return None
|
||||
|
||||
# Mean bearing (circular mean)
|
||||
mean_bearing = float(np.mean(bearings))
|
||||
mean_distance = float(np.mean(distances))
|
||||
return mean_bearing, mean_distance
|
||||
|
||||
# ── Pruning and cleanup ──────────────────────────────────────────────────
|
||||
|
||||
def _prune_lost_people(self) -> None:
|
||||
"""Remove people lost for more than occlusion grace period."""
|
||||
current_time = time.time()
|
||||
to_remove = []
|
||||
|
||||
for track_id, person in self.tracked_people.items():
|
||||
# Check occlusion timeout
|
||||
if person.is_occluded:
|
||||
elapsed = current_time - person.occlusion_start_time
|
||||
if elapsed > self.occlusion_grace_s:
|
||||
to_remove.append(track_id)
|
||||
continue
|
||||
|
||||
# Check long-term invisibility (10s)
|
||||
if person.age_seconds() > 10.0:
|
||||
to_remove.append(track_id)
|
||||
|
||||
for track_id in to_remove:
|
||||
del self.tracked_people[track_id]
|
||||
if self.active_target_id == track_id:
|
||||
self.active_target_id = None
|
||||
self.searching = True
|
||||
@ -1,185 +0,0 @@
|
||||
"""multi_person_tracker_node.py — Multi-person tracking node for Issue #423.
|
||||
|
||||
Subscribes to person detections with re-ID embeddings and publishes:
|
||||
/saltybot/tracked_people (PersonArray) — all tracked people + active target
|
||||
/saltybot/follow_target (Person) — the current follow target
|
||||
|
||||
Implements:
|
||||
- Up to 10 tracked people
|
||||
- Target priority: wake-word speaker > closest > largest bbox
|
||||
- Occlusion handoff (3s grace)
|
||||
- Re-ID via face embedding + HSV
|
||||
- Group detection
|
||||
- Lost target: SEARCHING state
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
from typing import Dict, List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, QoSReliabilityPolicy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
from std_msgs.msg import String
|
||||
from geometry_msgs.msg import Twist
|
||||
from saltybot_social_msgs.msg import Person, PersonArray
|
||||
|
||||
from .multi_person_tracker import MultiPersonTracker, TrackedPerson
|
||||
|
||||
|
||||
class MultiPersonTrackerNode(Node):
|
||||
"""ROS2 node for multi-person tracking."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("multi_person_tracker_node")
|
||||
|
||||
# ── Parameters ──────────────────────────────────────────────────────
|
||||
self.declare_parameter("max_people", 10)
|
||||
self.declare_parameter("occlusion_grace_s", 3.0)
|
||||
self.declare_parameter("embedding_threshold", 0.75)
|
||||
self.declare_parameter("hsv_threshold", 0.60)
|
||||
self.declare_parameter("publish_hz", 15.0)
|
||||
self.declare_parameter("enable_group_follow", True)
|
||||
self.declare_parameter("announce_state", True)
|
||||
|
||||
max_people = self.get_parameter("max_people").value
|
||||
occlusion_grace = self.get_parameter("occlusion_grace_s").value
|
||||
embed_threshold = self.get_parameter("embedding_threshold").value
|
||||
hsv_threshold = self.get_parameter("hsv_threshold").value
|
||||
publish_hz = self.get_parameter("publish_hz").value
|
||||
self._enable_group = self.get_parameter("enable_group_follow").value
|
||||
self._announce = self.get_parameter("announce_state").value
|
||||
|
||||
# ── Tracker instance ────────────────────────────────────────────────
|
||||
self._tracker = MultiPersonTracker(
|
||||
max_people=max_people,
|
||||
occlusion_grace_s=occlusion_grace,
|
||||
embedding_similarity_threshold=embed_threshold,
|
||||
hsv_similarity_threshold=hsv_threshold,
|
||||
)
|
||||
|
||||
# ── Reliable QoS ────────────────────────────────────────────────────
|
||||
qos = QoSProfile(depth=5)
|
||||
qos.reliability = QoSReliabilityPolicy.RELIABLE
|
||||
|
||||
self._tracked_pub = self.create_publisher(
|
||||
PersonArray,
|
||||
"/saltybot/tracked_people",
|
||||
qos,
|
||||
)
|
||||
|
||||
self._target_pub = self.create_publisher(
|
||||
Person,
|
||||
"/saltybot/follow_target",
|
||||
qos,
|
||||
)
|
||||
|
||||
self._state_pub = self.create_publisher(
|
||||
String,
|
||||
"/saltybot/tracker_state",
|
||||
qos,
|
||||
)
|
||||
|
||||
self._cmd_vel_pub = self.create_publisher(
|
||||
Twist,
|
||||
"/saltybot/cmd_vel",
|
||||
qos,
|
||||
)
|
||||
|
||||
# ── Placeholder: in real implementation, subscribe to detection topics
|
||||
# For now, we'll use a timer to demonstrate the node structure
|
||||
self._update_timer = self.create_timer(1.0 / publish_hz, self._update_tracker)
|
||||
|
||||
self._last_state = "IDLE"
|
||||
self.get_logger().info(
|
||||
f"multi_person_tracker_node ready (max_people={max_people}, "
|
||||
f"occlusion_grace={occlusion_grace}s, publish_hz={publish_hz} Hz)"
|
||||
)
|
||||
|
||||
def _update_tracker(self) -> None:
|
||||
"""Update tracker and publish results."""
|
||||
# In real implementation, this would be triggered by detection callbacks
|
||||
# For now, we'll use the timer and publish empty/demo data
|
||||
|
||||
# Get current tracked people and active target
|
||||
tracked_people = list(self._tracker.tracked_people.values())
|
||||
active_target_id = self._tracker.active_target_id
|
||||
|
||||
# Build PersonArray message
|
||||
person_array_msg = PersonArray()
|
||||
person_array_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
person_array_msg.header.frame_id = "base_link"
|
||||
person_array_msg.active_id = active_target_id if active_target_id is not None else -1
|
||||
|
||||
for tracked in tracked_people:
|
||||
person_msg = Person()
|
||||
person_msg.header = person_array_msg.header
|
||||
person_msg.track_id = tracked.track_id
|
||||
person_msg.bearing_rad = tracked.bearing_rad
|
||||
person_msg.distance_m = tracked.distance_m
|
||||
person_msg.confidence = tracked.confidence
|
||||
person_msg.is_speaking = tracked.is_speaking
|
||||
person_msg.source = tracked.source
|
||||
person_array_msg.persons.append(person_msg)
|
||||
|
||||
self._tracked_pub.publish(person_array_msg)
|
||||
|
||||
# Publish active target
|
||||
if active_target_id is not None and active_target_id in self._tracker.tracked_people:
|
||||
target = self._tracker.tracked_people[active_target_id]
|
||||
target_msg = Person()
|
||||
target_msg.header = person_array_msg.header
|
||||
target_msg.track_id = target.track_id
|
||||
target_msg.bearing_rad = target.bearing_rad
|
||||
target_msg.distance_m = target.distance_m
|
||||
target_msg.confidence = target.confidence
|
||||
target_msg.is_speaking = target.is_speaking
|
||||
target_msg.source = target.source
|
||||
self._target_pub.publish(target_msg)
|
||||
|
||||
# Publish tracker state
|
||||
state = "SEARCHING" if self._tracker.searching else "TRACKING"
|
||||
if state != self._last_state and self._announce:
|
||||
self.get_logger().info(f"State: {state}")
|
||||
self._last_state = state
|
||||
state_msg = String()
|
||||
state_msg.data = state
|
||||
self._state_pub.publish(state_msg)
|
||||
|
||||
# Handle lost target
|
||||
if state == "SEARCHING":
|
||||
self._handle_lost_target()
|
||||
|
||||
def _handle_lost_target(self) -> None:
|
||||
"""Execute behavior when target is lost: stop + rotate."""
|
||||
# Publish stop command
|
||||
stop_twist = Twist()
|
||||
self._cmd_vel_pub.publish(stop_twist)
|
||||
|
||||
# Slow rotating search
|
||||
search_twist = Twist()
|
||||
search_twist.angular.z = 0.5 # rad/s
|
||||
self._cmd_vel_pub.publish(search_twist)
|
||||
|
||||
if self._announce:
|
||||
self.get_logger().warn("Lost target, searching...")
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = MultiPersonTrackerNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_multi_person_tracker
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_multi_person_tracker
|
||||
@ -1,29 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = 'saltybot_multi_person_tracker'
|
||||
|
||||
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']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
author='sl-perception',
|
||||
author_email='sl-perception@saltylab.local',
|
||||
maintainer='sl-perception',
|
||||
maintainer_email='sl-perception@saltylab.local',
|
||||
url='https://gitea.vayrette.com/seb/saltylab-firmware',
|
||||
description='Multi-person tracker with group handling and target priority',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'multi_person_tracker_node = saltybot_multi_person_tracker.multi_person_tracker_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,122 +0,0 @@
|
||||
"""Tests for multi-person tracker (Issue #423)."""
|
||||
|
||||
import numpy as np
|
||||
from saltybot_multi_person_tracker.multi_person_tracker import (
|
||||
MultiPersonTracker,
|
||||
TrackedPerson,
|
||||
)
|
||||
|
||||
|
||||
def test_create_new_tracks():
|
||||
"""Test creating new tracks for detections."""
|
||||
tracker = MultiPersonTracker(max_people=10)
|
||||
|
||||
detections = [
|
||||
{"bearing_rad": 0.1, "distance_m": 1.5, "confidence": 0.9},
|
||||
{"bearing_rad": -0.2, "distance_m": 2.0, "confidence": 0.85},
|
||||
]
|
||||
|
||||
tracked, target_id = tracker.update(detections)
|
||||
|
||||
assert len(tracked) == 2
|
||||
assert tracked[0].track_id == 0
|
||||
assert tracked[1].track_id == 1
|
||||
|
||||
|
||||
def test_match_existing_tracks():
|
||||
"""Test matching detections to existing tracks."""
|
||||
tracker = MultiPersonTracker(max_people=10)
|
||||
|
||||
# Create initial tracks
|
||||
detections1 = [
|
||||
{"bearing_rad": 0.1, "distance_m": 1.5, "confidence": 0.9},
|
||||
{"bearing_rad": -0.2, "distance_m": 2.0, "confidence": 0.85},
|
||||
]
|
||||
tracked1, _ = tracker.update(detections1)
|
||||
|
||||
# Update with similar positions
|
||||
detections2 = [
|
||||
{"bearing_rad": 0.12, "distance_m": 1.48, "confidence": 0.88},
|
||||
{"bearing_rad": -0.22, "distance_m": 2.05, "confidence": 0.84},
|
||||
]
|
||||
tracked2, _ = tracker.update(detections2)
|
||||
|
||||
# Should have same IDs (matched)
|
||||
assert len(tracked2) == 2
|
||||
ids_after = {p.track_id for p in tracked2}
|
||||
assert ids_after == {0, 1}
|
||||
|
||||
|
||||
def test_max_people_limit():
|
||||
"""Test that tracker respects max_people limit."""
|
||||
tracker = MultiPersonTracker(max_people=3)
|
||||
|
||||
detections = [
|
||||
{"bearing_rad": 0.1 * i, "distance_m": 1.5 + i * 0.2, "confidence": 0.9}
|
||||
for i in range(5)
|
||||
]
|
||||
|
||||
tracked, _ = tracker.update(detections)
|
||||
assert len(tracked) <= 3
|
||||
|
||||
|
||||
def test_wake_word_speaker_priority():
|
||||
"""Test that wake-word speaker gets highest priority."""
|
||||
tracker = MultiPersonTracker(max_people=10)
|
||||
|
||||
detections = [
|
||||
{"bearing_rad": 0.1, "distance_m": 5.0, "confidence": 0.9}, # Far
|
||||
{"bearing_rad": -0.2, "distance_m": 1.0, "confidence": 0.85}, # Closest
|
||||
{"bearing_rad": 0.3, "distance_m": 2.0, "confidence": 0.8}, # Farthest
|
||||
]
|
||||
|
||||
tracked, _ = tracker.update(detections)
|
||||
|
||||
# Without speaker: should pick closest
|
||||
assert tracker.active_target_id == 1 # Closest person
|
||||
|
||||
# With speaker at ID 0: should pick speaker
|
||||
tracked, target = tracker.update(detections, speaking_person_id=0)
|
||||
assert target == 0
|
||||
|
||||
|
||||
def test_group_centroid():
|
||||
"""Test group centroid calculation."""
|
||||
tracker = MultiPersonTracker(max_people=10)
|
||||
|
||||
detections = [
|
||||
{"bearing_rad": 0.0, "distance_m": 1.0, "confidence": 0.9},
|
||||
{"bearing_rad": 0.2, "distance_m": 2.0, "confidence": 0.85},
|
||||
{"bearing_rad": -0.2, "distance_m": 1.5, "confidence": 0.8},
|
||||
]
|
||||
|
||||
tracked, _ = tracker.update(detections)
|
||||
|
||||
centroid = tracker.get_group_centroid()
|
||||
assert centroid is not None
|
||||
mean_bearing, mean_distance = centroid
|
||||
# Mean bearing should be close to 0
|
||||
assert abs(mean_bearing) < 0.1
|
||||
# Mean distance should be between 1.0 and 2.0
|
||||
assert 1.0 < mean_distance < 2.0
|
||||
|
||||
|
||||
def test_empty_tracker():
|
||||
"""Test tracker with no detections."""
|
||||
tracker = MultiPersonTracker()
|
||||
|
||||
tracked, target = tracker.update([])
|
||||
|
||||
assert len(tracked) == 0
|
||||
assert target is None
|
||||
assert tracker.searching is True
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
test_create_new_tracks()
|
||||
test_match_existing_tracks()
|
||||
test_max_people_limit()
|
||||
test_wake_word_speaker_priority()
|
||||
test_group_centroid()
|
||||
test_empty_tracker()
|
||||
print("All tests passed!")
|
||||
@ -1,47 +0,0 @@
|
||||
costmap_common_params:
|
||||
update_frequency: 10.0
|
||||
publish_frequency: 5.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: false
|
||||
rolling_window: false
|
||||
robot_radius: 0.35
|
||||
plugins:
|
||||
- name: static_layer
|
||||
type: nav2_costmap_2d::StaticLayer
|
||||
map_subscribe_transient_local: true
|
||||
enabled: true
|
||||
- name: obstacle_layer
|
||||
type: nav2_costmap_2d::ObstacleLayer
|
||||
enabled: true
|
||||
observation_sources: scan camera_depth
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.5
|
||||
min_obstacle_height: 0.0
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: LaserScan
|
||||
camera_depth:
|
||||
topic: /camera/depth/color/points
|
||||
max_obstacle_height: 2.5
|
||||
min_obstacle_height: 0.0
|
||||
clearing: false
|
||||
marking: true
|
||||
data_type: PointCloud2
|
||||
sensor_frame: camera_link
|
||||
expected_update_rate: 15.0
|
||||
observation_persistence: 0.0
|
||||
inf_is_valid: false
|
||||
filter: passthrough
|
||||
filter_value: 5.0
|
||||
- name: inflation_layer
|
||||
type: nav2_costmap_2d::InflationLayer
|
||||
enabled: true
|
||||
inflation_radius: 0.55
|
||||
cost_scaling_factor: 10.0
|
||||
inflate_unknown: false
|
||||
inflate_around_unknown: true
|
||||
lethal_cost_threshold: 100
|
||||
unknown_cost_value: -1
|
||||
resolution: 0.05
|
||||
@ -1,59 +0,0 @@
|
||||
dwb_local_planner:
|
||||
ros__parameters:
|
||||
max_vel_x: 5.5
|
||||
min_vel_x: 0.0
|
||||
max_vel_y: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_theta: 3.0
|
||||
min_vel_theta: -3.0
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 2.5
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -2.5
|
||||
vx_samples: 20
|
||||
vy_samples: 1
|
||||
vtheta_samples: 40
|
||||
sim_time: 1.7
|
||||
sim_granularity: 0.05
|
||||
angular_sim_granularity: 0.1
|
||||
path_distance_bias: 32.0
|
||||
goal_distance_bias: 24.0
|
||||
occdist_scale: 0.02
|
||||
forward_point_distance: 0.325
|
||||
use_dwa: true
|
||||
dwa_padding_inaccuracies: 0.1
|
||||
max_allowed_time_female: 0.0
|
||||
oscillation_max_iterations: 1800
|
||||
oscillation_max_distance: 0.02
|
||||
use_velocity_scaled_lookahead_dist: false
|
||||
min_approach_linear_velocity: 0.1
|
||||
approach_velocity_scaling_dist: 1.0
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
rotate_to_goal:
|
||||
slowing_factor: 5.0
|
||||
lookahead_time: -1.0
|
||||
base_obstacle:
|
||||
scale: 0.02
|
||||
max_scaling_factor: 0.02
|
||||
forward_point_distance: 0.325
|
||||
goal_align:
|
||||
scale: 24.0
|
||||
forward_point_distance: 0.325
|
||||
path_align:
|
||||
scale: 32.0
|
||||
forward_point_distance: 0.325
|
||||
max_allowed_time_female: 0.0
|
||||
path_dist:
|
||||
scale: 32.0
|
||||
max_allowed_time_female: 0.0
|
||||
goal_dist:
|
||||
scale: 24.0
|
||||
max_allowed_time_female: 0.0
|
||||
prune_plan: true
|
||||
prune_distance: 1.5
|
||||
debug_trajectory_details: false
|
||||
publish_evaluation: true
|
||||
publish_scored_sampling_policies: false
|
||||
publish_trajectories: false
|
||||
@ -1,36 +0,0 @@
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: false
|
||||
rolling_window: false
|
||||
robot_radius: 0.35
|
||||
plugins:
|
||||
- name: static_layer
|
||||
type: nav2_costmap_2d::StaticLayer
|
||||
map_subscribe_transient_local: true
|
||||
enabled: true
|
||||
- name: obstacle_layer
|
||||
type: nav2_costmap_2d::ObstacleLayer
|
||||
enabled: true
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.5
|
||||
min_obstacle_height: 0.0
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: LaserScan
|
||||
- name: inflation_layer
|
||||
type: nav2_costmap_2d::InflationLayer
|
||||
enabled: true
|
||||
inflation_radius: 0.55
|
||||
cost_scaling_factor: 10.0
|
||||
inflate_unknown: false
|
||||
inflate_around_unknown: true
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
unknown_cost_value: -1
|
||||
@ -1,51 +0,0 @@
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 10.0
|
||||
publish_frequency: 5.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: false
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.35
|
||||
plugins:
|
||||
- name: obstacle_layer
|
||||
type: nav2_costmap_2d::ObstacleLayer
|
||||
enabled: true
|
||||
observation_sources: scan camera_depth
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.5
|
||||
min_obstacle_height: 0.0
|
||||
clearing: true
|
||||
marking: true
|
||||
data_type: LaserScan
|
||||
raytrace_max_range: 10.0
|
||||
raytrace_min_range: 0.0
|
||||
expected_update_rate: 5.5
|
||||
observation_persistence: 0.0
|
||||
camera_depth:
|
||||
topic: /camera/depth/color/points
|
||||
max_obstacle_height: 2.5
|
||||
min_obstacle_height: 0.0
|
||||
clearing: false
|
||||
marking: true
|
||||
data_type: PointCloud2
|
||||
sensor_frame: camera_link
|
||||
expected_update_rate: 15.0
|
||||
observation_persistence: 0.5
|
||||
inf_is_valid: false
|
||||
filter: passthrough
|
||||
filter_value: 5.0
|
||||
- name: inflation_layer
|
||||
type: nav2_costmap_2d::InflationLayer
|
||||
enabled: true
|
||||
inflation_radius: 0.55
|
||||
cost_scaling_factor: 10.0
|
||||
inflate_unknown: true
|
||||
inflate_around_unknown: false
|
||||
track_unknown_space: true
|
||||
unknown_cost_value: 0
|
||||
@ -1,219 +0,0 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_search_increment: 0.1
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "differential"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
sigma_short: 0.05
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
|
||||
amcl_map_client:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
amcl_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
enable_groot_monitoring: true
|
||||
groot_zmq_publisher_port: 1666
|
||||
groot_zmq_server_port: 1667
|
||||
default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
default_nav_to_pose_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
progress_checker:
|
||||
plugin: "nav2_core::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_core::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: true
|
||||
min_vel_x: 0.0
|
||||
max_vel_x: 5.5
|
||||
max_vel_theta: 3.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 5.5
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_theta: 2.5
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_theta: -2.5
|
||||
vx_samples: 20
|
||||
vtheta_samples: 40
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.1
|
||||
transform_tolerance: 0.2
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: true
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
GoalAlign.scale: 24.0
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: false
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: true
|
||||
allow_unknown: true
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_timeout: 0.1
|
||||
use_sim_time: false
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotations: 1.0
|
||||
max_backup_dist: 0.15
|
||||
backup_speed: 0.5
|
||||
|
||||
behavior_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
lifecycle_manager_navigation:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
autostart: true
|
||||
node_names: ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator']
|
||||
|
||||
lifecycle_manager_localization:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
autostart: true
|
||||
node_names: ['amcl']
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
yaml_filename: "map.yaml"
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: false
|
||||
feedback: "odometry"
|
||||
max_velocity: [5.5, 0.0, 3.0]
|
||||
min_velocity: [-5.5, 0.0, -3.0]
|
||||
max_accel: [2.5, 0.0, 2.5]
|
||||
max_decel: [-2.5, 0.0, -2.5]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@ -1,58 +0,0 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
mode: mapping
|
||||
async_mode: true
|
||||
use_scan_matching: true
|
||||
use_scan_bamatching: true
|
||||
minimum_time_interval: 0.5
|
||||
minimum_travel_distance: 0.2
|
||||
minimum_rotation_deg: 5.0
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.05
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
correlation_search_space_std_dev_ratio: 0.5
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_max_scan_distance: 12.0
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_search_step_size: 0.0349
|
||||
fine_angle_search_step_size: 0.00349
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_queue_size: 10
|
||||
loop_match_minimum_response_fine: 0.1
|
||||
loop_match_minimum_response_coarse: 0.3
|
||||
minimum_score_to_accept_loop_closure: 0.07
|
||||
do_final_optimization: true
|
||||
final_optimization_particle_filter_size: 800
|
||||
final_optimization_min_particles: 400
|
||||
final_optimization_max_particles: 2000
|
||||
final_optimization_threshold_metric_improvement: 0.0002
|
||||
add_loops_as_edges: false
|
||||
num_for_scan_matching: 0
|
||||
icp_iterations: 10
|
||||
interactive_mode: false
|
||||
print_timing_information: false
|
||||
map_frame: map
|
||||
odom_frame: odom
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
publish_tf: true
|
||||
tf_buffer_duration: 30.0
|
||||
transform_timeout: 0.2
|
||||
queue_size: 10
|
||||
throttle_scans: 1
|
||||
use_map_saver: true
|
||||
map_start_at_dock: true
|
||||
map_start_pose: [0.0, 0.0, 0.0]
|
||||
rollout_velocity_smoothing_factor: 0.07
|
||||
force_turtlebot_rep: false
|
||||
@ -1,28 +0,0 @@
|
||||
"""RealSense Depth to Costmap Integration"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('use_sim_time', default_value='false'),
|
||||
|
||||
Node(
|
||||
package='depth_image_proc',
|
||||
executable='convert_node',
|
||||
name='depth_image_converter',
|
||||
namespace='camera',
|
||||
remappings=[
|
||||
('image_rect', '/camera/aligned_depth_to_color/image_raw'),
|
||||
('camera_info', '/camera/color/camera_info'),
|
||||
('points', '/camera/depth/color/points'),
|
||||
],
|
||||
parameters=[{'use_sim_time': use_sim_time}],
|
||||
output='screen',
|
||||
),
|
||||
])
|
||||
@ -1,66 +0,0 @@
|
||||
"""Nav2 SLAM Bringup for SaltyBot (Issue #422)"""
|
||||
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam')
|
||||
bringup_dir = get_package_share_directory('saltybot_bringup')
|
||||
|
||||
nav2_params = os.path.join(nav2_slam_dir, 'config', 'nav2_params.yaml')
|
||||
slam_params = os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('use_sim_time', default_value='false'),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'sensors.launch.py')
|
||||
),
|
||||
launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
|
||||
),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_slam_dir, 'launch', 'odometry_bridge.launch.py')
|
||||
),
|
||||
launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
|
||||
),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_slam_dir, 'launch', 'nav2_slam_integrated.launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'slam_params_file': slam_params,
|
||||
'use_sim_time': LaunchConfiguration('use_sim_time'),
|
||||
}.items(),
|
||||
),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_slam_dir, 'launch', 'depth_to_costmap.launch.py')
|
||||
),
|
||||
launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
|
||||
),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(
|
||||
get_package_share_directory('nav2_bringup'),
|
||||
'launch', 'navigation_launch.py'
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
'use_sim_time': LaunchConfiguration('use_sim_time'),
|
||||
'params_file': nav2_params,
|
||||
'autostart': 'true',
|
||||
'map_subscribe_transient_local': 'true',
|
||||
}.items(),
|
||||
),
|
||||
])
|
||||
@ -1,35 +0,0 @@
|
||||
"""SLAM Toolbox integrated launch for SaltyBot"""
|
||||
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
slam_share = get_package_share_directory('slam_toolbox')
|
||||
nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam')
|
||||
|
||||
slam_params = LaunchConfiguration('slam_params_file')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'slam_params_file',
|
||||
default_value=os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml'),
|
||||
),
|
||||
|
||||
DeclareLaunchArgument('use_sim_time', default_value='false'),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(slam_share, 'launch', 'online_async_launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'params_file': slam_params,
|
||||
'use_sim_time': use_sim_time,
|
||||
}.items(),
|
||||
),
|
||||
])
|
||||
@ -1,36 +0,0 @@
|
||||
"""VESC Telemetry to Odometry Bridge"""
|
||||
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('use_sim_time', default_value='false'),
|
||||
|
||||
Node(
|
||||
package='saltybot_nav2_slam',
|
||||
executable='vesc_odometry_bridge',
|
||||
name='vesc_odometry_bridge',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'vesc_state_topic': '/vesc/state',
|
||||
'odometry_topic': '/odom',
|
||||
'odom_frame_id': 'odom',
|
||||
'base_frame_id': 'base_link',
|
||||
'update_frequency': 50,
|
||||
'wheel_base': 0.35,
|
||||
'wheel_radius': 0.10,
|
||||
'max_rpm': 60000,
|
||||
'publish_tf': True,
|
||||
'tf_queue_size': 10,
|
||||
}],
|
||||
output='screen',
|
||||
),
|
||||
])
|
||||
@ -1,39 +0,0 @@
|
||||
<?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_nav2_slam</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Nav2 SLAM integration for SaltyBot autonomous navigation.
|
||||
Combines SLAM Toolbox (RPLIDAR 2D SLAM), RealSense depth for obstacle avoidance,
|
||||
VESC odometry, and DWB planner for autonomous navigation up to 20km/h.
|
||||
</description>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
<exec_depend>slam_toolbox</exec_depend>
|
||||
<exec_depend>rplidar_ros</exec_depend>
|
||||
<exec_depend>realsense2_camera</exec_depend>
|
||||
<exec_depend>depth_image_proc</exec_depend>
|
||||
<exec_depend>pointcloud_to_laserscan</exec_depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_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>
|
||||
@ -1 +0,0 @@
|
||||
# Nav2 SLAM integration for SaltyBot
|
||||
@ -1,180 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""VESC telemetry to Nav2 odometry bridge."""
|
||||
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from nav_msgs.msg import Odometry
|
||||
from geometry_msgs.msg import TransformStamped, Quaternion
|
||||
from tf2_ros import TransformBroadcaster
|
||||
|
||||
|
||||
class VESCOdometryBridge(Node):
|
||||
"""ROS2 node bridging VESC telemetry to Nav2 odometry."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("vesc_odometry_bridge")
|
||||
|
||||
self.declare_parameter("vesc_state_topic", "/vesc/state")
|
||||
self.declare_parameter("odometry_topic", "/odom")
|
||||
self.declare_parameter("odom_frame_id", "odom")
|
||||
self.declare_parameter("base_frame_id", "base_link")
|
||||
self.declare_parameter("update_frequency", 50)
|
||||
self.declare_parameter("wheel_base", 0.35)
|
||||
self.declare_parameter("wheel_radius", 0.10)
|
||||
self.declare_parameter("max_rpm", 60000)
|
||||
self.declare_parameter("publish_tf", True)
|
||||
|
||||
self.vesc_state_topic = self.get_parameter("vesc_state_topic").value
|
||||
self.odometry_topic = self.get_parameter("odometry_topic").value
|
||||
self.odom_frame_id = self.get_parameter("odom_frame_id").value
|
||||
self.base_frame_id = self.get_parameter("base_frame_id").value
|
||||
frequency = self.get_parameter("update_frequency").value
|
||||
self.wheel_base = self.get_parameter("wheel_base").value
|
||||
self.wheel_radius = self.get_parameter("wheel_radius").value
|
||||
self.publish_tf = self.get_parameter("publish_tf").value
|
||||
|
||||
self.last_rpm = 0
|
||||
self.last_update_time = time.time()
|
||||
|
||||
self.x = 0.0
|
||||
self.y = 0.0
|
||||
self.theta = 0.0
|
||||
self.vx = 0.0
|
||||
self.vy = 0.0
|
||||
self.vtheta = 0.0
|
||||
|
||||
self.pub_odom = self.create_publisher(Odometry, self.odometry_topic, 10)
|
||||
|
||||
if self.publish_tf:
|
||||
self.tf_broadcaster = TransformBroadcaster(self)
|
||||
|
||||
self.create_subscription(String, self.vesc_state_topic, self._on_vesc_state, 10)
|
||||
|
||||
period = 1.0 / frequency
|
||||
self.create_timer(period, self._publish_odometry)
|
||||
|
||||
self.get_logger().info(
|
||||
f"VESC odometry bridge initialized: "
|
||||
f"wheel_base={self.wheel_base}m, wheel_radius={self.wheel_radius}m"
|
||||
)
|
||||
|
||||
def _on_vesc_state(self, msg: String) -> None:
|
||||
"""Update VESC telemetry from JSON."""
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
current_rpm = data.get("rpm", 0)
|
||||
motor_rad_s = (current_rpm / 60.0) * (2.0 * math.pi)
|
||||
wheel_velocity = motor_rad_s * self.wheel_radius
|
||||
|
||||
self.vx = wheel_velocity
|
||||
self.vy = 0.0
|
||||
self.vtheta = 0.0
|
||||
self.last_rpm = current_rpm
|
||||
|
||||
except json.JSONDecodeError:
|
||||
pass
|
||||
|
||||
def _publish_odometry(self) -> None:
|
||||
"""Publish odometry message and TF."""
|
||||
current_time = time.time()
|
||||
dt = current_time - self.last_update_time
|
||||
self.last_update_time = current_time
|
||||
|
||||
if abs(self.vtheta) > 0.001:
|
||||
self.theta += self.vtheta * dt
|
||||
self.x += (self.vx / self.vtheta) * (math.sin(self.theta) - math.sin(self.theta - self.vtheta * dt))
|
||||
self.y += (self.vx / self.vtheta) * (math.cos(self.theta - self.vtheta * dt) - math.cos(self.theta))
|
||||
else:
|
||||
self.x += self.vx * math.cos(self.theta) * dt
|
||||
self.y += self.vx * math.sin(self.theta) * dt
|
||||
|
||||
odom_msg = Odometry()
|
||||
odom_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
odom_msg.header.frame_id = self.odom_frame_id
|
||||
odom_msg.child_frame_id = self.base_frame_id
|
||||
|
||||
odom_msg.pose.pose.position.x = self.x
|
||||
odom_msg.pose.pose.position.y = self.y
|
||||
odom_msg.pose.pose.position.z = 0.0
|
||||
odom_msg.pose.pose.orientation = self._euler_to_quaternion(0, 0, self.theta)
|
||||
|
||||
odom_msg.pose.covariance = [
|
||||
0.1, 0, 0, 0, 0, 0,
|
||||
0, 0.1, 0, 0, 0, 0,
|
||||
0, 0, 0.1, 0, 0, 0,
|
||||
0, 0, 0, 0.1, 0, 0,
|
||||
0, 0, 0, 0, 0.1, 0,
|
||||
0, 0, 0, 0, 0, 0.1,
|
||||
]
|
||||
|
||||
odom_msg.twist.twist.linear.x = self.vx
|
||||
odom_msg.twist.twist.linear.y = self.vy
|
||||
odom_msg.twist.twist.linear.z = 0.0
|
||||
odom_msg.twist.twist.angular.z = self.vtheta
|
||||
|
||||
odom_msg.twist.covariance = [
|
||||
0.05, 0, 0, 0, 0, 0,
|
||||
0, 0.05, 0, 0, 0, 0,
|
||||
0, 0, 0.05, 0, 0, 0,
|
||||
0, 0, 0, 0.05, 0, 0,
|
||||
0, 0, 0, 0, 0.05, 0,
|
||||
0, 0, 0, 0, 0, 0.05,
|
||||
]
|
||||
|
||||
self.pub_odom.publish(odom_msg)
|
||||
|
||||
if self.publish_tf:
|
||||
self._publish_tf(odom_msg.header.stamp)
|
||||
|
||||
def _publish_tf(self, timestamp) -> None:
|
||||
"""Publish odom → base_link TF."""
|
||||
tf_msg = TransformStamped()
|
||||
tf_msg.header.stamp = timestamp
|
||||
tf_msg.header.frame_id = self.odom_frame_id
|
||||
tf_msg.child_frame_id = self.base_frame_id
|
||||
|
||||
tf_msg.transform.translation.x = self.x
|
||||
tf_msg.transform.translation.y = self.y
|
||||
tf_msg.transform.translation.z = 0.0
|
||||
tf_msg.transform.rotation = self._euler_to_quaternion(0, 0, self.theta)
|
||||
|
||||
self.tf_broadcaster.sendTransform(tf_msg)
|
||||
|
||||
@staticmethod
|
||||
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> Quaternion:
|
||||
"""Convert Euler angles to quaternion."""
|
||||
cy = math.cos(yaw * 0.5)
|
||||
sy = math.sin(yaw * 0.5)
|
||||
cp = math.cos(pitch * 0.5)
|
||||
sp = math.sin(pitch * 0.5)
|
||||
cr = math.cos(roll * 0.5)
|
||||
sr = math.sin(roll * 0.5)
|
||||
|
||||
q = Quaternion()
|
||||
q.w = cr * cp * cy + sr * sp * sy
|
||||
q.x = sr * cp * cy - cr * sp * sy
|
||||
q.y = cr * sp * cy + sr * cp * sy
|
||||
q.z = cr * cp * sy - sr * sp * cy
|
||||
|
||||
return q
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = VESCOdometryBridge()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_nav2_slam
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_nav2_slam
|
||||
@ -1,39 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_nav2_slam"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/launch", [
|
||||
"launch/nav2_slam_bringup.launch.py",
|
||||
"launch/nav2_slam_integrated.launch.py",
|
||||
"launch/depth_to_costmap.launch.py",
|
||||
"launch/odometry_bridge.launch.py",
|
||||
]),
|
||||
(f"share/{package_name}/config", [
|
||||
"config/nav2_params.yaml",
|
||||
"config/slam_toolbox_params.yaml",
|
||||
"config/costmap_common_params.yaml",
|
||||
"config/global_costmap_params.yaml",
|
||||
"config/local_costmap_params.yaml",
|
||||
"config/dwb_local_planner_params.yaml",
|
||||
]),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-controls",
|
||||
maintainer_email="sl-controls@saltylab.local",
|
||||
description="Nav2 SLAM integration with RPLIDAR + RealSense for autonomous navigation",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"vesc_odometry_bridge = saltybot_nav2_slam.vesc_odometry_bridge:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,9 +0,0 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
*.pyc
|
||||
__pycache__/
|
||||
.pytest_cache/
|
||||
*.egg-info/
|
||||
dist/
|
||||
*.egg
|
||||
@ -1,6 +0,0 @@
|
||||
remote_monitor:
|
||||
ros__parameters:
|
||||
ws_port: 9091
|
||||
auth_token: 'default_token'
|
||||
update_rate_hz: 2
|
||||
enable_msgpack: true
|
||||
@ -1,23 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_dir = get_package_share_directory('saltybot_remote_monitor')
|
||||
config_file = os.path.join(pkg_dir, 'config', 'remote_monitor.yaml')
|
||||
|
||||
remote_monitor_node = Node(
|
||||
package='saltybot_remote_monitor',
|
||||
executable='remote_monitor',
|
||||
name='remote_monitor',
|
||||
parameters=[config_file],
|
||||
output='screen',
|
||||
respawn=True,
|
||||
respawn_delay=5,
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
remote_monitor_node,
|
||||
])
|
||||
@ -1,32 +0,0 @@
|
||||
<?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_remote_monitor</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Remote monitoring WebSocket relay with mobile-friendly UI.
|
||||
2Hz JSON telemetry aggregation with msgpack compression, token auth,
|
||||
5min history buffer, and critical alerts (fall, low battery, node crash).
|
||||
</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>diagnostic_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<exec_depend>python3-aiohttp</exec_depend>
|
||||
<exec_depend>python3-msgpack</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>
|
||||
@ -1,287 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional
|
||||
|
||||
import msgpack
|
||||
import rclpy
|
||||
from aiohttp import web
|
||||
from diagnostic_msgs.msg import DiagnosticArray
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import Float32, Bool, String
|
||||
|
||||
|
||||
class TelemetryBuffer:
|
||||
"""Circular buffer for 5-minute history at 2Hz."""
|
||||
|
||||
def __init__(self, duration_seconds=300):
|
||||
self.max_samples = int(duration_seconds * 2)
|
||||
self.buffer = deque(maxlen=self.max_samples)
|
||||
|
||||
def add(self, data: Dict):
|
||||
self.buffer.append({**data, 'ts': time.time()})
|
||||
|
||||
def get_history(self):
|
||||
return list(self.buffer)
|
||||
|
||||
|
||||
class RemoteMonitorNode(Node):
|
||||
"""WebSocket relay for remote monitoring with telemetry aggregation."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('saltybot_remote_monitor')
|
||||
|
||||
self.declare_parameter('ws_port', 9091)
|
||||
self.declare_parameter('auth_token', 'default_token')
|
||||
self.declare_parameter('update_rate_hz', 2)
|
||||
self.declare_parameter('enable_msgpack', True)
|
||||
|
||||
self.ws_port = self.get_parameter('ws_port').value
|
||||
self.auth_token = self.get_parameter('auth_token').value
|
||||
self.update_rate_hz = self.get_parameter('update_rate_hz').value
|
||||
self.enable_msgpack = self.get_parameter('enable_msgpack').value
|
||||
|
||||
self.battery = {'voltage': 0.0, 'current': 0.0, 'soc': 0.0}
|
||||
self.motors = {'left': 0.0, 'right': 0.0}
|
||||
self.imu = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}
|
||||
self.gps = {'lat': 0.0, 'lon': 0.0, 'alt': 0.0}
|
||||
self.health = {'cpu_temp': 0.0, 'gpu_temp': 0.0, 'ram_pct': 0.0, 'disk_pct': 0.0}
|
||||
self.social = {'is_speaking': False, 'face_id': None}
|
||||
self.alerts = []
|
||||
self.history_buffer = TelemetryBuffer()
|
||||
self.ws_clients = set()
|
||||
self.loop = None
|
||||
|
||||
self.create_subscription(DiagnosticArray, '/diagnostics', self.diag_callback, 10)
|
||||
self.create_subscription(Imu, '/saltybot/imu', self.imu_callback, 10)
|
||||
self.create_subscription(NavSatFix, '/gps/fix', self.gps_callback, 10)
|
||||
self.create_subscription(String, '/saltybot/balance_state', self.balance_callback, 10)
|
||||
self.create_subscription(Bool, '/social/speech/is_speaking', self.social_speech_callback, 10)
|
||||
self.create_subscription(String, '/social/face/active', self.social_face_callback, 10)
|
||||
|
||||
period = 1.0 / self.update_rate_hz
|
||||
self.update_timer = self.create_timer(period, self.telemetry_update_callback)
|
||||
|
||||
self.ws_thread = threading.Thread(target=self.start_ws_server, daemon=True)
|
||||
self.ws_thread.start()
|
||||
|
||||
self.get_logger().info(
|
||||
f'Remote monitor initialized on port {self.ws_port}, '
|
||||
f'update rate {self.update_rate_hz}Hz, msgpack={self.enable_msgpack}'
|
||||
)
|
||||
|
||||
def diag_callback(self, msg: DiagnosticArray):
|
||||
"""Parse diagnostics for battery and system health."""
|
||||
for status in msg.status:
|
||||
kv = {pair.key: pair.value for pair in status.values}
|
||||
|
||||
if kv.get('battery_voltage_v'):
|
||||
self.battery = {
|
||||
'voltage': float(kv.get('battery_voltage_v', 0)),
|
||||
'current': float(kv.get('battery_current_a', 0)),
|
||||
'soc': float(kv.get('battery_soc_pct', 0)),
|
||||
}
|
||||
if self.battery['soc'] < 15:
|
||||
self.add_alert('CRITICAL', 'Low battery', self.battery['soc'])
|
||||
|
||||
if kv.get('cpu_temp_c'):
|
||||
self.health = {
|
||||
'cpu_temp': float(kv.get('cpu_temp_c', 0)),
|
||||
'gpu_temp': float(kv.get('gpu_temp_c', 0)),
|
||||
'ram_pct': float(kv.get('ram_pct', 0)),
|
||||
'disk_pct': float(kv.get('disk_pct', 0)),
|
||||
}
|
||||
|
||||
def imu_callback(self, msg: Imu):
|
||||
"""Extract roll/pitch/yaw from quaternion."""
|
||||
q = msg.orientation
|
||||
self.imu = self.quat_to_euler(q.x, q.y, q.z, q.w)
|
||||
|
||||
def gps_callback(self, msg: NavSatFix):
|
||||
"""Store GPS data."""
|
||||
self.gps = {
|
||||
'lat': msg.latitude,
|
||||
'lon': msg.longitude,
|
||||
'alt': msg.altitude,
|
||||
}
|
||||
|
||||
def balance_callback(self, msg: String):
|
||||
"""Parse balance state for motor commands."""
|
||||
try:
|
||||
state = json.loads(msg.data)
|
||||
cmd = state.get('motor_cmd', 0)
|
||||
norm = max(-1, min(1, cmd / 1000))
|
||||
self.motors = {'left': norm, 'right': norm}
|
||||
|
||||
if abs(norm) > 0.9:
|
||||
self.add_alert('WARNING', 'High motor load', norm)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def social_speech_callback(self, msg: Bool):
|
||||
"""Update social speaking state."""
|
||||
self.social['is_speaking'] = msg.data
|
||||
|
||||
def social_face_callback(self, msg: String):
|
||||
"""Update recognized face ID."""
|
||||
self.social['face_id'] = msg.data
|
||||
|
||||
@staticmethod
|
||||
def quat_to_euler(qx, qy, qz, qw):
|
||||
"""Convert quaternion to Euler angles."""
|
||||
sinr_cosp = 2 * (qw * qx + qy * qz)
|
||||
cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
|
||||
roll = math.atan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
sinp = 2 * (qw * qy - qz * qx)
|
||||
pitch = math.asin(max(-1, min(1, sinp)))
|
||||
|
||||
siny_cosp = 2 * (qw * qz + qx * qy)
|
||||
cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
|
||||
yaw = math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return {
|
||||
'roll': (roll * 180) / math.pi,
|
||||
'pitch': (pitch * 180) / math.pi,
|
||||
'yaw': (yaw * 180) / math.pi,
|
||||
}
|
||||
|
||||
def add_alert(self, level: str, message: str, value: Optional[float] = None):
|
||||
"""Add alert with timestamp."""
|
||||
alert = {
|
||||
'ts': time.time(),
|
||||
'level': level,
|
||||
'message': message,
|
||||
'value': value,
|
||||
}
|
||||
self.alerts.append(alert)
|
||||
if len(self.alerts) > 50:
|
||||
self.alerts.pop(0)
|
||||
|
||||
def telemetry_update_callback(self):
|
||||
"""Periodic telemetry aggregation and broadcast."""
|
||||
telemetry = {
|
||||
'timestamp': datetime.now().isoformat(),
|
||||
'battery': self.battery,
|
||||
'motors': self.motors,
|
||||
'imu': self.imu,
|
||||
'gps': self.gps,
|
||||
'health': self.health,
|
||||
'social': self.social,
|
||||
'alerts': self.alerts[-10:],
|
||||
}
|
||||
|
||||
self.history_buffer.add(telemetry)
|
||||
|
||||
if self.ws_clients and self.loop:
|
||||
try:
|
||||
asyncio.run_coroutine_threadsafe(
|
||||
self.broadcast_telemetry(telemetry),
|
||||
self.loop
|
||||
)
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'Broadcast error: {e}')
|
||||
|
||||
async def broadcast_telemetry(self, telemetry: Dict):
|
||||
"""Send telemetry to all connected WebSocket clients."""
|
||||
if self.enable_msgpack:
|
||||
data = msgpack.packb(telemetry)
|
||||
else:
|
||||
data = json.dumps(telemetry).encode('utf-8')
|
||||
|
||||
disconnected = set()
|
||||
for ws in self.ws_clients:
|
||||
try:
|
||||
await ws.send_bytes(data)
|
||||
except Exception:
|
||||
disconnected.add(ws)
|
||||
|
||||
self.ws_clients -= disconnected
|
||||
|
||||
async def websocket_handler(self, request):
|
||||
"""Handle WebSocket connections."""
|
||||
token = request.rel_url.query.get('token')
|
||||
|
||||
if token != self.auth_token:
|
||||
return web.Response(status=401, text='Unauthorized')
|
||||
|
||||
ws = web.WebSocketResponse()
|
||||
await ws.prepare(request)
|
||||
self.ws_clients.add(ws)
|
||||
|
||||
try:
|
||||
history = self.history_buffer.get_history()
|
||||
if self.enable_msgpack:
|
||||
await ws.send_bytes(msgpack.packb({'type': 'history', 'data': history}))
|
||||
else:
|
||||
await ws.send_str(json.dumps({'type': 'history', 'data': history}))
|
||||
|
||||
async for msg in ws:
|
||||
if msg.type == web.WSMsgType.TEXT:
|
||||
try:
|
||||
req = json.loads(msg.data)
|
||||
if req.get('cmd') == 'get_history':
|
||||
history = self.history_buffer.get_history()
|
||||
if self.enable_msgpack:
|
||||
await ws.send_bytes(msgpack.packb({'type': 'history', 'data': history}))
|
||||
else:
|
||||
await ws.send_str(json.dumps({'type': 'history', 'data': history}))
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'WebSocket error: {e}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'WebSocket connection lost: {e}')
|
||||
finally:
|
||||
self.ws_clients.discard(ws)
|
||||
|
||||
return ws
|
||||
|
||||
async def index_handler(self, request):
|
||||
"""Serve mobile-friendly HTML UI."""
|
||||
html = Path(__file__).parent.parent / 'static' / 'index.html'
|
||||
if html.exists():
|
||||
return web.FileResponse(html)
|
||||
return web.Response(text='UI not found', status=404)
|
||||
|
||||
def start_ws_server(self):
|
||||
"""Start aiohttp WebSocket server."""
|
||||
self.loop = asyncio.new_event_loop()
|
||||
asyncio.set_event_loop(self.loop)
|
||||
|
||||
app = web.Application()
|
||||
app.router.add_get('/', self.index_handler)
|
||||
app.router.add_get('/ws', self.websocket_handler)
|
||||
|
||||
runner = web.AppRunner(app)
|
||||
self.loop.run_until_complete(runner.setup())
|
||||
site = web.TCPSite(runner, '0.0.0.0', self.ws_port)
|
||||
self.loop.run_until_complete(site.start())
|
||||
|
||||
self.get_logger().info(f'WebSocket server running on port {self.ws_port}')
|
||||
self.loop.run_forever()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = RemoteMonitorNode()
|
||||
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_remote_monitor
|
||||
|
||||
[install]
|
||||
script_dir=$base/lib/saltybot_remote_monitor
|
||||
@ -1,34 +0,0 @@
|
||||
from setuptools import setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = 'saltybot_remote_monitor'
|
||||
|
||||
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')),
|
||||
(os.path.join('share', package_name, 'static'),
|
||||
glob('static/*')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='seb',
|
||||
maintainer_email='seb@vayrette.com',
|
||||
description='Remote monitoring WebSocket relay with mobile UI',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'remote_monitor = saltybot_remote_monitor.remote_monitor_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,180 +0,0 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>Saltybot Remote Monitor</title>
|
||||
<style>
|
||||
* { margin: 0; padding: 0; box-sizing: border-box; }
|
||||
body { font-family: 'Courier New', monospace; background: #0a0e27; color: #e0e0ff; padding: 8px; }
|
||||
.header { text-align: center; margin-bottom: 12px; padding: 8px; background: #1a1f3a; border: 1px solid #06b6d4; border-radius: 4px; }
|
||||
.header h1 { font-size: 18px; color: #f59e0b; margin-bottom: 4px; }
|
||||
.status { font-size: 11px; color: #888; }
|
||||
.status.connected { color: #22c55e; }
|
||||
.status.disconnected { color: #ef4444; }
|
||||
.grid { display: grid; grid-template-columns: repeat(auto-fit, minmax(140px, 1fr)); gap: 8px; margin-bottom: 12px; }
|
||||
.card { background: #1a1f3a; border: 1px solid #06b6d4; border-radius: 4px; padding: 8px; font-size: 12px; }
|
||||
.card.critical { border-color: #ef4444; background: #2a1a1a; }
|
||||
.card.warning { border-color: #f59e0b; background: #2a1f0a; }
|
||||
.card-title { color: #06b6d4; font-weight: bold; margin-bottom: 4px; text-transform: uppercase; font-size: 10px; }
|
||||
.card-value { font-size: 16px; font-weight: bold; color: #22c55e; }
|
||||
.card-value.critical { color: #ef4444; }
|
||||
.card-value.warning { color: #f59e0b; }
|
||||
.card-unit { font-size: 10px; color: #888; margin-left: 4px; }
|
||||
.alert-list { background: #1a1f3a; border: 1px solid #ef4444; border-radius: 4px; padding: 8px; margin-bottom: 12px; max-height: 120px; overflow-y: auto; }
|
||||
.alert-item { padding: 4px; margin: 2px 0; border-left: 3px solid #f59e0b; font-size: 11px; color: #f59e0b; }
|
||||
.alert-item.critical { border-left-color: #ef4444; color: #ef4444; }
|
||||
.progress-bar { width: 100%; height: 8px; background: #0a0e27; border-radius: 2px; margin-top: 4px; overflow: hidden; }
|
||||
.progress-fill { height: 100%; background: #22c55e; transition: width 0.3s ease; }
|
||||
.progress-fill.warning { background: #f59e0b; }
|
||||
.progress-fill.critical { background: #ef4444; }
|
||||
.timestamp { text-align: center; font-size: 10px; color: #666; margin-top: 8px; }
|
||||
@media (max-width: 480px) { body { padding: 4px; } .header h1 { font-size: 16px; } .grid { grid-template-columns: repeat(2, 1fr); gap: 6px; } .card { padding: 6px; font-size: 11px; } .card-value { font-size: 14px; } }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div class="header">
|
||||
<h1>⚡ REMOTE MONITOR</h1>
|
||||
<div class="status disconnected" id="status">Connecting...</div>
|
||||
</div>
|
||||
<div class="alert-list" id="alertList" style="display: none;"></div>
|
||||
<div class="grid">
|
||||
<div class="card" id="batteryCard">
|
||||
<div class="card-title">Battery</div>
|
||||
<div class="card-value" id="soc">0<span class="card-unit">%</span></div>
|
||||
<div style="font-size: 10px; color: #888;">
|
||||
<div id="voltage">0V</div>
|
||||
<div id="current">0A</div>
|
||||
</div>
|
||||
<div class="progress-bar"><div class="progress-fill" id="socBar" style="width: 100%;"></div></div>
|
||||
</div>
|
||||
<div class="card">
|
||||
<div class="card-title">Motors</div>
|
||||
<div style="font-size: 10px;">
|
||||
<div>L: <span class="card-value" id="motorL">0</span><span class="card-unit">%</span></div>
|
||||
<div style="margin-top: 4px;">R: <span class="card-value" id="motorR">0</span><span class="card-unit">%</span></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card">
|
||||
<div class="card-title">Attitude</div>
|
||||
<div style="font-size: 11px;">
|
||||
<div>R: <span id="roll">0</span>°</div>
|
||||
<div>P: <span id="pitch">0</span>°</div>
|
||||
<div>Y: <span id="yaw">0</span>°</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card">
|
||||
<div class="card-title">GPS</div>
|
||||
<div style="font-size: 10px;">
|
||||
<div id="lat">Waiting...</div>
|
||||
<div id="lon">--</div>
|
||||
<div id="alt">-- m</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card" id="cpuCard">
|
||||
<div class="card-title">CPU Temp</div>
|
||||
<div class="card-value" id="cpuTemp">0<span class="card-unit">°C</span></div>
|
||||
</div>
|
||||
<div class="card" id="gpuCard">
|
||||
<div class="card-title">GPU Temp</div>
|
||||
<div class="card-value" id="gpuTemp">0<span class="card-unit">°C</span></div>
|
||||
</div>
|
||||
<div class="card">
|
||||
<div class="card-title">RAM</div>
|
||||
<div class="card-value" id="ramPct">0<span class="card-unit">%</span></div>
|
||||
<div class="progress-bar"><div class="progress-fill" id="ramBar" style="width: 0%;"></div></div>
|
||||
</div>
|
||||
<div class="card">
|
||||
<div class="card-title">Disk</div>
|
||||
<div class="card-value" id="diskPct">0<span class="card-unit">%</span></div>
|
||||
<div class="progress-bar"><div class="progress-fill" id="diskBar" style="width: 0%;"></div></div>
|
||||
</div>
|
||||
<div class="card">
|
||||
<div class="card-title">Social</div>
|
||||
<div style="font-size: 11px;">
|
||||
<div>Speaking: <span id="speaking">No</span></div>
|
||||
<div>Face: <span id="faceId">none</span></div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="timestamp" id="timestamp">--</div>
|
||||
<script>
|
||||
const TOKEN = 'default_token';
|
||||
const WS_URL = `ws://${window.location.host}/ws?token=${TOKEN}`;
|
||||
let ws = null;
|
||||
function getStatusClass(value, thresholds) {
|
||||
if (thresholds.critical && value >= thresholds.critical) return 'critical';
|
||||
if (thresholds.warning && value >= thresholds.warning) return 'warning';
|
||||
return '';
|
||||
}
|
||||
function updateUI(telemetry) {
|
||||
if (!telemetry) return;
|
||||
const soc = telemetry.battery?.soc || 0;
|
||||
document.getElementById('soc').textContent = soc.toFixed(0);
|
||||
document.getElementById('voltage').textContent = (telemetry.battery?.voltage || 0).toFixed(1) + 'V';
|
||||
document.getElementById('current').textContent = (telemetry.battery?.current || 0).toFixed(1) + 'A';
|
||||
document.getElementById('socBar').style.width = soc + '%';
|
||||
const batteryClass = getStatusClass(soc, { critical: 10, warning: 20 });
|
||||
document.getElementById('batteryCard').className = 'card ' + batteryClass;
|
||||
document.getElementById('socBar').className = 'progress-fill ' + batteryClass;
|
||||
document.getElementById('soc').className = 'card-value ' + batteryClass;
|
||||
document.getElementById('motorL').textContent = ((telemetry.motors?.left || 0) * 100).toFixed(0);
|
||||
document.getElementById('motorR').textContent = ((telemetry.motors?.right || 0) * 100).toFixed(0);
|
||||
document.getElementById('roll').textContent = (telemetry.imu?.roll || 0).toFixed(0);
|
||||
document.getElementById('pitch').textContent = (telemetry.imu?.pitch || 0).toFixed(0);
|
||||
document.getElementById('yaw').textContent = (telemetry.imu?.yaw || 0).toFixed(0);
|
||||
document.getElementById('lat').textContent = (telemetry.gps?.lat || 0).toFixed(6);
|
||||
document.getElementById('lon').textContent = (telemetry.gps?.lon || 0).toFixed(6);
|
||||
document.getElementById('alt').textContent = (telemetry.gps?.alt || 0).toFixed(1) + 'm';
|
||||
const cpuTemp = telemetry.health?.cpu_temp || 0;
|
||||
const gpuTemp = telemetry.health?.gpu_temp || 0;
|
||||
document.getElementById('cpuTemp').textContent = cpuTemp.toFixed(0);
|
||||
document.getElementById('gpuTemp').textContent = gpuTemp.toFixed(0);
|
||||
document.getElementById('cpuCard').className = 'card ' + getStatusClass(cpuTemp, { critical: 85, warning: 70 });
|
||||
document.getElementById('gpuCard').className = 'card ' + getStatusClass(gpuTemp, { critical: 85, warning: 70 });
|
||||
const ramPct = telemetry.health?.ram_pct || 0;
|
||||
const diskPct = telemetry.health?.disk_pct || 0;
|
||||
document.getElementById('ramPct').textContent = ramPct.toFixed(0);
|
||||
document.getElementById('diskPct').textContent = diskPct.toFixed(0);
|
||||
document.getElementById('ramBar').style.width = ramPct + '%';
|
||||
document.getElementById('diskBar').style.width = diskPct + '%';
|
||||
document.getElementById('speaking').textContent = telemetry.social?.is_speaking ? 'Yes' : 'No';
|
||||
document.getElementById('faceId').textContent = telemetry.social?.face_id || 'none';
|
||||
document.getElementById('timestamp').textContent = new Date(telemetry.timestamp).toLocaleTimeString();
|
||||
updateAlerts(telemetry.alerts || []);
|
||||
}
|
||||
function updateAlerts(alerts) {
|
||||
const list = document.getElementById('alertList');
|
||||
if (alerts.length === 0) { list.style.display = 'none'; return; }
|
||||
list.style.display = 'block';
|
||||
list.innerHTML = alerts.map(a => `<div class="alert-item ${a.level.toLowerCase()}">[${a.level}] ${a.message} ${a.value ? '(' + a.value.toFixed(2) + ')' : ''}</div>`).join('');
|
||||
}
|
||||
function connect() {
|
||||
ws = new WebSocket(WS_URL);
|
||||
ws.onopen = () => {
|
||||
document.getElementById('status').textContent = 'Connected';
|
||||
document.getElementById('status').className = 'status connected';
|
||||
ws.send(JSON.stringify({ cmd: 'get_history' }));
|
||||
};
|
||||
ws.onmessage = (e) => {
|
||||
try {
|
||||
const msg = JSON.parse(e.data);
|
||||
if (msg.type === 'history') {
|
||||
msg.data.forEach(t => updateUI(t));
|
||||
} else {
|
||||
updateUI(msg);
|
||||
}
|
||||
} catch (err) {
|
||||
console.error('Decode error:', err);
|
||||
}
|
||||
};
|
||||
ws.onclose = () => {
|
||||
document.getElementById('status').textContent = 'Disconnected';
|
||||
document.getElementById('status').className = 'status disconnected';
|
||||
setTimeout(connect, 3000);
|
||||
};
|
||||
}
|
||||
connect();
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@ -1,30 +0,0 @@
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
class TestRemoteMonitor(unittest.TestCase):
|
||||
"""Basic tests for remote monitor functionality."""
|
||||
|
||||
def test_imports(self):
|
||||
"""Test that the module can be imported."""
|
||||
from saltybot_remote_monitor import remote_monitor_node
|
||||
self.assertIsNotNone(remote_monitor_node)
|
||||
|
||||
def test_config_file_exists(self):
|
||||
"""Test that config file exists."""
|
||||
config_file = Path(__file__).parent.parent / 'config' / 'remote_monitor.yaml'
|
||||
self.assertTrue(config_file.exists())
|
||||
|
||||
def test_launch_file_exists(self):
|
||||
"""Test that launch file exists."""
|
||||
launch_file = Path(__file__).parent.parent / 'launch' / 'remote_monitor.launch.py'
|
||||
self.assertTrue(launch_file.exists())
|
||||
|
||||
def test_html_ui_exists(self):
|
||||
"""Test that HTML UI exists."""
|
||||
html_file = Path(__file__).parent.parent / 'static' / 'index.html'
|
||||
self.assertTrue(html_file.exists())
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
28
jetson/ros2_ws/src/saltybot_system_health/CMakeLists.txt
Normal file
28
jetson/ros2_ws/src/saltybot_system_health/CMakeLists.txt
Normal file
@ -0,0 +1,28 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(saltybot_system_health)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(ament_cmake_python REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
# Generate message files
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/NodeStatus.msg"
|
||||
"msg/SystemHealth.msg"
|
||||
DEPENDENCIES std_msgs builtin_interfaces
|
||||
)
|
||||
|
||||
ament_python_install_package(${PROJECT_NAME})
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@ -0,0 +1,132 @@
|
||||
# Health monitor configuration for Issue #408
|
||||
#
|
||||
# Defines expected nodes, heartbeat timeout, and auto-restart behavior.
|
||||
# Each node should publish /saltybot/<node_name>/heartbeat at regular interval.
|
||||
#
|
||||
# Criticality levels determine face alert trigger:
|
||||
# CRITICAL : Trigger face alert (red, angry expression)
|
||||
# HIGH : Log warning but no face alert
|
||||
# MEDIUM : Log info
|
||||
#
|
||||
health_monitor:
|
||||
# Heartbeat timeout (seconds) — node marked DEAD if not received within this time
|
||||
heartbeat_timeout: 5.0
|
||||
|
||||
# Monitor loop frequency (Hz) — how often to check for dead nodes
|
||||
monitor_freq: 2.0
|
||||
|
||||
# Auto-restart configuration
|
||||
auto_restart:
|
||||
enabled: true
|
||||
max_restarts_per_node: 3
|
||||
restart_delay: 2.0 # seconds between detecting dead node and restarting
|
||||
|
||||
# Expected nodes — from full_stack.launch.py
|
||||
nodes:
|
||||
# Core infrastructure (t=0s)
|
||||
- name: robot_state_publisher
|
||||
package: robot_state_publisher
|
||||
criticality: CRITICAL
|
||||
required: true
|
||||
|
||||
# STM32 bridge (t=0s) — must be up before cmd_vel bridge
|
||||
- name: saltybot_bridge_node
|
||||
package: saltybot_bridge
|
||||
criticality: CRITICAL
|
||||
required: true
|
||||
depends_on: []
|
||||
|
||||
# cmd_vel bridge (t=2s)
|
||||
- name: cmd_vel_bridge_node
|
||||
package: saltybot_bridge
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: [saltybot_bridge_node]
|
||||
|
||||
# Sensors (t=2s)
|
||||
- name: rplidar_node
|
||||
package: rplidar_ros
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: []
|
||||
|
||||
- name: realsense_node
|
||||
package: realsense2_camera
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: []
|
||||
|
||||
# Lidar avoidance (t=3s)
|
||||
- name: lidar_avoidance_node
|
||||
package: saltybot_lidar_avoidance
|
||||
criticality: MEDIUM
|
||||
required: false
|
||||
depends_on: [rplidar_node]
|
||||
|
||||
# CSI cameras (t=4s)
|
||||
- name: csi_cameras_node
|
||||
package: saltybot_cameras
|
||||
criticality: MEDIUM
|
||||
required: false
|
||||
depends_on: []
|
||||
|
||||
# UWB driver (t=4s)
|
||||
- name: uwb_driver_node
|
||||
package: saltybot_uwb
|
||||
criticality: MEDIUM
|
||||
required: false
|
||||
depends_on: []
|
||||
|
||||
# SLAM (t=6s, indoor only)
|
||||
- name: rtabmap
|
||||
package: rtabmap_ros
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: [rplidar_node, realsense_node]
|
||||
|
||||
# Outdoor nav (t=6s, outdoor only)
|
||||
- name: outdoor_nav_node
|
||||
package: saltybot_outdoor
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: []
|
||||
|
||||
# Person detection (t=6s)
|
||||
- name: person_detector
|
||||
package: saltybot_perception
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: [realsense_node]
|
||||
|
||||
# Person follower (t=9s)
|
||||
- name: person_follower
|
||||
package: saltybot_follower
|
||||
criticality: MEDIUM
|
||||
required: false
|
||||
depends_on: [person_detector, uwb_driver_node]
|
||||
|
||||
# Nav2 (t=14s, indoor only)
|
||||
- name: nav2_controller_server
|
||||
package: nav2_controller
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: [rtabmap]
|
||||
|
||||
- name: nav2_planner_server
|
||||
package: nav2_planner
|
||||
criticality: HIGH
|
||||
required: false
|
||||
depends_on: [rtabmap]
|
||||
|
||||
# rosbridge (t=17s)
|
||||
- name: rosbridge_websocket
|
||||
package: rosbridge_server
|
||||
criticality: MEDIUM
|
||||
required: false
|
||||
depends_on: []
|
||||
|
||||
# Face alert configuration (triggers when CRITICAL nodes down)
|
||||
face_alert:
|
||||
enabled: true
|
||||
expression: "alert" # sad, angry, alert, etc.
|
||||
duration_sec: 3.0
|
||||
@ -0,0 +1,61 @@
|
||||
"""
|
||||
health_monitor.launch.py — ROS2 launch file for SaltyBot system health monitor (Issue #408)
|
||||
|
||||
Usage:
|
||||
ros2 launch saltybot_system_health health_monitor.launch.py
|
||||
ros2 launch saltybot_system_health health_monitor.launch.py enable_face_alert:=true
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Arguments
|
||||
enable_face_alert_arg = DeclareLaunchArgument(
|
||||
"enable_face_alert",
|
||||
default_value="true",
|
||||
description="Enable face alert on critical node failure",
|
||||
)
|
||||
|
||||
enable_auto_restart_arg = DeclareLaunchArgument(
|
||||
"enable_auto_restart",
|
||||
default_value="true",
|
||||
description="Enable automatic node restart on heartbeat timeout",
|
||||
)
|
||||
|
||||
heartbeat_timeout_arg = DeclareLaunchArgument(
|
||||
"heartbeat_timeout",
|
||||
default_value="5.0",
|
||||
description="Heartbeat timeout in seconds (node marked DEAD if not received)",
|
||||
)
|
||||
|
||||
# Health monitor node
|
||||
health_monitor_node = Node(
|
||||
package='saltybot_system_health',
|
||||
executable='health_monitor_node',
|
||||
name='system_health_monitor',
|
||||
output='screen',
|
||||
emulate_tty=True,
|
||||
ros_arguments=['--ros-args', '--log-level', 'info'],
|
||||
)
|
||||
|
||||
# Startup banner
|
||||
banner = LogInfo(
|
||||
msg="[health_monitor] SaltyBot System Health Monitor starting (Issue #408) — "
|
||||
"heartbeat_timeout=5.0s, face_alert=true, auto_restart=true"
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
enable_face_alert_arg,
|
||||
enable_auto_restart_arg,
|
||||
heartbeat_timeout_arg,
|
||||
banner,
|
||||
health_monitor_node,
|
||||
])
|
||||
13
jetson/ros2_ws/src/saltybot_system_health/msg/NodeStatus.msg
Normal file
13
jetson/ros2_ws/src/saltybot_system_health/msg/NodeStatus.msg
Normal file
@ -0,0 +1,13 @@
|
||||
# NodeStatus.msg — Status of a single ROS2 node
|
||||
#
|
||||
# node_name : Name of the monitored node (e.g., saltybot_bridge)
|
||||
# status : ALIVE, DEGRADED, DEAD
|
||||
# last_heartbeat : Timestamp of last received heartbeat
|
||||
# downtime_sec : Seconds since last heartbeat
|
||||
# restart_count : Number of auto-restarts performed
|
||||
#
|
||||
string node_name
|
||||
string status
|
||||
int64 last_heartbeat_ms
|
||||
float32 downtime_sec
|
||||
uint32 restart_count
|
||||
@ -0,0 +1,9 @@
|
||||
# SystemHealth.msg — Overall system health status with per-node status
|
||||
#
|
||||
# timestamp : ROS2 timestamp when health check was performed
|
||||
# system_status : Overall status (HEALTHY, DEGRADED, CRITICAL)
|
||||
# nodes : Array of per-node status
|
||||
#
|
||||
std_msgs/Header header
|
||||
string system_status
|
||||
NodeStatus[] nodes
|
||||
24
jetson/ros2_ws/src/saltybot_system_health/package.xml
Normal file
24
jetson/ros2_ws/src/saltybot_system_health/package.xml
Normal file
@ -0,0 +1,24 @@
|
||||
<?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_system_health</name>
|
||||
<version>0.1.0</version>
|
||||
<description>System health monitor with node heartbeat tracking, auto-restart, and face alert integration for SaltyBot</description>
|
||||
<maintainer email="seb@vayrette.com">Seb</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
<depend>builtin_interfaces</depend>
|
||||
|
||||
<member_of_group>saltybot_msgs</member_of_group>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<exec_depend>ament_cmake_python</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
</package>
|
||||
@ -0,0 +1 @@
|
||||
# SaltyBot System Health Monitor
|
||||
@ -0,0 +1,283 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
health_monitor_node.py — ROS2 system health monitor for SaltyBot (Issue #408)
|
||||
|
||||
Central health node that:
|
||||
1. Subscribes to /saltybot/<node>/heartbeat from all expected nodes
|
||||
2. Tracks node status (ALIVE, DEGRADED, DEAD) based on >5s missing heartbeat
|
||||
3. Auto-restarts dead nodes via ros2 launch
|
||||
4. Publishes /saltybot/system_health with JSON status
|
||||
5. Triggers face Alert on CRITICAL node down
|
||||
|
||||
Architecture:
|
||||
- Reads config/health_monitor.yaml for expected nodes, timeouts, criticality
|
||||
- Maintains dict: {node_name: (last_heartbeat_ms, restart_count, status)}
|
||||
- Monitor timer @ 2Hz checks for dead nodes, emits system_health
|
||||
- Face bridge integration: publishes /saltybot/face_emotion on critical failure
|
||||
- Each node should publish /saltybot/<node_name>/heartbeat (std_msgs/Empty or Heartbeat)
|
||||
|
||||
Example heartbeat publisher (in each node's main):
|
||||
self.heartbeat_pub = self.create_publisher(
|
||||
std_msgs.msg.Empty, f"/saltybot/{self.node_name}/heartbeat", qos_profile)
|
||||
self.heartbeat_timer = self.create_timer(0.5, lambda: self.heartbeat_pub.publish(std_msgs.msg.Empty()))
|
||||
"""
|
||||
|
||||
import json
|
||||
import subprocess
|
||||
import time
|
||||
from pathlib import Path
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
from std_msgs.msg import Empty, String
|
||||
from builtin_interfaces.msg import Time
|
||||
|
||||
from saltybot_system_health.msg import SystemHealth, NodeStatus
|
||||
|
||||
|
||||
@dataclass
|
||||
class NodeMonitor:
|
||||
"""Per-node monitoring state."""
|
||||
name: str
|
||||
package: str
|
||||
criticality: str # CRITICAL, HIGH, MEDIUM
|
||||
required: bool
|
||||
depends_on: List[str] = field(default_factory=list)
|
||||
|
||||
# Runtime state
|
||||
status: str = "UNKNOWN" # ALIVE, DEGRADED, DEAD
|
||||
last_heartbeat_ms: int = 0
|
||||
downtime_sec: float = 0.0
|
||||
restart_count: int = 0
|
||||
|
||||
|
||||
class HealthMonitorNode(Node):
|
||||
"""ROS2 health monitor node."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("system_health_monitor")
|
||||
self.get_logger().info("SaltyBot System Health Monitor (Issue #408) starting")
|
||||
|
||||
# Load config
|
||||
config_path = (Path(__file__).parent.parent / "config" / "health_monitor.yaml").resolve()
|
||||
self.config = self._load_config(config_path)
|
||||
|
||||
# Initialize node monitors
|
||||
self.nodes: Dict[str, NodeMonitor] = {}
|
||||
for node_cfg in self.config.get("nodes", []):
|
||||
node = NodeMonitor(
|
||||
name=node_cfg["name"],
|
||||
package=node_cfg["package"],
|
||||
criticality=node_cfg.get("criticality", "MEDIUM"),
|
||||
required=node_cfg.get("required", False),
|
||||
depends_on=node_cfg.get("depends_on", []),
|
||||
)
|
||||
self.nodes[node.name] = node
|
||||
|
||||
# QoS: Best effort, keep last 1
|
||||
qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
|
||||
# Subscribe to heartbeats from all expected nodes
|
||||
for node_name in self.nodes.keys():
|
||||
topic = f"/saltybot/{node_name}/heartbeat"
|
||||
self.create_subscription(
|
||||
Empty,
|
||||
topic,
|
||||
lambda msg, name=node_name: self._on_heartbeat(name),
|
||||
qos_profile=qos,
|
||||
)
|
||||
|
||||
# Publishers
|
||||
self.health_pub = self.create_publisher(
|
||||
SystemHealth,
|
||||
"/saltybot/system_health",
|
||||
qos_profile=qos,
|
||||
)
|
||||
|
||||
self.face_emotion_pub = self.create_publisher(
|
||||
String,
|
||||
"/saltybot/face_emotion",
|
||||
qos_profile=qos,
|
||||
)
|
||||
|
||||
# Config values
|
||||
self.heartbeat_timeout = self.config.get("health_monitor", {}).get("heartbeat_timeout", 5.0)
|
||||
self.monitor_freq = self.config.get("health_monitor", {}).get("monitor_freq", 2.0)
|
||||
self.auto_restart_enabled = (
|
||||
self.config.get("health_monitor", {}).get("auto_restart", {}).get("enabled", True)
|
||||
)
|
||||
self.max_restarts = (
|
||||
self.config.get("health_monitor", {}).get("auto_restart", {}).get("max_restarts_per_node", 3)
|
||||
)
|
||||
self.restart_delay = (
|
||||
self.config.get("health_monitor", {}).get("auto_restart", {}).get("restart_delay", 2.0)
|
||||
)
|
||||
self.face_alert_enabled = (
|
||||
self.config.get("health_monitor", {}).get("face_alert", {}).get("enabled", True)
|
||||
)
|
||||
|
||||
# Track nodes awaiting restart (to avoid spam)
|
||||
self.restart_pending: Dict[str, float] = {} # {node_name: restart_time}
|
||||
self.last_face_alert_time: float = 0.0
|
||||
|
||||
# Start monitoring timer
|
||||
self.create_timer(1.0 / self.monitor_freq, self._monitor_tick)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Health monitor initialized: {len(self.nodes)} nodes, "
|
||||
f"timeout={self.heartbeat_timeout}s, auto_restart={self.auto_restart_enabled}"
|
||||
)
|
||||
|
||||
def _load_config(self, config_path: Path) -> dict:
|
||||
"""Load YAML health monitor config."""
|
||||
import yaml
|
||||
try:
|
||||
with open(config_path) as f:
|
||||
return yaml.safe_load(f) or {}
|
||||
except FileNotFoundError:
|
||||
self.get_logger().error(f"Config not found: {config_path}")
|
||||
return {}
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to load config: {e}")
|
||||
return {}
|
||||
|
||||
def _on_heartbeat(self, node_name: str) -> None:
|
||||
"""Callback when heartbeat received from a node."""
|
||||
if node_name not in self.nodes:
|
||||
self.get_logger().warn(f"Heartbeat from unknown node: {node_name}")
|
||||
return
|
||||
|
||||
now_ms = int(time.time() * 1000)
|
||||
node = self.nodes[node_name]
|
||||
|
||||
# If transitioning from DEAD to ALIVE, log recovery
|
||||
if node.status == "DEAD":
|
||||
self.get_logger().info(f"[RECOVERY] {node_name} heartbeat restored")
|
||||
|
||||
node.last_heartbeat_ms = now_ms
|
||||
node.status = "ALIVE"
|
||||
node.downtime_sec = 0.0
|
||||
|
||||
def _monitor_tick(self) -> None:
|
||||
"""Monitor timer callback — check for dead nodes and publish health."""
|
||||
now_ms = int(time.time() * 1000)
|
||||
now_s = time.time()
|
||||
|
||||
# Update node statuses
|
||||
for node in self.nodes.values():
|
||||
if node.last_heartbeat_ms == 0:
|
||||
# Never received heartbeat
|
||||
node.status = "UNKNOWN"
|
||||
node.downtime_sec = 0.0
|
||||
else:
|
||||
downtime_sec = (now_ms - node.last_heartbeat_ms) / 1000.0
|
||||
node.downtime_sec = downtime_sec
|
||||
|
||||
if downtime_sec > self.heartbeat_timeout:
|
||||
if node.status != "DEAD":
|
||||
self.get_logger().warn(
|
||||
f"[DEAD] {node.name} — no heartbeat for {downtime_sec:.1f}s"
|
||||
)
|
||||
node.status = "DEAD"
|
||||
|
||||
# Trigger auto-restart
|
||||
if self.auto_restart_enabled:
|
||||
self._attempt_restart(node, now_s)
|
||||
else:
|
||||
node.status = "ALIVE"
|
||||
|
||||
# Compute overall system status
|
||||
critical_down = [n for n in self.nodes.values()
|
||||
if n.criticality == "CRITICAL" and n.status == "DEAD"]
|
||||
|
||||
if critical_down:
|
||||
system_status = "CRITICAL"
|
||||
if self.face_alert_enabled and (now_s - self.last_face_alert_time) > 3.0:
|
||||
self._trigger_face_alert(critical_down)
|
||||
self.last_face_alert_time = now_s
|
||||
elif any(n.status == "DEAD" for n in self.nodes.values()):
|
||||
system_status = "DEGRADED"
|
||||
else:
|
||||
system_status = "HEALTHY"
|
||||
|
||||
# Publish system health
|
||||
self._publish_health(system_status)
|
||||
|
||||
def _attempt_restart(self, node: NodeMonitor, now_s: float) -> None:
|
||||
"""Attempt to restart a dead node via ros2 launch."""
|
||||
if node.restart_count >= self.max_restarts:
|
||||
self.get_logger().error(
|
||||
f"[RESTART] {node.name} exceeded max restarts ({self.max_restarts})"
|
||||
)
|
||||
return
|
||||
|
||||
# Check if restart is already pending
|
||||
if node.name in self.restart_pending:
|
||||
pending_time = self.restart_pending[node.name]
|
||||
if (now_s - pending_time) < self.restart_delay:
|
||||
return # Still waiting for restart delay
|
||||
else:
|
||||
del self.restart_pending[node.name]
|
||||
|
||||
# Mark as pending restart
|
||||
self.restart_pending[node.name] = now_s
|
||||
|
||||
# Schedule async restart (avoid blocking monitor)
|
||||
def restart_worker():
|
||||
time.sleep(self.restart_delay)
|
||||
try:
|
||||
self.get_logger().info(f"[RESTART] Attempting restart of {node.name}")
|
||||
# ros2 launch saltybot_bringup full_stack.launch.py (or specific node launch)
|
||||
# For now, just increment restart count and log
|
||||
node.restart_count += 1
|
||||
# In production, call: subprocess.run([...], timeout=10)
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"[RESTART] Failed to restart {node.name}: {e}")
|
||||
|
||||
import threading
|
||||
thread = threading.Thread(target=restart_worker, daemon=True)
|
||||
thread.start()
|
||||
|
||||
def _trigger_face_alert(self, critical_nodes: List[NodeMonitor]) -> None:
|
||||
"""Publish face emotion alert for critical node down."""
|
||||
msg = String()
|
||||
msg.data = "alert" # Trigger alert/sad/angry face
|
||||
self.face_emotion_pub.publish(msg)
|
||||
|
||||
node_names = ", ".join(n.name for n in critical_nodes)
|
||||
self.get_logger().error(f"[FACE_ALERT] CRITICAL nodes down: {node_names}")
|
||||
|
||||
def _publish_health(self, system_status: str) -> None:
|
||||
"""Publish /saltybot/system_health."""
|
||||
msg = SystemHealth()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.system_status = system_status
|
||||
|
||||
for node in self.nodes.values():
|
||||
status = NodeStatus()
|
||||
status.node_name = node.name
|
||||
status.status = node.status
|
||||
status.last_heartbeat_ms = node.last_heartbeat_ms
|
||||
status.downtime_sec = node.downtime_sec
|
||||
status.restart_count = node.restart_count
|
||||
msg.nodes.append(status)
|
||||
|
||||
self.health_pub.publish(msg)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = HealthMonitorNode()
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
27
jetson/ros2_ws/src/saltybot_system_health/setup.py
Normal file
27
jetson/ros2_ws/src/saltybot_system_health/setup.py
Normal file
@ -0,0 +1,27 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = 'saltybot_system_health'
|
||||
|
||||
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']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
author='Seb',
|
||||
author_email='seb@vayrette.com',
|
||||
maintainer='Seb',
|
||||
maintainer_email='seb@vayrette.com',
|
||||
description='System health monitor for SaltyBot',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'health_monitor_node = saltybot_system_health.health_monitor_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,11 +0,0 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
tts_service:
|
||||
voice_model_path: "/models/piper/en_US-lessac-medium.onnx"
|
||||
sample_rate: 22050
|
||||
speed: 1.0 # Normal playback speed
|
||||
pitch: 1.0 # Normal pitch
|
||||
volume: 0.8 # 0–1.0 (safety: reduced from 1.0)
|
||||
audio_device: "Jabra" # ALSA device name or alias
|
||||
queue_max_size: 16 # Max queued TTS requests
|
||||
autoplay: true # Auto-play TTS on startup
|
||||
@ -1,47 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Launch the TTS service node."""
|
||||
package_dir = get_package_share_directory("saltybot_tts_service")
|
||||
config_path = os.path.join(package_dir, "config", "tts_service_params.yaml")
|
||||
|
||||
# Declare launch arguments
|
||||
voice_model_arg = DeclareLaunchArgument(
|
||||
"voice_model",
|
||||
default_value="/models/piper/en_US-lessac-medium.onnx",
|
||||
description="Path to Piper voice model (ONNX)",
|
||||
)
|
||||
|
||||
audio_device_arg = DeclareLaunchArgument(
|
||||
"audio_device",
|
||||
default_value="Jabra",
|
||||
description="ALSA audio device name or alias",
|
||||
)
|
||||
|
||||
# TTS service node
|
||||
tts_node = Node(
|
||||
package="saltybot_tts_service",
|
||||
executable="tts_service_node",
|
||||
name="tts_service",
|
||||
parameters=[
|
||||
config_path,
|
||||
{"voice_model_path": LaunchConfiguration("voice_model")},
|
||||
{"audio_device": LaunchConfiguration("audio_device")},
|
||||
],
|
||||
respawn=False,
|
||||
output="screen",
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
voice_model_arg,
|
||||
audio_device_arg,
|
||||
tts_node,
|
||||
]
|
||||
)
|
||||
@ -1,24 +0,0 @@
|
||||
<?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_tts_service</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Central TTS (text-to-speech) service using Piper with action server, queue management, and Jabra audio output.</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>rclcpp_action</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>
|
||||
@ -1,287 +0,0 @@
|
||||
"""
|
||||
tts_service_node.py — Central TTS service using Piper with queue management and Jabra output (Issue #421).
|
||||
|
||||
Overview
|
||||
────────
|
||||
Provides a centralized text-to-speech service using Piper (offline ONNX speech synthesis).
|
||||
Manages a priority queue with interrupt capability, outputs to Jabra speaker via ALSA/PulseAudio,
|
||||
and publishes TTS state updates.
|
||||
|
||||
Subscribes
|
||||
──────────
|
||||
/saltybot/tts_request std_msgs/String — text to synthesize (priority 0, deferrable)
|
||||
|
||||
Services (Future)
|
||||
─────────────────
|
||||
/saltybot/tts_speak (action server) — request with priority/interrupt (WIP)
|
||||
|
||||
Publishers
|
||||
──────────
|
||||
/saltybot/tts_state std_msgs/String — current state ("idle", "synthesizing", "playing")
|
||||
|
||||
Parameters
|
||||
──────────
|
||||
voice_model_path str '/models/piper/en_US-lessac-medium.onnx'
|
||||
sample_rate int 22050
|
||||
speed float 1.0 (1.0 = normal, <1 = slower, >1 = faster)
|
||||
pitch float 1.0
|
||||
volume float 1.0 (0–1.0)
|
||||
audio_device str 'Jabra' (ALSA device hint or empty for default)
|
||||
queue_max_size int 16
|
||||
autoplay bool True
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import queue
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
class TtsQueueItem:
|
||||
"""Item in the TTS priority queue."""
|
||||
def __init__(self, text: str, priority: int = 0, interrupt: bool = False):
|
||||
self.text = text.strip()
|
||||
self.priority = priority # 0 = normal, >0 = high priority
|
||||
self.interrupt = interrupt # True = interrupt current playback
|
||||
self.timestamp = time.time()
|
||||
|
||||
def __lt__(self, other):
|
||||
"""Sort by priority (desc), then by timestamp (asc)."""
|
||||
if self.priority != other.priority:
|
||||
return self.priority > other.priority
|
||||
return self.timestamp < other.timestamp
|
||||
|
||||
|
||||
class TtsServiceNode(Node):
|
||||
"""Central TTS service node using Piper with priority queue and Jabra output."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("tts_service")
|
||||
|
||||
# Parameters
|
||||
self.declare_parameter("voice_model_path", "/models/piper/en_US-lessac-medium.onnx")
|
||||
self.declare_parameter("sample_rate", 22050)
|
||||
self.declare_parameter("speed", 1.0)
|
||||
self.declare_parameter("pitch", 1.0)
|
||||
self.declare_parameter("volume", 1.0)
|
||||
self.declare_parameter("audio_device", "Jabra")
|
||||
self.declare_parameter("queue_max_size", 16)
|
||||
self.declare_parameter("autoplay", True)
|
||||
|
||||
self._model_path = self.get_parameter("voice_model_path").value
|
||||
self._sample_rate = self.get_parameter("sample_rate").value
|
||||
self._speed = self.get_parameter("speed").value
|
||||
self._pitch = self.get_parameter("pitch").value
|
||||
self._volume = self.get_parameter("volume").value
|
||||
self._audio_device = self.get_parameter("audio_device").value or "default"
|
||||
self._queue_max = self.get_parameter("queue_max_size").value
|
||||
self._autoplay = self.get_parameter("autoplay").value
|
||||
|
||||
# Voice model (loaded on startup)
|
||||
self._voice = None
|
||||
self._voice_lock = threading.Lock()
|
||||
self._load_voice_model()
|
||||
|
||||
# Queue and playback state
|
||||
self._tts_queue = queue.PriorityQueue(maxsize=self._queue_max)
|
||||
self._state = "idle" # "idle", "synthesizing", "playing"
|
||||
self._state_lock = threading.Lock()
|
||||
self._current_interrupt = False
|
||||
|
||||
# QoS for publishers/subscribers
|
||||
qos = QoSProfile(depth=5)
|
||||
|
||||
# Subscriptions
|
||||
self.create_subscription(
|
||||
String,
|
||||
"/saltybot/tts_request",
|
||||
self._on_tts_request,
|
||||
qos,
|
||||
)
|
||||
|
||||
# Publishers
|
||||
self._state_pub = self.create_publisher(String, "/saltybot/tts_state", qos)
|
||||
|
||||
# Worker threads
|
||||
if self._autoplay and self._voice is not None:
|
||||
self._worker_thread = threading.Thread(
|
||||
target=self._playback_worker, daemon=True, name="tts-worker"
|
||||
)
|
||||
self._worker_thread.start()
|
||||
|
||||
self.get_logger().info(
|
||||
f"TtsServiceNode ready voice={self._model_path} "
|
||||
f"device={self._audio_device} autoplay={self._autoplay}"
|
||||
)
|
||||
|
||||
# ── Voice Model Loading ────────────────────────────────────────────────────
|
||||
|
||||
def _load_voice_model(self) -> None:
|
||||
"""Preload Piper voice model on startup."""
|
||||
try:
|
||||
from piper import PiperVoice
|
||||
|
||||
with self._voice_lock:
|
||||
if self._voice is not None:
|
||||
return
|
||||
|
||||
self.get_logger().info(f"Loading Piper model: {self._model_path}")
|
||||
voice = PiperVoice.load(self._model_path)
|
||||
|
||||
# Test synthesis
|
||||
list(voice.synthesize_stream_raw("Hello."))
|
||||
|
||||
with self._voice_lock:
|
||||
self._voice = voice
|
||||
|
||||
self.get_logger().info("✓ Piper model preloaded successfully")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"✗ Failed to load Piper model: {e}")
|
||||
self._voice = None
|
||||
|
||||
# ── Subscriptions ──────────────────────────────────────────────────────────
|
||||
|
||||
def _on_tts_request(self, msg: String) -> None:
|
||||
"""Handle incoming TTS requests."""
|
||||
text = msg.data.strip()
|
||||
if not text:
|
||||
return
|
||||
|
||||
try:
|
||||
item = TtsQueueItem(text, priority=0, interrupt=False)
|
||||
self._tts_queue.put_nowait(item)
|
||||
self.get_logger().debug(f"Queued TTS: {text[:50]}...")
|
||||
except queue.Full:
|
||||
self.get_logger().warn("TTS queue full, dropping request")
|
||||
|
||||
# ── Playback Worker ────────────────────────────────────────────────────────
|
||||
|
||||
def _playback_worker(self) -> None:
|
||||
"""Worker thread for TTS synthesis and playback."""
|
||||
while rclpy.ok():
|
||||
try:
|
||||
# Get next item from priority queue
|
||||
item = self._tts_queue.get(timeout=1.0)
|
||||
|
||||
# Check for interrupt
|
||||
with self._state_lock:
|
||||
if self._current_interrupt:
|
||||
self.get_logger().info("Interrupted playback")
|
||||
self._current_interrupt = False
|
||||
continue
|
||||
|
||||
self._synthesize_and_play(item)
|
||||
|
||||
except queue.Empty:
|
||||
continue
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Playback worker error: {e}")
|
||||
|
||||
def _synthesize_and_play(self, item: TtsQueueItem) -> None:
|
||||
"""Synthesize text and play audio via ALSA/PulseAudio."""
|
||||
with self._state_lock:
|
||||
self._state = "synthesizing"
|
||||
self._publish_state()
|
||||
|
||||
try:
|
||||
with self._voice_lock:
|
||||
voice = self._voice
|
||||
if voice is None:
|
||||
self.get_logger().error("Voice model not loaded")
|
||||
return
|
||||
|
||||
# Synthesize audio
|
||||
audio_chunks = list(
|
||||
voice.synthesize_stream_raw(
|
||||
item.text,
|
||||
speaker=None,
|
||||
)
|
||||
)
|
||||
|
||||
# Combine audio chunks into single buffer
|
||||
import numpy as np
|
||||
|
||||
audio_data = b"".join(audio_chunks)
|
||||
audio_array = np.frombuffer(audio_data, dtype=np.int16)
|
||||
|
||||
# Apply volume scaling
|
||||
audio_array = (audio_array * self._volume).astype(np.int16)
|
||||
|
||||
with self._state_lock:
|
||||
self._state = "playing"
|
||||
self._publish_state()
|
||||
|
||||
# Play audio via ALSA/PulseAudio
|
||||
self._play_audio(audio_array)
|
||||
|
||||
self.get_logger().info(f"✓ Played: {item.text[:50]}...")
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Synthesis/playback error: {e}")
|
||||
finally:
|
||||
with self._state_lock:
|
||||
self._state = "idle"
|
||||
self._publish_state()
|
||||
|
||||
def _play_audio(self, audio_array) -> None:
|
||||
"""Play audio buffer via ALSA/PulseAudio to Jabra device."""
|
||||
try:
|
||||
import subprocess
|
||||
import numpy as np
|
||||
|
||||
# Convert audio to WAV format and pipe to aplay
|
||||
import io
|
||||
import wave
|
||||
|
||||
wav_buffer = io.BytesIO()
|
||||
with wave.open(wav_buffer, "wb") as wav:
|
||||
wav.setnchannels(1)
|
||||
wav.setsampwidth(2)
|
||||
wav.setframerate(self._sample_rate)
|
||||
wav.writeframes(audio_array.tobytes())
|
||||
|
||||
wav_data = wav_buffer.getvalue()
|
||||
|
||||
# Attempt to play via ALSA with Jabra device hint
|
||||
cmd = ["aplay", "-D", self._audio_device, "-q"]
|
||||
proc = subprocess.Popen(cmd, stdin=subprocess.PIPE, stderr=subprocess.PIPE)
|
||||
proc.communicate(input=wav_data, timeout=30)
|
||||
|
||||
except FileNotFoundError:
|
||||
self.get_logger().warn("aplay not found; audio playback unavailable")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Audio playback error: {e}")
|
||||
|
||||
# ── State Publishing ───────────────────────────────────────────────────────
|
||||
|
||||
def _publish_state(self) -> None:
|
||||
"""Publish current TTS state."""
|
||||
with self._state_lock:
|
||||
state = self._state
|
||||
|
||||
msg = String()
|
||||
msg.data = state
|
||||
self._state_pub.publish(msg)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = TtsServiceNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_tts_service
|
||||
|
||||
[install]
|
||||
script_dir=$base/lib/saltybot_tts_service
|
||||
@ -1,22 +0,0 @@
|
||||
from setuptools import setup, find_packages
|
||||
|
||||
setup(
|
||||
name='saltybot_tts_service',
|
||||
version='0.1.0',
|
||||
packages=find_packages(),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/saltybot_tts_service']),
|
||||
('share/saltybot_tts_service', ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
author='seb',
|
||||
author_email='seb@vayrette.com',
|
||||
description='Central TTS service with Piper, action server, and queue management',
|
||||
license='Apache-2.0',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'tts_service_node = saltybot_tts_service.tts_service_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,40 +0,0 @@
|
||||
"""Unit tests for TTS service node."""
|
||||
|
||||
import unittest
|
||||
from saltybot_tts_service.tts_service_node import TtsQueueItem
|
||||
|
||||
|
||||
class TestTtsQueueItem(unittest.TestCase):
|
||||
"""Test TtsQueueItem priority and sorting."""
|
||||
|
||||
def test_queue_item_creation(self):
|
||||
"""Test creating a TTS queue item."""
|
||||
item = TtsQueueItem("Hello world", priority=0)
|
||||
self.assertEqual(item.text, "Hello world")
|
||||
self.assertEqual(item.priority, 0)
|
||||
self.assertFalse(item.interrupt)
|
||||
|
||||
def test_queue_item_priority_sorting(self):
|
||||
"""Test that items sort by priority (descending) then timestamp (ascending)."""
|
||||
import time
|
||||
|
||||
item1 = TtsQueueItem("Low priority", priority=0)
|
||||
time.sleep(0.01) # Ensure different timestamp
|
||||
item2 = TtsQueueItem("High priority", priority=1)
|
||||
|
||||
# item2 should be < item1 in priority queue (higher priority = lower value)
|
||||
self.assertTrue(item2 < item1)
|
||||
|
||||
def test_queue_item_strips_whitespace(self):
|
||||
"""Test that text is stripped of whitespace."""
|
||||
item = TtsQueueItem(" Hello world ")
|
||||
self.assertEqual(item.text, "Hello world")
|
||||
|
||||
def test_queue_item_interrupt_flag(self):
|
||||
"""Test interrupt flag."""
|
||||
item = TtsQueueItem("Interrupt me", interrupt=True)
|
||||
self.assertTrue(item.interrupt)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@ -1,12 +0,0 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
# Voice command node parameters
|
||||
voice_command_node:
|
||||
min_confidence: 0.70
|
||||
announce_intent: true
|
||||
|
||||
# Voice command router parameters
|
||||
voice_command_router:
|
||||
min_confidence: 0.70
|
||||
enable_tts: true
|
||||
battery_topic: "/saltybot/battery_status"
|
||||
@ -1,51 +0,0 @@
|
||||
"""voice_command.launch.py — Launch file for voice command system (Issue #409)."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate launch description for voice command system."""
|
||||
|
||||
# Declare launch arguments
|
||||
config_arg = DeclareLaunchArgument(
|
||||
"config_file",
|
||||
default_value=os.path.join(
|
||||
get_package_share_directory("saltybot_voice_command"),
|
||||
"config",
|
||||
"voice_command_params.yaml",
|
||||
),
|
||||
description="Path to voice command parameters YAML file",
|
||||
)
|
||||
|
||||
# Voice command parser node
|
||||
voice_command_node = Node(
|
||||
package="saltybot_voice_command",
|
||||
executable="voice_command_node",
|
||||
name="voice_command_node",
|
||||
parameters=[LaunchConfiguration("config_file")],
|
||||
remappings=[],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
# Voice command router node
|
||||
voice_command_router = Node(
|
||||
package="saltybot_voice_command",
|
||||
executable="voice_command_router",
|
||||
name="voice_command_router",
|
||||
parameters=[LaunchConfiguration("config_file")],
|
||||
remappings=[],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
config_arg,
|
||||
voice_command_node,
|
||||
voice_command_router,
|
||||
]
|
||||
)
|
||||
@ -1,26 +0,0 @@
|
||||
<?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_voice_command</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Simple voice command interpreter for SaltyBot (Issue #409)</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>saltybot_social_msgs</depend>
|
||||
|
||||
<exec_depend>python3-pip</exec_depend>
|
||||
<exec_depend>python3-fuzzy</exec_depend>
|
||||
<exec_depend>piper-tts</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1 +0,0 @@
|
||||
"""saltybot_voice_command — Simple voice command interpreter for SaltyBot (Issue #409)."""
|
||||
@ -1,108 +0,0 @@
|
||||
"""voice_command_node.py — Voice command interpreter for SaltyBot (Issue #409).
|
||||
|
||||
Subscribes to /saltybot/speech_text, runs keyword+fuzzy intent classification,
|
||||
and publishes parsed commands to /saltybot/voice_command.
|
||||
|
||||
ROS2 topics
|
||||
-----------
|
||||
Subscribe: /saltybot/speech_text (std_msgs/String)
|
||||
Publish: /saltybot/voice_command (saltybot_social_msgs/VoiceCommand)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
min_confidence float 0.70 Intent confidence threshold for execution
|
||||
announce_intent bool true Log intent at INFO level
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, QoSReliabilityPolicy
|
||||
|
||||
from std_msgs.msg import String
|
||||
from saltybot_social_msgs.msg import VoiceCommand
|
||||
|
||||
from .voice_command_parser import parse
|
||||
|
||||
|
||||
class VoiceCommandNode(Node):
|
||||
"""Voice command interpreter node."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("voice_command_node")
|
||||
|
||||
# ── Parameters ──────────────────────────────────────────────────────
|
||||
self.declare_parameter("min_confidence", 0.70)
|
||||
self.declare_parameter("announce_intent", True)
|
||||
|
||||
self._min_conf: float = self.get_parameter("min_confidence").value
|
||||
self._announce: bool = self.get_parameter("announce_intent").value
|
||||
|
||||
# ── Reliable QoS — voice commands must not be dropped ───────────────
|
||||
qos = QoSProfile(depth=10)
|
||||
qos.reliability = QoSReliabilityPolicy.RELIABLE
|
||||
|
||||
self._cmd_pub = self.create_publisher(
|
||||
VoiceCommand,
|
||||
"/saltybot/voice_command",
|
||||
qos,
|
||||
)
|
||||
|
||||
self._speech_sub = self.create_subscription(
|
||||
String,
|
||||
"/saltybot/speech_text",
|
||||
self._on_speech_text,
|
||||
qos,
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f"voice_command_node ready (min_confidence={self._min_conf})"
|
||||
)
|
||||
|
||||
def _on_speech_text(self, msg: String) -> None:
|
||||
"""Process incoming speech text."""
|
||||
text = msg.data.strip()
|
||||
if not text:
|
||||
return
|
||||
|
||||
parsed = parse(text)
|
||||
|
||||
if self._announce:
|
||||
self.get_logger().info(
|
||||
f"[VoiceCmd] '{text}' → {parsed.intent} (conf={parsed.confidence:.2f})"
|
||||
)
|
||||
|
||||
# Filter by confidence threshold
|
||||
if parsed.confidence < self._min_conf and parsed.intent != "fallback":
|
||||
self.get_logger().debug(
|
||||
f"Confidence {parsed.confidence:.2f} below threshold {self._min_conf}"
|
||||
)
|
||||
return
|
||||
|
||||
# Publish the command
|
||||
cmd_msg = VoiceCommand()
|
||||
cmd_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
cmd_msg.intent = parsed.intent
|
||||
cmd_msg.raw_text = parsed.raw_text
|
||||
cmd_msg.speaker_id = "voice_command"
|
||||
cmd_msg.confidence = float(parsed.confidence)
|
||||
cmd_msg.requires_confirmation = False
|
||||
|
||||
self._cmd_pub.publish(cmd_msg)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = VoiceCommandNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,116 +0,0 @@
|
||||
"""Simple voice command parser for Issue #409.
|
||||
|
||||
Commands supported:
|
||||
- nav.follow_me: "follow me", "come with me", "follow along"
|
||||
- nav.stop: "stop", "halt", "freeze", "don't move"
|
||||
- nav.come_here: "come here", "come to me", "approach me"
|
||||
- nav.go_home: "go home", "return home", "go to base"
|
||||
- system.battery: "battery", "battery status", "how's the battery"
|
||||
- nav.spin: "spin", "turn around", "spin around"
|
||||
- system.quiet_mode: "quiet mode", "be quiet", "quiet"
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from difflib import SequenceMatcher
|
||||
from typing import Dict, Optional
|
||||
|
||||
|
||||
@dataclass
|
||||
class ParsedVoiceCommand:
|
||||
"""Result of voice command parsing."""
|
||||
intent: str # e.g. "nav.follow_me"
|
||||
confidence: float # 0.0-1.0
|
||||
raw_text: str # Normalized input
|
||||
|
||||
|
||||
# Text normalization
|
||||
_STRIP_PUNCT = re.compile(r"[^\w\s'-]")
|
||||
_MULTI_SPACE = re.compile(r"\s+")
|
||||
|
||||
|
||||
def _normalize(text: str) -> str:
|
||||
"""Lowercase, strip punctuation, collapse whitespace."""
|
||||
t = text.lower()
|
||||
t = _STRIP_PUNCT.sub(" ", t)
|
||||
t = _MULTI_SPACE.sub(" ", t)
|
||||
return t.strip()
|
||||
|
||||
|
||||
# Simple keyword-based command definitions
|
||||
_COMMANDS: Dict[str, Dict[str, list]] = {
|
||||
"nav.follow_me": {
|
||||
"keywords": ["follow me", "come with me", "follow along", "stick with me", "stay with me"],
|
||||
},
|
||||
"nav.stop": {
|
||||
"keywords": ["stop", "halt", "freeze", "don't move", "stop moving"],
|
||||
},
|
||||
"nav.come_here": {
|
||||
"keywords": ["come here", "come to me", "approach me", "get here"],
|
||||
},
|
||||
"nav.go_home": {
|
||||
"keywords": ["go home", "return home", "go to base", "go to dock", "go to charging"],
|
||||
},
|
||||
"system.battery": {
|
||||
"keywords": ["battery", "battery status", "how's the battery", "what's the battery", "check battery"],
|
||||
},
|
||||
"nav.spin": {
|
||||
"keywords": ["spin", "turn around", "spin around", "rotate", "do a spin"],
|
||||
},
|
||||
"system.quiet_mode": {
|
||||
"keywords": ["quiet mode", "be quiet", "quiet", "silence", "mute"],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _fuzzy_match(text: str, patterns: list, threshold: float = 0.75) -> Optional[float]:
|
||||
"""Fuzzy match text against a list of patterns.
|
||||
|
||||
Returns confidence score (0.0-1.0) if above threshold, else None.
|
||||
"""
|
||||
best_score = 0.0
|
||||
for pattern in patterns:
|
||||
# Check if pattern is a substring (keyword match)
|
||||
if pattern in text:
|
||||
return 1.0 # Exact keyword match
|
||||
|
||||
# Fuzzy matching
|
||||
ratio = SequenceMatcher(None, text, pattern).ratio()
|
||||
best_score = max(best_score, ratio)
|
||||
|
||||
return best_score if best_score >= threshold else None
|
||||
|
||||
|
||||
def parse(text: str) -> ParsedVoiceCommand:
|
||||
"""Parse voice command text and return ParsedVoiceCommand.
|
||||
|
||||
Tries exact keyword matching first, then fuzzy matching.
|
||||
Returns fallback intent with confidence=0.0 if no match.
|
||||
"""
|
||||
normalized = _normalize(text)
|
||||
if not normalized:
|
||||
return ParsedVoiceCommand(intent="fallback", confidence=0.0, raw_text=text)
|
||||
|
||||
best_intent = "fallback"
|
||||
best_confidence = 0.0
|
||||
|
||||
# Try exact keyword match first
|
||||
for intent, cmd_def in _COMMANDS.items():
|
||||
for keyword in cmd_def["keywords"]:
|
||||
if keyword in normalized:
|
||||
return ParsedVoiceCommand(intent=intent, confidence=1.0, raw_text=normalized)
|
||||
|
||||
# Fall back to fuzzy matching
|
||||
for intent, cmd_def in _COMMANDS.items():
|
||||
conf = _fuzzy_match(normalized, cmd_def["keywords"], threshold=0.70)
|
||||
if conf and conf > best_confidence:
|
||||
best_intent = intent
|
||||
best_confidence = conf
|
||||
|
||||
return ParsedVoiceCommand(
|
||||
intent=best_intent,
|
||||
confidence=best_confidence,
|
||||
raw_text=normalized,
|
||||
)
|
||||
@ -1,181 +0,0 @@
|
||||
"""voice_command_router.py — Command execution router for SaltyBot (Issue #409).
|
||||
|
||||
Subscribes to /saltybot/voice_command and executes commands by publishing
|
||||
appropriate navigation and system commands. Includes TTS confirmation via Piper.
|
||||
|
||||
ROS2 topics
|
||||
-----------
|
||||
Subscribe: /saltybot/voice_command (saltybot_social_msgs/VoiceCommand)
|
||||
Publish: /saltybot/cmd_vel (geometry_msgs/Twist) for nav commands
|
||||
Publish: /saltybot/tts_text (std_msgs/String) for TTS output
|
||||
|
||||
Parameters
|
||||
----------
|
||||
min_confidence float 0.70 Only execute commands above this confidence
|
||||
enable_tts bool true Enable TTS confirmation
|
||||
battery_topic str "/battery_status" Topic for battery status
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
from typing import Dict, Callable, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, QoSReliabilityPolicy
|
||||
|
||||
from std_msgs.msg import String
|
||||
from geometry_msgs.msg import Twist
|
||||
from saltybot_social_msgs.msg import VoiceCommand
|
||||
|
||||
|
||||
class VoiceCommandRouter(Node):
|
||||
"""Routes voice commands to appropriate ROS2 topics/services."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("voice_command_router")
|
||||
|
||||
# ── Parameters ──────────────────────────────────────────────────────
|
||||
self.declare_parameter("min_confidence", 0.70)
|
||||
self.declare_parameter("enable_tts", True)
|
||||
|
||||
self._min_conf: float = self.get_parameter("min_confidence").value
|
||||
self._enable_tts: bool = self.get_parameter("enable_tts").value
|
||||
|
||||
# ── Reliable QoS ────────────────────────────────────────────────────
|
||||
qos = QoSProfile(depth=10)
|
||||
qos.reliability = QoSReliabilityPolicy.RELIABLE
|
||||
|
||||
self._cmd_vel_pub = self.create_publisher(Twist, "/saltybot/cmd_vel", qos)
|
||||
self._tts_pub = self.create_publisher(String, "/saltybot/tts_text", qos)
|
||||
|
||||
self._cmd_sub = self.create_subscription(
|
||||
VoiceCommand,
|
||||
"/saltybot/voice_command",
|
||||
self._on_voice_command,
|
||||
qos,
|
||||
)
|
||||
|
||||
# ── Command handlers ────────────────────────────────────────────────
|
||||
self._handlers: Dict[str, Callable] = {
|
||||
"nav.follow_me": self._handle_follow_me,
|
||||
"nav.stop": self._handle_stop,
|
||||
"nav.come_here": self._handle_come_here,
|
||||
"nav.go_home": self._handle_go_home,
|
||||
"system.battery": self._handle_battery,
|
||||
"nav.spin": self._handle_spin,
|
||||
"system.quiet_mode": self._handle_quiet_mode,
|
||||
}
|
||||
|
||||
self.get_logger().info("voice_command_router ready")
|
||||
|
||||
def _on_voice_command(self, msg: VoiceCommand) -> None:
|
||||
"""Process incoming voice command."""
|
||||
if msg.confidence < self._min_conf:
|
||||
self.get_logger().debug(
|
||||
f"Ignoring '{msg.intent}' (confidence={msg.confidence:.2f} < {self._min_conf})"
|
||||
)
|
||||
return
|
||||
|
||||
self.get_logger().info(f"Executing command: {msg.intent}")
|
||||
|
||||
if msg.intent == "fallback":
|
||||
self._tts_speak("I didn't understand that command. Try 'follow me', 'stop', or 'go home'.")
|
||||
return
|
||||
|
||||
handler = self._handlers.get(msg.intent)
|
||||
if handler:
|
||||
handler()
|
||||
else:
|
||||
self.get_logger().warn(f"No handler for intent: {msg.intent}")
|
||||
|
||||
# ── Navigation Command Handlers ─────────────────────────────────────────
|
||||
|
||||
def _handle_follow_me(self) -> None:
|
||||
"""Start following the speaker."""
|
||||
self._tts_speak("Following you.")
|
||||
# Publish a marker to indicate follow mode
|
||||
# (actual following behavior is handled by nav stack)
|
||||
twist = Twist()
|
||||
twist.linear.x = 0.5 # Start moving forward
|
||||
self._cmd_vel_pub.publish(twist)
|
||||
|
||||
def _handle_stop(self) -> None:
|
||||
"""Stop all motion."""
|
||||
self._tts_speak("Stopping.")
|
||||
twist = Twist() # Zero velocity stops motion
|
||||
self._cmd_vel_pub.publish(twist)
|
||||
|
||||
def _handle_come_here(self) -> None:
|
||||
"""Come towards the speaker."""
|
||||
self._tts_speak("Coming to you.")
|
||||
# Command navigation system to approach speaker
|
||||
twist = Twist()
|
||||
twist.linear.x = 0.3 # Approach speed
|
||||
self._cmd_vel_pub.publish(twist)
|
||||
|
||||
def _handle_go_home(self) -> None:
|
||||
"""Return to base/dock."""
|
||||
self._tts_speak("Going home.")
|
||||
# Trigger homing routine (specific intent/service call)
|
||||
# For now, just indicate intention
|
||||
|
||||
def _handle_battery(self) -> None:
|
||||
"""Announce battery status."""
|
||||
# In a real implementation, would read from battery_status topic
|
||||
self._tts_speak("Battery status: Full charge. Ready to go.")
|
||||
|
||||
def _handle_spin(self) -> None:
|
||||
"""Execute a spin."""
|
||||
self._tts_speak("Spinning.")
|
||||
twist = Twist()
|
||||
twist.angular.z = 1.0 # Spin at 1 rad/s
|
||||
self._cmd_vel_pub.publish(twist)
|
||||
# Would need a timer to stop spin after timeout
|
||||
|
||||
def _handle_quiet_mode(self) -> None:
|
||||
"""Suppress TTS output."""
|
||||
self._enable_tts = False
|
||||
if self._enable_tts: # Announce before going quiet
|
||||
self._tts_speak("Quiet mode enabled.")
|
||||
|
||||
# ── TTS Helper ──────────────────────────────────────────────────────────
|
||||
|
||||
def _tts_speak(self, text: str) -> None:
|
||||
"""Queue text for TTS output via Piper."""
|
||||
if not self._enable_tts:
|
||||
return
|
||||
|
||||
try:
|
||||
# Try to speak using piper command if available
|
||||
subprocess.Popen(
|
||||
["echo", text, "|", "piper", "--model", "en_US-ryan-high"],
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
)
|
||||
except (FileNotFoundError, OSError):
|
||||
# Fall back to publishing message (external TTS node will handle)
|
||||
pass
|
||||
|
||||
# Always publish the text message for external TTS nodes
|
||||
msg = String()
|
||||
msg.data = text
|
||||
self._tts_pub.publish(msg)
|
||||
self.get_logger().info(f"[TTS] {text}")
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = VoiceCommandRouter()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_voice_command
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_voice_command
|
||||
@ -1,30 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = 'saltybot_voice_command'
|
||||
|
||||
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']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
author='sl-perception',
|
||||
author_email='sl-perception@saltylab.local',
|
||||
maintainer='sl-perception',
|
||||
maintainer_email='sl-perception@saltylab.local',
|
||||
url='https://gitea.vayrette.com/seb/saltylab-firmware',
|
||||
description='Simple voice command interpreter for SaltyBot',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'voice_command_node = saltybot_voice_command.voice_command_node:main',
|
||||
'voice_command_router = saltybot_voice_command.voice_command_router:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,127 +0,0 @@
|
||||
"""Tests for voice command parser (Issue #409)."""
|
||||
|
||||
from saltybot_voice_command.voice_command_parser import parse
|
||||
|
||||
|
||||
def test_follow_me():
|
||||
"""Test 'follow me' command."""
|
||||
r = parse("follow me")
|
||||
assert r.intent == "nav.follow_me"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse("come with me")
|
||||
assert r.intent == "nav.follow_me"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
def test_stop():
|
||||
"""Test 'stop' command."""
|
||||
r = parse("stop")
|
||||
assert r.intent == "nav.stop"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse("halt")
|
||||
assert r.intent == "nav.stop"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
def test_come_here():
|
||||
"""Test 'come here' command."""
|
||||
r = parse("come here")
|
||||
assert r.intent == "nav.come_here"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse("approach me")
|
||||
assert r.intent == "nav.come_here"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
def test_go_home():
|
||||
"""Test 'go home' command."""
|
||||
r = parse("go home")
|
||||
assert r.intent == "nav.go_home"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse("return home")
|
||||
assert r.intent == "nav.go_home"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
def test_battery():
|
||||
"""Test 'battery' command."""
|
||||
r = parse("battery")
|
||||
assert r.intent == "system.battery"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse("battery status")
|
||||
assert r.intent == "system.battery"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
def test_spin():
|
||||
"""Test 'spin' command."""
|
||||
r = parse("spin")
|
||||
assert r.intent == "nav.spin"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse("turn around")
|
||||
assert r.intent == "nav.spin"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
def test_quiet_mode():
|
||||
"""Test 'quiet mode' command."""
|
||||
r = parse("quiet mode")
|
||||
assert r.intent == "system.quiet_mode"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse("be quiet")
|
||||
assert r.intent == "system.quiet_mode"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
def test_fallback():
|
||||
"""Test unrecognized commands."""
|
||||
r = parse("xyzabc")
|
||||
assert r.intent == "fallback"
|
||||
assert r.confidence == 0.0
|
||||
|
||||
r = parse("")
|
||||
assert r.intent == "fallback"
|
||||
assert r.confidence == 0.0
|
||||
|
||||
|
||||
def test_fuzzy_matching():
|
||||
"""Test fuzzy matching for similar commands."""
|
||||
# Similar but not exact
|
||||
r = parse("stap") # Typo for "stop"
|
||||
assert r.intent == "nav.stop"
|
||||
assert r.confidence > 0.7
|
||||
|
||||
r = parse("comhere") # Slurred version
|
||||
assert r.intent in ("nav.come_here", "fallback")
|
||||
|
||||
|
||||
def test_normalization():
|
||||
"""Test text normalization."""
|
||||
r = parse("FOLLOW ME!")
|
||||
assert r.intent == "nav.follow_me"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
r = parse(" go home ")
|
||||
assert r.intent == "nav.go_home"
|
||||
assert r.confidence == 1.0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
test_follow_me()
|
||||
test_stop()
|
||||
test_come_here()
|
||||
test_go_home()
|
||||
test_battery()
|
||||
test_spin()
|
||||
test_quiet_mode()
|
||||
test_fallback()
|
||||
test_fuzzy_matching()
|
||||
test_normalization()
|
||||
print("All tests passed!")
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
x
Reference in New Issue
Block a user