diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..6444229 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,155 @@ +name: CI + +on: + push: + branches: [ main, dev ] + pull_request: + branches: [ main, dev ] + +jobs: + lint: + name: Lint + runs-on: ubuntu-latest + container: + image: osrf/ros:humble-desktop + steps: + - uses: actions/checkout@v4 + + - name: Install dependencies + run: | + apt-get update && apt-get install -y \ + python3-pip \ + python3-pytest \ + python3-pytest-mock \ + python3-opencv \ + gstreamer1.0-tools \ + gstreamer1.0-plugins-base \ + gstreamer1.0-plugins-good \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-plugins-ugly \ + v4l-utils \ + build-essential \ + cmake \ + git \ + && rm -rf /var/lib/apt/lists/* + + - name: Setup workspace structure + run: | + mkdir -p ws/src + cp -r camera_ros lucy_bringup ws/src/ + + - name: Build workspace + working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws + shell: bash + run: | + source /opt/ros/humble/setup.bash + colcon build --packages-select camera_ros lucy_bringup --cmake-args -DBUILD_TESTING=ON + + - name: Run linters + working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws + shell: bash + run: | + source /opt/ros/humble/setup.bash + source install/setup.bash + colcon test --packages-select camera_ros lucy_bringup --event-handlers console_direct+ || true + + - name: Check lint results + working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws + shell: bash + run: | + source /opt/ros/humble/setup.bash + source install/setup.bash + colcon test-result --verbose || true + + test: + name: Unit Tests + runs-on: ubuntu-latest + container: + image: osrf/ros:humble-desktop + steps: + - uses: actions/checkout@v4 + + - name: Install dependencies + run: | + apt-get update && apt-get install -y \ + python3-pip \ + python3-pytest \ + python3-pytest-mock \ + python3-opencv \ + gstreamer1.0-tools \ + gstreamer1.0-plugins-base \ + gstreamer1.0-plugins-good \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-plugins-ugly \ + v4l-utils \ + build-essential \ + cmake \ + git \ + && rm -rf /var/lib/apt/lists/* + + - name: Install Python test dependencies + run: | + pip3 install --no-cache-dir pytest pytest-mock pytest-cov + + - name: Setup workspace structure + run: | + mkdir -p ws/src + cp -r camera_ros lucy_bringup ws/src/ + + - name: Build workspace with tests + working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws + shell: bash + run: | + source /opt/ros/humble/setup.bash + colcon build --packages-select camera_ros lucy_bringup --cmake-args -DBUILD_TESTING=ON + + - name: Run unit tests + working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws + shell: bash + run: | + source /opt/ros/humble/setup.bash + source install/setup.bash + colcon test --packages-select camera_ros lucy_bringup --event-handlers console_direct+ + + - name: Check test results + working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws + shell: bash + run: | + source /opt/ros/humble/setup.bash + source install/setup.bash + colcon test-result --verbose + # Fail if any tests failed + if colcon test-result --verbose | grep -q "failed"; then + echo "Some tests failed!" + exit 1 + fi + + - name: Generate coverage report + working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws + shell: bash + run: | + source /opt/ros/humble/setup.bash + source install/setup.bash + cd build/camera_ros + python3 -m pytest ../../src/camera_ros/test/ \ + --cov=../../src/camera_ros/scripts \ + --cov-report=xml:coverage.xml \ + --cov-report=html:htmlcov \ + --cov-report=term-missing \ + --cov-branch || true + + - name: Upload coverage to Codecov + uses: codecov/codecov-action@v3 + with: + file: ${{ github.workspace }}/ws/build/camera_ros/coverage.xml + flags: unittests + name: codecov-umbrella + fail_ci_if_error: false + + - name: Upload coverage HTML report + uses: actions/upload-artifact@v4 + if: always() + with: + name: coverage-report + path: ${{ github.workspace }}/ws/build/camera_ros/htmlcov/ + retention-days: 30 diff --git a/.gitignore b/.gitignore index 1d17dae..d1e1aa1 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,57 @@ +# Python +__pycache__/ +*.py[cod] +*$py.class +*.so +.Python +*.egg-info/ +dist/ +build/ +.eggs/ + +# Virtual environments .venv +venv/ +ENV/ +env/ + +# Coverage reports +htmlcov/ +.coverage +.coverage.* +coverage.xml +*.cover +.hypothesis/ + +# Test artifacts +.pytest_cache/ +test_results/ +*.xunit.xml +.tox/ +.nox/ + +# ROS2 build artifacts +install/ +log/ +build/ + +# IDE +.vscode/ +.idea/ +*.swp +*.swo +*~ +.DS_Store + +# OS +Thumbs.db +.DS_Store + +# ROS2 specific +.ros/ + +# Temporary files +*.tmp +*.log +*.bak +*.orig diff --git a/camera_ros/CMakeLists.txt b/camera_ros/CMakeLists.txt index e6ed2ce..a66c993 100644 --- a/camera_ros/CMakeLists.txt +++ b/camera_ros/CMakeLists.txt @@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} # Install Python scripts install(PROGRAMS scripts/camera_publisher.py + scripts/camera_stream_controller.py DESTINATION lib/${PROJECT_NAME} ) @@ -54,7 +55,32 @@ install(FILES if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) ament_lint_auto_find_test_dependencies() + + # Install test files + install(DIRECTORY test + DESTINATION share/${PROJECT_NAME} + ) + + # Install pytest.ini for coverage configuration + install(FILES + pytest.ini + DESTINATION share/${PROJECT_NAME} + ) + + # Add pytest tests with coverage + ament_add_pytest_test(test_camera_publisher test/test_camera_publisher.py + TIMEOUT 60 + PYTHON_EXECUTABLE "${PYTHON3_EXECUTABLE}" + APPEND_ENV PYTHONPATH="${CMAKE_CURRENT_SOURCE_DIR}/scripts:${CMAKE_CURRENT_SOURCE_DIR}" + ) + + ament_add_pytest_test(test_camera_stream_controller test/test_camera_stream_controller.py + TIMEOUT 60 + PYTHON_EXECUTABLE "${PYTHON3_EXECUTABLE}" + APPEND_ENV PYTHONPATH="${CMAKE_CURRENT_SOURCE_DIR}/scripts:${CMAKE_CURRENT_SOURCE_DIR}" + ) endif() # Export the generated interfaces diff --git a/camera_ros/camera_ros/__init__.py b/camera_ros/camera_ros/__init__.py index c9b4174..138a49c 100644 --- a/camera_ros/camera_ros/__init__.py +++ b/camera_ros/camera_ros/__init__.py @@ -16,6 +16,7 @@ """ Camera ROS Package. -Zero-copy MJPEG camera publisher optimized for NVIDIA Jetson AGX Orin.""" +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 index f718b68..c018142 100644 --- a/camera_ros/launch/camera.launch.py +++ b/camera_ros/launch/camera.launch.py @@ -17,18 +17,21 @@ """ Launch file for zero-copy MJPEG camera publisher. -This launch file starts the camera node with optimized settings for NVIDIA Jetson AGX Orin.""" +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', + default_value='10.0', description='Camera frame rate (1.0-30.0 FPS)' ) @@ -38,6 +41,24 @@ def generate_launch_description(): description='Camera device path' ) + vendor_id_arg = DeclareLaunchArgument( + 'vendor_id', + default_value='', + description='USB vendor ID for camera identification (e.g., 0x046d)' + ) + + product_id_arg = DeclareLaunchArgument( + 'product_id', + default_value='', + description='USB product ID for camera identification' + ) + + serial_number_arg = DeclareLaunchArgument( + 'serial_number', + default_value='', + description='Camera serial number for identification' + ) + # Camera node camera_node = Node( package='camera_ros', @@ -47,12 +68,30 @@ def generate_launch_description(): parameters=[{ 'fps': LaunchConfiguration('fps'), 'device': LaunchConfiguration('device'), + 'vendor_id': LaunchConfiguration('vendor_id'), + 'product_id': LaunchConfiguration('product_id'), + 'serial_number': LaunchConfiguration('serial_number'), }], ) + # Camera stream controller node + camera_controller_node = Node( + package='camera_ros', + executable='camera_stream_controller.py', + name='camera_stream_controller', + output='screen', + ) + return LaunchDescription([ fps_arg, device_arg, - LogInfo(msg='Starting zero-copy MJPEG camera publisher with automatic client-based activation'), + vendor_id_arg, + product_id_arg, + serial_number_arg, + LogInfo( + msg='Starting zero-copy MJPEG camera publisher with ' + 'service-based streaming control' + ), camera_node, + camera_controller_node, ]) diff --git a/camera_ros/package.xml b/camera_ros/package.xml index 14c8b35..ce2e256 100644 --- a/camera_ros/package.xml +++ b/camera_ros/package.xml @@ -28,6 +28,10 @@ ament_lint_auto ament_lint_common + ament_cmake_pytest + python3-pytest + python3-pytest-mock + python3-pytest-cov rosidl_interface_packages diff --git a/camera_ros/pytest.ini b/camera_ros/pytest.ini new file mode 100644 index 0000000..783d60c --- /dev/null +++ b/camera_ros/pytest.ini @@ -0,0 +1,17 @@ +[pytest] +# Pytest configuration for camera_ros package +testpaths = test +python_files = test_*.py +python_classes = Test* +python_functions = test_* +addopts = + -v + --strict-markers + --tb=short + --cov=scripts + --cov-report=term-missing + --cov-report=html:htmlcov + --cov-report=xml:coverage.xml + --cov-branch + --no-cov-on-fail + diff --git a/camera_ros/scripts/camera_publisher.py b/camera_ros/scripts/camera_publisher.py index 5291095..335a0b4 100644 --- a/camera_ros/scripts/camera_publisher.py +++ b/camera_ros/scripts/camera_publisher.py @@ -18,81 +18,277 @@ from rclpy.node import Node from sensor_msgs.msg import CompressedImage from std_msgs.msg import Int32 +from std_srvs.srv import SetBool from camera_ros.srv import GetInt import cv2 -FPS = 15.0 +FPS = 10.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.""" + Publishes compressed JPEG images from camera with zero CPU transcoding + overhead. Supports service-based streaming control + (start_streaming/stop_streaming). 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') - + super().__init__('camera_publisher') + self.declare_parameter('fps', FPS) self.declare_parameter('device', CAMERA_DEVICE) - + self.declare_parameter('vendor_id', '') + self.declare_parameter('product_id', '') + self.declare_parameter('serial_number', '') + # 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.vendor_id = self.get_parameter('vendor_id').get_parameter_value().string_value + self.product_id = self.get_parameter('product_id').get_parameter_value().string_value + self.serial_number = self.get_parameter('serial_number').get_parameter_value().string_value + + # Find camera by vendor ID/serial if provided, or by default look for + # "webcamproduct: usb-webcam". If no specific ID provided, try to find + # the external webcam by name + if self.vendor_id or self.product_id or self.serial_number: + detected_device = self.find_camera_by_id( + self.vendor_id, self.product_id, self.serial_number + ) + if detected_device: + self.camera_device = detected_device + self.get_logger().info(f"Found camera by ID: {self.camera_device}") + else: + self.get_logger().warn( + f"Could not find camera by ID, using device path: " + f"{self.camera_device}" + ) + else: + # Default: try to find the external webcam by name + detected_device = self.find_camera_by_id() + if detected_device: + self.camera_device = detected_device + self.get_logger().info( + f"Found external camera (webcamproduct: usb-webcam): " + f"{self.camera_device}" + ) + else: + self.get_logger().info(f"Using default device path: {self.camera_device}") + + # Initialize camera hardware self.init_cap() + # Create publisher + self.jpg_pub = self.create_publisher(CompressedImage, 'ext_camera/jpg', 10) + + # Streaming state + self.is_streaming = False + self.client_count = 0 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") - + # Create timer for frame publishing (will be started/stopped by services) + self.timer = None - self.create_subscription(Int32, '/client_count', self.client_count_callback, 10) - + # Create services for streaming control + self.create_service(SetBool, 'start_streaming', self.start_streaming_callback) + self.create_service(SetBool, 'stop_streaming', self.stop_streaming_callback) self.create_service(GetInt, 'get_client_count', self.get_client_count) + # Subscribe to client count for automatic control + self.create_subscription(Int32, '/client_count', self.client_count_callback, 10) + + self.get_logger().info( + f"Camera publisher node started using device {self.camera_device} " + f"at {self.target_fps} FPS" + ) + def get_client_count(self, request, response): response.value = self.client_count return response + def start_streaming_callback(self, request, response): + """Service: Start streaming.""" + if not self.is_streaming: + self.is_streaming = True + # Start the timer if not already running + if self.timer is None: + self.timer = self.create_timer(0.01, self.publish_frame) # 100Hz timer + self.get_logger().info("Streaming started via service") + response.success = True + response.message = "Streaming started" + else: + response.success = True + response.message = "Streaming already active" + return response + + def stop_streaming_callback(self, request, response): + """Service: Stop streaming.""" + if self.is_streaming: + self.is_streaming = False + if self.timer is not None: + self.timer.cancel() + self.timer = None + self.get_logger().info("Streaming stopped via service") + response.success = True + response.message = "Streaming stopped" + return response + + def start_streaming_internal(self): + """Start streaming internally.""" + if not self.is_streaming: + self.is_streaming = True + if self.timer is None: + self.timer = self.create_timer(0.01, self.publish_frame) + self.get_logger().info( + f"{self.client_count} client(s) detected - starting camera " + f"streaming on {self.camera_device}" + ) + + def stop_streaming_internal(self): + """Stop streaming internally.""" + if self.is_streaming: + self.is_streaming = False + if self.timer is not None: + self.timer.cancel() + self.timer = None + self.get_logger().info( + f"No clients detected - stopping camera streaming on " + f"{self.camera_device}" + ) + def client_count_callback(self, msg): - """Automatically activate/deactivate camera based on client count""" + """Automatically start/stop streaming 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)") + + if msg.data > 0 and not self.is_streaming: + self.start_streaming_internal() + elif msg.data == 0 and self.is_streaming: + self.stop_streaming_internal() 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 find_camera_by_id(self, vendor_id=None, product_id=None, serial_number=None): + """ + Find camera device by vendor ID, product ID, serial number, or by name. + + Uses v4l2-ctl to enumerate devices and match identifiers. + Specifically looks for "webcamproduct: usb-webcam" to identify the + external camera. Returns device path (e.g., /dev/video6) or None if + not found. + """ + import subprocess + import re + + try: + # List all video devices + result = subprocess.run( + ['v4l2-ctl', '--list-devices'], + capture_output=True, text=True, timeout=5 + ) + # Check if stdout contains valid device information + # v4l2-ctl may return non-zero if it can't open /dev/video0, + # but still provide valid output for other devices + if not result.stdout or 'video' not in result.stdout.lower(): + self.get_logger().warn("v4l2-ctl not available or no output, using device path") + return None + + # Parse output to find matching device + lines = result.stdout.split('\n') + current_device_name = None + current_devices = [] + + for i, line in enumerate(lines): + # Check if line contains device name (before device paths) + if 'webcamproduct' in line.lower() or 'usb-webcam' in line.lower(): + # Found the external webcam - extract name + current_device_name = line.strip() + current_devices = [] + # Look ahead for device paths + for j in range(i + 1, min(i + 5, len(lines))): + if '/dev/video' in lines[j]: + match = re.search(r'/dev/video\d+', lines[j]) + if match: + current_devices.append(match.group(0)) + # Return the first video device (usually /dev/video6) + if current_devices: + self.get_logger().info( + f"Found external camera: {current_device_name} " + f"at {current_devices[0]}" + ) + return current_devices[0] + + # Check if line contains device path + elif '/dev/video' in line: + # Extract device path + match = re.search(r'/dev/video\d+', line) + if match: + # If we're in a section with a device name, associate it + if (current_device_name and + 'webcamproduct' in current_device_name.lower()): + self.get_logger().info( + f"Found external camera device: {match.group(0)}" + ) + return match.group(0) + + # If we have identifiers, try to match via sysfs + if vendor_id or product_id or serial_number: + try: + # Use sysfs to find matching device + import os + import glob + + for video_dev in glob.glob('/dev/video*'): + dev_num = video_dev.replace('/dev/video', '') + sysfs_path = f'/sys/class/video4linux/video{dev_num}/device' + + if os.path.exists(sysfs_path): + # Check vendor and product IDs + vendor_file = os.path.join(sysfs_path, 'idVendor') + product_file = os.path.join(sysfs_path, 'idProduct') + + if os.path.exists(vendor_file) and vendor_id: + with open(vendor_file, 'r') as f: + if vendor_id.lower() in f.read().lower(): + return video_dev + + if os.path.exists(product_file) and product_id: + with open(product_file, 'r') as f: + if product_id.lower() in f.read().lower(): + return video_dev + except Exception as e: + self.get_logger().debug(f"Error matching device by ID: {e}") + + # If no match found, return None to use fallback device path + return None + + except Exception as e: + self.get_logger().warn(f"Error finding camera by ID: {e}") + return None + def init_cap(self): # Create GStreamer pipeline with parameterized device + # Optimized for 1920x1080 at 30 FPS gst_pipeline = ( f"v4l2src device={self.camera_device} ! " - "image/jpeg,width=1280,height=720,framerate=30/1 ! " + "image/jpeg,width=1920,height=1080,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") + self.get_logger().error( + f"Failed to open camera on {self.camera_device} " + f"with GStreamer pipeline" + ) return else: self.get_logger().info(f"Camera on {self.camera_device} opened successfully") @@ -101,11 +297,11 @@ def init_cap(self): try: self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG')) - except: + except Exception: pass def publish_frame(self): - if not self.active: + if not self.is_streaming: return current_time = self.get_clock().now().nanoseconds / 1e9 @@ -114,7 +310,10 @@ def publish_frame(self): 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.get_logger().error( + f"Frame read failed on {self.camera_device}, " + f"reconnecting camera..." + ) self.cap.release() self.init_cap() return @@ -130,11 +329,13 @@ def publish_frame(self): 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/scripts/camera_stream_controller.py b/camera_ros/scripts/camera_stream_controller.py new file mode 100644 index 0000000..5c51728 --- /dev/null +++ b/camera_ros/scripts/camera_stream_controller.py @@ -0,0 +1,129 @@ +#!/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 std_msgs.msg import Int32 +from std_srvs.srv import SetBool + + +class CameraStreamController(Node): + """ + Controller node that manages camera streaming based on client count. + + Subscribes to /client_count topic and uses services to start/stop + camera streaming when clients connect/disconnect. + """ + + def __init__(self): + super().__init__('camera_stream_controller') + + # Subscribe to client count + self.create_subscription( + Int32, '/client_count', self.client_count_callback, 10 + ) + + # Service clients for streaming control + self.start_streaming_client = self.create_client( + SetBool, '/camera_publisher/start_streaming' + ) + self.stop_streaming_client = self.create_client( + SetBool, '/camera_publisher/stop_streaming' + ) + + self.current_client_count = 0 + self.is_streaming = False + + self.get_logger().info("Camera stream controller started") + + def client_count_callback(self, msg): + """Handle client count changes.""" + self.current_client_count = msg.data + + if msg.data > 0 and not self.is_streaming: + self.start_streaming() + elif msg.data == 0 and self.is_streaming: + self.stop_streaming() + + def start_streaming(self): + """Start streaming via service.""" + if not self.start_streaming_client.wait_for_service(timeout_sec=1.0): + self.get_logger().warn("start_streaming service not available") + return + + request = SetBool.Request() + request.data = True + future = self.start_streaming_client.call_async(request) + + def response_callback(future): + try: + response = future.result() + if response.success: + self.is_streaming = True + self.get_logger().info( + f"Camera streaming started " + f"({self.current_client_count} client(s))" + ) + else: + self.get_logger().warn( + f"Failed to start streaming: {response.message}" + ) + except Exception as e: + self.get_logger().error( + f"Exception calling start_streaming service: {e}" + ) + + future.add_done_callback(response_callback) + + def stop_streaming(self): + """Stop streaming via service.""" + if not self.stop_streaming_client.wait_for_service(timeout_sec=1.0): + self.get_logger().warn("stop_streaming service not available") + return + + request = SetBool.Request() + request.data = True + future = self.stop_streaming_client.call_async(request) + + def response_callback(future): + try: + response = future.result() + if response.success: + self.is_streaming = False + self.get_logger().info( + "Camera streaming stopped (no clients)" + ) + else: + self.get_logger().warn( + f"Failed to stop streaming: {response.message}" + ) + except Exception as e: + self.get_logger().error( + f"Exception calling stop_streaming service: {e}" + ) + + future.add_done_callback(response_callback) + + +def main(args=None): + rclpy.init(args=args) + node = CameraStreamController() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/camera_ros/test/README.md b/camera_ros/test/README.md new file mode 100644 index 0000000..c70929b --- /dev/null +++ b/camera_ros/test/README.md @@ -0,0 +1,62 @@ +# Camera ROS Unit Tests + +This directory contains unit tests for the `camera_ros` package. + +## Test Files + +- `test_camera_publisher.py` - Tests for CameraPublisher node +- `test_camera_stream_controller.py` - Tests for CameraStreamController node + +## Running Tests + +### Run all tests +```bash +cd ~/lucy_ws +colcon build +source install/setup.zsh +colcon test --packages-select camera_ros +colcon test-result --verbose +``` + +### Run specific test file +```bash +python3 -m pytest src/lucy_ros_packages/camera_ros/test/test_camera_publisher.py -v +``` + +### Run specific test +```bash +python3 -m pytest src/lucy_ros_packages/camera_ros/test/test_camera_publisher.py::TestCameraPublisher::test_camera_detection_webcamproduct -v +``` + +## Test Coverage + +The tests cover: +- Camera detection by webcamproduct name +- Fallback to default device +- Service-based streaming control (start/stop) +- Client count callback functionality +- Topic naming (ext_camera/jpg) +- Service responses + +### Generate Coverage Report + +**Note:** For best results, run tests through `colcon test` first, which properly sets up the ROS2 environment. + +**Option 1: Using colcon test (recommended)** +```bash +cd ~/lucy_ws + +colcon build +source install/setup.zsh + +colcon test --packages-select camera_ros +colcon test-result --verbose +``` + +## Notes + +- Tests use mocking to avoid requiring actual camera hardware +- cv2.VideoCapture and subprocess.run are mocked +- rclpy is initialized/shutdown for each test via fixture +- Coverage reports are generated automatically when using pytest directly + diff --git a/camera_ros/test/conftest.py b/camera_ros/test/conftest.py new file mode 100644 index 0000000..469d725 --- /dev/null +++ b/camera_ros/test/conftest.py @@ -0,0 +1,37 @@ +# 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 . + +"""Pytest configuration for camera_ros tests.""" + +import sys +from pathlib import Path + +# Set up Python path at module level to ensure it's available before any imports +# This is needed when running tests directly (not through colcon test) +workspace_path = Path("/home/dev/lucy_ws") +install_path = workspace_path / "install" / "camera_ros" + +if install_path.exists(): + python_version = f"{sys.version_info.major}.{sys.version_info.minor}" + dist_packages = install_path / "local" / "lib" / f"python{python_version}" / "dist-packages" + + if dist_packages.exists() and str(dist_packages) not in sys.path: + sys.path.insert(0, str(dist_packages)) + +# Add scripts directory +package_path = Path(__file__).parent.parent +scripts_path = package_path / "scripts" +if scripts_path.exists() and str(scripts_path) not in sys.path: + sys.path.insert(0, str(scripts_path)) diff --git a/camera_ros/test/test_camera_publisher.py b/camera_ros/test/test_camera_publisher.py new file mode 100644 index 0000000..d6812c9 --- /dev/null +++ b/camera_ros/test/test_camera_publisher.py @@ -0,0 +1,309 @@ +#!/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 . + +import pytest +import rclpy +import sys +import os +from unittest.mock import patch, MagicMock +from std_msgs.msg import Int32 +from std_srvs.srv import SetBool + +# Add scripts directory to path for imports +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'scripts')) + + +@pytest.fixture +def rclpy_init_shutdown(): + """Fixture to initialize and shutdown rclpy for each test.""" + rclpy.init() + yield + rclpy.shutdown() + + +class TestCameraPublisher: + """Unit tests for CameraPublisher node.""" + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_camera_detection_webcamproduct( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test that camera is detected by webcamproduct name.""" + from camera_publisher import CameraPublisher + + # Mock v4l2-ctl output + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = """NVIDIA Tegra Video Input Device (platform:tegra-camrtc-ca): + /dev/media0 + +webcamproduct: usb-webcam (usb-3610000.usb-4.1.2.2): + /dev/video6 + /dev/video7 + /dev/media3 +""" + + # Mock VideoCapture + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + + assert node.camera_device == '/dev/video6' + mock_videocapture.assert_called_once() + topic_name = node.jpg_pub.topic_name + assert topic_name == '/ext_camera/jpg' or topic_name == 'ext_camera/jpg' + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_camera_detection_fallback( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test fallback to default device when camera not found.""" + from camera_publisher import CameraPublisher + + # Mock v4l2-ctl output without webcamproduct + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = """NVIDIA Tegra Video Input Device: + /dev/video0 +""" + + # Mock VideoCapture + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + + # Should use default device + assert node.camera_device == '/dev/video0' + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_camera_detection_nonzero_exit_code( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test that camera detection works even when v4l2-ctl returns non-zero.""" + from camera_publisher import CameraPublisher + + # Mock v4l2-ctl with non-zero return code but valid stdout + # This simulates the real behavior where v4l2-ctl returns 1 if it + # can't open /dev/video0, but still lists available devices + mock_subprocess.return_value.returncode = 1 + mock_subprocess.return_value.stdout = """NVIDIA Tegra Video Input Device (platform:tegra-camrtc-ca): + /dev/media0 + +webcamproduct: usb-webcam (usb-3610000.usb-4.1.2.2): + /dev/video6 + /dev/video7 + /dev/media3 +""" + + # Mock VideoCapture + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + + # Should still detect the camera despite non-zero return code + assert node.camera_device == '/dev/video6' + mock_videocapture.assert_called_once() + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_camera_detection_no_output( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test fallback when v4l2-ctl produces no output.""" + from camera_publisher import CameraPublisher + + # Mock v4l2-ctl with no output (truly unavailable) + mock_subprocess.return_value.returncode = 1 + mock_subprocess.return_value.stdout = "" + + # Mock VideoCapture + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + + # Should fall back to default device when no output + assert node.camera_device == '/dev/video0' + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_start_streaming_service( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test start_streaming service.""" + from camera_publisher import CameraPublisher + + # Mock subprocess and VideoCapture + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = ( + "webcamproduct: usb-webcam:\n /dev/video6\n" + ) + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + node.is_streaming = False + + request = SetBool.Request() + request.data = True + response = node.start_streaming_callback(request, SetBool.Response()) + + assert response.success is True + assert node.is_streaming is True + assert "started" in response.message.lower() + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_stop_streaming_service( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test stop_streaming service.""" + from camera_publisher import CameraPublisher + + # Mock subprocess and VideoCapture + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = ( + "webcamproduct: usb-webcam:\n /dev/video6\n" + ) + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + node.is_streaming = True + node.timer = MagicMock() + + request = SetBool.Request() + request.data = True + response = node.stop_streaming_callback(request, SetBool.Response()) + + assert response.success is True + assert node.is_streaming is False + assert "stopped" in response.message.lower() + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_client_count_callback_start( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test client count callback starts streaming.""" + from camera_publisher import CameraPublisher + + # Mock subprocess and VideoCapture + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = ( + "webcamproduct: usb-webcam:\n /dev/video6\n" + ) + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + node.is_streaming = False + + msg = Int32() + msg.data = 1 + node.client_count_callback(msg) + + assert node.is_streaming is True + assert node.client_count == 1 + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_client_count_callback_stop( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test client count callback stops streaming.""" + from camera_publisher import CameraPublisher + + # Mock subprocess and VideoCapture + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = ( + "webcamproduct: usb-webcam:\n /dev/video6\n" + ) + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + node.is_streaming = True + node.timer = MagicMock() + + msg = Int32() + msg.data = 0 + node.client_count_callback(msg) + + assert node.is_streaming is False + assert node.client_count == 0 + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_get_client_count_service( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test get_client_count service.""" + # Import here to ensure path is set up + from camera_ros.srv import GetInt + from camera_publisher import CameraPublisher + + # Mock subprocess and VideoCapture + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = ( + "webcamproduct: usb-webcam:\n /dev/video6\n" + ) + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + node.client_count = 5 + + request = GetInt.Request() + response = node.get_client_count(request, GetInt.Response()) + + assert response.value == 5 + + @patch('cv2.VideoCapture') + @patch('subprocess.run') + def test_topic_name_ext_camera( + self, mock_subprocess, mock_videocapture, rclpy_init_shutdown + ): + """Test that topic name is ext_camera/jpg.""" + from camera_publisher import CameraPublisher + + # Mock subprocess and VideoCapture + mock_subprocess.return_value.returncode = 0 + mock_subprocess.return_value.stdout = ( + "webcamproduct: usb-webcam:\n /dev/video6\n" + ) + mock_cap = MagicMock() + mock_cap.isOpened.return_value = True + mock_videocapture.return_value = mock_cap + + node = CameraPublisher() + + # Topic name may have leading slash + topic_name = node.jpg_pub.topic_name + assert topic_name == '/ext_camera/jpg' or topic_name == 'ext_camera/jpg' diff --git a/camera_ros/test/test_camera_stream_controller.py b/camera_ros/test/test_camera_stream_controller.py new file mode 100644 index 0000000..d3d087e --- /dev/null +++ b/camera_ros/test/test_camera_stream_controller.py @@ -0,0 +1,137 @@ +#!/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 . + +import pytest +import rclpy +import sys +import os +from unittest.mock import Mock, patch, MagicMock +from std_msgs.msg import Int32 + +# Add scripts directory to path for imports +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'scripts')) + + +@pytest.fixture +def rclpy_init_shutdown(): + """Fixture to initialize and shutdown rclpy for each test.""" + rclpy.init() + yield + rclpy.shutdown() + + +class TestCameraStreamController: + """Unit tests for CameraStreamController node.""" + + def test_node_initialization(self, rclpy_init_shutdown): + """Test that controller node initializes correctly.""" + from camera_stream_controller import CameraStreamController + + node = CameraStreamController() + + assert node.get_name() == 'camera_stream_controller' + assert node.current_client_count == 0 + assert node.is_streaming is False + assert node.start_streaming_client is not None + assert node.stop_streaming_client is not None + + def test_client_count_callback_start(self, rclpy_init_shutdown): + """Test client count callback starts streaming when clients > 0.""" + from camera_stream_controller import CameraStreamController + with patch.object(CameraStreamController, 'start_streaming') as mock_start: + node = CameraStreamController() + node.is_streaming = False + + msg = Int32() + msg.data = 1 + node.client_count_callback(msg) + + mock_start.assert_called_once() + assert node.current_client_count == 1 + + def test_client_count_callback_stop(self, rclpy_init_shutdown): + """Test client count callback stops streaming when clients == 0.""" + from camera_stream_controller import CameraStreamController + with patch.object(CameraStreamController, 'stop_streaming') as mock_stop: + node = CameraStreamController() + node.is_streaming = True + + msg = Int32() + msg.data = 0 + node.client_count_callback(msg) + + mock_stop.assert_called_once() + assert node.current_client_count == 0 + + def test_start_streaming_service_available(self, rclpy_init_shutdown): + """Test start_streaming when service is available.""" + from camera_stream_controller import CameraStreamController + + node = CameraStreamController() + node.start_streaming_client.wait_for_service = Mock(return_value=True) + node.start_streaming_client.call_async = Mock(return_value=MagicMock()) + + node.start_streaming() + + node.start_streaming_client.wait_for_service.assert_called_once_with( + timeout_sec=1.0 + ) + node.start_streaming_client.call_async.assert_called_once() + + def test_start_streaming_service_unavailable(self, rclpy_init_shutdown): + """Test start_streaming when service is unavailable.""" + from camera_stream_controller import CameraStreamController + + node = CameraStreamController() + node.start_streaming_client.wait_for_service = Mock(return_value=False) + node.get_logger = Mock() + + node.start_streaming() + + node.start_streaming_client.wait_for_service.assert_called_once_with( + timeout_sec=1.0 + ) + node.get_logger().warn.assert_called() + + def test_stop_streaming_service_available(self, rclpy_init_shutdown): + """Test stop_streaming when service is available.""" + from camera_stream_controller import CameraStreamController + + node = CameraStreamController() + node.stop_streaming_client.wait_for_service = Mock(return_value=True) + node.stop_streaming_client.call_async = Mock(return_value=MagicMock()) + + node.stop_streaming() + + node.stop_streaming_client.wait_for_service.assert_called_once_with( + timeout_sec=1.0 + ) + node.stop_streaming_client.call_async.assert_called_once() + + def test_stop_streaming_service_unavailable(self, rclpy_init_shutdown): + """Test stop_streaming when service is unavailable.""" + from camera_stream_controller import CameraStreamController + + node = CameraStreamController() + node.stop_streaming_client.wait_for_service = Mock(return_value=False) + node.get_logger = Mock() + + node.stop_streaming() + + node.stop_streaming_client.wait_for_service.assert_called_once_with( + timeout_sec=1.0 + ) + node.get_logger().warn.assert_called() diff --git a/lucy_bringup/launch/lucy.launch.py b/lucy_bringup/launch/lucy.launch.py index 87d5990..fcf2348 100755 --- a/lucy_bringup/launch/lucy.launch.py +++ b/lucy_bringup/launch/lucy.launch.py @@ -22,6 +22,7 @@ - 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) +- External USB webcam (for additional vision stream) Optimized for NVIDIA Jetson AGX Orin. """ @@ -96,59 +97,71 @@ def create_audio_nodes(sample_rate, capture_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') - ) - + + # Audio nodes are created but currently disabled + # 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) + + # External USB webcam (camera_ros package) + camera_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('camera_ros'), + 'launch', + 'camera.launch.py' + ]) + ]) + ) + + # RealSense D435i Camera realsense_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ @@ -158,7 +171,7 @@ def generate_launch_description(): ]) ]) ) - + return LaunchDescription([ # Launch arguments device0_arg, @@ -166,25 +179,26 @@ def generate_launch_description(): 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 + # *audio_nodes, # Unpack audio nodes (currently disabled) + camera_launch, # External USB webcam 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=' - External USB Webcam: Stream 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 index 1675bf0..f19f836 100644 --- a/lucy_bringup/launch/realsense.launch.py +++ b/lucy_bringup/launch/realsense.launch.py @@ -15,17 +15,21 @@ # along with this program. If not, see . """ -Minimal launch file for Intel RealSense D435i camera. +Launch file for Intel RealSense D435i camera. + +Optimized for hand tracking at 30-60cm range (robot head-mounted 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. +Features: +- Depth-to-color alignment for RGBD +- Colorizer filter for JPEG compression +- Post-processing filters optimized for hand tracking: + * Spatial filter: Edge-preserving smoothing + * Temporal filter: Noise reduction over time + * Hole filling: Fills gaps in depth data +- Depth module preset optimized for close-range hand detection """ from launch import LaunchDescription @@ -44,13 +48,44 @@ def generate_launch_description(): '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 + + # Depth module optimization for hand tracking (30-60cm range) + # Preset values: 0=Custom, 1=Default, 2=Hand, 3=High Accuracy, + # 4=High Density, 5=Medium Density + 'depth_module.visual_preset': 2, # 2=Hand preset (optimized for hand tracking) + + # Enable colorizer filter to colorize depth for JPEG compression + 'colorizer.enable': True, + 'colorizer.visual_preset': 2, # 0=Jet, 1=Classic, 2=WhiteToBlack, 3=BlackToWhite, etc. + + # Post-processing filters for improved depth quality at close range + # Spatial filter: Edge-preserving smoothing (reduces noise, maintains edges) + 'spatial_filter.enable': True, + 'spatial_filter.filter_magnitude': 2, # 1-5, higher = more smoothing + 'spatial_filter.filter_smooth_alpha': 0.5, # 0.25-0.75, edge-preserving strength + 'spatial_filter.filter_smooth_delta': 20, # 1-50, edge threshold + 'spatial_filter.holes_fill': 1, # 0-5, fill small holes + + # Temporal filter: Reduces noise by averaging over time (good for hand tracking) + 'temporal_filter.enable': True, + 'temporal_filter.filter_smooth_alpha': 0.4, # 0.0-1.0, persistence weight + 'temporal_filter.filter_smooth_delta': 20, # Edge threshold + 'temporal_filter.holes_fill': 1, # Fill temporal holes + + # Hole filling filter: Fills remaining gaps in depth data + 'hole_filling_filter.enable': True, + # 0=fill_from_left, 1=farest_from_around, 2=nearest_from_around + 'hole_filling_filter.holes_fill': 1, + + # Disable decimation filter (not needed for close-range, reduces resolution) + 'decimation_filter.enable': False, } - + realsense_node = Node( package='realsense2_camera', executable='realsense2_camera_node', @@ -61,7 +96,7 @@ def generate_launch_description(): respawn=True, respawn_delay=2.0, ) - + return LaunchDescription([ realsense_node, ]) diff --git a/lucy_bringup/package.xml b/lucy_bringup/package.xml index c89d7e1..f28ed54 100644 --- a/lucy_bringup/package.xml +++ b/lucy_bringup/package.xml @@ -18,6 +18,11 @@ launch launch_ros + ament_lint_auto + ament_cmake_pytest + python3-pytest + python3-pytest-mock + ament_cmake diff --git a/lucy_bringup/system_scripts/check_lucy.sh b/lucy_bringup/system_scripts/check_lucy.sh index 811ba84..d76669d 100755 --- a/lucy_bringup/system_scripts/check_lucy.sh +++ b/lucy_bringup/system_scripts/check_lucy.sh @@ -1,5 +1,5 @@ #!/usr/bin/zsh -# Copyright 2024 Sentience Robotics Team +# Copyright 2025 Sentience Robotics Team # Health check script for Lucy Robot System SESSION_NAME="lucy" @@ -55,6 +55,14 @@ else echo -e "${RED}❌ Not found${NC}" fi +# Check camera stream controller +echo -n " Camera Stream Controller: " +if ros2 node list 2>/dev/null | grep -q "camera_stream_controller"; 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 @@ -63,6 +71,14 @@ else echo -e "${RED}❌ Not found${NC}" fi +# Check realsense node +echo -n " Realsense Camera: " +if ros2 node list 2>/dev/null | grep -q "realsense2_camera"; 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 @@ -78,11 +94,24 @@ else echo -e "${RED}❌ Not found${NC}" fi +echo "" +echo -e "${BLUE}Camera Services:${NC}" + +# Check camera services +for service in "/camera_publisher/start_streaming" "/camera_publisher/stop_streaming"; do + echo -n " $service: " + if ros2 service list 2>/dev/null | grep -q "$service"; then + echo -e "${GREEN}✅ Available${NC}" + else + echo -e "${YELLOW}⚠️ Not found${NC}" + fi +done + 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 +for topic in "/joints/right_arm" "/joints/left_arm" "/trace_publisher" "/ext_camera/jpg" "/audio"; do echo -n " $topic: " if ros2 topic list 2>/dev/null | grep -q "$topic"; then echo -e "${GREEN}✅ Available${NC}" diff --git a/lucy_bringup/system_scripts/launch_lucy.sh b/lucy_bringup/system_scripts/launch_lucy.sh index 281cb6f..81d8f02 100755 --- a/lucy_bringup/system_scripts/launch_lucy.sh +++ b/lucy_bringup/system_scripts/launch_lucy.sh @@ -1,5 +1,5 @@ #!/usr/bin/zsh -# Copyright 2024 Sentience Robotics Team +# Copyright 2025 Sentience Robotics Team # Launch script for Lucy Robot System using tmux set -e # Exit on error @@ -83,12 +83,6 @@ tmux resize-pane -t $SESSION_NAME:0.1 -x 20 # ============================================ 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 # ============================================ @@ -97,12 +91,8 @@ tmux send-keys -t $SESSION_NAME:0.0 "ros2 launch lucy_bringup lucy.launch.py" C- 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 + tmux send-keys -t $SESSION_NAME:0.1 "yarn dev --host" 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 @@ -115,18 +105,6 @@ fi 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 diff --git a/lucy_bringup/system_scripts/stop_lucy.sh b/lucy_bringup/system_scripts/stop_lucy.sh index 4a56163..45a25ac 100755 --- a/lucy_bringup/system_scripts/stop_lucy.sh +++ b/lucy_bringup/system_scripts/stop_lucy.sh @@ -23,6 +23,18 @@ fi echo -e "${BLUE}📋 Gracefully stopping processes...${NC}" +# Source ROS2 workspace for service calls +source $HOME/lucy_ws/install/setup.zsh 2>/dev/null || true + +# Stop camera streaming via service (with timeout) +echo -e "${BLUE} → Stopping camera streaming...${NC}" +if ros2 service list 2>/dev/null | grep -q "/camera_publisher/stop_streaming"; then + timeout 2 ros2 service call /camera_publisher/stop_streaming std_srvs/srv/SetBool "{data: true}" 2>/dev/null || true +else + echo -e "${YELLOW} ⚠️ Camera service not available${NC}" +fi +sleep 1 + # 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