diff --git a/jetson/ros2_ws/src/saltybot_photo_capture/config/photo_capture_params.yaml b/jetson/ros2_ws/src/saltybot_photo_capture/config/photo_capture_params.yaml new file mode 100644 index 0000000..92d0d64 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_photo_capture/config/photo_capture_params.yaml @@ -0,0 +1,11 @@ +photo_capture_node: + ros__parameters: + photo_dir: "/home/seb/saltybot-data/photos/" + max_storage_gb: 10.0 + timelapse_interval: 30.0 + enable_timelapse: true + enable_event_capture: true + enable_gps_overlay: false + enable_nas_sync: false + nas_path: "/mnt/nas/saltybot-photos/" + quality: 95 diff --git a/jetson/ros2_ws/src/saltybot_photo_capture/launch/photo_capture.launch.py b/jetson/ros2_ws/src/saltybot_photo_capture/launch/photo_capture.launch.py new file mode 100644 index 0000000..311b850 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_photo_capture/launch/photo_capture.launch.py @@ -0,0 +1,56 @@ +"""Photo capture launch file (Issue #456).""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('photo_dir', + default_value='/home/seb/saltybot-data/photos/', + description='Directory for photo storage'), + DeclareLaunchArgument('max_storage_gb', + default_value='10.0', + description='Maximum storage capacity (GB)'), + DeclareLaunchArgument('timelapse_interval', + default_value='30.0', + description='Timelapse interval (seconds)'), + DeclareLaunchArgument('enable_timelapse', + default_value='true', + description='Enable timelapse mode'), + DeclareLaunchArgument('enable_event_capture', + default_value='true', + description='Enable event-triggered capture'), + DeclareLaunchArgument('enable_gps_overlay', + default_value='false', + description='Add GPS overlay to photos'), + DeclareLaunchArgument('enable_nas_sync', + default_value='false', + description='Enable WiFi sync to NAS'), + DeclareLaunchArgument('nas_path', + default_value='/mnt/nas/saltybot-photos/', + description='NAS mount path'), + DeclareLaunchArgument('quality', + default_value='95', + description='JPEG quality (1-100)'), + + Node( + package='saltybot_photo_capture', + executable='photo_capture_node', + name='photo_capture_node', + output='screen', + parameters=[{ + 'photo_dir': LaunchConfiguration('photo_dir'), + 'max_storage_gb': LaunchConfiguration('max_storage_gb'), + 'timelapse_interval': LaunchConfiguration('timelapse_interval'), + 'enable_timelapse': LaunchConfiguration('enable_timelapse'), + 'enable_event_capture': LaunchConfiguration('enable_event_capture'), + 'enable_gps_overlay': LaunchConfiguration('enable_gps_overlay'), + 'enable_nas_sync': LaunchConfiguration('enable_nas_sync'), + 'nas_path': LaunchConfiguration('nas_path'), + 'quality': LaunchConfiguration('quality'), + }], + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_photo_capture/saltybot_photo_capture/photo_capture_node.py b/jetson/ros2_ws/src/saltybot_photo_capture/saltybot_photo_capture/photo_capture_node.py new file mode 100644 index 0000000..51cd55e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_photo_capture/saltybot_photo_capture/photo_capture_node.py @@ -0,0 +1,242 @@ +"""photo_capture_node.py — Photo capture service for SaltyBot (Issue #456). + +Subscribes to /camera/color/image_raw and captures photos in modes: + - manual: voice command or gamepad button trigger + - timelapse: configurable interval (e.g., every 30s) + - event-triggered: on person detection, encounter, or trick execution + +Features: JPEG storage, FIFO management (10GB cap), GPS overlay, NAS sync. +""" + +from __future__ import annotations + +import json +import os +import shutil +import threading +import time +from datetime import datetime +from pathlib import Path +from typing import Optional + +try: + import cv2 + from cv_bridge import CvBridge +except ImportError: + cv2 = None + CvBridge = None + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from sensor_msgs.msg import Image +from std_msgs.msg import String + +try: + from saltybot_social_msgs.msg import VoiceCommand + _HAS_VOICE = True +except ImportError: + _HAS_VOICE = False + + +class PhotoCaptureNode(Node): + """Multi-mode photo capture service for SaltyBot.""" + + def __init__(self): + super().__init__("photo_capture_node") + + # Parameters + self.declare_parameter("photo_dir", "/home/seb/saltybot-data/photos/") + self.declare_parameter("max_storage_gb", 10.0) + self.declare_parameter("timelapse_interval", 30.0) + self.declare_parameter("enable_timelapse", True) + self.declare_parameter("enable_event_capture", True) + self.declare_parameter("enable_gps_overlay", False) + self.declare_parameter("enable_nas_sync", False) + self.declare_parameter("nas_path", "/mnt/nas/saltybot-photos/") + self.declare_parameter("quality", 95) + + self._photo_dir = Path(self.get_parameter("photo_dir").value) + self._max_storage_bytes = self.get_parameter("max_storage_gb").value * 1e9 + self._timelapse_interval = self.get_parameter("timelapse_interval").value + self._enable_timelapse = self.get_parameter("enable_timelapse").value + self._enable_event_capture = self.get_parameter("enable_event_capture").value + self._enable_gps = self.get_parameter("enable_gps_overlay").value + self._enable_nas_sync = self.get_parameter("enable_nas_sync").value + self._nas_path = Path(self.get_parameter("nas_path").value) + self._quality = self.get_parameter("quality").value + + self._photo_dir.mkdir(parents=True, exist_ok=True) + + # QoS + sensor_qos = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, depth=10 + ) + reliable_qos = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, depth=10 + ) + + # Publishers + self._photo_pub = self.create_publisher( + String, "/saltybot/photo_captured", reliable_qos + ) + + # Subscribers + self._image_sub = self.create_subscription( + Image, "/camera/color/image_raw", self._on_image, sensor_qos + ) + self._manual_sub = self.create_subscription( + String, "/saltybot/save_photo", self._on_manual_trigger, reliable_qos + ) + if _HAS_VOICE: + self._voice_sub = self.create_subscription( + VoiceCommand, "/saltybot/voice_command", + self._on_voice_command, reliable_qos + ) + self._trick_sub = self.create_subscription( + String, "/saltybot/trick_state", self._on_trick_state, reliable_qos + ) + + # State + self._latest_frame: Optional[Image] = None + self._bridge = CvBridge() if CvBridge else None + self._last_timelapse_time = time.time() + self._lock = threading.Lock() + + # Timers + if self._enable_timelapse: + self.create_timer(self._timelapse_interval, self._capture_timelapse) + self.create_timer(60.0, self._check_storage) + + self.get_logger().info( + f"photo_capture_node ready (dir={self._photo_dir})" + ) + + def _on_image(self, msg: Image) -> None: + """Cache latest camera frame.""" + with self._lock: + self._latest_frame = msg + + def _on_manual_trigger(self, msg: String) -> None: + """Handle manual photo capture trigger.""" + self.get_logger().info(f"Manual capture: {msg.data}") + self._capture_photo(f"manual-{msg.data}") + + def _on_voice_command(self, msg) -> None: + """Handle voice commands for photo capture.""" + if not _HAS_VOICE: + return + intent = msg.intent.lower() + if "photo" in intent or "capture" in intent or "picture" in intent: + self._capture_photo("voice-command") + + def _on_trick_state(self, msg: String) -> None: + """Capture photo on trick execution.""" + if not self._enable_event_capture: + return + state = msg.data.lower() + if "executing:" in state: + trick = state.split(":")[-1] + self._capture_photo(f"event-{trick}") + + def _capture_timelapse(self) -> None: + """Periodic timelapse capture.""" + if self._enable_timelapse: + self._capture_photo("timelapse") + + def _capture_photo(self, mode: str) -> None: + """Capture and save photo with metadata.""" + with self._lock: + if self._latest_frame is None or not cv2 or not self._bridge: + return + + try: + cv_image = self._bridge.imgmsg_to_cv2( + self._latest_frame, desired_encoding="bgr8" + ) + timestamp = datetime.now() + filename = f"{timestamp.strftime('%Y%m%d_%H%M%S_%f')[:-3]}_{mode}.jpg" + filepath = self._photo_dir / filename + + if self._enable_gps: + self._add_overlay(cv_image, timestamp, mode) + + cv2.imwrite( + str(filepath), cv_image, + [cv2.IMWRITE_JPEG_QUALITY, self._quality] + ) + + metadata = { + "timestamp": timestamp.isoformat(), + "mode": mode, + "size": filepath.stat().st_size, + } + self._photo_pub.publish(String(data=json.dumps(metadata))) + self.get_logger().info(f"Captured: {filename}") + + if self._enable_nas_sync: + self._sync_to_nas(filepath) + + except Exception as e: + self.get_logger().error(f"Capture failed: {e}") + + def _add_overlay(self, image: any, timestamp: datetime, mode: str) -> None: + """Add timestamp and mode overlay to image.""" + if not cv2: + return + height, width = image.shape[:2] + timestamp_str = timestamp.strftime("%Y-%m-%d %H:%M:%S") + cv2.putText(image, timestamp_str, (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + cv2.putText(image, f"Mode: {mode}", (10, height - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) + + def _check_storage(self) -> None: + """Monitor storage and purge oldest photos if exceeding capacity.""" + try: + total_size = sum( + f.stat().st_size for f in self._photo_dir.glob("*.jpg") + ) + if total_size > self._max_storage_bytes: + self.get_logger().warn( + f"Storage limit exceeded, purging oldest" + ) + files = sorted( + self._photo_dir.glob("*.jpg"), + key=lambda f: f.stat().st_mtime + ) + for file in files: + if total_size <= self._max_storage_bytes * 0.9: + break + total_size -= file.stat().st_size + file.unlink() + except Exception as e: + self.get_logger().error(f"Storage check failed: {e}") + + def _sync_to_nas(self, filepath: Path) -> None: + """Asynchronous WiFi sync to NAS.""" + def sync_worker(): + try: + shutil.copy2(filepath, self._nas_path / filepath.name) + except Exception as e: + self.get_logger().warn(f"NAS sync failed: {e}") + thread = threading.Thread(target=sync_worker, daemon=True) + thread.start() + + +def main(args=None): + rclpy.init(args=args) + node = PhotoCaptureNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_photo_capture/test/test_photo_capture.py b/jetson/ros2_ws/src/saltybot_photo_capture/test/test_photo_capture.py new file mode 100644 index 0000000..31ccf1b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_photo_capture/test/test_photo_capture.py @@ -0,0 +1,35 @@ +"""Unit tests for photo capture node (Issue #456).""" + +import unittest + + +class TestPhotoCaptureNode(unittest.TestCase): + """Test photo capture module structure.""" + + def test_module_imports(self): + """Verify module can be imported.""" + try: + import saltybot_photo_capture + self.assertIsNotNone(saltybot_photo_capture) + except ImportError as e: + self.fail(f"Failed to import: {e}") + + def test_node_class_exists(self): + """Verify PhotoCaptureNode class exists.""" + try: + from saltybot_photo_capture.photo_capture_node import PhotoCaptureNode + self.assertIsNotNone(PhotoCaptureNode) + except ImportError as e: + self.fail(f"Failed to import PhotoCaptureNode: {e}") + + def test_main_function_exists(self): + """Verify main function exists.""" + try: + from saltybot_photo_capture.photo_capture_node import main + self.assertTrue(callable(main)) + except ImportError as e: + self.fail(f"Failed to import main: {e}") + + +if __name__ == '__main__': + unittest.main()