feat(camera): add configurable Jetson camera module for LMS visualization
This commit is contained in:
@@ -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"]
|
||||
}
|
||||
@@ -0,0 +1,5 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
@@ -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.
|
||||
@@ -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'"
|
||||
@@ -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/*
|
||||
@@ -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 "$@"
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
}]),
|
||||
])
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>camera_test_pkg</name>
|
||||
<version>1.0.0</version>
|
||||
<description>Isolated V4L2 camera input test for the LMS Nav2 runtime.</description>
|
||||
<maintainer email="chyoo@futuredrive.net">chyoo</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-gi</exec_depend>
|
||||
<export><build_type>ament_python</build_type></export>
|
||||
</package>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/camera_test_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/camera_test_pkg
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user