feat: photo capture service (Issue #456)
This commit is contained in:
parent
3ea19fbb99
commit
5e62e81a97
@ -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
|
||||
@ -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'),
|
||||
}],
|
||||
),
|
||||
])
|
||||
@ -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()
|
||||
@ -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()
|
||||
Loading…
x
Reference in New Issue
Block a user