diff --git a/camera_ros/CMakeLists.txt b/camera_ros/CMakeLists.txt index 7da345f..e6ed2ce 100644 --- a/camera_ros/CMakeLists.txt +++ b/camera_ros/CMakeLists.txt @@ -5,22 +5,59 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +# Find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Generate Python classes for the custom service +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/GetInt.srv" + DEPENDENCIES std_msgs + ADD_LINTER_TESTS +) + +# Install Python scripts +install(PROGRAMS + scripts/camera_publisher.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +# Install launch files +install(FILES + launch/camera.launch.py + DESTINATION share/${PROJECT_NAME}/launch +) + +# Install package.xml +install(FILES + package.xml + DESTINATION share/${PROJECT_NAME}/ +) + +# Install README +install(FILES + README.md + DESTINATION share/${PROJECT_NAME}/ +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() +# Export the generated interfaces +ament_export_dependencies(rosidl_default_runtime) + ament_package() diff --git a/camera_ros/README.md b/camera_ros/README.md new file mode 100644 index 0000000..bc5224c --- /dev/null +++ b/camera_ros/README.md @@ -0,0 +1,207 @@ +# Zero-Copy MJPEG Camera Publisher for ROS2 + +A high-performance camera publisher optimized for NVIDIA Jetson AGX Orin that provides zero-copy MJPEG streaming with minimal CPU overhead. + +## Features + +- 🚀 **Zero-copy MJPEG streaming** - No CPU transcoding overhead +- ⚡ **High performance** - Optimized for NVIDIA Jetson AGX Orin +- 🎯 **Smart frame rate control** - Dynamic FPS adjustment +- 🔄 **Automatic client management** - Activates when clients detected, pauses when none +- 📡 **Client count monitoring** - Listens to `/client_count` topic for automatic control +- 🛠️ **GStreamer pipeline** - Hardware-accelerated video processing + +## Requirements + +- NVIDIA Jetson AGX Orin (tested on 64GB model) +- ROS2 Humble +- Python 3.10+ +- OpenCV with GStreamer support +- GStreamer 1.0+ + +## Usage + +### Prerequisites + +**Source the ROS environment:** +```bash +# Navigate to your workspace +cd /home/dev/lucy_ws + +# Source ROS Humble +source /opt/ros/humble/setup.zsh + +# Source your workspace +source install/setup.zsh +``` + +### Launch Methods + +**Basic launch (camera activates automatically when clients connect):** +```bash +ros2 launch camera_ros camera.launch.py +``` + +**Launch with custom parameters:** +```bash +ros2 launch camera_ros camera.launch.py fps:=20.0 device:=/dev/video0 +``` + +### Automatic Client Management + +The camera automatically activates and deactivates based on client count: + +**Client count monitoring:** +- Camera **activates** when `client_count > 0` +- Camera **pauses** when `client_count = 0` +- Monitors `/client_count` topic for automatic control + +### Topics + +**Published topics:** +- `/camera/mobius/jpg` - CompressedImage (JPEG format, published only when clients are present) + +**Subscribed topics:** +- `/client_count` - Int32 (monitors client count for automatic activation/deactivation) + +## Configuration + +### Camera Settings + +Edit `src/camera_publisher.py` to modify: + +```python +# Frame rate (1.0 - 30.0 FPS) +FPS = 10.0 + +# GStreamer pipeline +GST_PIPELINE = ( + "v4l2src device=/dev/video0 ! " + "image/jpeg,width=1280,height=720,framerate=30/1 ! " + "jpegparse ! " + "appsink drop=true emit-signals=true sync=false" +) +``` + +### Resolution Options + +For different resolutions, modify the GStreamer pipeline: + +```python +# 640x480 (lower CPU usage) +GST_PIPELINE = "v4l2src device=/dev/video0 ! image/jpeg,width=640,height=480,framerate=30/1 ! jpegparse ! appsink drop=true emit-signals=true sync=false" + +# 1920x1080 (higher quality, more CPU) +GST_PIPELINE = "v4l2src device=/dev/video0 ! image/jpeg,width=1920,height=1080,framerate=30/1 ! jpegparse ! appsink drop=true emit-signals=true sync=false" +``` + +## Performance Optimization + +### System-Level Optimizations + +1. **Set CPU governor to performance mode:** + ```bash + sudo jetson_clocks + ``` + +2. **Check camera device:** + ```bash + ls /dev/video* + v4l2-ctl --device=/dev/video0 --list-formats-ext + ``` + +3. **Monitor performance:** + ```bash + # Monitor CPU usage + htop + + # Monitor Jetson stats + tegrastats + + # Check frame rate + ros2 topic hz /camera/mobius/jpg + ``` + +### Troubleshooting + +#### Common Issues + +**Camera not found:** +```bash +# Check USB devices +lsusb + +# Check video devices +ls /dev/video* + +# Check camera capabilities +v4l2-ctl --list-devices +v4l2-ctl --device=/dev/video0 --list-formats-ext + +# Reload camera driver +sudo modprobe uvcvideo +``` + +**High CPU usage:** +- Reduce frame rate to 10-15 FPS +- Use lower resolution (640x480) +- Check for background processes +- Ensure CPU is in performance mode +- Use `jetson_clocks` for maximum performance + +**ExternalShutdownException error:** +This usually occurs when the node is terminated externally. To prevent this: +```bash +# Use Ctrl+C to stop the node gracefully +# Or use the service to disable camera first +ros2 service call /camera/mobius/enable std_srvs/srv/SetBool "{data: false}" +``` + +## Architecture + +``` +Camera (/dev/video0) + ↓ +GStreamer Pipeline (v4l2src → jpegparse → appsink) + ↓ +OpenCV VideoCapture (zero-copy) + ↓ +ROS2 CompressedImage Publisher + ↓ +Client Applications +``` + +## API Reference + +### CameraPublisher Class + +**Methods:** +- `set_fps(fps)` - Set frame rate (1.0-30.0) +- `set_active(active)` - Enable/disable camera + +**Parameters:** +- `fps` - Target frame rate +- `device` - Camera device path + +## Contributing + +1. Fork the repository +2. Create a feature branch +3. Make your changes +4. Test on Jetson AGX Orin +5. Submit a pull request + +## License + +This project is licensed under the GNU General Public License v3.0 - see the LICENSE file for details. + +## Authors + +- **Sentience Robotics Team** - [contact@sentience-robotics.fr](mailto:contact@sentience-robotics.fr) + +## Support + +For issues and questions: +- Check the troubleshooting section +- Open an issue on GitHub +- Contact the development team diff --git a/camera_ros/camera_ros/__init__.py b/camera_ros/camera_ros/__init__.py new file mode 100644 index 0000000..c9b4174 --- /dev/null +++ b/camera_ros/camera_ros/__init__.py @@ -0,0 +1,21 @@ +# Copyright 2024 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Camera ROS Package. + +Zero-copy MJPEG camera publisher optimized for NVIDIA Jetson AGX Orin.""" + +__version__ = "1.0.0" diff --git a/camera_ros/launch/camera.launch.py b/camera_ros/launch/camera.launch.py new file mode 100644 index 0000000..f718b68 --- /dev/null +++ b/camera_ros/launch/camera.launch.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +# Copyright 2024 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Launch file for zero-copy MJPEG camera publisher. + +This launch file starts the camera node with optimized settings for NVIDIA Jetson AGX Orin.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + # Launch arguments + fps_arg = DeclareLaunchArgument( + 'fps', + default_value='15.0', + description='Camera frame rate (1.0-30.0 FPS)' + ) + + device_arg = DeclareLaunchArgument( + 'device', + default_value='/dev/video0', + description='Camera device path' + ) + + # Camera node + camera_node = Node( + package='camera_ros', + executable='camera_publisher.py', + name='camera_publisher', + output='screen', + parameters=[{ + 'fps': LaunchConfiguration('fps'), + 'device': LaunchConfiguration('device'), + }], + ) + + return LaunchDescription([ + fps_arg, + device_arg, + LogInfo(msg='Starting zero-copy MJPEG camera publisher with automatic client-based activation'), + camera_node, + ]) diff --git a/camera_ros/package.xml b/camera_ros/package.xml index 8d5ec4c..14c8b35 100644 --- a/camera_ros/package.xml +++ b/camera_ros/package.xml @@ -2,17 +2,36 @@ camera_ros - 0.0.0 - TODO: Package description - dev - TODO: License declaration - + 1.0.0 + Zero-copy MJPEG camera publisher optimized for NVIDIA Jetson AGX Orin + + Sentience Robotics Team + GPL-3.0 + + Sentience Robotics Team + + ament_cmake - + ament_cmake_python + + rclpy + sensor_msgs + std_srvs + std_msgs + cv_bridge + rosidl_default_generators + rosidl_default_runtime + + python3-opencv + gstreamer1.0-tools + gstreamer1.0-plugins-good + ament_lint_auto ament_lint_common - + + rosidl_interface_packages + ament_cmake - + \ No newline at end of file diff --git a/camera_ros/scripts/__init__.py b/camera_ros/scripts/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/camera_ros/scripts/camera_publisher.py b/camera_ros/scripts/camera_publisher.py new file mode 100644 index 0000000..5291095 --- /dev/null +++ b/camera_ros/scripts/camera_publisher.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +# Copyright 2024 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CompressedImage +from std_msgs.msg import Int32 +from camera_ros.srv import GetInt +import cv2 + +FPS = 15.0 +CAMERA_DEVICE = "/dev/video0" + +class CameraPublisher(Node): + """ + Zero-copy MJPEG camera publisher using GStreamer pipeline. + + Publishes compressed JPEG images from camera with zero CPU transcoding overhead. + Automatically activates when clients are detected and pauses when no clients are present. + Listens to /client_count topic for automatic activation/deactivation.""" + + def __init__(self): + super().__init__('sensor_interface') + + self.declare_parameter('fps', FPS) + self.declare_parameter('device', CAMERA_DEVICE) + + # Get parameter values + self.target_fps = self.get_parameter('fps').get_parameter_value().double_value + self.camera_device = self.get_parameter('device').get_parameter_value().string_value + + self.jpg_pub = self.create_publisher(CompressedImage, 'camera/mobius/jpg', 10) + self.active = False + self.client_count = 0 + + self.init_cap() + + self.last_publish_time = 0.0 + self.frame_skip_counter = 0 + + self.timer = self.create_timer(0.01, self.publish_frame) # 100Hz timer + self.get_logger().info(f"Camera publisher node started using device {self.camera_device} at {self.target_fps} FPS") + + + self.create_subscription(Int32, '/client_count', self.client_count_callback, 10) + + self.create_service(GetInt, 'get_client_count', self.get_client_count) + + def get_client_count(self, request, response): + response.value = self.client_count + return response + + def client_count_callback(self, msg): + """Automatically activate/deactivate camera based on client count""" + self.client_count = msg.data + + if self.client_count == 0 and self.active: + self.active = False + self.get_logger().info(f"No clients detected - pausing camera on {self.camera_device}") + elif self.client_count > 0 and not self.active: + self.active = True + self.get_logger().info(f"{self.client_count} client(s) detected - activating camera on {self.camera_device}") + elif self.client_count > 0 and self.active: + self.get_logger().debug(f"Camera on {self.camera_device} active with {self.client_count} client(s)") + + def set_fps(self, fps): + self.target_fps = max(1.0, min(30.0, fps)) + self.get_logger().info(f"Frame rate set to {self.target_fps} FPS") + + def init_cap(self): + # Create GStreamer pipeline with parameterized device + gst_pipeline = ( + f"v4l2src device={self.camera_device} ! " + "image/jpeg,width=1280,height=720,framerate=30/1 ! " + "jpegparse ! " + "appsink drop=true emit-signals=true sync=false" + ) + + self.cap = cv2.VideoCapture(gst_pipeline, cv2.CAP_GSTREAMER) + + if not self.cap.isOpened(): + self.get_logger().error(f"Failed to open camera on {self.camera_device} with GStreamer pipeline") + return + else: + self.get_logger().info(f"Camera on {self.camera_device} opened successfully") + + # Optimize OpenCV settings for Jetson + try: + self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG')) + except: + pass + + def publish_frame(self): + if not self.active: + return + + current_time = self.get_clock().now().nanoseconds / 1e9 + if current_time - self.last_publish_time < (1.0 / self.target_fps): + return + + ret, frame = self.cap.read() + if not ret or frame is None: + self.get_logger().error(f"Frame read failed on {self.camera_device}, reconnecting camera...") + self.cap.release() + self.init_cap() + return + + if frame.shape[0] == 1: # Raw JPEG data + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.format = 'jpeg' + msg.data = frame.flatten().tobytes() + self.jpg_pub.publish(msg) + + self.last_publish_time = current_time + else: + self.get_logger().error(f"Unexpected frame shape: {frame.shape}") + +def main(args=None): + rclpy.init(args=args) + node = CameraPublisher() + + rclpy.spin(node) + +if __name__ == '__main__': + main() diff --git a/camera_ros/src/publish_jpg.py b/camera_ros/src/publish_jpg.py deleted file mode 100755 index 782e596..0000000 --- a/camera_ros/src/publish_jpg.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image, CameraInfo, CompressedImage -from std_srvs.srv import SetBool -from std_msgs.msg import Int32 -from cv_bridge import CvBridge -import cv2 -import signal -import sys - -FPS = 10.0 -CAMERA_RESOLUTION = (640, 480) -CAMERA_ENDPOINT = '/dev/video0' - -# Trying to get hardware acceleration with GStreamer (NVIDIA Jetson) - not working yet -# GST_PIPELINE = ( -# "v4l2src device=/dev/video0 ! " -# "image/jpeg,width=1280,height=720,framerate=30/1 ! " -# "nvjpegdec ! " -# "nvvidconv ! " -# "video/x-raw,format=BGRx ! " -# "videoconvert ! " -# "video/x-raw,format=BGR ! " -# "appsink drop=true" -# ) - -class CameraPublisher(Node): - def __init__(self): - super().__init__('sensor_interface') - # self.pub = self.create_publisher(Image, 'camera/mobius/raw', 10) - self.jpg_pub = self.create_publisher(CompressedImage, 'camera/mobius/jpg', 10) - self.bridge = CvBridge() - self.active = False - self.init_cap() - self.timer = self.create_timer(1/FPS, self.publish_frame) - self.enable_srv = self.create_service(SetBool, 'camera/mobius/enable', self.set_active) - self.get_logger().info("Camera publisher node started.") - - # Pausing camera when no more ros bridge clients - self.cli = self.create_client(SetBool, 'camera/mobius/enable') - while not self.cli.wait_for_service(timeout_sec=10.0): - self.get_logger().warn('Waiting for camera/mobius/enable service...') - self.create_subscription(Int32, '/client_count', self.client_count_callback, 10) - - def client_count_callback(self, msg): - count = msg.data - - if count == 0: - self.get_logger().info('No clients detected — pausing camera.') - self.pause_camera() - - def pause_camera(self): - req = SetBool.Request() - req.data = False - future = self.cli.call_async(req) - future.add_done_callback(self.service_response_callback) - - def service_response_callback(self, future): - try: - response = future.result() - if response.success: - self.get_logger().info(f"Camera paused successfully: {response.message}") - else: - self.get_logger().warn(f"Camera pause failed: {response.message}") - except Exception as e: - self.get_logger().error(f"Service call failed: {e}") - - def set_active(self, request, response): - self.active = request.data - response.success = True - response.message = f"Camera {'enabled' if self.active else 'disabled'}" - self.get_logger().info(response.message) - return response - - def init_cap(self): - # self.cap = cv2.VideoCapture(GST_PIPELINE, cv2.CAP_GSTREAMER) - - self.cap = cv2.VideoCapture(CAMERA_ENDPOINT) - self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_RESOLUTION[0]) - self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_RESOLUTION[1]) - self.cap.set(cv2.CAP_PROP_FPS, FPS) - # self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG')) - - if not self.cap.isOpened(): - self.get_logger().error(f"Failed to open camera at {CAMERA_ENDPOINT}") - else: - self.get_logger().info(f"Camera opened successfully at {CAMERA_ENDPOINT}") - - def publish_frame(self): - if not self.active: - return - ret, frame = self.cap.read() - if not ret or frame is None: - self.get_logger().warn("Frame empty, reconnecting camera...") - self.cap.release() - self.init_cap() - return - - # Raw image - # msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') - # self.pub.publish(msg) - - # Encode as JPEG - ret, jpg = cv2.imencode('.jpg', frame) - if not ret: - self.get_logger().warn("Failed to encode frame as JPEG.") - return - - msg = CompressedImage() - msg.format = 'jpeg' - msg.data = jpg.tobytes() - self.jpg_pub.publish(msg) - -def main(args=None): - rclpy.init(args=args) - node = CameraPublisher() - - rclpy.spin(node) - -if __name__ == '__main__': - main() - diff --git a/camera_ros/srv/GetInt.srv b/camera_ros/srv/GetInt.srv new file mode 100644 index 0000000..92f2246 --- /dev/null +++ b/camera_ros/srv/GetInt.srv @@ -0,0 +1,2 @@ +--- +int32 value diff --git a/lucy_bringup/AUDIO.md b/lucy_bringup/AUDIO.md new file mode 100644 index 0000000..b793675 --- /dev/null +++ b/lucy_bringup/AUDIO.md @@ -0,0 +1,793 @@ +# Audio System Documentation - Lucy Robot + +**Version:** 1.0.0 +**Last Updated:** December 2024 +**Platform:** NVIDIA Jetson AGX Orin, Ubuntu 22.04 LTS, ROS2 Humble + +--- + +## Table of Contents + +1. [Overview](#overview) +2. [Architecture](#architecture) +3. [Hardware Requirements](#hardware-requirements) +4. [Software Stack](#software-stack) +5. [Installation & Setup](#installation--setup) +6. [Configuration](#configuration) +7. [Usage](#usage) +8. [ROS2 Topics & Messages](#ros2-topics--messages) +9. [Node Details](#node-details) +10. [Troubleshooting](#troubleshooting) +11. [Performance Optimization](#performance-optimization) +12. [Best Practices](#best-practices) +13. [API Reference](#api-reference) + +--- + +## Overview + +The Lucy robot audio system provides **stereo audio capture and playback** capabilities for humanoid robotics applications. It is designed to work with the NVIDIA Jetson AGX Orin development kit's audio panel, supporting real-time audio processing for: + +- **Voice interaction** - Capturing human speech for processing +- **Sound localization** - Using stereo microphones for spatial audio awareness +- **Audio feedback** - Playing sounds, speech synthesis, or music through stereo speakers +- **Audio streaming** - Real-time audio transmission over ROS2 topics + +### Key Features + +- ✅ **Stereo capture** - Dual microphone input (2 channels) +- ✅ **Stereo playback** - Dual speaker output (2 channels) +- ✅ **Low latency** - PortAudio-based processing optimized for embedded systems +- ✅ **ROS2 native** - Built on `audio_common` package (ROS2 Humble compatible) +- ✅ **Configurable** - Sample rates, device selection, buffer sizes +- ✅ **Resilient** - Auto-respawn on node failures +- ✅ **Real-time** - Optimized for Jetson AGX Orin hardware + +--- + +## Architecture + +### System Diagram + +``` +┌────────────────────────────────────────────────────┐ +│ Jetson AGX Orin │ +│ | +│ ┌──────────────────────────────────────────────┐ │ +│ │ Audio Hardware (Audio Panel) │ │ +│ │ ├─ Stereo Microphones (Input) │ │ +│ │ └─ Stereo Speakers (Output) │ │ +│ └──────────────────────────────────────────────┘ │ +│ │ │ +│ ▼ │ +│ ┌──────────────────────────────────────────────┐ │ +│ │ ALSA / PortAudio Layer │ │ +│ └──────────────────────────────────────────────┘ │ +│ │ │ +│ ▼ │ +│ ┌──────────────────────────────────────────────┐ │ +│ │ ROS2 Audio Nodes │ │ +│ │ ├─ audio_capturer_node │ │ +│ │ │ └─ Publishes: /audio (AudioStamped) │ │ +│ │ └─ audio_player_node │ │ +│ │ └─ Subscribes: /audio (AudioStamped) │ │ +│ └──────────────────────────────────────────────┘ │ +│ │ │ +│ ▼ │ +│ ┌──────────────────────────────────────────────┐ │ +│ │ ROS2 Topics │ │ +│ │ └─ /audio (audio_common_msgs/AudioStamped) │ │ +│ └──────────────────────────────────────────────┘ │ +└────────────────────────────────────────────────────┘ +``` + +### Data Flow + +1. **Capture Path:** + ``` + Microphones → ALSA → PortAudio → audio_capturer_node → /audio topic + ``` + +2. **Playback Path:** + ``` + /audio topic → audio_player_node → PortAudio → ALSA → Speakers + ``` + +### Component Overview + +| Component | Purpose | Package | +|-----------|---------|---------| +| `audio_capturer_node` | Captures audio from microphones and publishes to ROS2 | `audio_common` | +| `audio_player_node` | Subscribes to audio topic and plays through speakers | `audio_common` | +| `audio_common_msgs` | ROS2 message definitions for audio data | `audio_common_msgs` | +| PortAudio | Cross-platform audio I/O library | System dependency | +| ALSA | Linux audio subsystem | System dependency | + +--- + +## Hardware Requirements + +### Required Hardware + +1. **NVIDIA Jetson AGX Orin Development Kit** + - Integrated audio panel + - Stereo line-in (for microphones) + - Stereo line-out (for speakers) + +2. **Stereo Microphones** + - 2-channel input device + - Compatible with 3.5mm audio jack or USB or analog interface + - Recommended: Condenser microphones with phantom power (if using USB interface) + +3. **Stereo Speakers** + - 2-channel output device + - Compatible with 3.5mm audio jack or USB or analog audio interface + - Recommended: Powered speakers or headphones + +### Audio Panel Connections + +- Jetson AGX Orin [Hardware layout](https://developer.nvidia.com/embedded/learn/jetson-agx-orin-devkit-user-guide/developer_kit_layout.html) for audio panel position +- Jetson AGX Orin [Carrier Board datasheet](https://developer.nvidia.com/assets/embedded/secure/jetson/agx_orin/jetson_agx_orin_devkit_carrier_board_specification_sp) for audio panel header pins specs + +### USB Audio Interfaces (Optional) + +For higher quality audio or additional channels, USB audio interfaces are supported: +- Any USB Audio Class compliant device + +--- + +## Software Stack + +### ROS2 Packages + +| Package | Version | Purpose | +|---------|---------|---------| +| `audio_common` | Latest (mgonzs13) | Audio capture and playback nodes | +| `audio_common_msgs` | Latest | Audio message definitions | +| `lucy_bringup` | 1.0.0 | Launch files and system integration | + +--- + +## Installation & Setup + +### 1. Install System Dependencies + +```bash +sudo apt update +sudo apt install -y \ + libportaudio2 \ + libportaudio-dev \ + alsa-utils \ + libasound2-dev +``` + +### 2. Clone Audio Packages + +```bash +cd ~/lucy_ws/src + +# Clone audio_common (ROS2 compatible) +git clone https://github.com/mgonzs13/audio_common.git +``` + +### 3. Build Packages + +```bash +cd ~/lucy_ws + +# Build audio packages first +colcon build --packages-select audio_common_msgs + +# Then build audio_common +colcon build --packages-select audio_common + +# Finally build lucy_bringup +colcon build --packages-select lucy_bringup +``` + +### 4. Source Workspace + +```bash +source ~/lucy_ws/install/setup.zsh +``` + +### 5. Verify Installation + +```bash +# Check if nodes are available +ros2 run audio_common audio_capturer_node --help +ros2 run audio_common audio_player_node --help + +# List available audio devices +arecord -l # List capture devices +aplay -l # List playback devices +``` + +--- + +## Configuration + +### Launch Arguments + +The audio system is configured through launch arguments in `lucy.launch.py`: + +| Argument | Default | Description | Example | +|----------|---------|-------------|---------| +| `audio_sample_rate` | `48000` | Sample rate in Hz | `44100`, `48000` | +| `audio_capture_device` | `-1` | Capture device index (-1 = default) | `0`, `1`, `-1` | +| `audio_playback_device` | `-1` | Playback device index (-1 = default) | `0`, `1`, `-1` | + +### Audio Capturer Parameters + +```python +{ + 'format': 8, # paInt16 (PortAudio format constant) + 'channels': 2, # Stereo microphones + 'rate': 48000, # Sample rate (Hz) + 'chunk': 1024, # Buffer size (frames) + 'device': -1, # Device index (-1 = default) + 'frame_id': 'audio_capture' # TF frame ID +} +``` + +### Audio Player Parameters + +```python +{ + 'channels': 2, # Stereo speakers + 'device': -1 # Device index (-1 = default) +} +``` + +### PortAudio Format Constants + +| Constant | Value | Description | +|----------|-------|-------------| +| `paFloat32` | 1 | 32-bit floating point | +| `paInt32` | 2 | 32-bit integer | +| `paInt16` | 8 | 16-bit integer (default) | +| `paInt8` | 16 | 8-bit integer | +| `paUInt8` | 32 | 8-bit unsigned integer | + +**Note:** Current configuration uses `paInt16` (format: 8) for optimal performance on embedded systems. + +### Finding Audio Device Indices + +```bash +# List all audio capture devices +arecord -l + +# Example output: +# card 0: PCH [HDA Intel PCH], device 0: ALC892 Analog [ALC892 Analog] +# Subdevices: 1/1 +# Subdevice #0: subdevice #0 +# +# card 1: USB [USB Audio], device 0: USB Audio [USB Audio] +# Subdevices: 1/1 +# Subdevice #0: subdevice #0 + +# List all audio playback devices +aplay -l + +# Test a specific device +arecord -D hw:1,0 -d 5 test.wav # Record from card 1, device 0 +aplay -D hw:1,0 test.wav # Play to card 1, device 0 +``` + +**Device Index Mapping:** +- `-1` = Default system device +- `0` = First device (card 0, device 0) +- `1` = Second device (card 1, device 0) +- etc. + +--- + +## Usage + +### Basic Launch + +Launch the entire Lucy system (includes audio): + +```bash +source ~/lucy_ws/install/setup.zsh +ros2 launch lucy_bringup lucy.launch.py +``` + +### Custom Audio Configuration + +```bash +# Launch with custom sample rate +ros2 launch lucy_bringup lucy.launch.py audio_sample_rate:=44100 + +# Launch with specific audio devices +ros2 launch lucy_bringup lucy.launch.py \ + audio_capture_device:=1 \ + audio_playback_device:=1 + +# Combined configuration +ros2 launch lucy_bringup lucy.launch.py \ + audio_sample_rate:=48000 \ + audio_capture_device:=0 \ + audio_playback_device:=0 +``` + +### Standalone Audio Nodes + +Run audio nodes independently for testing: + +```bash +# Audio capturer only +ros2 run audio_common audio_capturer_node \ + --ros-args \ + -p format:=8 \ + -p channels:=2 \ + -p rate:=48000 \ + -p chunk:=1024 \ + -p device:=-1 + +# Audio player only +ros2 run audio_common audio_player_node \ + --ros-args \ + -p channels:=2 \ + -p device:=-1 +``` + +### Using tmux Launcher + +The recommended way to launch the full system: + +```bash +~/launch_lucy.sh +``` + +This starts all nodes including audio in a tmux session. + +### Monitoring Audio System + +```bash +# Check if audio nodes are running +ros2 node list | grep audio + +# Monitor audio topic +ros2 topic echo /audio + +# Check audio topic frequency +ros2 topic hz /audio + +# View audio topic info +ros2 topic info /audio + +# Record audio to file (using ROS2 bag) +ros2 bag record /audio +``` + +--- + +## ROS2 Topics & Messages + +### Topics + +| Topic | Type | Direction | Description | +|-------|------|-----------|-------------| +| `/audio` | `audio_common_msgs/AudioStamped` | Publisher: `audio_capturer_node`
Subscriber: `audio_player_node` | Audio data stream with timestamp | + +### Message Structure + +**`audio_common_msgs/AudioStamped`:** + +```yaml +std_msgs/Header header + uint32 seq + time stamp + string frame_id +audio_common_msgs/Audio audio + audio_common_msgs/AudioInformation info + uint8 format # PortAudio format constant + uint8 channels # Number of channels (2 for stereo) + uint32 rate # Sample rate (Hz) + string coding_format # Audio coding format + audio_common_msgs/AudioData audio_data + float32[] float32_data # For paFloat32 format + int32[] int32_data # For paInt32 format + int16[] int16_data # For paInt16 format (default) + int8[] int8_data # For paInt8 format + uint8[] uint8_data # For paUInt8 format +``` + +### Message Example + +```python +# Example AudioStamped message (paInt16, stereo, 48kHz) +header: + seq: 1234 + stamp: + sec: 1701964800 + nanosec: 123456789 + frame_id: "audio_capture" +audio: + info: + format: 8 # paInt16 + channels: 2 # Stereo + rate: 48000 # 48 kHz + coding_format: "wave" + audio_data: + int16_data: [1234, -5678, 9012, -3456, ...] # Interleaved stereo samples +``` + +**Data Format:** +- Audio samples are **interleaved** for stereo: `[L, R, L, R, L, R, ...]` +- For `paInt16`: Values range from -32768 to 32767 +- Buffer size: 1024 frames = 2048 samples (for stereo) + +--- + +## Node Details + +### audio_capturer_node + +**Purpose:** Captures audio from microphones and publishes to `/audio` topic. + +**Parameters:** +- `format` (uint8): PortAudio format constant (default: 8 = paInt16) +- `channels` (uint8): Number of channels (default: 2 = stereo) +- `rate` (uint32): Sample rate in Hz (default: 48000) +- `chunk` (uint32): Buffer size in frames (default: 1024) +- `device` (int32): Audio device index (default: -1 = default device) +- `frame_id` (string): TF frame ID (default: "audio_capture") + +**Published Topics:** +- `/audio` (`audio_common_msgs/AudioStamped`): Audio data stream + +**Behavior:** +- Continuously captures audio from the specified device +- Publishes audio chunks at the configured sample rate +- Auto-respawns on failure (2 second delay) +- Handles device errors gracefully + +### audio_player_node + +**Purpose:** Subscribes to `/audio` topic and plays audio through speakers. + +**Parameters:** +- `channels` (uint8): Number of channels (default: 2 = stereo) +- `device` (int32): Audio device index (default: -1 = default device) + +**Subscribed Topics:** +- `/audio` (`audio_common_msgs/AudioStamped`): Audio data stream + +**Behavior:** +- Continuously plays audio from the `/audio` topic +- Handles format conversion automatically +- Auto-respawns on failure (2 second delay) +- Reports underrun warnings when buffer is empty (normal when no audio is published) + +--- + +## Troubleshooting + +### Common Issues + +#### 1. No Audio Input/Output + +**Symptoms:** +- No audio data on `/audio` topic +- Audio player reports no data + +**Solutions:** +```bash +# Check if audio devices are connected +arecord -l +aplay -l + +# Test audio hardware directly +arecord -d 5 test.wav && aplay test.wav + +# Check audio device permissions +ls -l /dev/snd/ + +# Verify ALSA configuration +alsamixer +``` + +#### 2. PortAudio Underrun Warnings + +**Symptoms:** +``` +PortAudio underrun detected, retrying... +``` + +**Explanation:** +These warnings are **normal and expected** when: +- No audio is being published to `/audio` topic +- Audio source stops temporarily +- System is idle + +**Action:** No action needed. Warnings will stop when audio data starts flowing. + +#### 3. ALSA Configuration Errors + +**Symptoms:** +``` +ALSA lib confmisc.c:1369:(snd_func_refer) Unable to find definition 'cards.0.pcm.front.0:CARD=0' +ALSA lib pcm.c:2664:(snd_pcm_open_noupdate) Unknown PCM front +``` + +**Explanation:** +These are ALSA configuration warnings, not errors. They occur when ALSA tries to use default device names that don't exist. + +**Solutions:** +```bash +# Create/update ALSA configuration +sudo alsa force-reload + +# Or ignore (system will use available devices) +``` + +#### 4. Device Not Found + +**Symptoms:** +``` +Error opening audio device +``` + +**Solutions:** +```bash +# List available devices +arecord -l +aplay -l + +# Use correct device index in launch arguments +ros2 launch lucy_bringup lucy.launch.py \ + audio_capture_device:=0 \ + audio_playback_device:=0 + +# Test device directly +arecord -D hw:0,0 -d 5 test.wav +``` + +#### 5. High CPU Usage + +**Symptoms:** +- System becomes sluggish +- Audio dropouts + +**Solutions:** +```bash +# Reduce sample rate +ros2 launch lucy_bringup lucy.launch.py audio_sample_rate:=44100 + +# Increase buffer size (in code, modify chunk parameter) +# Default: 1024, try: 2048 or 4096 + +# Enable Jetson performance mode +sudo jetson_clocks +``` + +#### 6. Audio Nodes Not Starting + +**Symptoms:** +- Nodes don't appear in `ros2 node list` +- Launch fails silently + +**Solutions:** +```bash +# Check if audio_common is built +ros2 pkg list | grep audio_common + +# Rebuild if needed +cd ~/lucy_ws +colcon build --packages-select audio_common + +# Check node executables +ros2 run audio_common audio_capturer_node --help +ros2 run audio_common audio_player_node --help + +# Check launch file syntax +python3 -m py_compile ~/lucy_ws/src/lucy_ros_packages/lucy_bringup/launch/lucy.launch.py +``` + +### Debug Commands + +```bash +# Check audio nodes status +ros2 node list | grep audio +ros2 node info /audio_capturer +ros2 node info /audio_player + +# Monitor audio topic +ros2 topic echo /audio --no-arr +ros2 topic hz /audio + +# Check system audio +alsamixer # Audio mixer +speaker-test -t wav -c 2 # Test speakers +arecord -d 5 test.wav # Test microphones + +# View node logs +ros2 run rqt_console rqt_console +``` + +--- + +## Performance Optimization + +### Sample Rate Selection + +| Sample Rate | Use Case | CPU Usage | Quality | +|-------------|----------|-----------|---------| +| 44100 Hz | Standard audio, music | Low | Good | +| 48000 Hz | Professional audio, video sync | Medium | Excellent | +| 96000 Hz | High-fidelity recording | High | Superior | + +**Recommendation:** Use 48000 Hz for balanced performance and quality on Jetson AGX Orin. + +### Buffer Size Tuning + +| Buffer Size | Latency | Stability | CPU Usage | +|-------------|---------|-----------|-----------| +| 512 frames | Low (~10ms) | Lower | Higher | +| 1024 frames | Medium (~21ms) | Good | Medium | +| 2048 frames | Higher (~42ms) | Excellent | Lower | + +**Current Setting:** 1024 frames (optimal for real-time applications) + +### Jetson-Specific Optimizations + +```bash +# Enable maximum performance mode +sudo jetson_clocks + +# Set CPU governor to performance +echo performance | sudo tee /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor + +# Disable power management throttling +sudo nvpmodel -m 0 # MAXN mode +``` + +### Memory Considerations + +- **Audio buffer memory:** ~4 KB per 1024-frame chunk (stereo, 16-bit) +- **Topic queue:** Default 10 messages (~40 KB) +- **Total per node:** ~50 KB memory footprint + +--- + +## Best Practices + +### 1. Device Selection + +- Always test devices before deployment +- Use device indices instead of names for reliability +- Document device mappings for your hardware setup + +### 2. Sample Rate Consistency + +- Use the same sample rate for capture and playback +- Match sample rate to your application requirements +- Consider downstream processing needs (speech recognition, etc.) + +### 3. Error Handling + +- Monitor node status regularly +- Check topic activity: `ros2 topic hz /audio` +- Implement health checks in your application + +### 4. Resource Management + +- Don't run unnecessary audio processing +- Use appropriate buffer sizes for your use case +- Monitor CPU usage during development + +### 5. Testing + +- Test audio hardware independently before ROS2 integration +- Verify audio quality with known test files +- Test with actual microphones and speakers, not just software loops + +### 6. Documentation + +- Document your specific hardware configuration +- Keep track of device indices for your setup +- Note any custom ALSA configurations + +--- + +## API Reference + +### Launch File Integration + +The audio system is integrated into `lucy.launch.py` via the `create_audio_nodes()` helper function: + +```python +def create_audio_nodes(sample_rate, capture_device, playback_device): + """Create audio capture and playback nodes.""" + return [ + Node( + package='audio_common', + executable='audio_capturer_node', + name='audio_capturer', + # ... parameters ... + ), + Node( + package='audio_common', + executable='audio_player_node', + name='audio_player', + # ... parameters ... + ) + ] +``` + +### Programmatic Usage + +To use audio in your own ROS2 nodes: + +```python +import rclpy +from rclpy.node import Node +from audio_common_msgs.msg import AudioStamped + +class MyAudioProcessor(Node): + def __init__(self): + super().__init__('my_audio_processor') + self.subscription = self.create_subscription( + AudioStamped, + '/audio', + self.audio_callback, + 10 + ) + + def audio_callback(self, msg): + # Process audio data + audio_data = msg.audio.audio_data.int16_data + sample_rate = msg.audio.info.rate + channels = msg.audio.info.channels + # ... your processing ... +``` + +### Service Integration + +Currently, audio nodes don't expose ROS2 services. For on-demand control, consider: +- Publishing to control topics +- Using lifecycle nodes +- Implementing custom service wrappers + +--- + +## References + +- **audio_common GitHub:** https://github.com/mgonzs13/audio_common +- **PortAudio Documentation:** http://www.portaudio.com/docs.html +- **ALSA Documentation:** https://www.alsa-project.org/wiki/Documentation +- **Jetson AGX Orin Audio:** https://developer.nvidia.com/embedded/jetson-agx-orin + +--- + +## License + +This documentation is part of the Lucy Robot project and is licensed under **GPL-3.0**. + +**Copyright 2024 Sentience Robotics Team** + +--- + +## Changelog + +### Version 1.0.0 (December 2024) +- Initial comprehensive documentation +- Audio system integration with lucy_bringup +- Support for stereo capture and playback +- PortAudio-based implementation +- Jetson AGX Orin optimization + +--- + +## Support + +For issues, questions, or contributions: +- **Email:** contact@sentience-robotics.fr +- **Repository:** [Lucy ROS Packages](https://github.com/sentience-robotics/lucy_ros_packages) + +--- + +*Last updated: December 2024* + diff --git a/lucy_bringup/CMakeLists.txt b/lucy_bringup/CMakeLists.txt new file mode 100644 index 0000000..c39b4ae --- /dev/null +++ b/lucy_bringup/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(lucy_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) + +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +# Install system scripts +install(PROGRAMS + system_scripts/launch_lucy.sh + system_scripts/stop_lucy.sh + system_scripts/check_lucy.sh + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() + diff --git a/lucy_bringup/README.md b/lucy_bringup/README.md new file mode 100644 index 0000000..8f7d5a2 --- /dev/null +++ b/lucy_bringup/README.md @@ -0,0 +1,188 @@ +# Lucy Bringup Package + +System launch files and scripts for the Lucy robot on NVIDIA Jetson AGX Orin. + +## Overview + +This package provides a unified way to launch all components of the Lucy robot system: +- Two micro-ROS agents (for RP2040 controllers on left and right arms) +- ROSBridge WebSocket server (for web interface communication) +- Audio capture and playback nodes (for stereo microphones and speakers) +- Intel RealSense D435i camera (for vision system with depth sensing) +- Web control panel interface + +## Quick Start + +### Using the tmux Launcher (Recommended for Development) + +```bash +# Launch everything +~/launch_lucy.sh + +# Check system status +~/check_lucy.sh + +# Stop everything +~/stop_lucy.sh +``` + +### Using ROS2 Launch Directly + +```bash +# Source workspace +source ~/lucy_ws/install/setup.zsh + +# Launch only ROS nodes (no web interface) +ros2 launch lucy_bringup lucy.launch.py + +# Launch with custom devices +ros2 launch lucy_bringup lucy.launch.py device0:=/dev/ttyACM2 device1:=/dev/ttyACM3 +``` + +## Architecture + +### tmux Layout + +``` +┌─────────────────────────────────────────────────────────────┐ +│ Pane 0: ROS2 Nodes (Top 60%) │ +│ ├─ micro_ros_agent_right │ +│ ├─ micro_ros_agent_left │ +│ ├─ rosbridge_server │ +│ ├─ audio_capturer_node │ +│ └─ audio_player_node │ +│ └─ realsense2_camera │ +├──────┬──────────────────────────────────────────────────────┤ +│ Web │ Pane 2: Debug Terminal │ +│ ~12% │ ~88% │ +└──────┴──────────────────────────────────────────────────────┘ +``` + +### tmux Controls + +**Navigation:** +- **Detach (keep running)**: `Ctrl+B` then `D` +- **Switch panes**: `Ctrl+B` then arrow keys +- **Cycle panes**: `Ctrl+B` then `O` +- **Zoom pane**: `Ctrl+B` then `Z` (toggle fullscreen) +- **Reattach**: `tmux attach -t lucy` + +**Scrolling (Copy Mode):** +- **Enter scroll mode**: `Ctrl+B` then `[` +- **Navigate**: Arrow keys, `PgUp`/`PgDn`, mouse wheel (if enabled) +- **Search forward**: `/` then type search term +- **Search backward**: `?` then type search term +- **Exit scroll mode**: `q` or `Esc` + +## Launch Arguments + +The `lucy.launch.py` file accepts the following arguments: + +- `device0` - Serial device for right arm (default: `/dev/ttyACM0`) +- `device1` - Serial device for left arm (default: `/dev/ttyACM1`) +- `realsense_serial` - RealSense camera serial number (default: `''` = auto-detect) + +Audio launch arguments (passed to `audio.launch.py`): +- `sample_rate` - Audio sample rate in Hz (default: `48000`) +- `capture_device` - Audio capture device index (default: `-1` for default) +- `playback_device` - Audio playback device index (default: `-1` for default) + +## System Requirements + +- Ubuntu 22.04 on NVIDIA Jetson AGX Orin +- ROS2 Humble +- tmux +- Two RP2040 controllers connected via USB +- Intel RealSense D435i camera (see [REALSENSE.md](REALSENSE.md) for details) + +## Troubleshooting + +### Serial Devices Not Found + +```bash +# List USB devices +ls -l /dev/ttyACM* + +# Check which device is which +ros2 topic echo /trace_publisher +``` + +### Web Interface Not Starting + +Check if the web directory exists: +```bash +ls ~/web_control_panel +``` + +### ROS Nodes Not Communicating + +```bash +# Check all nodes +ros2 node list + +# Check topics +ros2 topic list + +# Echo joint commands +ros2 topic echo /joints/right_arm +ros2 topic echo /joints/left_arm + +# Check audio topics +ros2 topic echo /audio +ros2 topic hz /audio +``` + +### Audio Underrun Warnings + +**Note:** PortAudio underrun warnings are **normal and expected** when: +- No audio is being published to `/audio` topic +- Audio source stops temporarily +- System is idle + +These are informational warnings, not errors. The system continues to function normally. You can safely ignore them. They will stop when audio data starts flowing again. + +## Files + +``` +lucy_bringup/ +├── launch/ +│ ├── lucy.launch.py # Main ROS2 launch file +│ └── realsense.launch.py # RealSense D435i camera launch file +├── system_scripts/ +│ ├── launch_lucy.sh # tmux launcher +│ ├── stop_lucy.sh # Graceful shutdown +│ └── check_lucy.sh # Health check +├── CMakeLists.txt +├── package.xml +├── README.md # This file +└── REALSENSE.md # RealSense D435i integration documentation +``` + +## Symlinks + +For convenience, symlinks are created in `/home/dev/`: +- `~/launch_lucy.sh` → `lucy_ws/src/lucy_ros_packages/lucy_bringup/system_scripts/launch_lucy.sh` +- `~/stop_lucy.sh` → `lucy_ws/src/lucy_ros_packages/lucy_bringup/system_scripts/stop_lucy.sh` +- `~/check_lucy.sh` → `lucy_ws/src/lucy_ros_packages/lucy_bringup/system_scripts/check_lucy.sh` + +## Camera System + +The Lucy robot uses an **Intel RealSense D435i** stereo depth camera for vision and depth sensing. + +**Key Features:** +- Color stream: 1920x1080 @ 30fps +- Depth stream: 1280x720 @ 30fps +- Aligned depth-to-color images +- IMU data (accelerometer + gyroscope @ 400Hz) +- Spatial and temporal filters for depth quality + +**Topics:** All camera topics are published under `/realsense/` namespace. + +For complete documentation, installation instructions, troubleshooting, and usage examples, see **[REALSENSE.md](REALSENSE.md)**. + +## License + +GPL-3.0 - See LICENSE file for details. + +Copyright 2025 Sentience Robotics Team + diff --git a/lucy_bringup/REALSENSE.md b/lucy_bringup/REALSENSE.md new file mode 100644 index 0000000..b46b80c --- /dev/null +++ b/lucy_bringup/REALSENSE.md @@ -0,0 +1,412 @@ +# Intel RealSense D435i Integration + +Complete documentation for the Intel RealSense D435i stereo depth camera integration in the Lucy ROS2 system. + +> **Official ROS2 Wrapper**: This integration uses the [official realsense-ros](https://github.com/realsenseai/realsense-ros) ROS2 wrapper maintained by Intel RealSense. For complete documentation, see the [official repository](https://github.com/realsenseai/realsense-ros). + +## Table of Contents + +- [Overview](#overview) +- [Installation](#installation) +- [Camera Specifications](#camera-specifications) +- [Configuration](#configuration) +- [Usage](#usage) +- [Published Topics](#published-topics) +- [Integration with Lucy System](#integration-with-lucy-system) +- [Troubleshooting](#troubleshooting) +- [Performance Optimization](#performance-optimization) + +## Overview + +The Intel RealSense D435i is a stereo depth camera that provides: +- **RGB Color Stream**: High-resolution color images +- **Depth Stream**: Stereo depth information +- **IMU Data**: Accelerometer and gyroscope for motion tracking +- **Aligned Depth-to-Color**: Depth data aligned with color image pixels + +## Installation + +### Official Installation Guide for Jetson AGX Orin + +**Tested System:** +- **Board**: Jetson AGX Orin +- **L4T Version**: R36.4.7 (L4T 36.4.7) +- **Kernel**: 5.15.148-tegra +- **Ubuntu**: 22.04.5 LTS +- **JetPack**: 6.0 + +**Important**: For Jetson AGX Orin with JetPack 6, the **libuvc-backend** installation method is required. This method avoids kernel patching and works on a wider range of platforms. See the [official libuvc-backend installation guide](https://github.com/realsenseai/librealsense/blob/master/doc/libuvc_installation.md). + +### Step 1: Install Librealsense2 SDK (libuvc-backend Method) + +**Prerequisites:** +- Make sure **no RealSense device is connected** before starting +- Internet connection required +- Network proxy settings configured if needed + +**Installation steps:** + +```bash +# Download the installation script +wget https://github.com/realsenseai/librealsense/raw/master/scripts/libuvc_installation.sh + +# Make script executable +chmod +x ./libuvc_installation.sh + +# Run the installation script +./libuvc_installation.sh +``` + +**Wait for completion:** +- The script will take some time to complete +- Wait until you see `Librealsense script completed` message + +**After installation:** +1. Connect the RealSense device +2. Verify installation: + ```bash + rs-enumerate-devices + ``` + +**Note**: This method uses the libuvc-backend which doesn't require kernel patching. It's the recommended approach for Jetson devices when kernel patching is not possible or fails. The installation script automatically handles udev rules setup. + +### Step 2: Install ROS2 RealSense Packages + +After installing the librealsense2 SDK, install the ROS2 wrapper packages. The official ROS2 wrapper is maintained at [realsense-ros](https://github.com/realsenseai/realsense-ros). + +```bash +sudo apt-get update +sudo apt-get install -y \ + ros-humble-realsense2-camera \ + ros-humble-realsense2-camera-msgs \ + ros-humble-realsense2-description +``` + + +**Official ROS2 Wrapper Repository**: [realsenseai/realsense-ros](https://github.com/realsenseai/realsense-ros) + +**Important**: After installing both the librealsense2 SDK and ROS2 packages, you may encounter a "bad optional access" error if multiple librealsense installations exist (libuvc and ROS wrapper). The ROS wrapper version may be loaded by default, but it doesn't work with the libuvc-backend installation. + +**Solution**: Add the libuvc installation path to `LD_LIBRARY_PATH` to prioritize it: + +```bash +# Add to ~/.zshrc (or ~/.bashrc if using bash) +export LD_LIBRARY_PATH=/usr/local/lib:${LD_LIBRARY_PATH} +``` + +Then reload your shell configuration: +```bash +source ~/.zshrc # or source ~/.bashrc +``` + +## Camera Specifications + +For complete specifications, see the [official D435i product page](https://www.intelrealsense.com/depth-camera-d435i/). + +**Key Specifications:** +- **Model**: D435i (with IMU) +- **USB ID**: `8086:0b3a` +- **Color**: 1920x1080 @ 30fps (maximum) +- **Depth**: 1280x720 @ 30fps (maximum) +- **IMU**: Accelerometer + Gyroscope @ 400Hz + +## Configuration + +### Current Configuration + +The RealSense D435i is configured with optimal settings in `lucy_bringup/launch/realsense.launch.py`: + +- **Color Stream**: 1920x1080 @ 30fps (maximum) +- **Depth Stream**: 1280x720 @ 30fps (maximum) +- **Aligned Depth**: Enabled (depth aligned to color image plane) +- **IMU**: Separate topics for accelerometer and gyroscope @ 400Hz +- **Point Cloud**: Disabled +- **Infrared**: Disabled +- **Filters**: Spatial + Temporal (recommended for depth quality) + +For complete parameter documentation, see the [official realsense-ros parameters documentation](https://github.com/realsenseai/realsense-ros#parameters). + +## Usage + +### Launch RealSense Standalone + +Launch only the RealSense camera: + +```bash +# Source workspace +source ~/lucy_ws/install/setup.zsh + +# Launch RealSense camera +ros2 launch lucy_bringup realsense.launch.py +``` + +### Launch with Custom Serial Number + +If you have multiple RealSense cameras: + +```bash +ros2 launch lucy_bringup realsense.launch.py serial_no:= +``` + +### Launch Full Lucy System + +The RealSense camera is automatically launched as part of the full Lucy system: + +```bash +# Using launch script +~/launch_lucy.sh + +# Or using ROS2 launch directly +ros2 launch lucy_bringup lucy.launch.py +``` + +## Published Topics + +All topics are published under the `/realsense` namespace. + +**For complete topic documentation**, see the [official realsense-ros repository](https://github.com/realsenseai/realsense-ros). + +### Main Topics (This Configuration) + +- `/realsense/color/image_raw` - RGB color images (1920x1080 @ 30fps) +- `/realsense/color/camera_info` - Color camera calibration +- `/realsense/depth/image_rect_raw` - Native depth images (1280x720 @ 30fps) +- `/realsense/depth/camera_info` - Depth camera calibration +- `/realsense/aligned_depth_to_color/image_raw` - Aligned depth (1920x1080 @ 30fps) +- `/realsense/aligned_depth_to_color/camera_info` - Aligned depth calibration +- `/realsense/gyro/sample` - Gyroscope data (400 Hz) +- `/realsense/accel/sample` - Accelerometer data (400 Hz) + +### Useful Commands + +```bash +# List all topics +ros2 topic list | grep realsense + +# Check frame rates +ros2 topic hz /realsense/color/image_raw +ros2 topic hz /realsense/depth/image_rect_raw + +# View images +ros2 run rqt_image_view rqt_image_view /realsense/color/image_raw +``` + +## Services and Actions + +For complete documentation on available services and actions, see: +- [Available Services](https://github.com/realsenseai/realsense-ros#available-services) +- [Available Actions](https://github.com/realsenseai/realsense-ros#available-actions) + +### Common Services + +- `/realsense/realsense2_camera/hw_reset` - Reset the device +- `/realsense/realsense2_camera/device_info` - Get device information (serial number, firmware version, etc.) + +**Example:** +```bash +ros2 service call /realsense/realsense2_camera/device_info realsense2_camera_msgs/srv/DeviceInfo +``` + +## Integration with Lucy System + +### Launch File Integration + +The RealSense camera is integrated into the main Lucy launch file (`lucy.launch.py`): + +```python +# RealSense D435i Camera (replaces camera_ros) +realsense_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('lucy_bringup'), + 'launch', + 'realsense.launch.py' + ]) + ]), + launch_arguments={ + 'serial_no': LaunchConfiguration('realsense_serial'), + }.items() +) +``` + +### Namespace + +All RealSense topics use the `/realsense/...` namespace to avoid conflicts with other systems. + +## Troubleshooting + +For common issues and solutions, see the [official realsense-ros troubleshooting](https://github.com/realsenseai/realsense-ros) and [Intel RealSense support](https://support.intelrealsense.com/). + +### Common Issues + +**Camera Not Detected:** + +**Other checks:** +- Ensure the installation script completed successfully +- Check USB connection: `lsusb | grep -i intel` +- Verify user is in `video` and `plugdev` groups +- Reload camera (unplug/replug) after installation + +**IMU Data Not Publishing:** +- Verify camera permissions (the installation script should handle this) +- Check HID device permissions: `ls -la /dev/hidraw*` +- Ensure `enable_gyro` and `enable_accel` are set to `true` in launch file + + +### Alignment Issues + +**Problem**: Aligned depth doesn't match color image. + +**Solutions**: + +1. **Verify alignment is enabled**: + ```python + 'align_depth.enable': True, + ``` + +2. **Check camera calibration**: + - RealSense cameras are factory calibrated + - If issues persist, may need recalibration + +3. **Verify topics are synchronized**: + ```bash + # Check timestamps are close + ros2 topic echo /realsense/color/image_raw --once | grep stamp + ros2 topic echo /realsense/aligned_depth_to_color/image_raw --once | grep stamp + ``` + +## Performance Optimization + +### Jetson AGX Orin Optimizations + +1. **Set CPU governor to performance**: + ```bash + sudo jetson_clocks + ``` + +2. **Monitor system resources**: + ```bash + # CPU usage + htop + + # Jetson stats + tegrastats + + # ROS2 node CPU usage + ros2 run resource_monitor resource_monitor + ``` + +3. **USB 3.0 Connection**: + - Ensure camera is connected to USB 3.0 port + - Check: `lsusb -t` should show "5000M" speed + +### Reducing CPU Usage + +If CPU usage is too high: + +1. **Lower resolution**: + ```python + 'rgb_camera.profile': '1280x720x30', + 'depth_module.profile': '640x480x30', + ``` + +2. **Lower frame rate**: + ```python + 'rgb_camera.profile': '1920x1080x15', + 'depth_module.profile': '1280x720x15', + ``` + +3. **Disable filters** (if depth quality is acceptable): + ```python + 'filters': '', + ``` + +### Memory Usage + +RealSense node typically uses: +- ~200-300 MB RAM for image buffers +- Additional memory for filters and processing + +Monitor with: +```bash +ros2 run resource_monitor resource_monitor +``` + +## Advanced Configuration + +### Custom Filter Parameters + +Edit `lucy_bringup/launch/realsense.launch.py` to customize filters: + +```python +# Spatial filter - stronger smoothing +'spatial_filter.magnitude': 3, +'spatial_filter.smooth_alpha': 0.6, +'spatial_filter.smooth_delta': 30, + +# Temporal filter - more aggressive temporal smoothing +'temporal_filter.alpha': 0.5, +'temporal_filter.delta': 30, +``` + +### Enable Point Cloud (if needed) + +To enable point cloud generation: + +```python +'enable_pointcloud': True, +``` + +This will publish: +- `/realsense/depth/color/points` (sensor_msgs/PointCloud2) + +**Note**: Point cloud generation is CPU-intensive and disabled by default. + +### Enable Infrared Streams (if needed) + +To enable infrared streams: + +```python +'enable_infra1': True, +'enable_infra2': True, +``` + +This will publish: +- `/realsense/infra1/image_rect_raw` +- `/realsense/infra2/image_rect_raw` + +**Note**: Infrared streams are typically not needed when depth is enabled. + +## References + +### Official ROS2 Wrapper + +- **[realsense-ros GitHub Repository](https://github.com/realsenseai/realsense-ros)** - Official ROS/ROS2 wrapper for RealSense cameras + +### Official SDK and Installation + +- **[libuvc-backend Installation Guide](https://github.com/realsenseai/librealsense/blob/master/doc/libuvc_installation.md)** - Official libuvc-backend installation for Jetson (recommended for JetPack 6) +- [librealsense GitHub Repository](https://github.com/realsenseai/librealsense) - Official SDK source code +- [libuvc Installation Script](https://github.com/realsenseai/librealsense/raw/master/scripts/libuvc_installation.sh) - Direct link to installation script + + +### Additional Resources + +- [RealSense ROS2 Services and Actions](https://github.com/realsenseai/realsense-ros#available-services) - Complete list of available services and actions +- [RealSense ROS2 Parameters](https://github.com/realsenseai/realsense-ros#parameters) - Configuration parameters documentation + +## Support + +For issues and questions: +- Check the troubleshooting section above +- Review Intel RealSense documentation +- Check ROS2 RealSense wrapper issues on GitHub +- Contact: contact@sentience-robotics.fr + +--- + +**Last Updated**: December 2025 +**Camera Model**: Intel RealSense D435i +**ROS2 Distribution**: Humble +**Platform**: NVIDIA Jetson AGX Orin + diff --git a/lucy_bringup/launch/lucy.launch.py b/lucy_bringup/launch/lucy.launch.py new file mode 100755 index 0000000..87d5990 --- /dev/null +++ b/lucy_bringup/launch/lucy.launch.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 +# Copyright 2025 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Launch file for Lucy Robot System. + +This launch file starts all core ROS2 components: +- Two micro-ROS agents (for left and right arm RP2040 controllers) +- ROSBridge WebSocket server (for web interface communication) +- Audio capture and playback nodes (for stereo microphones and speakers) +- RealSense D435i camera (for vision system with depth sensing) + +Optimized for NVIDIA Jetson AGX Orin. +""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, LogInfo, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def create_micro_ros_nodes(device0, device1): + """Create micro-ROS agent nodes for left and right arms.""" + return [ + Node( + package='micro_ros_agent', + executable='micro_ros_agent', + name='micro_ros_agent_right', + arguments=['serial', '--dev', device0], + output='screen', + respawn=True, + respawn_delay=2.0, + emulate_tty=True + ), + Node( + package='micro_ros_agent', + executable='micro_ros_agent', + name='micro_ros_agent_left', + arguments=['serial', '--dev', device1], + output='screen', + respawn=True, + respawn_delay=2.0, + emulate_tty=True + ) + ] + + +def create_audio_nodes(sample_rate, capture_device, playback_device): + """Create audio capture and playback nodes.""" + return [ + Node( + package='audio_common', + executable='audio_capturer_node', + name='audio_capturer', + output='screen', + respawn=True, + respawn_delay=2.0, + parameters=[{ + 'format': 8, # paInt16 (PortAudio format constant) + 'channels': 2, # Stereo microphones + 'rate': sample_rate, + 'chunk': 1024, # Buffer size + 'device': capture_device, + 'frame_id': 'audio_capture' + }] + ), + Node( + package='audio_common', + executable='audio_player_node', + name='audio_player', + output='screen', + respawn=True, + respawn_delay=2.0, + parameters=[{ + 'channels': 2, # Stereo speakers + 'device': playback_device + }] + ) + ] + + +def generate_launch_description(): + """Generate launch description for Lucy robot system.""" + + # Declare launch arguments for flexibility + device0_arg = DeclareLaunchArgument( + 'device0', + default_value='/dev/ttyACM0', + description='Serial device for first micro-ROS agent (right arm)' + ) + + device1_arg = DeclareLaunchArgument( + 'device1', + default_value='/dev/ttyACM1', + description='Serial device for second micro-ROS agent (left arm)' + ) + + # Audio launch arguments + audio_sample_rate_arg = DeclareLaunchArgument( + 'audio_sample_rate', + default_value='48000', + description='Audio sample rate in Hz (e.g., 44100, 48000)' + ) + + audio_capture_device_arg = DeclareLaunchArgument( + 'audio_capture_device', + default_value='-1', + description='Audio capture device index (-1 for default)' + ) + + audio_playback_device_arg = DeclareLaunchArgument( + 'audio_playback_device', + default_value='-1', + description='Audio playback device index (-1 for default)' + ) + + # Create subsystem nodes using helper functions + micro_ros_nodes = create_micro_ros_nodes( + LaunchConfiguration('device0'), + LaunchConfiguration('device1') + ) + + audio_nodes = create_audio_nodes( + LaunchConfiguration('audio_sample_rate'), + LaunchConfiguration('audio_capture_device'), + LaunchConfiguration('audio_playback_device') + ) + + # ROSBridge WebSocket Server (for web interface) + rosbridge_server = ExecuteProcess( + cmd=['ros2', 'launch', 'rosbridge_server', 'rosbridge_websocket_launch.xml'], + output='screen', + shell=True + ) + + # Node 4: RealSense D435i Camera (replaces camera_ros) + realsense_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('lucy_bringup'), + 'launch', + 'realsense.launch.py' + ]) + ]) + ) + + return LaunchDescription([ + # Launch arguments + device0_arg, + device1_arg, + audio_sample_rate_arg, + audio_capture_device_arg, + audio_playback_device_arg, + + # Startup message + LogInfo(msg='========================================'), + LogInfo(msg='🤖 Starting Lucy Robot System...'), + LogInfo(msg='========================================'), + LogInfo(msg='Note: Audio underrun warnings are normal when no audio is published'), + + # Launch all subsystems + *micro_ros_nodes, # Unpack micro-ROS nodes + rosbridge_server, + *audio_nodes, # Unpack audio nodes + realsense_launch, # RealSense D435i camera + + # Success message + LogInfo(msg='✅ All ROS nodes launched successfully!'), + LogInfo(msg=' - Micro-ROS Agents: right & left arm'), + LogInfo(msg=' - ROSBridge Server: WebSocket ready'), + LogInfo(msg=' - Audio System: Capture & playback ready'), + LogInfo(msg=' - RealSense D435i: Vision system active'), + LogInfo(msg='========================================'), + ]) + diff --git a/lucy_bringup/launch/realsense.launch.py b/lucy_bringup/launch/realsense.launch.py new file mode 100644 index 0000000..1675bf0 --- /dev/null +++ b/lucy_bringup/launch/realsense.launch.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +# Copyright 2025 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Minimal launch file for Intel RealSense D435i camera. + +Based on official realsense-ros documentation: +https://github.com/realsenseai/realsense-ros + +This launch file uses only required parameters: +- Enable streams (color, depth, IMU) +- Disable unused streams (infrared, pointcloud) +- Namespace: /realsense + +All other parameters use defaults from the realsense-ros package. +""" + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + params = { + # Enable required streams + 'enable_color': True, + 'enable_depth': True, + + # Disable unused streams + 'enable_gyro': False, + 'enable_accel': False, + 'enable_infra1': False, + 'enable_infra2': False, + 'enable_pointcloud': False, + + # Enable RGBD topic (combines color and depth into RGB-D image) + 'enable_rgbd': True, + 'align_depth.enable': True, # Required for RGBD + 'enable_sync': True, # Required for RGBD synchronization + } + + realsense_node = Node( + package='realsense2_camera', + executable='realsense2_camera_node', + name='realsense2_camera', + namespace='realsense', + parameters=[params], + output='screen', + respawn=True, + respawn_delay=2.0, + ) + + return LaunchDescription([ + realsense_node, + ]) diff --git a/lucy_bringup/package.xml b/lucy_bringup/package.xml new file mode 100644 index 0000000..c89d7e1 --- /dev/null +++ b/lucy_bringup/package.xml @@ -0,0 +1,25 @@ + + + + lucy_bringup + 1.0.0 + Launch files and scripts for bringing up the Lucy robot system + + Sentience Robotics Team + GPL-3.0 + + ament_cmake + + micro_ros_agent + rosbridge_server + camera_ros + audio_common + realsense2_camera + launch + launch_ros + + + ament_cmake + + + diff --git a/lucy_bringup/system_scripts/check_lucy.sh b/lucy_bringup/system_scripts/check_lucy.sh new file mode 100755 index 0000000..811ba84 --- /dev/null +++ b/lucy_bringup/system_scripts/check_lucy.sh @@ -0,0 +1,113 @@ +#!/usr/bin/zsh +# Copyright 2024 Sentience Robotics Team +# Health check script for Lucy Robot System + +SESSION_NAME="lucy" +WORKSPACE="$HOME/lucy_ws" + +# Colors for output +GREEN='\033[0;32m' +BLUE='\033[0;34m' +YELLOW='\033[1;33m' +RED='\033[0;31m' +NC='\033[0m' # No Color + +echo -e "${BLUE}========================================${NC}" +echo -e "${BLUE}🔍 Checking Lucy Robot System Status...${NC}" +echo -e "${BLUE}========================================${NC}" +echo "" + +# Check if tmux session exists +echo -n "tmux session: " +if tmux has-session -t $SESSION_NAME 2>/dev/null; then + echo -e "${GREEN}✅ Running${NC}" + TMUX_RUNNING=true +else + echo -e "${RED}❌ Not running${NC}" + TMUX_RUNNING=false + echo "" + echo "Start with: ~/launch_lucy.sh" + exit 1 +fi + +# Source ROS2 workspace +source $WORKSPACE/install/setup.zsh 2>/dev/null || true + +echo "" +echo -e "${BLUE}ROS2 Nodes:${NC}" + +# Check micro-ROS pico nodes (should be 2) +echo -n " Pico Nodes (2 expected): " +PICO_COUNT=$(ros2 node list 2>/dev/null | grep -c "pico_node" || echo "0") +if [ "$PICO_COUNT" -eq 2 ]; then + echo -e "${GREEN}✅ Active ($PICO_COUNT/2)${NC}" +elif [ "$PICO_COUNT" -eq 1 ]; then + echo -e "${YELLOW}⚠️ Partial ($PICO_COUNT/2)${NC}" +else + echo -e "${RED}❌ Not found (0/2)${NC}" +fi + +# Check camera node +echo -n " Camera Publisher: " +if ros2 node list 2>/dev/null | grep -q "camera_publisher"; then + echo -e "${GREEN}✅ Active${NC}" +else + echo -e "${RED}❌ Not found${NC}" +fi + +# Check rosbridge +echo -n " ROSBridge Server: " +if ros2 node list 2>/dev/null | grep -q "rosbridge"; then + echo -e "${GREEN}✅ Active${NC}" +else + echo -e "${RED}❌ Not found${NC}" +fi + +# Check audio nodes +echo -n " Audio Capturer: " +if ros2 node list 2>/dev/null | grep -q "audio_capturer"; then + echo -e "${GREEN}✅ Active${NC}" +else + echo -e "${RED}❌ Not found${NC}" +fi + +echo -n " Audio Player: " +if ros2 node list 2>/dev/null | grep -q "audio_player"; then + echo -e "${GREEN}✅ Active${NC}" +else + echo -e "${RED}❌ Not found${NC}" +fi + +echo "" +echo -e "${BLUE}Key Topics:${NC}" + +# Check important topics +for topic in "/joints/right_arm" "/joints/left_arm" "/trace_publisher" "/camera/mobius/jpg" "/audio"; do + echo -n " $topic: " + if ros2 topic list 2>/dev/null | grep -q "$topic"; then + echo -e "${GREEN}✅ Available${NC}" + else + echo -e "${YELLOW}⚠️ Not found${NC}" + fi +done + +echo "" +echo -e "${BLUE}Serial Devices:${NC}" + +# Check serial devices +for device in "/dev/ttyACM0" "/dev/ttyACM1"; do + echo -n " $device: " + if [ -e "$device" ]; then + echo -e "${GREEN}✅ Connected${NC}" + else + echo -e "${RED}❌ Not found${NC}" + fi +done + +echo "" +echo -e "${BLUE}========================================${NC}" +echo -e "${BLUE}Commands:${NC}" +echo " Attach: tmux attach -t $SESSION_NAME" +echo " Stop: ~/stop_lucy.sh" +echo -e "${BLUE}========================================${NC}" + diff --git a/lucy_bringup/system_scripts/launch_lucy.sh b/lucy_bringup/system_scripts/launch_lucy.sh new file mode 100755 index 0000000..281cb6f --- /dev/null +++ b/lucy_bringup/system_scripts/launch_lucy.sh @@ -0,0 +1,160 @@ +#!/usr/bin/zsh +# Copyright 2024 Sentience Robotics Team +# Launch script for Lucy Robot System using tmux + +set -e # Exit on error + +SESSION_NAME="lucy" +WORKSPACE="$HOME/lucy_ws" +WEB_DIR="$HOME/web_control_panel" + +# Colors for output +GREEN='\033[0;32m' +BLUE='\033[0;34m' +YELLOW='\033[1;33m' +RED='\033[0;31m' +NC='\033[0m' # No Color + +echo -e "${GREEN}========================================${NC}" +echo -e "${GREEN}🤖 Launching Lucy Robot System...${NC}" +echo -e "${GREEN}========================================${NC}" + +# Check if tmux is installed +if ! command -v tmux &> /dev/null; then + echo -e "${RED}❌ tmux is not installed!${NC}" + echo "Install with: sudo apt install tmux" + exit 1 +fi + +# Check if tmux session already exists +if tmux has-session -t $SESSION_NAME 2>/dev/null; then + echo -e "${YELLOW}⚠️ Session '$SESSION_NAME' already exists!${NC}" + echo "" + echo "Options:" + echo " 1. Attach to existing session:" + echo -e " ${BLUE}tmux attach -t $SESSION_NAME${NC}" + echo "" + echo " 2. Kill and recreate:" + echo -e " ${BLUE}tmux kill-session -t $SESSION_NAME${NC}" + echo -e " ${BLUE}./launch_lucy.sh${NC}" + echo "" + echo " 3. Use the stop script:" + echo -e " ${BLUE}~/stop_lucy.sh${NC}" + exit 1 +fi + +# Check if workspace exists +if [ ! -d "$WORKSPACE" ]; then + echo -e "${RED}❌ Workspace not found: $WORKSPACE${NC}" + exit 1 +fi + +# Check if web directory exists +if [ ! -d "$WEB_DIR" ]; then + echo -e "${YELLOW}⚠️ Web directory not found: $WEB_DIR${NC}" + echo "Web interface will not be started." + WEB_AVAILABLE=false +else + WEB_AVAILABLE=true +fi + +# Source ROS2 workspace +echo -e "${BLUE}📦 Sourcing ROS2 workspace...${NC}" +source $WORKSPACE/install/setup.zsh + +# Create new tmux session with 3 panes +echo -e "${BLUE}🖥️ Creating tmux session with 3 panes...${NC}" + +# Create session with first pane +tmux new-session -d -s $SESSION_NAME -n "Lucy" + +# Split vertically (top/bottom) +tmux split-window -v -t $SESSION_NAME + +# Split the bottom pane horizontally (left/right) +tmux split-window -h -t $SESSION_NAME:0.1 + +# Adjust pane sizes (pane 0 gets 60% height, pane 1 gets 20 cols, pane 2 gets the rest) +tmux resize-pane -t $SESSION_NAME:0.0 -y 30 +tmux resize-pane -t $SESSION_NAME:0.1 -x 20 + +# ============================================ +# PANE 0 (Top - 60%): ROS2 Launch File +# ============================================ +echo -e "${BLUE}🤖 Setting up ROS2 nodes pane...${NC}" +tmux send-keys -t $SESSION_NAME:0.0 "source $WORKSPACE/install/setup.zsh" C-m +tmux send-keys -t $SESSION_NAME:0.0 "clear" C-m +tmux send-keys -t $SESSION_NAME:0.0 "echo '========================================'" C-m +tmux send-keys -t $SESSION_NAME:0.0 "echo '🤖 ROS2 System Nodes'" C-m +tmux send-keys -t $SESSION_NAME:0.0 "echo '========================================'" C-m +tmux send-keys -t $SESSION_NAME:0.0 "echo 'Note: Audio underrun warnings are normal when no audio is published'" C-m +tmux send-keys -t $SESSION_NAME:0.0 "sleep 1" C-m +tmux send-keys -t $SESSION_NAME:0.0 "ros2 launch lucy_bringup lucy.launch.py" C-m + +# ============================================ +# PANE 1 (Bottom-Left): Web Interface +# ============================================ +if [ "$WEB_AVAILABLE" = true ]; then + echo -e "${BLUE}🌐 Setting up web interface pane...${NC}" + tmux send-keys -t $SESSION_NAME:0.1 "cd $WEB_DIR" C-m + tmux send-keys -t $SESSION_NAME:0.1 "clear" C-m + tmux send-keys -t $SESSION_NAME:0.1 "echo '========================================'" C-m + tmux send-keys -t $SESSION_NAME:0.1 "echo '🌐 Web Interface'" C-m + tmux send-keys -t $SESSION_NAME:0.1 "echo '========================================'" C-m + tmux send-keys -t $SESSION_NAME:0.1 "sleep 2" C-m + tmux send-keys -t $SESSION_NAME:0.1 "yarn dev" C-m +else + tmux send-keys -t $SESSION_NAME:0.1 "clear" C-m + tmux send-keys -t $SESSION_NAME:0.1 "echo '⚠️ Web interface directory not found'" C-m + tmux send-keys -t $SESSION_NAME:0.1 "echo 'Expected: $WEB_DIR'" C-m +fi + +# ============================================ +# PANE 2 (Bottom-Right): Debug Terminal +# ============================================ +echo -e "${BLUE}📟 Setting up debug terminal pane...${NC}" +tmux send-keys -t $SESSION_NAME:0.2 "source $WORKSPACE/install/setup.zsh" C-m +tmux send-keys -t $SESSION_NAME:0.2 "clear" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo '========================================'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo '📟 Debug Terminal'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo '========================================'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ''" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo 'Useful commands:'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ' ros2 node list # List all nodes'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ' ros2 topic list # List all topics'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ' ros2 topic echo /joints/right_arm'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ' ros2 topic echo /joints/left_arm'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ' ros2 topic echo /trace_publisher'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ' ros2 topic hz /audio # Audio topic rate'" C-m +tmux send-keys -t $SESSION_NAME:0.2 "echo ''" C-m + +# Select the ROS pane as default +tmux select-pane -t $SESSION_NAME:0.0 + +echo "" +echo -e "${GREEN}========================================${NC}" +echo -e "${GREEN}✅ Lucy system started successfully!${NC}" +echo -e "${GREEN}========================================${NC}" +echo "" +echo -e "${BLUE}Session Control:${NC}" +echo " Attach to session: tmux attach -t $SESSION_NAME" +echo " Detach (keep running): Ctrl+B then D" +echo " Stop system: ~/stop_lucy.sh" +echo "" +echo -e "${BLUE}Pane Navigation:${NC}" +echo " Switch panes: Ctrl+B then arrow keys" +echo " Cycle panes: Ctrl+B then O" +echo " Zoom pane: Ctrl+B then Z (toggle fullscreen)" +echo " Scroll/search: Ctrl+B then [ (then q to exit)" +echo "" +echo -e "${BLUE}Pane Layout:${NC}" +echo " Pane 0 (Top 60%): ROS2 nodes" +echo " Pane 1 (Bottom-L ~12%): Web interface" +echo " Pane 2 (Bottom-R ~88%): Debug terminal" +echo "" +echo -e "${YELLOW}Attaching to session in 2 seconds...${NC}" +sleep 2 + +# Auto-attach to the session +tmux attach -t $SESSION_NAME + diff --git a/lucy_bringup/system_scripts/stop_lucy.sh b/lucy_bringup/system_scripts/stop_lucy.sh new file mode 100755 index 0000000..4a56163 --- /dev/null +++ b/lucy_bringup/system_scripts/stop_lucy.sh @@ -0,0 +1,44 @@ +#!/usr/bin/zsh +# Copyright 2024 Sentience Robotics Team +# Stop script for Lucy Robot System + +SESSION_NAME="lucy" + +# Colors for output +GREEN='\033[0;32m' +BLUE='\033[0;34m' +YELLOW='\033[1;33m' +RED='\033[0;31m' +NC='\033[0m' # No Color + +echo -e "${BLUE}========================================${NC}" +echo -e "${BLUE}🛑 Stopping Lucy Robot System...${NC}" +echo -e "${BLUE}========================================${NC}" + +# Check if tmux session exists +if ! tmux has-session -t $SESSION_NAME 2>/dev/null; then + echo -e "${YELLOW}⚠️ Session '$SESSION_NAME' is not running${NC}" + exit 0 +fi + +echo -e "${BLUE}📋 Gracefully stopping processes...${NC}" + +# Stop ROS nodes (pane 0) +echo -e "${BLUE} → Stopping ROS2 nodes...${NC}" +tmux send-keys -t $SESSION_NAME:0.0 C-c 2>/dev/null || true +sleep 2 + +# Stop web interface (pane 1) +echo -e "${BLUE} → Stopping web interface...${NC}" +tmux send-keys -t $SESSION_NAME:0.1 C-c 2>/dev/null || true +sleep 1 + +# Kill the tmux session +echo -e "${BLUE} → Terminating tmux session...${NC}" +tmux kill-session -t $SESSION_NAME 2>/dev/null || true + +echo "" +echo -e "${GREEN}========================================${NC}" +echo -e "${GREEN}✅ Lucy system stopped successfully!${NC}" +echo -e "${GREEN}========================================${NC}" +