commit 7167f559e8d66f2442ae9e1c4bf63531d5c5e03e Author: chyoo Date: Wed Sep 23 13:52:13 2026 +0900 feat(camera): add configurable Jetson camera module for LMS visualization diff --git a/.autonomy-studio-module.json b/.autonomy-studio-module.json new file mode 100644 index 0000000..bea9070 --- /dev/null +++ b/.autonomy-studio-module.json @@ -0,0 +1,7 @@ +{ + "packageName": "camera_test_pkg", + "launchFile": "camera.launch.py", + "dockerImage": "future-nav2-camera-test:humble-jetson_orin", + "dockerBuildContext": "docker", + "devices": ["/dev/video0"] +} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6d02bdc --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +build/ +install/ +log/ +__pycache__/ +*.py[cod] diff --git a/README.md b/README.md new file mode 100644 index 0000000..445c9bd --- /dev/null +++ b/README.md @@ -0,0 +1,55 @@ +# NAV2-Camera (Jetson) + +Jetson ARM64 USB/V4L2 camera module for the LMS visualization camera panel. +The template folder remains `[lms-nav2-camera-test]nav2-camera-test_v1.0.0_ws`. + +## Runtime + +- Image: `future-nav2-camera-test:humble-jetson_orin` +- Package: `camera_test_pkg` +- Launch: `camera.launch.py` +- Viewer topic: `/camera_test/image_jpeg` (`sensor_msgs/msg/CompressedImage`, sensor-data QoS) +- Only the selected `/dev/videoN` device is passed into the container. +- No vehicle commands, fabricated calibration, or fixed camera mounting TF are published. + +## Parameters + +| Parameter | Default | Meaning | +| --- | --- | --- | +| `video_device` | `/dev/video0` | V4L2 camera device | +| `width` / `height` | `640` / `480` | Capture resolution supported by the camera | +| `fps` | `30` | Capture frame rate | +| `publish_fps` | `10` | Maximum publication rate | +| `pixel_format` | `auto` | `auto`, `MJPEG`, `UYVY`, or `YUY2` | +| `jpeg_width` | `640` | Maximum preview width | +| `jpeg_topic` | `/camera_test/image_jpeg` | Keep the default for the LMS viewer | +| `frame_id` | `camera_test_link` | Image frame identifier | +| `publish_raw` | `false` | Also publish `/camera_test/image_raw` | + +`auto` negotiates raw/JPEG encoding, not resolution or frame rate. Use a mode +reported by `v4l2-ctl --device /dev/video0 --list-formats-ext` when defaults are unsupported. +This module provides a video preview, not calibrated visual SLAM input. + +## LMS Installation + +The environment profile supplies the runtime image artifact; the module supplies +the ARM64 build/install artifact. Installing these avoids an initial source build. +The LMS app needs camera-test module routing support before running this template. +ROS domain and DDS settings are supplied by the LMS workspace runtime. + +## Manual Development + +Provision the image from its published artifact first. For image maintainers only: + +```sh +docker build -t future-nav2-camera-test:humble-jetson_orin docker +``` + +```sh +./build.sh +ROS_DOMAIN_ID=77 ./run.sh video_device:=/dev/video0 pixel_format:=auto +``` + +The standalone script defaults to domain 77, host networking, and +`ROS_LOCALHOST_ONLY=0`. LMS execution uses workspace configuration instead. +Tests in `tests/test_camera.py` use synthetic video and never access a camera. diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..7e96281 --- /dev/null +++ b/build.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +MODULE_ROOT="$(dirname "$MODULE_PATH")" +MODULE_FOLDER="$(basename "$MODULE_PATH")" +IMAGE="${NAV2_DOCKER_IMAGE:-future-nav2-camera-test:humble-jetson_orin}" + +docker image inspect "$IMAGE" >/dev/null +docker run --rm \ + -v "$MODULE_ROOT:/modules" \ + -w "/modules/$MODULE_FOLDER" \ + "$IMAGE" bash -lc ' + set -e + source /opt/ros/humble/setup.bash + rm -rf build install log + colcon --log-base log build --base-paths src --build-base build --install-base install \ + --packages-up-to camera_test_pkg --cmake-args -DBUILD_TESTING=OFF + ' + +exec docker run --rm -v "$MODULE_ROOT:/modules" "$IMAGE" bash -lc \ + "chown -R $(id -u):$(id -g) '/modules/$MODULE_FOLDER/build' '/modules/$MODULE_FOLDER/install' '/modules/$MODULE_FOLDER/log'" diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..c224f19 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,16 @@ +FROM future-nav2:humble-jetson_orin + +SHELL ["/bin/bash", "-o", "pipefail", "-c"] +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + python3-numpy \ + python3-opencv \ + python3-gi \ + gir1.2-gstreamer-1.0 \ + gir1.2-gst-plugins-base-1.0 \ + gstreamer1.0-plugins-base \ + gstreamer1.0-plugins-good \ + gstreamer1.0-tools \ + && rm -rf /var/lib/apt/lists/* diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..d76cf71 --- /dev/null +++ b/run.sh @@ -0,0 +1,27 @@ +#!/usr/bin/env bash +set -euo pipefail + +MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +FOLDER_NAME="$(basename "$MODULE_PATH")" +CONTAINER_NAME="${FOLDER_NAME#[}"; CONTAINER_NAME="${CONTAINER_NAME/]/-}" +CONTAINER_NAME="${CONTAINER_NAME%_ws}" +MODULE_ROOT="$(dirname "$MODULE_PATH")" +IMAGE="${NAV2_DOCKER_IMAGE:-future-nav2-camera-test:humble-jetson_orin}" +DEVICE=/dev/video0 +for arg in "$@"; do + case "$arg" in video_device:=*) DEVICE=${arg#video_device:=} ;; esac +done +[[ "$DEVICE" =~ ^/dev/video[0-9]+$ ]] && [ -c "$DEVICE" ] || { echo "Camera device not found: $DEVICE" >&2; exit 1; } +test -f "$MODULE_PATH/install/setup.bash" || { echo 'Module is not built' >&2; exit 1; } + +docker rm -f "$CONTAINER_NAME" >/dev/null 2>&1 || true +exec docker run --rm -d --device "$DEVICE:$DEVICE" --network host --ipc=host \ + --name "$CONTAINER_NAME" \ + -e "ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-77}" \ + -e "ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0}" \ + -v "$MODULE_ROOT:/modules" \ + "$IMAGE" bash -lc " + source /opt/ros/humble/setup.bash + source '/modules/$FOLDER_NAME/install/setup.bash' + exec ros2 launch camera_test_pkg camera.launch.py \"\$@\" + " bash "$@" diff --git a/src/camera_test_pkg/camera_test_pkg/__init__.py b/src/camera_test_pkg/camera_test_pkg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/camera_test_pkg/camera_test_pkg/gstreamer_camera.py b/src/camera_test_pkg/camera_test_pkg/gstreamer_camera.py new file mode 100644 index 0000000..406eb36 --- /dev/null +++ b/src/camera_test_pkg/camera_test_pkg/gstreamer_camera.py @@ -0,0 +1,125 @@ +import re + +import cv2 +import gi +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import CompressedImage, Image + +gi.require_version('Gst', '1.0') +gi.require_version('GstVideo', '1.0') +from gi.repository import Gst, GstVideo + + +def capture_pipeline(device, width, height, fps, pixel_format): + if not re.fullmatch(r'/dev/video[0-9]+', device): + raise ValueError('video_device must be /dev/videoN') + if not (1 <= width <= 4096 and 1 <= height <= 2160 and 1 <= fps <= 120): + raise ValueError('Invalid camera resolution or fps') + size = f'width={width},height={height},framerate={fps}/1' + formats = { + 'auto': f'video/x-raw,{size};image/jpeg,{size}', + 'UYVY': f'video/x-raw,format=UYVY,{size}', + 'YUY2': f'video/x-raw,format=YUY2,{size}', + 'MJPEG': f'image/jpeg,{size}', + } + if pixel_format not in formats: + raise ValueError('pixel_format must be auto, UYVY, YUY2 or MJPEG') + return (f'v4l2src device={device} ! {formats[pixel_format]} ! ' + 'decodebin ! videoconvert ! video/x-raw,format=RGB ! ' + 'appsink name=camera_sink max-buffers=1 drop=true sync=false') + + +class GstreamerCameraTest(Node): + def __init__(self): + super().__init__('camera_test') + defaults = { + 'video_device': '/dev/video0', 'width': 640, 'height': 480, + 'fps': 30, 'publish_fps': 10, 'pixel_format': 'auto', + 'frame_id': 'camera_test_link', 'jpeg_width': 640, + 'jpeg_topic': '/camera_test/image_jpeg', 'publish_raw': False, + } + for key, value in defaults.items(): + self.declare_parameter(key, value) + p = {key: self.get_parameter(key).value for key in defaults} + if not 1 <= p['publish_fps'] <= 60 or not 160 <= p['jpeg_width'] <= 4096: + raise ValueError('Invalid publish_fps or jpeg_width') + self.frame_id = p['frame_id'] + self.jpeg_width = p['jpeg_width'] + self.jpeg_publisher = self.create_publisher( + CompressedImage, p['jpeg_topic'], qos_profile_sensor_data) + self.raw_publisher = self.create_publisher( + Image, '/camera_test/image_raw', qos_profile_sensor_data) if p['publish_raw'] else None + Gst.init(None) + self.pipeline = Gst.parse_launch(capture_pipeline( + p['video_device'], p['width'], p['height'], p['fps'], p['pixel_format'])) + self.sink = self.pipeline.get_by_name('camera_sink') + self.bus = self.pipeline.get_bus() + if self.pipeline.set_state(Gst.State.PLAYING) == Gst.StateChangeReturn.FAILURE: + self.pipeline.set_state(Gst.State.NULL) + raise RuntimeError(f"Cannot open camera: {p['video_device']}") + self.timer = self.create_timer(1.0 / p['publish_fps'], self.capture) + self.get_logger().info(f"Camera {p['video_device']} -> {p['jpeg_topic']}") + + def capture(self): + message = self.bus.pop_filtered(Gst.MessageType.ERROR | Gst.MessageType.EOS) + if message is not None: + detail = message.parse_error()[0].message if message.type == Gst.MessageType.ERROR else 'End of stream' + raise RuntimeError(f'Camera stopped: {detail}') + sample = self.sink.emit('try-pull-sample', 0) + if sample is None: + return + info = GstVideo.VideoInfo.new_from_caps(sample.get_caps()) + buffer = sample.get_buffer() + success, mapped = buffer.map(Gst.MapFlags.READ) + if not success: + return + try: + # V4L2/GStreamer RGB rows can include alignment padding. + rgb = np.ndarray((info.height, info.width, 3), dtype=np.uint8, + buffer=mapped.data, offset=info.offset[0], + strides=(info.stride[0], 3, 1)).copy() + finally: + buffer.unmap(mapped) + stamp = self.get_clock().now().to_msg() + if self.raw_publisher is not None: + raw = Image() + raw.header.stamp, raw.header.frame_id = stamp, self.frame_id + raw.height, raw.width = info.height, info.width + raw.encoding, raw.step, raw.data = 'rgb8', info.width * 3, rgb.tobytes() + self.raw_publisher.publish(raw) + if info.width > self.jpeg_width: + rgb = cv2.resize(rgb, (self.jpeg_width, max(1, round(info.height * self.jpeg_width / info.width))), + interpolation=cv2.INTER_AREA) + success, encoded = cv2.imencode('.jpg', cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR), + [cv2.IMWRITE_JPEG_QUALITY, 78]) + if success: + image = CompressedImage() + image.header.stamp, image.header.frame_id = stamp, self.frame_id + image.format, image.data = 'jpeg', encoded.tobytes() + self.jpeg_publisher.publish(image) + + def destroy_node(self): + self.pipeline.set_state(Gst.State.NULL) + return super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = None + try: + node = GstreamerCameraTest() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/camera_test_pkg/launch/camera.launch.py b/src/camera_test_pkg/launch/camera.launch.py new file mode 100644 index 0000000..49d6f60 --- /dev/null +++ b/src/camera_test_pkg/launch/camera.launch.py @@ -0,0 +1,26 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + defaults = { + 'video_device': ('/dev/video0', str), + 'width': ('640', int), 'height': ('480', int), + 'fps': ('30', int), 'publish_fps': ('10', int), + 'pixel_format': ('auto', str), 'jpeg_width': ('640', int), + 'frame_id': ('camera_test_link', str), + 'jpeg_topic': ('/camera_test/image_jpeg', str), + 'publish_raw': ('false', bool), + } + return LaunchDescription([ + *[DeclareLaunchArgument(key, default_value=value) + for key, (value, _) in defaults.items()], + Node(package='camera_test_pkg', executable='gstreamer_camera_test', + name='camera_test', output='screen', parameters=[{ + key: ParameterValue(LaunchConfiguration(key), value_type=kind) + for key, (_, kind) in defaults.items() + }]), + ]) diff --git a/src/camera_test_pkg/package.xml b/src/camera_test_pkg/package.xml new file mode 100644 index 0000000..e139884 --- /dev/null +++ b/src/camera_test_pkg/package.xml @@ -0,0 +1,17 @@ + + + camera_test_pkg + 1.0.0 + Isolated V4L2 camera input test for the LMS Nav2 runtime. + chyoo + Apache-2.0 + ament_python + launch + launch_ros + rclpy + sensor_msgs + python3-opencv + python3-numpy + python3-gi + ament_python + diff --git a/src/camera_test_pkg/resource/camera_test_pkg b/src/camera_test_pkg/resource/camera_test_pkg new file mode 100644 index 0000000..e69de29 diff --git a/src/camera_test_pkg/setup.cfg b/src/camera_test_pkg/setup.cfg new file mode 100644 index 0000000..a416d96 --- /dev/null +++ b/src/camera_test_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/camera_test_pkg +[install] +install_scripts=$base/lib/camera_test_pkg diff --git a/src/camera_test_pkg/setup.py b/src/camera_test_pkg/setup.py new file mode 100644 index 0000000..9089a70 --- /dev/null +++ b/src/camera_test_pkg/setup.py @@ -0,0 +1,27 @@ +from glob import glob +from setuptools import setup + + +package_name = 'camera_test_pkg' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), + (f'share/{package_name}', ['package.xml']), + (f'share/{package_name}/launch', glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='chyoo', + maintainer_email='chyoo@futuredrive.net', + description='Isolated V4L2 camera input test for the LMS Nav2 runtime.', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'gstreamer_camera_test = camera_test_pkg.gstreamer_camera:main', + ], + }, +) diff --git a/tests/test_camera.py b/tests/test_camera.py new file mode 100644 index 0000000..6a4341a --- /dev/null +++ b/tests/test_camera.py @@ -0,0 +1,52 @@ +import time +import unittest +from unittest.mock import patch + +import cv2 +import numpy as np +import rclpy +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import CompressedImage +from camera_test_pkg import gstreamer_camera as camera + + +class CameraTest(unittest.TestCase): + def test_pipeline_formats_and_validation(self): + camera.Gst.init(None) + for fmt in ['auto', 'UYVY', 'YUY2', 'MJPEG']: + pipeline = camera.Gst.parse_launch(camera.capture_pipeline('/dev/video0', 640, 480, 30, fmt)) + pipeline.set_state(camera.Gst.State.NULL) + for args in [('/tmp/other', 640, 480, 30, 'auto'), + ('/dev/video0', 0, 480, 30, 'auto'), + ('/dev/video0', 640, 480, 30, 'bad')]: + with self.assertRaises(ValueError): + camera.capture_pipeline(*args) + + def test_synthetic_frame_to_ros_jpeg(self): + # Odd width exercises RGB row padding without accessing physical hardware. + pipeline = ('videotestsrc is-live=true ! video/x-raw,format=RGB,width=641,height=480,framerate=30/1 ! ' + 'appsink name=camera_sink max-buffers=1 drop=true sync=false') + rclpy.init() + node = None + try: + with patch.object(camera, 'capture_pipeline', return_value=pipeline): + node = camera.GstreamerCameraTest() + frames = [] + subscription = node.create_subscription(CompressedImage, '/camera_test/image_jpeg', frames.append, + qos_profile_sensor_data) + end = time.monotonic() + 8 + while not frames and time.monotonic() < end: + rclpy.spin_once(node, timeout_sec=0.1) + self.assertTrue(frames, 'No ROS JPEG received') + decoded = cv2.imdecode(np.frombuffer(frames[0].data, dtype=np.uint8), cv2.IMREAD_COLOR) + self.assertEqual(decoded.shape[1], 640) + self.assertEqual(frames[0].format, 'jpeg') + node.destroy_subscription(subscription) + finally: + if node: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + unittest.main()