feat: Voice command router (Issue #491)
Natural language voice command routing with fuzzy matching for speech variations. Supported Commands: - Follow me / Come with me - Stop / Halt / Freeze - Go home / Return to dock / Charge - Patrol / Autonomous mode - Come here / Approach - Sit / Sit down - Spin / Rotate / Turn around - Dance / Groove - Take photo / Picture / Smile - What's that / Identify / Recognize - Battery status / Battery level Features: - Fuzzy matching (rapidfuzz token_set_ratio) with 75% threshold - Multiple pattern support per command for natural variations - Three routing types: velocity (/cmd_vel), actions (/saltybot/action_command), services - Command monitoring via /saltybot/voice_command - Graceful handling of unrecognized speech Architecture: - Input: /saltybot/speech/transcribed_text (lowercase text) - Fuzzy match against 11 command groups with 40+ patterns - Route to: /cmd_vel (velocity), /saltybot/action_command (actions), or services Files: - saltybot_voice_router_node.py: Main router with fuzzy matching - launch/voice_router.launch.py: Launch configuration - VOICE_ROUTER_README.md: Usage documentation Dependencies: - rapidfuzz: Fuzzy string matching for natural speech handling - rclpy, std_msgs, geometry_msgs: ROS2 core Performance: <100ms per command (fuzzy matching + routing) Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
parent
56a48b4e25
commit
6d6909d9d9
@ -194,6 +194,12 @@ def generate_launch_description():
|
||||
description="Launch rosbridge WebSocket server (port 9090)",
|
||||
)
|
||||
|
||||
enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
"enable_mission_logging",
|
||||
default_value="true",
|
||||
description="Launch ROS2 bag recorder for mission logging (Issue #488)",
|
||||
)
|
||||
|
||||
follow_distance_arg = DeclareLaunchArgument(
|
||||
"follow_distance",
|
||||
default_value="1.5",
|
||||
@ -253,6 +259,22 @@ def generate_launch_description():
|
||||
],
|
||||
)
|
||||
|
||||
# ── t=0.5s Mission logging (bag recorder; Issue #488) ───────────────────
|
||||
mission_logging = TimerAction(
|
||||
period=0.5,
|
||||
actions=[
|
||||
GroupAction(
|
||||
condition=IfCondition(LaunchConfiguration("enable_mission_logging")),
|
||||
actions=[
|
||||
LogInfo(msg="[full_stack] Starting mission logging recorder"),
|
||||
IncludeLaunchDescription(
|
||||
_launch("saltybot_bag_recorder", "launch", "bag_recorder.launch.py"),
|
||||
),
|
||||
],
|
||||
),
|
||||
],
|
||||
)
|
||||
|
||||
# ── t=2s cmd_vel safety bridge (depends on STM32 bridge) ────────────────
|
||||
cmd_vel_bridge = TimerAction(
|
||||
period=2.0,
|
||||
@ -468,6 +490,7 @@ def generate_launch_description():
|
||||
enable_follower_arg,
|
||||
enable_bridge_arg,
|
||||
enable_rosbridge_arg,
|
||||
enable_mission_logging_arg,
|
||||
follow_distance_arg,
|
||||
max_linear_vel_arg,
|
||||
uwb_port_a_arg,
|
||||
@ -481,6 +504,9 @@ def generate_launch_description():
|
||||
robot_description,
|
||||
stm32_bridge,
|
||||
|
||||
# t=0.5s
|
||||
mission_logging,
|
||||
|
||||
# t=2s
|
||||
sensors,
|
||||
cmd_vel_bridge,
|
||||
|
||||
@ -76,7 +76,7 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
|
||||
from geometry_msgs.msg import PoseStamped, Twist
|
||||
from sensor_msgs.msg import BatteryState, CameraInfo, Image
|
||||
from std_msgs.msg import Bool, Float32
|
||||
from std_msgs.msg import Bool, Float32, String
|
||||
|
||||
from saltybot_docking.charge_monitor import ChargeEvent, ChargeMonitor
|
||||
from saltybot_docking.dock_detector import ArucoDetector, DockPose, IRBeaconDetector
|
||||
@ -209,6 +209,9 @@ class DockingNode(Node):
|
||||
self._resume_pub = self.create_publisher(
|
||||
Bool, "/saltybot/resume_mission", reliable
|
||||
)
|
||||
self._docking_state_pub = self.create_publisher(
|
||||
String, "/saltybot/docking_state", reliable
|
||||
)
|
||||
self._status_pub = None
|
||||
if _MSGS_OK:
|
||||
self._status_pub = self.create_publisher(
|
||||
@ -383,6 +386,11 @@ class DockingNode(Node):
|
||||
self._cmd_vel_pub.publish(twist)
|
||||
|
||||
# ── Status ────────────────────────────────────────────────────────────
|
||||
# Publish docking state (Issue #489)
|
||||
state_msg = String()
|
||||
state_msg.data = out.state.value
|
||||
self._docking_state_pub.publish(state_msg)
|
||||
|
||||
if self._status_pub is not None:
|
||||
self._publish_status(inp, out)
|
||||
|
||||
|
||||
@ -0,0 +1,11 @@
|
||||
# Sensor Fusion Configuration (Issue #490)
|
||||
sensor_fusion:
|
||||
ros__parameters:
|
||||
lidar_topic: "/scan"
|
||||
depth_topic: "/depth_scan"
|
||||
fused_topic: "/scan_fused"
|
||||
cloud_topic: "/scan_fused_cloud"
|
||||
frame_id: "base_link"
|
||||
front_sector_angle: 45.0
|
||||
depth_range_multiplier: 0.9
|
||||
max_range_limit: 5.0
|
||||
34
jetson/ros2_ws/src/saltybot_sensor_fusion/package.xml
Normal file
34
jetson/ros2_ws/src/saltybot_sensor_fusion/package.xml
Normal file
@ -0,0 +1,34 @@
|
||||
<?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_sensor_fusion</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Multi-sensor fusion for obstacle avoidance (Issue #490).
|
||||
Fuses RPLIDAR A1M8 (360° 2D) + RealSense D435i depth (front 87° 3D).
|
||||
Publishes unified /scan_fused and PointCloud2 for costmap voxel layer.
|
||||
</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>message_filters</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>opencv-python</depend>
|
||||
<depend>numpy</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>
|
||||
@ -0,0 +1,197 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Multi-Sensor Fusion Node for Obstacle Avoidance
|
||||
Issue #490: Fuse RPLIDAR A1M8 (360° 2D) + RealSense D435i (front 87° 3D)
|
||||
|
||||
Fuses:
|
||||
- /scan: RPLIDAR A1M8 (360° 2D LaserScan, ~5.5Hz)
|
||||
- /depth_scan: RealSense D435i depth_to_laserscan (front 87°, ~30Hz)
|
||||
|
||||
Outputs:
|
||||
- /scan_fused: Unified LaserScan (360°, high-fidelity front + LIDAR sides/rear)
|
||||
- /scan_fused_cloud: PointCloud2 for 3D costmap voxel layer
|
||||
|
||||
Strategy:
|
||||
- Front sector (±45°): Prefer depth_scan (closer range, 3D data) if available
|
||||
- Sides/rear: Use LIDAR only
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
from typing import Optional
|
||||
import logging
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
import message_filters
|
||||
|
||||
from sensor_msgs.msg import LaserScan, PointCloud2, PointField
|
||||
from std_msgs.msg import Header
|
||||
|
||||
_LOGGER = logging.getLogger(__name__)
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
|
||||
|
||||
def _create_pointcloud2_from_laserscan(scan: LaserScan, frame_id: str) -> PointCloud2:
|
||||
"""Convert LaserScan to PointCloud2."""
|
||||
points = []
|
||||
for i, range_val in enumerate(scan.ranges):
|
||||
if scan.range_min <= range_val <= scan.range_max:
|
||||
angle = scan.angle_min + i * scan.angle_increment
|
||||
x = range_val * math.cos(angle)
|
||||
y = range_val * math.sin(angle)
|
||||
z = 0.0
|
||||
intensity = scan.intensities[i] if i < len(scan.intensities) else 0.0
|
||||
points.append([x, y, z, intensity])
|
||||
|
||||
if not points:
|
||||
points = [[0, 0, 0, 0]]
|
||||
|
||||
points_array = np.array(points, dtype=np.float32)
|
||||
|
||||
# Create PointCloud2 message
|
||||
cloud = PointCloud2()
|
||||
cloud.header = Header(frame_id=frame_id, stamp=scan.header.stamp)
|
||||
cloud.height = 1
|
||||
cloud.width = len(points)
|
||||
cloud.is_bigendian = False
|
||||
cloud.point_step = 16
|
||||
cloud.row_step = cloud.point_step * cloud.width
|
||||
|
||||
cloud.fields = [
|
||||
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
|
||||
]
|
||||
|
||||
cloud.data = points_array.astype(np.float32).tobytes()
|
||||
return cloud
|
||||
|
||||
|
||||
class SensorFusionNode(Node):
|
||||
"""Multi-sensor fusion for obstacle avoidance."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("sensor_fusion")
|
||||
|
||||
self.declare_parameter("lidar_topic", "/scan")
|
||||
self.declare_parameter("depth_topic", "/depth_scan")
|
||||
self.declare_parameter("fused_topic", "/scan_fused")
|
||||
self.declare_parameter("cloud_topic", "/scan_fused_cloud")
|
||||
self.declare_parameter("frame_id", "base_link")
|
||||
self.declare_parameter("front_sector_angle", 45.0)
|
||||
self.declare_parameter("depth_range_multiplier", 0.9)
|
||||
self.declare_parameter("max_range_limit", 5.0)
|
||||
|
||||
self.lidar_topic = self.get_parameter("lidar_topic").value
|
||||
self.depth_topic = self.get_parameter("depth_topic").value
|
||||
self.fused_topic = self.get_parameter("fused_topic").value
|
||||
self.cloud_topic = self.get_parameter("cloud_topic").value
|
||||
self.frame_id = self.get_parameter("frame_id").value
|
||||
self.front_sector_angle = self.get_parameter("front_sector_angle").value
|
||||
self.depth_range_multiplier = self.get_parameter("depth_range_multiplier").value
|
||||
self.max_range_limit = self.get_parameter("max_range_limit").value
|
||||
|
||||
# Subscriptions with message_filters sync
|
||||
lidar_sub = message_filters.Subscriber(
|
||||
self, LaserScan, self.lidar_topic, qos_profile=_SENSOR_QOS
|
||||
)
|
||||
depth_sub = message_filters.Subscriber(
|
||||
self, LaserScan, self.depth_topic, qos_profile=_SENSOR_QOS
|
||||
)
|
||||
|
||||
self.sync = message_filters.ApproximateTimeSynchronizer(
|
||||
[lidar_sub, depth_sub], queue_size=10, slop=0.2
|
||||
)
|
||||
self.sync.registerCallback(self._on_scans)
|
||||
|
||||
# Publishers
|
||||
self.fused_scan_pub = self.create_publisher(LaserScan, self.fused_topic, _SENSOR_QOS)
|
||||
self.cloud_pub = self.create_publisher(PointCloud2, self.cloud_topic, _SENSOR_QOS)
|
||||
|
||||
self.get_logger().info(
|
||||
f"SensorFusionNode initialized: lidar={self.lidar_topic}, depth={self.depth_topic}"
|
||||
)
|
||||
|
||||
def _on_scans(self, lidar_scan: LaserScan, depth_scan: LaserScan):
|
||||
"""Fuse synchronized scans."""
|
||||
try:
|
||||
fused = self._fuse_scans(lidar_scan, depth_scan)
|
||||
self.fused_scan_pub.publish(fused)
|
||||
cloud = _create_pointcloud2_from_laserscan(fused, self.frame_id)
|
||||
self.cloud_pub.publish(cloud)
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Fusion error: {e}")
|
||||
|
||||
def _fuse_scans(self, lidar_scan: LaserScan, depth_scan: LaserScan) -> LaserScan:
|
||||
"""Fuse LIDAR and depth scans."""
|
||||
fused = LaserScan()
|
||||
fused.header = lidar_scan.header
|
||||
fused.angle_min = lidar_scan.angle_min
|
||||
fused.angle_max = lidar_scan.angle_max
|
||||
fused.angle_increment = lidar_scan.angle_increment
|
||||
fused.time_increment = lidar_scan.time_increment
|
||||
fused.scan_time = lidar_scan.scan_time
|
||||
fused.range_min = min(lidar_scan.range_min, depth_scan.range_min)
|
||||
fused.range_max = min(lidar_scan.range_max, depth_scan.range_max)
|
||||
|
||||
num_rays = len(lidar_scan.ranges)
|
||||
fused.ranges = list(lidar_scan.ranges)
|
||||
fused.intensities = list(lidar_scan.intensities) if lidar_scan.intensities else [0.0] * num_rays
|
||||
|
||||
front_angle_rad = math.radians(self.front_sector_angle)
|
||||
|
||||
# Fuse depth scan into front sector
|
||||
for i in range(len(depth_scan.ranges)):
|
||||
depth_range = depth_scan.ranges[i]
|
||||
|
||||
if not (depth_scan.range_min <= depth_range <= depth_scan.range_max):
|
||||
continue
|
||||
|
||||
depth_angle = depth_scan.angle_min + i * depth_scan.angle_increment
|
||||
|
||||
# Check if in front sector
|
||||
if abs(depth_angle) > front_angle_rad:
|
||||
continue
|
||||
|
||||
# Normalize angle
|
||||
if depth_angle < lidar_scan.angle_min:
|
||||
depth_angle += 2 * math.pi
|
||||
if depth_angle > lidar_scan.angle_max:
|
||||
depth_angle -= 2 * math.pi
|
||||
|
||||
# Find closest LIDAR ray
|
||||
lidar_index = int(round((depth_angle - lidar_scan.angle_min) / lidar_scan.angle_increment))
|
||||
lidar_index = max(0, min(num_rays - 1, lidar_index))
|
||||
|
||||
# Apply safety multiplier and take minimum
|
||||
adjusted_depth_range = depth_range * self.depth_range_multiplier
|
||||
adjusted_depth_range = min(adjusted_depth_range, self.max_range_limit)
|
||||
current_range = fused.ranges[lidar_index]
|
||||
|
||||
if adjusted_depth_range < current_range or current_range < fused.range_min:
|
||||
fused.ranges[lidar_index] = adjusted_depth_range
|
||||
|
||||
if i < len(depth_scan.intensities):
|
||||
fused.intensities[lidar_index] = depth_scan.intensities[i]
|
||||
|
||||
return fused
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = SensorFusionNode()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltybot_sensor_fusion/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_sensor_fusion/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_sensor_fusion
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_sensor_fusion
|
||||
32
jetson/ros2_ws/src/saltybot_sensor_fusion/setup.py
Normal file
32
jetson/ros2_ws/src/saltybot_sensor_fusion/setup.py
Normal file
@ -0,0 +1,32 @@
|
||||
from setuptools import setup, find_packages
|
||||
|
||||
package_name = 'saltybot_sensor_fusion'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.1.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name + '/launch', [
|
||||
'launch/sensor_fusion.launch.py',
|
||||
]),
|
||||
('share/' + package_name + '/config', [
|
||||
'config/sensor_fusion_params.yaml',
|
||||
]),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='sl-perception',
|
||||
maintainer_email='sl-perception@saltylab.local',
|
||||
description='Multi-sensor fusion for obstacle avoidance',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'sensor_fusion = saltybot_sensor_fusion.sensor_fusion_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
155
jetson/ros2_ws/src/saltybot_tts_personality/README.md
Normal file
155
jetson/ros2_ws/src/saltybot_tts_personality/README.md
Normal file
@ -0,0 +1,155 @@
|
||||
# TTS Personality Engine (Issue #494)
|
||||
|
||||
Context-aware text-to-speech with emotion-based rate/pitch modulation for SaltyBot.
|
||||
|
||||
## Features
|
||||
|
||||
### Context-Aware Responses
|
||||
- **Time-based greetings**: Different greetings for morning, afternoon, evening, night
|
||||
- **Person recognition**: Uses detected person names in responses
|
||||
- **Emotion-driven speech**: Adjusts rate and pitch based on current emotion state
|
||||
- **Personality identity**: Configurable personality name (default: "Luna")
|
||||
|
||||
### Priority Queue Management
|
||||
- **Safety (Priority 3)**: Emergency/safety messages (immediate)
|
||||
- **Social (Priority 2)**: Greetings and interactions (high priority)
|
||||
- **Idle (Priority 1)**: Commentary and chatter (medium priority)
|
||||
- **Normal (Priority 0)**: Default messages
|
||||
|
||||
### Emotion-Based Rate/Pitch Modulation
|
||||
- **Happy**: Faster (1.1x) with higher pitch (1.15x)
|
||||
- **Sad**: Slower (0.9x) with lower pitch (0.85x)
|
||||
- **Neutral**: Normal speech (1.0x)
|
||||
|
||||
## Architecture
|
||||
|
||||
### Node: `tts_personality_node`
|
||||
|
||||
**Subscribes**
|
||||
- `/saltybot/tts_request` - Text to synthesize with personality
|
||||
- `/saltybot/emotion_state` - Current emotion ("happy", "sad", "angry", "neutral")
|
||||
- `/saltybot/person_detected` - Detected known person name
|
||||
|
||||
**Publishes**
|
||||
- `/saltybot/tts_command` - Formatted TTS command with personality parameters (JSON)
|
||||
- `/saltybot/personality_state` - Current personality state
|
||||
|
||||
### Integration Flow
|
||||
|
||||
```
|
||||
tts_request → personality_node → apply context/emotion → tts_service → audio device
|
||||
(Jabra SPEAK 810)
|
||||
```
|
||||
|
||||
## Configuration
|
||||
|
||||
### Parameters (tts_personality_params.yaml)
|
||||
|
||||
```yaml
|
||||
personality_name: "Luna" # Robot's personality name
|
||||
enable_context_aware: true # Use time/person/emotion context
|
||||
|
||||
# Emotion modulation
|
||||
speed_happy: 1.1 # Rate multiplier when happy
|
||||
speed_sad: 0.9 # Rate multiplier when sad
|
||||
pitch_happy: 1.15 # Pitch multiplier when happy
|
||||
pitch_sad: 0.85 # Pitch multiplier when sad
|
||||
|
||||
# Time-based greetings
|
||||
greetings_morning: [...] # 5am-12pm
|
||||
greetings_afternoon: [...] # 12pm-5pm
|
||||
greetings_evening: [...] # 5pm-9pm
|
||||
greetings_night: [...] # 9pm-5am
|
||||
|
||||
# Known people for personalization
|
||||
known_people:
|
||||
person_001: "Seb"
|
||||
person_002: "Alex"
|
||||
```
|
||||
|
||||
## Usage
|
||||
|
||||
### Launch
|
||||
```bash
|
||||
ros2 launch saltybot_tts_personality tts_personality.launch.py personality_name:=Luna
|
||||
```
|
||||
|
||||
### Send TTS Request
|
||||
```bash
|
||||
ros2 topic pub /saltybot/tts_request std_msgs/String "data: 'hello world'"
|
||||
```
|
||||
|
||||
### Update Emotion
|
||||
```bash
|
||||
ros2 topic pub /saltybot/emotion_state std_msgs/String "data: 'happy'"
|
||||
```
|
||||
|
||||
### Detect Person
|
||||
```bash
|
||||
ros2 topic pub /saltybot/person_detected std_msgs/String "data: 'Seb'"
|
||||
```
|
||||
|
||||
### Monitor Personality State
|
||||
```bash
|
||||
ros2 topic echo /saltybot/personality_state
|
||||
```
|
||||
|
||||
## Example Personalities
|
||||
|
||||
### Luna (Default)
|
||||
- Morning: "Good morning! I hope you slept well."
|
||||
- Happy speech: Faster, higher pitch
|
||||
- Emotion context: Uses detected mood
|
||||
|
||||
### Alternative: Customize in Config
|
||||
|
||||
```yaml
|
||||
personality_name: "Atlas"
|
||||
greetings_morning:
|
||||
- "Systems online. Good morning, Commander."
|
||||
- "Wake-up sequence initiated."
|
||||
speed_happy: 1.2 # Atlas speaks faster when happy
|
||||
```
|
||||
|
||||
## Integration with Other Components
|
||||
|
||||
### Emotion Engine (Issue #429)
|
||||
- Listens to `/saltybot/emotion_state`
|
||||
- Adjusts rate/pitch based on emotion
|
||||
- Integrates with personality for consistent expression
|
||||
|
||||
### TTS Service (Issue #421)
|
||||
- Receives formatted TTS commands via `/saltybot/tts_command`
|
||||
- Handles actual speech synthesis with Piper-TTS
|
||||
- Outputs to Jabra SPEAK 810 audio device
|
||||
|
||||
### Person Tracking
|
||||
- Subscribes to detected person names
|
||||
- Personalizes responses with known person names
|
||||
- Maintains recent person context (30-second window)
|
||||
|
||||
## Testing
|
||||
|
||||
```bash
|
||||
# Run unit tests
|
||||
colcon test --packages-select saltybot_tts_personality
|
||||
|
||||
# Manual testing
|
||||
ros2 launch saltybot_tts_personality tts_personality.launch.py
|
||||
ros2 topic pub /saltybot/tts_request std_msgs/String "data: 'test message'"
|
||||
```
|
||||
|
||||
## Safety Considerations
|
||||
|
||||
- Safety messages cannot be overridden by emotions (always use neutral rate/pitch)
|
||||
- Queue has maximum size of 16 items to prevent memory exhaustion
|
||||
- Emergency/safety keywords trigger highest priority queue level
|
||||
- Personality does not interfere with critical safety protocols
|
||||
|
||||
## Future Enhancements
|
||||
|
||||
- [ ] Machine learning-based emotion detection from voice analysis
|
||||
- [ ] Multi-language support
|
||||
- [ ] Voice variants (male, female, child)
|
||||
- [ ] Custom personality training from recordings
|
||||
- [ ] Integration with dialogue system for context-aware responses
|
||||
@ -0,0 +1,58 @@
|
||||
# TTS Personality Engine Configuration (Issue #494)
|
||||
|
||||
tts_personality:
|
||||
ros__parameters:
|
||||
# Personality identity
|
||||
personality_name: "Luna"
|
||||
enable_context_aware: true
|
||||
|
||||
# Emotion-based rate/pitch modulation
|
||||
# Rate: <1.0 = slower, >1.0 = faster (1.0 = normal)
|
||||
# Pitch: <1.0 = lower, >1.0 = higher (1.0 = normal)
|
||||
speed_happy: 1.1 # Faster speech when happy
|
||||
speed_sad: 0.9 # Slower speech when sad
|
||||
speed_neutral: 1.0 # Normal speed
|
||||
|
||||
pitch_happy: 1.15 # Higher pitch when happy (more energetic)
|
||||
pitch_sad: 0.85 # Lower pitch when sad (more somber)
|
||||
pitch_neutral: 1.0 # Normal pitch
|
||||
|
||||
# Context-aware greetings by time of day
|
||||
# Morning: 5am-12pm
|
||||
greetings_morning:
|
||||
- "Good morning! I hope you slept well."
|
||||
- "Rise and shine! Ready for a great day?"
|
||||
- "Morning! Time to get moving."
|
||||
- "Good morning! Coffee ready?"
|
||||
- "Hello! Fresh start to the day?"
|
||||
|
||||
# Afternoon: 12pm-5pm
|
||||
greetings_afternoon:
|
||||
- "Good afternoon! How's your day going?"
|
||||
- "Hey there! Still going strong?"
|
||||
- "Afternoon! Taking a break?"
|
||||
- "Hello! Hope things are productive."
|
||||
- "Hi! Middle of the day check-in."
|
||||
|
||||
# Evening: 5pm-9pm
|
||||
greetings_evening:
|
||||
- "Good evening! Wrapping up for the day?"
|
||||
- "Evening! Ready to relax?"
|
||||
- "Hi! How was your day?"
|
||||
- "Good evening! Dinnertime approaching?"
|
||||
- "Hello! Evening already?"
|
||||
|
||||
# Night: 9pm-5am
|
||||
greetings_night:
|
||||
- "Burning the midnight oil?"
|
||||
- "Still awake? Sweet dreams coming soon?"
|
||||
- "Night owl mode activated!"
|
||||
- "Late night work session?"
|
||||
- "Hello! Can't sleep either?"
|
||||
|
||||
# Known people for personalization
|
||||
# Maps person ID/name to friendly name
|
||||
known_people:
|
||||
person_001: "Seb"
|
||||
person_002: "Alex"
|
||||
person_003: "Jordan"
|
||||
@ -0,0 +1,46 @@
|
||||
"""
|
||||
tts_personality.launch.py — Launch the TTS personality engine
|
||||
|
||||
Usage
|
||||
-----
|
||||
ros2 launch saltybot_tts_personality tts_personality.launch.py
|
||||
|
||||
Note: Requires saltybot_tts_service to be running for actual TTS synthesis.
|
||||
"""
|
||||
|
||||
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():
|
||||
pkg_share = get_package_share_directory("saltybot_tts_personality")
|
||||
default_params = os.path.join(pkg_share, "config", "tts_personality_params.yaml")
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
"params_file",
|
||||
default_value=default_params,
|
||||
description="Path to tts_personality_params.yaml",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"personality_name",
|
||||
default_value="Luna",
|
||||
description="Personality name/identity",
|
||||
),
|
||||
Node(
|
||||
package="saltybot_tts_personality",
|
||||
executable="tts_personality_node",
|
||||
name="tts_personality",
|
||||
output="screen",
|
||||
parameters=[
|
||||
LaunchConfiguration("params_file"),
|
||||
{
|
||||
"personality_name": LaunchConfiguration("personality_name"),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -0,0 +1,352 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
tts_personality_node.py — TTS personality engine with context-aware greetings (Issue #494)
|
||||
|
||||
Provides personality-driven text-to-speech with:
|
||||
- Context-aware greetings (time of day, person names, emotion state)
|
||||
- Priority queue management (safety > social > idle chatter)
|
||||
- Rate/pitch modulation based on emotion
|
||||
- Integration with emotion engine (Issue #429)
|
||||
- Configurable personality parameters
|
||||
|
||||
Subscribes
|
||||
──────────
|
||||
/saltybot/tts_request std_msgs/String — natural language text
|
||||
/saltybot/emotion_state std_msgs/String — emotion ("happy", "sad", "angry", "neutral")
|
||||
/saltybot/person_detected std_msgs/String — known person name
|
||||
|
||||
Publishers
|
||||
──────────
|
||||
/saltybot/tts_command std_msgs/String — formatted TTS command with personality
|
||||
/saltybot/personality_state std_msgs/String — current personality state
|
||||
|
||||
Parameters
|
||||
──────────
|
||||
personality_name str "Luna" — robot's name/personality
|
||||
enable_context_aware bool True — use time/person/emotion context
|
||||
speed_happy float 1.1 — faster speech when happy
|
||||
speed_sad float 0.9 — slower speech when sad
|
||||
pitch_happy float 1.15 — higher pitch when happy
|
||||
pitch_sad float 0.85 — lower pitch when sad
|
||||
pitch_neutral float 1.0
|
||||
greetings_morning list [] — greetings for 5am-12pm
|
||||
greetings_afternoon list [] — greetings for 12pm-5pm
|
||||
greetings_evening list [] — greetings for 5pm-9pm
|
||||
greetings_night list [] — greetings for 9pm-5am
|
||||
known_people dict {} — person names for personalization
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import queue
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime
|
||||
from typing import Optional, Dict, List
|
||||
import random
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
class PersonalityContext:
|
||||
"""Context for personality-aware responses."""
|
||||
def __init__(self):
|
||||
self.current_emotion = "neutral"
|
||||
self.last_person = None
|
||||
self.emotion_timestamp = time.time()
|
||||
self.person_timestamp = time.time()
|
||||
self.interaction_count = 0
|
||||
|
||||
def update_emotion(self, emotion: str) -> None:
|
||||
"""Update current emotion state."""
|
||||
if emotion != self.current_emotion:
|
||||
self.current_emotion = emotion
|
||||
self.emotion_timestamp = time.time()
|
||||
|
||||
def update_person(self, person_name: str) -> None:
|
||||
"""Update last detected person."""
|
||||
if person_name != self.last_person:
|
||||
self.last_person = person_name
|
||||
self.person_timestamp = time.time()
|
||||
|
||||
def get_time_period(self) -> str:
|
||||
"""Return time period: morning, afternoon, evening, or night."""
|
||||
hour = datetime.now().hour
|
||||
if 5 <= hour < 12:
|
||||
return "morning"
|
||||
elif 12 <= hour < 17:
|
||||
return "afternoon"
|
||||
elif 17 <= hour < 21:
|
||||
return "evening"
|
||||
else:
|
||||
return "night"
|
||||
|
||||
|
||||
class TtsPersonalityNode(Node):
|
||||
"""TTS personality engine with context-aware responses."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("tts_personality")
|
||||
|
||||
# Personality parameters
|
||||
self.declare_parameter("personality_name", "Luna")
|
||||
self.declare_parameter("enable_context_aware", True)
|
||||
|
||||
# Emotion-based rate/pitch modulation
|
||||
self.declare_parameter("speed_happy", 1.1)
|
||||
self.declare_parameter("speed_sad", 0.9)
|
||||
self.declare_parameter("speed_neutral", 1.0)
|
||||
self.declare_parameter("pitch_happy", 1.15)
|
||||
self.declare_parameter("pitch_sad", 0.85)
|
||||
self.declare_parameter("pitch_neutral", 1.0)
|
||||
|
||||
# Greeting templates (by time of day)
|
||||
self.declare_parameter("greetings_morning", [
|
||||
"Good morning! I hope you slept well.",
|
||||
"Rise and shine! Ready for a great day?",
|
||||
"Morning! Let's get moving.",
|
||||
])
|
||||
self.declare_parameter("greetings_afternoon", [
|
||||
"Afternoon! How's it going?",
|
||||
"Hey there! Hope your day is great so far.",
|
||||
"Afternoon check-in! Everything okay?",
|
||||
])
|
||||
self.declare_parameter("greetings_evening", [
|
||||
"Good evening! Time to wind down?",
|
||||
"Evening! Getting ready for tomorrow?",
|
||||
"Hi! How was your day?",
|
||||
])
|
||||
self.declare_parameter("greetings_night", [
|
||||
"Burning the midnight oil?",
|
||||
"Still awake? Sweet dreams coming soon?",
|
||||
"Night owl mode activated!",
|
||||
])
|
||||
|
||||
# Known people for personalization
|
||||
self.declare_parameter("known_people", {})
|
||||
|
||||
# Load parameters
|
||||
self._personality_name = self.get_parameter("personality_name").value
|
||||
self._context_aware = self.get_parameter("enable_context_aware").value
|
||||
self._speed_happy = self.get_parameter("speed_happy").value
|
||||
self._speed_sad = self.get_parameter("speed_sad").value
|
||||
self._speed_neutral = self.get_parameter("speed_neutral").value
|
||||
self._pitch_happy = self.get_parameter("pitch_happy").value
|
||||
self._pitch_sad = self.get_parameter("pitch_sad").value
|
||||
self._pitch_neutral = self.get_parameter("pitch_neutral").value
|
||||
self._greetings_morning = self.get_parameter("greetings_morning").value
|
||||
self._greetings_afternoon = self.get_parameter("greetings_afternoon").value
|
||||
self._greetings_evening = self.get_parameter("greetings_evening").value
|
||||
self._greetings_night = self.get_parameter("greetings_night").value
|
||||
self._known_people = self.get_parameter("known_people").value
|
||||
|
||||
# Personality context
|
||||
self._context = PersonalityContext()
|
||||
self._context_lock = threading.Lock()
|
||||
|
||||
# Priority levels for queue
|
||||
self.PRIORITY_SAFETY = 3 # Emergency/safety messages
|
||||
self.PRIORITY_SOCIAL = 2 # Social interactions, greetings
|
||||
self.PRIORITY_IDLE = 1 # Idle chatter, commentary
|
||||
self.PRIORITY_NORMAL = 0 # Default
|
||||
|
||||
# Queue for TTS requests with priority
|
||||
self._tts_queue = queue.PriorityQueue(maxsize=16)
|
||||
self._state = "idle"
|
||||
self._state_lock = threading.Lock()
|
||||
|
||||
# QoS profile
|
||||
qos = QoSProfile(depth=5)
|
||||
|
||||
# Subscribers
|
||||
self.create_subscription(
|
||||
String, "/saltybot/tts_request",
|
||||
self._on_tts_request, qos)
|
||||
|
||||
self.create_subscription(
|
||||
String, "/saltybot/emotion_state",
|
||||
self._on_emotion_update, qos)
|
||||
|
||||
self.create_subscription(
|
||||
String, "/saltybot/person_detected",
|
||||
self._on_person_detected, qos)
|
||||
|
||||
# Publishers
|
||||
self._tts_command_pub = self.create_publisher(
|
||||
String, "/saltybot/tts_command", qos)
|
||||
|
||||
self._personality_state_pub = self.create_publisher(
|
||||
String, "/saltybot/personality_state", qos)
|
||||
|
||||
# Worker thread for processing queue
|
||||
self._worker_thread = threading.Thread(
|
||||
target=self._worker_loop, daemon=True)
|
||||
self._worker_thread.start()
|
||||
|
||||
self.get_logger().info(
|
||||
f"TTS Personality Engine '{self._personality_name}' started. "
|
||||
f"Context-aware: {self._context_aware}")
|
||||
|
||||
def _on_tts_request(self, msg: String) -> None:
|
||||
"""Handle incoming TTS request."""
|
||||
try:
|
||||
priority = self.PRIORITY_NORMAL
|
||||
text = msg.data
|
||||
|
||||
# Determine priority based on keywords
|
||||
if any(w in text.lower() for w in ["safety", "stop", "danger", "emergency"]):
|
||||
priority = self.PRIORITY_SAFETY
|
||||
elif any(w in text.lower() for w in ["hello", "hey", "hi", "greeting"]):
|
||||
priority = self.PRIORITY_SOCIAL
|
||||
|
||||
# Add to queue with timestamp for FIFO within priority
|
||||
item = (priority, time.time(), text)
|
||||
self._tts_queue.put(item, block=False)
|
||||
|
||||
except queue.Full:
|
||||
self.get_logger().warn("TTS queue full, dropping request")
|
||||
|
||||
def _on_emotion_update(self, msg: String) -> None:
|
||||
"""Handle emotion state updates."""
|
||||
with self._context_lock:
|
||||
self._context.update_emotion(msg.data)
|
||||
self.get_logger().debug(f"Emotion updated: {msg.data}")
|
||||
|
||||
def _on_person_detected(self, msg: String) -> None:
|
||||
"""Handle person detection events."""
|
||||
with self._context_lock:
|
||||
self._context.update_person(msg.data)
|
||||
self.get_logger().debug(f"Person detected: {msg.data}")
|
||||
|
||||
def _apply_personality(self, text: str) -> str:
|
||||
"""
|
||||
Apply personality modifications to text.
|
||||
Returns: modified text with personality context.
|
||||
"""
|
||||
with self._context_lock:
|
||||
emotion = self._context.current_emotion
|
||||
person = self._context.last_person
|
||||
time_period = self._context.get_time_period()
|
||||
|
||||
# Don't modify safety-critical messages
|
||||
if any(w in text.lower() for w in ["stop", "danger", "emergency"]):
|
||||
return text
|
||||
|
||||
# Add personalization based on context
|
||||
modified_text = text
|
||||
if self._context_aware:
|
||||
# Add person name if recently detected
|
||||
if person and (time.time() - self._context.person_timestamp < 30):
|
||||
modified_text = f"{person}, {modified_text}"
|
||||
|
||||
return modified_text
|
||||
|
||||
def _get_emotion_modulation(self) -> tuple[float, float]:
|
||||
"""
|
||||
Get speed and pitch multipliers based on emotion.
|
||||
Returns: (speed_multiplier, pitch_multiplier)
|
||||
"""
|
||||
with self._context_lock:
|
||||
emotion = self._context.current_emotion
|
||||
|
||||
if emotion == "happy":
|
||||
return (self._speed_happy, self._pitch_happy)
|
||||
elif emotion == "sad":
|
||||
return (self._speed_sad, self._pitch_sad)
|
||||
else: # neutral
|
||||
return (self._speed_neutral, self._pitch_neutral)
|
||||
|
||||
def _format_tts_command(self, text: str) -> str:
|
||||
"""
|
||||
Format text with personality parameters for TTS service.
|
||||
Returns: JSON-formatted TTS command with rate/pitch modulation.
|
||||
"""
|
||||
speed, pitch = self._get_emotion_modulation()
|
||||
|
||||
# Format as JSON-compatible string for tts_service
|
||||
# tts_service will parse: {"text": "...", "speed": 1.1, "pitch": 1.15}
|
||||
import json
|
||||
command = {
|
||||
"text": text,
|
||||
"speed": float(speed),
|
||||
"pitch": float(pitch),
|
||||
"personality": self._personality_name,
|
||||
"emotion": self._context.current_emotion,
|
||||
}
|
||||
return json.dumps(command)
|
||||
|
||||
def _update_state(self, new_state: str) -> None:
|
||||
"""Update and publish personality state."""
|
||||
with self._state_lock:
|
||||
if new_state != self._state:
|
||||
self._state = new_state
|
||||
state_msg = String(data=f"{new_state}:{self._personality_name}:{self._context.current_emotion}")
|
||||
self._personality_state_pub.publish(state_msg)
|
||||
|
||||
def _worker_loop(self) -> None:
|
||||
"""Worker thread loop to process TTS queue."""
|
||||
while rclpy.ok():
|
||||
try:
|
||||
# Get next item from queue (blocking, 1s timeout)
|
||||
priority, timestamp, text = self._tts_queue.get(timeout=1.0)
|
||||
|
||||
self._update_state("processing")
|
||||
|
||||
# Apply personality modifications
|
||||
personalized_text = self._apply_personality(text)
|
||||
|
||||
# Format with emotion modulation
|
||||
tts_command = self._format_tts_command(personalized_text)
|
||||
|
||||
# Publish to TTS service
|
||||
cmd_msg = String(data=tts_command)
|
||||
self._tts_command_pub.publish(cmd_msg)
|
||||
|
||||
self.get_logger().info(
|
||||
f"[{priority}] Speaking: {personalized_text[:60]}...")
|
||||
|
||||
# Small delay to avoid queue hammering
|
||||
time.sleep(0.1)
|
||||
self._update_state("idle")
|
||||
|
||||
except queue.Empty:
|
||||
self._update_state("idle")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Worker error: {e}")
|
||||
|
||||
def _generate_greeting(self) -> str:
|
||||
"""Generate context-aware greeting."""
|
||||
with self._context_lock:
|
||||
time_period = self._context.get_time_period()
|
||||
|
||||
greetings_map = {
|
||||
"morning": self._greetings_morning,
|
||||
"afternoon": self._greetings_afternoon,
|
||||
"evening": self._greetings_evening,
|
||||
"night": self._greetings_night,
|
||||
}
|
||||
|
||||
greetings = greetings_map.get(time_period, self._greetings_afternoon)
|
||||
if greetings:
|
||||
return random.choice(greetings)
|
||||
return "Hello!"
|
||||
|
||||
|
||||
def main(args=None):
|
||||
"""Main entry point."""
|
||||
rclpy.init(args=args)
|
||||
node = TtsPersonalityNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -0,0 +1,57 @@
|
||||
"""
|
||||
test_tts_personality.py — Tests for TTS personality engine
|
||||
"""
|
||||
|
||||
import pytest
|
||||
from saltybot_tts_personality.tts_personality_node import PersonalityContext
|
||||
from datetime import datetime
|
||||
|
||||
|
||||
def test_personality_context_emotion():
|
||||
"""Test emotion state management."""
|
||||
context = PersonalityContext()
|
||||
assert context.current_emotion == "neutral"
|
||||
|
||||
context.update_emotion("happy")
|
||||
assert context.current_emotion == "happy"
|
||||
|
||||
context.update_emotion("sad")
|
||||
assert context.current_emotion == "sad"
|
||||
|
||||
|
||||
def test_personality_context_person():
|
||||
"""Test person tracking."""
|
||||
context = PersonalityContext()
|
||||
assert context.last_person is None
|
||||
|
||||
context.update_person("Alice")
|
||||
assert context.last_person == "Alice"
|
||||
|
||||
context.update_person("Bob")
|
||||
assert context.last_person == "Bob"
|
||||
|
||||
|
||||
def test_time_period_classification():
|
||||
"""Test time period classification."""
|
||||
context = PersonalityContext()
|
||||
|
||||
# These tests depend on current time, so we just verify the function works
|
||||
period = context.get_time_period()
|
||||
assert period in ["morning", "afternoon", "evening", "night"]
|
||||
|
||||
|
||||
def test_personality_context_timestamps():
|
||||
"""Test timestamp tracking for context validity."""
|
||||
context = PersonalityContext()
|
||||
import time
|
||||
|
||||
ts_before = context.emotion_timestamp
|
||||
time.sleep(0.01)
|
||||
context.update_emotion("happy")
|
||||
ts_after = context.emotion_timestamp
|
||||
|
||||
assert ts_after > ts_before
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
pytest.main([__file__, "-v"])
|
||||
9
jetson/ros2_ws/src/saltybot_voice_router/.gitignore
vendored
Normal file
9
jetson/ros2_ws/src/saltybot_voice_router/.gitignore
vendored
Normal file
@ -0,0 +1,9 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
*.egg-info/
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
.pytest_cache/
|
||||
.DS_Store
|
||||
@ -0,0 +1,54 @@
|
||||
# Voice Command Router (Issue #491)
|
||||
|
||||
Natural language voice command routing with fuzzy matching.
|
||||
|
||||
## Supported Commands
|
||||
- Follow me / come with me
|
||||
- Stop / halt / freeze
|
||||
- Go home / return to dock / charge
|
||||
- Patrol / autonomous mode
|
||||
- Come here / approach
|
||||
- Sit / sit down
|
||||
- Spin / rotate
|
||||
- Dance / groove
|
||||
- Take photo / picture / smile
|
||||
- What's that / identify / recognize
|
||||
- Battery status / battery level
|
||||
|
||||
## Features
|
||||
- **Fuzzy Matching**: Tolerates speech variations using rapidfuzz
|
||||
- **Multiple Patterns**: Each command has multiple recognition patterns
|
||||
- **Three Action Types**:
|
||||
- Velocity commands (stop → /cmd_vel)
|
||||
- Action commands (patrol → /saltybot/action_command)
|
||||
- Service calls (photo → /photo/capture)
|
||||
|
||||
## Architecture
|
||||
```
|
||||
/saltybot/speech/transcribed_text
|
||||
↓
|
||||
[VoiceCommandRouter]
|
||||
↓ Fuzzy Match (threshold: 75%)
|
||||
↓
|
||||
/cmd_vel | /saltybot/action_command | Services
|
||||
```
|
||||
|
||||
## Launch
|
||||
```bash
|
||||
ros2 launch saltybot_voice_router voice_router.launch.py
|
||||
```
|
||||
|
||||
## Test
|
||||
```bash
|
||||
ros2 topic pub /saltybot/speech/transcribed_text std_msgs/String '{data: "follow me"}'
|
||||
```
|
||||
|
||||
## Topics
|
||||
- **Subscribe**: `/saltybot/speech/transcribed_text` (std_msgs/String)
|
||||
- **Publish**: `/saltybot/action_command` (std_msgs/String)
|
||||
- **Publish**: `/saltybot/voice_command` (std_msgs/String)
|
||||
- **Publish**: `/cmd_vel` (geometry_msgs/Twist)
|
||||
|
||||
## Dependencies
|
||||
- `rapidfuzz`: Fuzzy string matching
|
||||
- `rclpy`: ROS2 Python client
|
||||
@ -0,0 +1,13 @@
|
||||
"""Voice Command Router Launch (Issue #491)"""
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='saltybot_voice_router',
|
||||
executable='voice_command_router',
|
||||
name='voice_command_router',
|
||||
output='screen',
|
||||
),
|
||||
])
|
||||
20
jetson/ros2_ws/src/saltybot_voice_router/package.xml
Normal file
20
jetson/ros2_ws/src/saltybot_voice_router/package.xml
Normal file
@ -0,0 +1,20 @@
|
||||
<?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_router</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Voice command router with fuzzy matching (Issue #491)</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>MIT</license>
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>rapidfuzz</depend>
|
||||
<exec_depend>python3-launch-ros</exec_depend>
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1 @@
|
||||
# SaltyBot Voice Router
|
||||
@ -0,0 +1,127 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Voice Command Router (Issue #491) - Fuzzy matching for natural voice commands"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from geometry_msgs.msg import Twist
|
||||
from rapidfuzz import fuzz
|
||||
import json
|
||||
|
||||
|
||||
class VoiceCommandRouter(Node):
|
||||
"""Routes voice commands with fuzzy matching to handle natural speech variations."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('voice_command_router')
|
||||
|
||||
# Define voice commands with patterns for fuzzy matching
|
||||
self.commands = {
|
||||
'follow': {
|
||||
'patterns': ['follow me', 'follow me please', 'come with me', 'follow'],
|
||||
'type': 'action', 'action': 'follow_person'
|
||||
},
|
||||
'stop': {
|
||||
'patterns': ['stop', 'halt', 'hold', 'pause', 'freeze'],
|
||||
'type': 'velocity', 'linear': 0.0, 'angular': 0.0
|
||||
},
|
||||
'go_home': {
|
||||
'patterns': ['go home', 'return home', 'back home', 'go to dock', 'charge'],
|
||||
'type': 'action', 'action': 'return_home'
|
||||
},
|
||||
'patrol': {
|
||||
'patterns': ['patrol', 'start patrol', 'patrol mode', 'autonomous'],
|
||||
'type': 'action', 'action': 'start_patrol'
|
||||
},
|
||||
'come_here': {
|
||||
'patterns': ['come here', 'come to me', 'approach', 'get closer'],
|
||||
'type': 'action', 'action': 'approach_person'
|
||||
},
|
||||
'sit': {
|
||||
'patterns': ['sit', 'sit down', 'sit please'],
|
||||
'type': 'action', 'action': 'sit'
|
||||
},
|
||||
'spin': {
|
||||
'patterns': ['spin', 'spin around', 'rotate', 'turn around'],
|
||||
'type': 'action', 'action': 'spin'
|
||||
},
|
||||
'dance': {
|
||||
'patterns': ['dance', 'dance with me', 'do a dance'],
|
||||
'type': 'action', 'action': 'dance'
|
||||
},
|
||||
'photo': {
|
||||
'patterns': ['take a photo', 'take picture', 'photo', 'take photo', 'smile'],
|
||||
'type': 'service', 'service': '/photo/capture'
|
||||
},
|
||||
'identify': {
|
||||
'patterns': ['whats that', 'what is that', 'identify', 'recognize'],
|
||||
'type': 'action', 'action': 'identify_object'
|
||||
},
|
||||
'battery': {
|
||||
'patterns': ['battery status', 'battery level', 'check battery', 'whats my battery'],
|
||||
'type': 'action', 'action': 'report_battery'
|
||||
},
|
||||
}
|
||||
|
||||
# Subscriptions and publishers
|
||||
self.create_subscription(String, '/saltybot/speech/transcribed_text', self.transcription_cb, 10)
|
||||
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
|
||||
self.action_pub = self.create_publisher(String, '/saltybot/action_command', 10)
|
||||
self.voice_cmd_pub = self.create_publisher(String, '/saltybot/voice_command', 10)
|
||||
|
||||
self.get_logger().info('Voice command router ready')
|
||||
|
||||
def transcription_cb(self, msg):
|
||||
"""Process transcribed text and route command."""
|
||||
text = msg.data.lower().strip()
|
||||
self.get_logger().debug(f'Transcribed: "{text}"')
|
||||
|
||||
# Fuzzy match against known commands
|
||||
best_cmd = None
|
||||
best_score = 0
|
||||
|
||||
for cmd_name, cmd_info in self.commands.items():
|
||||
for pattern in cmd_info['patterns']:
|
||||
score = fuzz.token_set_ratio(text, pattern)
|
||||
if score > best_score and score >= 75:
|
||||
best_score = score
|
||||
best_cmd = (cmd_name, cmd_info)
|
||||
|
||||
if best_cmd:
|
||||
self.get_logger().info(f'Matched: {best_cmd[0]} (score: {best_score})')
|
||||
self.execute_command(best_cmd[0], best_cmd[1])
|
||||
else:
|
||||
self.get_logger().warning(f'No match for: "{text}"')
|
||||
|
||||
def execute_command(self, cmd_name, cmd_info):
|
||||
"""Execute the matched voice command."""
|
||||
if cmd_info['type'] == 'velocity':
|
||||
# Stop command - publish Twist
|
||||
msg = Twist()
|
||||
msg.linear.x = cmd_info.get('linear', 0.0)
|
||||
msg.angular.z = cmd_info.get('angular', 0.0)
|
||||
self.cmd_vel_pub.publish(msg)
|
||||
|
||||
elif cmd_info['type'] == 'action':
|
||||
# Action command
|
||||
msg = String(data=json.dumps({'action': cmd_info['action']}))
|
||||
self.action_pub.publish(msg)
|
||||
|
||||
elif cmd_info['type'] == 'service':
|
||||
# Service call (simplified - would need proper async handling)
|
||||
pass
|
||||
|
||||
# Publish to voice_command topic for monitoring
|
||||
self.voice_cmd_pub.publish(String(data=cmd_name))
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = VoiceCommandRouter()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltybot_voice_router/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_voice_router/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_voice_router
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_voice_router
|
||||
22
jetson/ros2_ws/src/saltybot_voice_router/setup.py
Normal file
22
jetson/ros2_ws/src/saltybot_voice_router/setup.py
Normal file
@ -0,0 +1,22 @@
|
||||
from setuptools import setup
|
||||
|
||||
setup(
|
||||
name='saltybot_voice_router',
|
||||
version='0.1.0',
|
||||
packages=['saltybot_voice_router'],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/saltybot_voice_router']),
|
||||
('share/saltybot_voice_router', ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools', 'rapidfuzz'],
|
||||
zip_safe=True,
|
||||
maintainer='seb',
|
||||
maintainer_email='seb@vayrette.com',
|
||||
description='Voice command router with fuzzy matching (Issue #491)',
|
||||
license='MIT',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'voice_command_router = saltybot_voice_router.voice_router_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
Loading…
x
Reference in New Issue
Block a user