From cf86e0198490edd427d0ef7957fbdfe7f71a1685 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Fri, 24 Jul 2026 17:12:38 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E4=BF=AF=E7=9E=B0=E5=9B=BE?= =?UTF-8?q?=E5=8F=98=E6=8D=A2=E5=8C=85=EF=BC=8C=E7=B1=BB=E4=BC=BC=E8=BD=A6?= =?UTF-8?q?=E8=BE=86=E7=8E=AF=E8=A7=86=E5=9B=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/origincar_birdseye/CMakeLists.txt | 44 +++ src/origincar_birdseye/README.md | 288 +++++++++++++++ .../config/birdseye_defaults.yaml | 41 +++ .../config/offline_birdseye_overrides.yaml | 4 + .../config/offline_camera.yaml | 5 + .../plans/2026-07-23-car-usb-cam-config.md | 80 +++++ .../origincar_birdseye/birdseye_geometry.hpp | 42 +++ .../configuration_validation.hpp | 65 ++++ .../origincar_birdseye/ground_projection.hpp | 81 +++++ .../origincar_birdseye/image_validation.hpp | 31 ++ .../launch/birdseye_runtime.launch.py | 26 ++ .../launch/car_usb_birdseye.launch.py | 40 +++ .../launch/offline_validation.launch.py | 42 +++ src/origincar_birdseye/package.xml | 27 ++ src/origincar_birdseye/src/birdseye_node.cpp | 340 ++++++++++++++++++ .../src/synthetic_camera_node.cpp | 68 ++++ .../test/test_ground_projection.cpp | 114 ++++++ 17 files changed, 1338 insertions(+) create mode 100644 src/origincar_birdseye/CMakeLists.txt create mode 100644 src/origincar_birdseye/README.md create mode 100644 src/origincar_birdseye/config/birdseye_defaults.yaml create mode 100644 src/origincar_birdseye/config/offline_birdseye_overrides.yaml create mode 100644 src/origincar_birdseye/config/offline_camera.yaml create mode 100644 src/origincar_birdseye/docs/superpowers/plans/2026-07-23-car-usb-cam-config.md create mode 100644 src/origincar_birdseye/include/origincar_birdseye/birdseye_geometry.hpp create mode 100644 src/origincar_birdseye/include/origincar_birdseye/configuration_validation.hpp create mode 100644 src/origincar_birdseye/include/origincar_birdseye/ground_projection.hpp create mode 100644 src/origincar_birdseye/include/origincar_birdseye/image_validation.hpp create mode 100644 src/origincar_birdseye/launch/birdseye_runtime.launch.py create mode 100644 src/origincar_birdseye/launch/car_usb_birdseye.launch.py create mode 100644 src/origincar_birdseye/launch/offline_validation.launch.py create mode 100644 src/origincar_birdseye/package.xml create mode 100644 src/origincar_birdseye/src/birdseye_node.cpp create mode 100644 src/origincar_birdseye/src/synthetic_camera_node.cpp create mode 100644 src/origincar_birdseye/test/test_ground_projection.cpp diff --git a/src/origincar_birdseye/CMakeLists.txt b/src/origincar_birdseye/CMakeLists.txt new file mode 100644 index 0000000..b8af442 --- /dev/null +++ b/src/origincar_birdseye/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(origincar_birdseye) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(OpenCV REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +add_executable(birdseye_node src/birdseye_node.cpp) +target_include_directories(birdseye_node PRIVATE include) +ament_target_dependencies(birdseye_node + cv_bridge + geometry_msgs + rclcpp + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros) +target_link_libraries(birdseye_node ${OpenCV_LIBS} tf2_geometry_msgs::tf2_geometry_msgs) + +add_executable(synthetic_camera_node src/synthetic_camera_node.cpp) +ament_target_dependencies(synthetic_camera_node rclcpp sensor_msgs) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_ground_projection test/test_ground_projection.cpp) + target_include_directories(test_ground_projection PRIVATE include) +endif() + +install(TARGETS birdseye_node synthetic_camera_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY include/ DESTINATION include) +install(FILES README.md DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/origincar_birdseye/README.md b/src/origincar_birdseye/README.md new file mode 100644 index 0000000..31ed558 --- /dev/null +++ b/src/origincar_birdseye/README.md @@ -0,0 +1,288 @@ +# origincar_birdseye 使用教程 + +`origincar_birdseye` 将车头斜向下安装的单目相机图像投影到地面鸟瞰平面，并提供两种输出： + +- 车体视角：车辆固定在画布中，车辆前后移动时地面图像相对车辆流动。 +- 地图视角：通过 TF 查询 `odom_combined -> base_footprint`，把每帧地面图像放到固定世界画布中。 + +运行时不需要棋盘格。棋盘格只在标定阶段用于得到相机内参以及 `camera_from_base_*` 外参。 + +## 1. 与 car_usb_cam 的关系 + +本包不会修改 `car_usb_cam`，也不会代替它控制相机。联合启动文件只是原样包含： + +```text +car_usb_cam/launch/hobot_usb_cam.launch.py +``` + +并只传递该启动文件当前公开的 `camera_config` 参数。因此相机设备、分辨率、帧率、像素格式以及相机自身的标定矫正仍由下面的文件负责： + +```text +car_usb_cam/config/usb_camera.yaml +car_usb_cam/config/usb_camera_calibration.yaml +``` + +当前实车检查到的 `usb_camera.yaml` 配置为： + +```yaml +hobot_usb_cam: + ros__parameters: + frame_id: default_usb_cam + framerate: 30 + image_height: 720 + image_width: 1280 + io_method: mmap + pixel_format: mjpeg + video_device: /dev/video0 + zero_copy: false +``` + +`hobot_usb_cam` 根据自己的参数和标定文件产生已矫正图像。当前 `pixel_format: mjpeg` 模式实测在 `/image` 发布 `sensor_msgs/msg/CompressedImage`；本包通过 `image_message_type: compressed` 订阅并解码 MJPEG。若相机改为发布 `sensor_msgs/msg/Image`，只需把该参数改成 `raw`。 + +本包默认配置 `input_is_rectified: true`，不会再次执行畸变矫正，避免双重矫正。 + +如果以后订阅的是未经矫正的原始图像，应将 `input_is_rectified` 改为 `false`，此时本包才使用 `fx/fy/cx/cy` 和 `distortion_coefficients` 执行 `cv::undistort`。 + +## 2. 数据流 + +```text +/dev/video0 + -> hobot_usb_cam + usb_camera.yaml + usb_camera_calibration.yaml + -> /image（当前为已矫正 sensor_msgs/msg/CompressedImage） + -> 本包解码 MJPEG + -> birdseye_node + -> /birdseye/vehicle/image + -> /birdseye/world/image + ^ + | TF: odom_combined -> base_footprint +``` + +世界视角缺少 TF 时不会影响车体视角输出；节点会跳过当前世界视角帧并节流打印警告。 + +## 3. 编译 + +WSL 开发环境： + +```bash +cd /home/hikos/Yiliao2026 +source /opt/ros/humble/setup.bash +colcon build --packages-select origincar_birdseye +source install/setup.bash +``` + +RDKx5 实车环境： + +```bash +cd /home/sunrise/yiliao_ws +source /opt/tros/humble/setup.bash +colcon build --packages-select origincar_birdseye +source install/setup.bash +``` + +## 4. 无摄像头离线验证 + +离线启动会发布 YAML 指定尺寸的彩色网格图，并关闭世界视角： + +```bash +ros2 launch origincar_birdseye offline_validation.launch.py +``` + +另开终端检查： + +```bash +source /opt/ros/humble/setup.bash +source /home/hikos/Yiliao2026/install/setup.bash +ros2 topic info /birdseye/vehicle/image +ros2 topic echo /birdseye/vehicle/image --once --field width +ros2 topic echo /birdseye/vehicle/image --once --field height +``` + +离线图像发布器的输入话题和尺寸位于 `config/offline_camera.yaml`。它们必须与鸟瞰 YAML 对应参数一致。离线发布器使用原始 `sensor_msgs/msg/Image`，所以 `config/offline_birdseye_overrides.yaml` 会把 `image_message_type` 覆盖为 `raw`，并关闭世界视角。 + +## 5. 只启动鸟瞰节点 + +相机已由其他启动文件运行时： + +```bash +ros2 launch origincar_birdseye birdseye_runtime.launch.py +``` + +指定另一份鸟瞰配置： + +```bash +ros2 launch origincar_birdseye birdseye_runtime.launch.py \ + birdseye_config:=/absolute/path/to/birdseye.yaml +``` + +## 6. 联合启动 car_usb_cam 和鸟瞰节点 + +使用两个包的默认 YAML： + +```bash +ros2 launch origincar_birdseye car_usb_birdseye.launch.py +``` + +使用自定义相机和鸟瞰配置： + +```bash +ros2 launch origincar_birdseye car_usb_birdseye.launch.py \ + camera_config:=/absolute/path/to/usb_camera.yaml \ + birdseye_config:=/absolute/path/to/birdseye.yaml +``` + +`camera_config` 被原样传给 `hobot_usb_cam.launch.py`，本包不会读取或重写它。如果修改了相机输出尺寸，还必须同步修改鸟瞰 YAML 的 `input_width`、`input_height` 和相应内参。 + +## 7. 鸟瞰 YAML 参数 + +默认文件：`config/birdseye_defaults.yaml`。 + +| 参数 | 含义 | +|---|---| +| `image_topic` | 输入图像话题，可在 YAML 中修改 | +| `image_message_type` | `compressed` 订阅 `CompressedImage`；`raw` 订阅 `Image` | +| `input_width` / `input_height` | 标定所对应的输入图像尺寸；每帧会严格校验 | +| `input_is_rectified` | `true` 表示上游已经矫正，本包不再去畸变 | +| `vehicle_output_topic` | 车体视角输出话题 | +| `world_output_topic` | 地图视角输出话题 | +| `base_frame` | 车辆基准坐标系，默认 `base_footprint` | +| `world_frame` | 世界坐标系，默认 `odom_combined` | +| `enable_world_view` | 是否生成地图视角 | +| `fx/fy/cx/cy` | 与实际输入尺寸对应的相机内参 | +| `distortion_coefficients` | 原始图像畸变参数；仅 `input_is_rectified: false` 时使用 | +| `camera_from_base_rotation` | 从车体坐标到相机坐标的 3x3 旋转矩阵，按行展开 | +| `camera_from_base_translation` | 从车体坐标到相机坐标的平移向量，单位米 | +| `output_width/height` | 车体视角输出像素尺寸 | +| `meters_per_pixel` | 每像素代表的米数，两个输出共用 | +| `rear_meters` | 车体原点距车体视角画布下边缘的距离 | +| `left_meters` | 车体视角画布左边界对应的车体左侧距离 | +| `world_origin_x/y` | 世界画布局部原点在 `world_frame` 中的位置 | +| `world_width/height` | 世界视角输出像素尺寸 | +| `world_left_meters` | 世界局部原点距画布左边缘的距离 | +| `world_rear_meters` | 世界局部原点距画布下边缘的距离 | + +输入话题和尺寸只需修改 YAML，例如： + +```yaml +birdseye_node: + ros__parameters: + image_topic: /front_camera/rectified_image + image_message_type: compressed + input_width: 1920 + input_height: 1080 + input_is_rectified: true +``` + +注意：改变话题只会改变鸟瞰节点的订阅目标，不会擅自改变 `car_usb_cam` 的发布话题。上游必须已经发布同名话题，或由系统级 ROS remap 将其映射到该名称。 + +## 8. 分辨率与标定的约束 + +下面四项必须一致： + +1. `car_usb_cam` 实际发布图像的宽高； +2. `birdseye_defaults.yaml` 的 `input_width/input_height`； +3. `fx/fy/cx/cy` 所属的标定分辨率； +4. `camera_from_base_*` 标定时所使用的成像模式和安装姿态。 + +收到尺寸不一致的图像时，本包不会继续投影，而是打印类似错误： + +```text +input image size mismatch: expected 1280x720, received 800x450 +``` + +默认内参是把项目中原来的 `800x450` 内参按相同 16:9 视场线性缩放到 `1280x720` 的过渡值。它可用于软件联调，但实车最终精度应以同一相机模式下重新确认的矫正后内参为准。 + +## 9. 标定到运行的流程 + +1. 固定相机在车头，铺设地面棋盘格。 +2. 用最终运行时相同的 `car_usb_cam` 分辨率、格式和矫正设置采集图像。 +3. 获取矫正后图像对应的 `fx/fy/cx/cy`。 +4. 运行 `camera_to_base_calibrator`，由棋盘格计算固定的 `T_camera_from_base`。 +5. 把结果写入 `birdseye_defaults.yaml`，检查实时鸟瞰图中的直线、比例和车辆位置。 +6. 固定结果后移除棋盘格；运行阶段只需要相机图像和地图模式所需的 TF。 + +`camera_to_base_calibrator` 的职责只是求相机相对于车体/地面的固定外参。它不需要在正常行驶时持续运行；相机安装没有松动时也不需要反复拍棋盘格。 + +## 10. 两种运行模式 + +### 车体居中模式 + +设置： + +```yaml +enable_world_view: false +``` + +使用 `/birdseye/vehicle/image`。车辆矩形始终位于固定像素位置，地面随车辆运动向后或向前流动。 + +### 地图模式 + +设置： + +```yaml +enable_world_view: true +world_frame: odom_combined +base_frame: base_footprint +``` + +系统必须提供可查询的 TF： + +```bash +ros2 run tf2_ros tf2_echo odom_combined base_footprint +``` + +节点按每帧图像时间戳查询车辆位置和航向，再把该帧映射到 `/birdseye/world/image`。 + +## 11. 话题检查 + +```bash +ros2 topic list +ros2 topic info /image -v +ros2 topic hz /image +ros2 topic info /birdseye/vehicle/image -v +ros2 topic info /birdseye/world/image -v +``` + +主要话题： + +| 默认话题 | 类型 | 方向 | +|---|---|---| +| `/image` | 当前为 `sensor_msgs/msg/CompressedImage` | 输入 | +| `/birdseye/vehicle/image` | `sensor_msgs/msg/Image` | 输出 | +| `/birdseye/world/image` | `sensor_msgs/msg/Image` | 输出 | +| `/tf`、`/tf_static` | `tf2_msgs/msg/TFMessage` | 地图模式输入 | + +## 12. 测试 + +```bash +cd /home/hikos/Yiliao2026 +source /opt/ros/humble/setup.bash +colcon build --packages-select origincar_birdseye +colcon test --packages-select origincar_birdseye +colcon test-result --test-result-base build/origincar_birdseye --verbose +``` + +## 13. 常见问题 + +### 没有收到图像 + +确认 `image_topic` 与实际话题完全一致，并用 `ros2 topic info <话题> -v` 检查类型。发布 `sensor_msgs/msg/CompressedImage` 时使用 `image_message_type: compressed`；发布 `sensor_msgs/msg/Image` 时使用 `raw`。WebSocket 数据不能作为输入。 + +### 报输入尺寸不一致 + +原始 `Image` 可直接查看 `width/height`；`CompressedImage` 不携带独立宽高字段，本包解码第一帧后会打印 `accepted input frame size=...`。让相机配置、鸟瞰输入尺寸和内参使用同一分辨率，不能只改宽高而保留另一分辨率的内参。 + +### 鸟瞰图扭曲或地面直线不直 + +检查是否重复矫正、内参是否对应当前分辨率，以及 `camera_from_base_*` 是否来自相机当前安装姿态。使用 `car_usb_cam` 已矫正输出时应保持 `input_is_rectified: true`。 + +### 车体视角正常但没有世界视角 + +检查 `enable_world_view`，然后确认图像时间戳处存在 `odom_combined -> base_footprint` TF。没有 TF 时节点会明确输出 `world view skipped`。 + +### `/dev/video0` 打不开 + +通常是另一个相机进程占用了设备。不要同时启动两份 `hobot_usb_cam`；联合启动前先停止旧相机节点。 + +### 输出大面积黑色 + +检查外参旋转/平移方向、相机是否真的朝向地面，以及参考地面范围是否落在相机 FOV 内。当前默认外参只是离线近似值，实车必须使用 `camera_to_base_calibrator` 的最终结果。 diff --git a/src/origincar_birdseye/config/birdseye_defaults.yaml b/src/origincar_birdseye/config/birdseye_defaults.yaml new file mode 100644 index 0000000..367c451 --- /dev/null +++ b/src/origincar_birdseye/config/birdseye_defaults.yaml @@ -0,0 +1,41 @@ +birdseye_node: + ros__parameters: + image_topic: /image + # Current car_usb_cam MJPEG mode publishes sensor_msgs/msg/CompressedImage. + # Use "raw" when the selected upstream publisher uses sensor_msgs/msg/Image. + image_message_type: compressed + # Must match the image actually published by car_usb_cam and the calibration matrix below. + input_width: 1280 + input_height: 720 + # hobot_usb_cam applies its configured camera calibration before publishing /image. + input_is_rectified: true + vehicle_output_topic: /birdseye/vehicle/image + world_output_topic: /birdseye/world/image + base_frame: base_footprint + world_frame: odom_combined + enable_world_view: true + + # Scaled from the existing 800x450 intrinsics to the current 1280x720 stream. + # Replace these values with the final rectified 1280x720 calibration when available. + fx: 450.55244482 + fy: 457.08553096 + cx: 583.36914605 + cy: 327.88183768 + distortion_coefficients: [0.03532389, -0.05576607, 0.00027529, 0.00241747, 0.01024998] + + # Approximate T_camera_from_base for offline validation. Replace with calibrator output on the vehicle. + camera_from_base_rotation: [0.0, -1.0, 0.0, -0.70710678, 0.0, -0.70710678, 0.70710678, 0.0, -0.70710678] + camera_from_base_translation: [0.0, 0.28284271, 0.14142136] + + output_width: 600 + output_height: 800 + meters_per_pixel: 0.01 + rear_meters: 2.0 + left_meters: 3.0 + + world_origin_x: 0.0 + world_origin_y: 0.0 + world_width: 800 + world_height: 800 + world_left_meters: 4.0 + world_rear_meters: 4.0 diff --git a/src/origincar_birdseye/config/offline_birdseye_overrides.yaml b/src/origincar_birdseye/config/offline_birdseye_overrides.yaml new file mode 100644 index 0000000..bd9e534 --- /dev/null +++ b/src/origincar_birdseye/config/offline_birdseye_overrides.yaml @@ -0,0 +1,4 @@ +birdseye_node: + ros__parameters: + image_message_type: raw + enable_world_view: false diff --git a/src/origincar_birdseye/config/offline_camera.yaml b/src/origincar_birdseye/config/offline_camera.yaml new file mode 100644 index 0000000..54ef54a --- /dev/null +++ b/src/origincar_birdseye/config/offline_camera.yaml @@ -0,0 +1,5 @@ +synthetic_camera_node: + ros__parameters: + image_topic: /image + input_width: 1280 + input_height: 720 diff --git a/src/origincar_birdseye/docs/superpowers/plans/2026-07-23-car-usb-cam-config.md b/src/origincar_birdseye/docs/superpowers/plans/2026-07-23-car-usb-cam-config.md new file mode 100644 index 0000000..ae8d624 --- /dev/null +++ b/src/origincar_birdseye/docs/superpowers/plans/2026-07-23-car-usb-cam-config.md @@ -0,0 +1,80 @@ +# car_usb_cam YAML Adaptation Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Make the bird's-eye node accept its image topic and expected input size from YAML, consume the already-rectified output of the current `car_usb_cam`, and document the complete workflow. + +**Architecture:** `car_usb_cam` remains an independent upstream package and is included through its public `camera_config` launch argument without modification. `origincar_birdseye` loads all of its own parameters from `birdseye_defaults.yaml`, validates every received frame against the configured calibration size, and optionally skips local undistortion when the upstream image is already rectified. + +**Tech Stack:** ROS 2 Humble, C++17, OpenCV, ROS 2 Python launch, YAML, ament/GoogleTest. + +--- + +### Task 1: Testable input-size validation + +**Files:** +- Create: `include/origincar_birdseye/image_validation.hpp` +- Modify: `test/test_ground_projection.cpp` + +- [ ] Add tests showing that an exact configured size succeeds and a mismatched frame returns a diagnostic containing both sizes. +- [ ] Run `colcon test --packages-select origincar_birdseye` and verify the new test fails before implementation. +- [ ] Add the minimal pure validation helper. +- [ ] Rebuild and verify the unit test passes. + +### Task 2: YAML-controlled runtime input + +**Files:** +- Modify: `src/birdseye_node.cpp` +- Modify: `config/birdseye_defaults.yaml` + +- [ ] Declare `input_width`, `input_height`, and `input_is_rectified` as node parameters. +- [ ] Reject non-positive configured dimensions during construction. +- [ ] Store the configured dimensions in `CameraIntrinsics` and reject mismatching frames with a throttled error before projection. +- [ ] Skip `cv::undistort` when `input_is_rectified: true`; retain the optional path for a raw camera source. +- [ ] Configure the current `car_usb_cam` defaults as `/image`, `1280`, `720`, and already rectified. + +### Task 3: Current car_usb_cam launch compatibility + +**Files:** +- Modify: `launch/car_usb_birdseye.launch.py` +- Modify: `launch/birdseye_runtime.launch.py` + +- [ ] Replace obsolete `usb_*` launch arguments with the current single `camera_config` argument. +- [ ] Add `camera_config` and `birdseye_config` launch arguments with package defaults. +- [ ] Include `car_usb_cam/launch/hobot_usb_cam.launch.py` unchanged and pass only `camera_config`. +- [ ] Start the bird's-eye node using `birdseye_config`; do not modify or remap the camera package internally. +- [ ] Verify both launch files are importable and expose the documented arguments. + +### Task 4: Package tutorial + +**Files:** +- Create: `README.md` + +- [ ] Document architecture, current car configuration, prerequisites, build commands, offline validation, bird's-eye-only launch, joint real-camera launch, and YAML overrides. +- [ ] Document every input/output topic and parameter, vehicle/world coordinate behavior, upstream rectification, calibration-size constraints, checkerboard-to-runtime workflow, and troubleshooting. +- [ ] State explicitly that `car_usb_cam` is not modified and its camera YAML/calibration remain authoritative for capture and upstream rectification. + +### Task 5: Local and robot verification + +**Files:** +- Verify only; no planned source additions. + +- [ ] Build the package in `/home/hikos/Yiliao2026` and run all package tests with full failure output. +- [ ] Run the synthetic offline pipeline and verify `/birdseye/vehicle/image` is published at the configured output size. +- [ ] Copy only `origincar_birdseye` to `/home/sunrise/yiliao_ws/src/`, preserving `car_usb_cam`. +- [ ] Build and test on RDKx5. +- [ ] Start the existing real camera through the joint launch, verify the current MJPEG `/image` is `sensor_msgs/msg/CompressedImage` decoded as `1280x720`, verify bird's-eye output, then stop the launch and confirm `/dev/video0` is released. + +### Task 6: MJPEG message compatibility discovered on the vehicle + +**Files:** +- Modify: `include/origincar_birdseye/image_validation.hpp` +- Modify: `src/birdseye_node.cpp` +- Modify: `config/birdseye_defaults.yaml` +- Create: `config/offline_birdseye_overrides.yaml` +- Modify: `README.md` + +- [ ] Test and validate the YAML value `image_message_type` as `raw` or `compressed`. +- [ ] Subscribe to `sensor_msgs/msg/CompressedImage` for the current MJPEG camera configuration and decode it before the common processing path. +- [ ] Retain configurable `sensor_msgs/msg/Image` support for raw publishers. +- [ ] Verify the decoded frame is `1280x720` and the vehicle output is published on the robot. diff --git a/src/origincar_birdseye/include/origincar_birdseye/birdseye_geometry.hpp b/src/origincar_birdseye/include/origincar_birdseye/birdseye_geometry.hpp new file mode 100644 index 0000000..f43bea2 --- /dev/null +++ b/src/origincar_birdseye/include/origincar_birdseye/birdseye_geometry.hpp @@ -0,0 +1,42 @@ +#ifndef ORIGINCAR_BIRDSEYE__BIRDSEYE_GEOMETRY_HPP_ +#define ORIGINCAR_BIRDSEYE__BIRDSEYE_GEOMETRY_HPP_ + +#include + +#include "origincar_birdseye/ground_projection.hpp" + +namespace origincar_birdseye +{ + +struct BirdseyeCanvas +{ + int width; + int height; + double meters_per_pixel; + double rear_meters; + double left_meters; +}; + +class BirdseyeGeometry +{ +public: + explicit BirdseyeGeometry(BirdseyeCanvas canvas) + : canvas_(canvas) + { + } + + std::optional basePointToCanvasPixel(const GroundPoint & point) const + { + const PixelPoint pixel{ + (canvas_.left_meters - point.y) / canvas_.meters_per_pixel, + (canvas_.rear_meters - point.x) / canvas_.meters_per_pixel}; + return pixel; + } + +private: + BirdseyeCanvas canvas_; +}; + +} // namespace origincar_birdseye + +#endif // ORIGINCAR_BIRDSEYE__BIRDSEYE_GEOMETRY_HPP_ diff --git a/src/origincar_birdseye/include/origincar_birdseye/configuration_validation.hpp b/src/origincar_birdseye/include/origincar_birdseye/configuration_validation.hpp new file mode 100644 index 0000000..d93c5d7 --- /dev/null +++ b/src/origincar_birdseye/include/origincar_birdseye/configuration_validation.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "origincar_birdseye/birdseye_geometry.hpp" +#include "origincar_birdseye/ground_projection.hpp" + +namespace origincar_birdseye +{ + +inline bool allFinite(const std::vector & values) +{ + return std::all_of( + values.begin(), values.end(), [](double value) { + return std::isfinite(value); + }); +} + +inline std::optional validateCameraConfiguration( + const CameraIntrinsics & intrinsics, const std::vector & rotation, + const std::vector & translation) +{ + if (intrinsics.width <= 0 || intrinsics.height <= 0 || + !std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) || + !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) || + intrinsics.fx <= 0.0 || intrinsics.fy <= 0.0) + { + return "camera intrinsics and input dimensions must be finite and positive"; + } + if (rotation.size() != 9 || translation.size() != 3 || + !allFinite(rotation) || !allFinite(translation)) + { + return "T_camera_from_base must contain finite 3x3 rotation and 3D translation values"; + } + return std::nullopt; +} + +inline std::optional validateCanvas( + const BirdseyeCanvas & canvas, const std::string & name) +{ + if (canvas.width <= 0 || canvas.height <= 0) { + return name + " canvas width and height must be positive"; + } + if (!std::isfinite(canvas.meters_per_pixel) || canvas.meters_per_pixel <= 0.0 || + !std::isfinite(canvas.rear_meters) || !std::isfinite(canvas.left_meters)) + { + return name + " canvas metric parameters must be finite and meters_per_pixel must be positive"; + } + return std::nullopt; +} + +inline std::optional validateCoefficientVector( + const std::vector & values, size_t minimum_size, const std::string & name) +{ + if (values.size() < minimum_size || !allFinite(values)) { + return name + " must contain the required number of finite values"; + } + return std::nullopt; +} + +} // namespace origincar_birdseye diff --git a/src/origincar_birdseye/include/origincar_birdseye/ground_projection.hpp b/src/origincar_birdseye/include/origincar_birdseye/ground_projection.hpp new file mode 100644 index 0000000..42c99ac --- /dev/null +++ b/src/origincar_birdseye/include/origincar_birdseye/ground_projection.hpp @@ -0,0 +1,81 @@ +#ifndef ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_ +#define ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_ + +#include +#include + +namespace origincar_birdseye +{ + +struct CameraIntrinsics +{ + double fx; + double fy; + double cx; + double cy; + int width; + int height; +}; + +struct GroundPoint +{ + double x; + double y; +}; + +struct PixelPoint +{ + double x; + double y; +}; + +class RigidTransform +{ +public: + RigidTransform(std::array rotation, std::array translation) + : rotation_(rotation), translation_(translation) + { + } + + std::array apply(const std::array & point) const + { + return { + rotation_[0] * point[0] + rotation_[1] * point[1] + rotation_[2] * point[2] + translation_[0], + rotation_[3] * point[0] + rotation_[4] * point[1] + rotation_[5] * point[2] + translation_[1], + rotation_[6] * point[0] + rotation_[7] * point[1] + rotation_[8] * point[2] + + translation_[2]}; + } + +private: + std::array rotation_; + std::array translation_; +}; + +class GroundProjection +{ +public: + GroundProjection(CameraIntrinsics intrinsics, RigidTransform camera_from_base) + : intrinsics_(intrinsics), camera_from_base_(camera_from_base) + { + } + + std::optional projectBaseGroundPoint(const GroundPoint & point) const + { + const auto camera_point = camera_from_base_.apply({point.x, point.y, 0.0}); + if (camera_point[2] <= 0.0) { + return std::nullopt; + } + + return PixelPoint{ + intrinsics_.fx * camera_point[0] / camera_point[2] + intrinsics_.cx, + intrinsics_.fy * camera_point[1] / camera_point[2] + intrinsics_.cy}; + } + +private: + CameraIntrinsics intrinsics_; + RigidTransform camera_from_base_; +}; + +} // namespace origincar_birdseye + +#endif // ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_ diff --git a/src/origincar_birdseye/include/origincar_birdseye/image_validation.hpp b/src/origincar_birdseye/include/origincar_birdseye/image_validation.hpp new file mode 100644 index 0000000..0685069 --- /dev/null +++ b/src/origincar_birdseye/include/origincar_birdseye/image_validation.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include + +namespace origincar_birdseye +{ + +inline std::optional validateImageSize( + int expected_width, int expected_height, int actual_width, int actual_height) +{ + if (expected_width == actual_width && expected_height == actual_height) { + return std::nullopt; + } + + std::ostringstream message; + message << "input image size mismatch: expected " << expected_width << 'x' << expected_height + << ", received " << actual_width << 'x' << actual_height; + return message.str(); +} + +inline std::optional validateImageMessageType(const std::string & value) +{ + if (value == "raw" || value == "compressed") { + return std::nullopt; + } + return "image_message_type must be raw or compressed"; +} + +} // namespace origincar_birdseye diff --git a/src/origincar_birdseye/launch/birdseye_runtime.launch.py b/src/origincar_birdseye/launch/birdseye_runtime.launch.py new file mode 100644 index 0000000..da256bd --- /dev/null +++ b/src/origincar_birdseye/launch/birdseye_runtime.launch.py @@ -0,0 +1,26 @@ +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +import os + + +def generate_launch_description(): + default_config = os.path.join( + get_package_share_directory('origincar_birdseye'), + 'config', + 'birdseye_defaults.yaml') + return LaunchDescription([ + DeclareLaunchArgument( + 'birdseye_config', + default_value=default_config, + description='Birdseye node ROS parameter YAML'), + Node( + package='origincar_birdseye', + executable='birdseye_node', + name='birdseye_node', + output='screen', + parameters=[LaunchConfiguration('birdseye_config')]) + ]) diff --git a/src/origincar_birdseye/launch/car_usb_birdseye.launch.py b/src/origincar_birdseye/launch/car_usb_birdseye.launch.py new file mode 100644 index 0000000..af72306 --- /dev/null +++ b/src/origincar_birdseye/launch/car_usb_birdseye.launch.py @@ -0,0 +1,40 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + birdseye_share = get_package_share_directory('origincar_birdseye') + camera_share = get_package_share_directory('car_usb_cam') + default_birdseye_config = os.path.join( + birdseye_share, 'config', 'birdseye_defaults.yaml') + default_camera_config = os.path.join( + camera_share, 'config', 'usb_camera.yaml') + camera_launch = os.path.join(camera_share, 'launch', 'hobot_usb_cam.launch.py') + + return LaunchDescription([ + DeclareLaunchArgument( + 'camera_config', + default_value=default_camera_config, + description='Path passed unchanged to car_usb_cam'), + DeclareLaunchArgument( + 'birdseye_config', + default_value=default_birdseye_config, + description='Birdseye node ROS parameter YAML'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(camera_launch), + launch_arguments={ + 'camera_config': LaunchConfiguration('camera_config'), + }.items()), + Node( + package='origincar_birdseye', + executable='birdseye_node', + name='birdseye_node', + output='screen', + parameters=[LaunchConfiguration('birdseye_config')]), + ]) diff --git a/src/origincar_birdseye/launch/offline_validation.launch.py b/src/origincar_birdseye/launch/offline_validation.launch.py new file mode 100644 index 0000000..c70619b --- /dev/null +++ b/src/origincar_birdseye/launch/offline_validation.launch.py @@ -0,0 +1,42 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + share = get_package_share_directory('origincar_birdseye') + birdseye_config = os.path.join( + share, + 'config', + 'birdseye_defaults.yaml') + synthetic_config = os.path.join(share, 'config', 'offline_camera.yaml') + birdseye_overrides = os.path.join( + share, 'config', 'offline_birdseye_overrides.yaml') + return LaunchDescription([ + DeclareLaunchArgument( + 'birdseye_config', + default_value=birdseye_config, + description='Birdseye node ROS parameter YAML'), + DeclareLaunchArgument( + 'synthetic_config', + default_value=synthetic_config, + description='Synthetic image publisher ROS parameter YAML'), + Node( + package='origincar_birdseye', + executable='synthetic_camera_node', + name='synthetic_camera_node', + output='screen', + parameters=[LaunchConfiguration('synthetic_config')]), + Node( + package='origincar_birdseye', + executable='birdseye_node', + name='birdseye_node', + output='screen', + parameters=[ + LaunchConfiguration('birdseye_config'), + birdseye_overrides]), + ]) diff --git a/src/origincar_birdseye/package.xml b/src/origincar_birdseye/package.xml new file mode 100644 index 0000000..c15b765 --- /dev/null +++ b/src/origincar_birdseye/package.xml @@ -0,0 +1,27 @@ + + + origincar_birdseye + 0.1.0 + Single front camera ground-plane bird's-eye projection for Origincar. + sunrise + Apache-2.0 + + ament_cmake + cv_bridge + geometry_msgs + libopencv-dev + rclcpp + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + ament_index_python + car_usb_cam + launch + launch_ros + ament_cmake_gtest + + + ament_cmake + + diff --git a/src/origincar_birdseye/src/birdseye_node.cpp b/src/origincar_birdseye/src/birdseye_node.cpp new file mode 100644 index 0000000..1555cea --- /dev/null +++ b/src/origincar_birdseye/src/birdseye_node.cpp @@ -0,0 +1,340 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "origincar_birdseye/birdseye_geometry.hpp" +#include "origincar_birdseye/configuration_validation.hpp" +#include "origincar_birdseye/ground_projection.hpp" +#include "origincar_birdseye/image_validation.hpp" + +namespace origincar_birdseye +{ + +class BirdseyeNode : public rclcpp::Node +{ +public: + BirdseyeNode() + : Node("birdseye_node"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + const auto image_topic = this->declare_parameter("image_topic", "/image"); + const auto image_message_type = this->declare_parameter( + "image_message_type", "raw"); + if (const auto error = validateImageMessageType(image_message_type)) { + throw std::runtime_error(*error); + } + const auto vehicle_output_topic = this->declare_parameter( + "vehicle_output_topic", "/birdseye/vehicle/image"); + const auto world_output_topic = this->declare_parameter( + "world_output_topic", "/birdseye/world/image"); + base_frame_ = this->declare_parameter("base_frame", "base_footprint"); + world_frame_ = this->declare_parameter("world_frame", "odom_combined"); + enable_world_view_ = this->declare_parameter("enable_world_view", true); + + const int input_width = this->declare_parameter("input_width", 0); + const int input_height = this->declare_parameter("input_height", 0); + input_is_rectified_ = this->declare_parameter("input_is_rectified", true); + if (input_width <= 0 || input_height <= 0) { + throw std::runtime_error("input_width and input_height must be positive YAML parameters"); + } + + const CameraIntrinsics intrinsics{ + this->declare_parameter("fx", 0.0), + this->declare_parameter("fy", 0.0), + this->declare_parameter("cx", 0.0), + this->declare_parameter("cy", 0.0), + input_width, + input_height}; + const auto rotation = this->declare_parameter>( + "camera_from_base_rotation", std::vector{}); + const auto translation = this->declare_parameter>( + "camera_from_base_translation", std::vector{}); + if (const auto error = validateCameraConfiguration(intrinsics, rotation, translation)) { + throw std::runtime_error(*error); + } + + intrinsics_ = intrinsics; + camera_from_base_ = RigidTransform( + {rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6], + rotation[7], rotation[8]}, + {translation[0], translation[1], translation[2]}); + projection_ = std::make_unique(intrinsics_, camera_from_base_); + + const BirdseyeCanvas vehicle_canvas{ + static_cast(this->declare_parameter("output_width", 600)), + static_cast(this->declare_parameter("output_height", 800)), + this->declare_parameter("meters_per_pixel", 0.01), + this->declare_parameter("rear_meters", 2.0), + this->declare_parameter("left_meters", 3.0)}; + if (const auto error = validateCanvas(vehicle_canvas, "vehicle")) { + throw std::runtime_error(*error); + } + vehicle_geometry_ = std::make_unique(vehicle_canvas); + vehicle_canvas_ = vehicle_canvas; + + world_canvas_.width = static_cast(this->declare_parameter("world_width", 800)); + world_canvas_.height = static_cast(this->declare_parameter("world_height", 800)); + world_canvas_.meters_per_pixel = vehicle_canvas.meters_per_pixel; + world_canvas_.rear_meters = this->declare_parameter("world_rear_meters", 4.0); + world_canvas_.left_meters = this->declare_parameter("world_left_meters", 4.0); + world_origin_x_ = this->declare_parameter("world_origin_x", 0.0); + world_origin_y_ = this->declare_parameter("world_origin_y", 0.0); + if (const auto error = validateCanvas(world_canvas_, "world")) { + throw std::runtime_error(*error); + } + if (!std::isfinite(world_origin_x_) || !std::isfinite(world_origin_y_)) { + throw std::runtime_error("world origin must contain finite values"); + } + + const auto distortion = this->declare_parameter>( + "distortion_coefficients", std::vector{0.0, 0.0, 0.0, 0.0, 0.0}); + if (const auto error = validateCoefficientVector( + distortion, 4, "distortion_coefficients")) + { + throw std::runtime_error(*error); + } + camera_matrix_ = (cv::Mat_(3, 3) << intrinsics.fx, 0.0, intrinsics.cx, 0.0, + intrinsics.fy, intrinsics.cy, 0.0, 0.0, 1.0); + distortion_ = cv::Mat(distortion).clone(); + + vehicle_publisher_ = this->create_publisher(vehicle_output_topic, 10); + world_publisher_ = this->create_publisher(world_output_topic, 10); + if (image_message_type == "compressed") { + compressed_image_subscription_ = + this->create_subscription( + image_topic, rclcpp::SensorDataQoS(), + std::bind(&BirdseyeNode::onCompressedImage, this, std::placeholders::_1)); + } else { + raw_image_subscription_ = this->create_subscription( + image_topic, rclcpp::SensorDataQoS(), + std::bind(&BirdseyeNode::onRawImage, this, std::placeholders::_1)); + } + + RCLCPP_INFO( + this->get_logger(), + "birdseye input=%s message_type=%s expected_size=%dx%d rectified=%s " + "world_frame=%s base_frame=%s", + image_topic.c_str(), image_message_type.c_str(), intrinsics_.width, intrinsics_.height, + input_is_rectified_ ? "true" : "false", world_frame_.c_str(), base_frame_.c_str()); + } + +private: + using BaseToCanvas = std::function(const GroundPoint &)>; + + cv::Mat render( + const cv::Mat & image, const BaseToCanvas & base_to_canvas, int width, + int height) const + { + const std::array reference_points{{ + {0.5, -2.0}, {0.5, 2.0}, {4.0, 2.0}, {4.0, -2.0}}}; + std::vector source; + std::vector destination; + source.reserve(reference_points.size()); + destination.reserve(reference_points.size()); + for (const auto & point : reference_points) { + const auto image_pixel = projection_->projectBaseGroundPoint(point); + const auto output_pixel = base_to_canvas(point); + if (!image_pixel || !output_pixel) { + return cv::Mat::zeros(height, width, CV_8UC3); + } + source.emplace_back(image_pixel->x, image_pixel->y); + destination.emplace_back(output_pixel->x, output_pixel->y); + } + cv::Mat output; + cv::warpPerspective( + image, output, cv::getPerspectiveTransform(source, destination), cv::Size(width, height), + cv::INTER_LINEAR, cv::BORDER_CONSTANT); + return output; + } + + void drawVehicle(cv::Mat & image, const BaseToCanvas & base_to_canvas) const + { + const std::array footprint{{ + {0.138, -0.095}, {0.138, 0.095}, {-0.138, 0.095}, {-0.138, -0.095}}}; + std::vector pixels; + for (const auto & point : footprint) { + const auto pixel = base_to_canvas(point); + if (!pixel) { + return; + } + pixels.emplace_back(static_cast(pixel->x), static_cast(pixel->y)); + } + cv::fillConvexPoly(image, pixels, cv::Scalar(0, 220, 255)); + } + + std::optional worldPixel(const GroundPoint & world_point) const + { + const GroundPoint local{world_point.x - world_origin_x_, world_point.y - world_origin_y_}; + return BirdseyeGeometry(world_canvas_).basePointToCanvasPixel(local); + } + + static GroundPoint baseToWorld( + const GroundPoint & point, const geometry_msgs::msg::TransformStamped & transform) + { + const double yaw = tf2::getYaw(transform.transform.rotation); + const double cosine = std::cos(yaw); + const double sine = std::sin(yaw); + return { + transform.transform.translation.x + cosine * point.x - sine * point.y, + transform.transform.translation.y + sine * point.x + cosine * point.y}; + } + + void publish( + const cv::Mat & image, const std_msgs::msg::Header & header, + const std::string & frame_id, + const rclcpp::Publisher::SharedPtr & publisher) const + { + auto message = + cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, image).toImageMsg(); + message->header.frame_id = frame_id; + publisher->publish(*message); + } + + void processImage(const cv::Mat & source, const std_msgs::msg::Header & header) + { + const auto size_error = validateImageSize( + intrinsics_.width, intrinsics_.height, + source.cols, source.rows); + if (size_error) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, "%s; update input_width/input_height " + "and matching camera intrinsics in birdseye YAML", size_error->c_str()); + return; + } + + if (!logged_input_size_) { + RCLCPP_INFO(this->get_logger(), "accepted input frame size=%dx%d", source.cols, source.rows); + logged_input_size_ = true; + } + + cv::Mat projection_input; + if (input_is_rectified_) { + projection_input = source; + } else { + cv::undistort(source, projection_input, camera_matrix_, distortion_); + } + + const auto vehicle_mapping = [this](const GroundPoint & point) { + return vehicle_geometry_->basePointToCanvasPixel(point); + }; + auto vehicle = render( + projection_input, vehicle_mapping, vehicle_canvas_.width, vehicle_canvas_.height); + drawVehicle(vehicle, vehicle_mapping); + publish(vehicle, header, base_frame_, vehicle_publisher_); + + if (!enable_world_view_) { + return; + } + geometry_msgs::msg::TransformStamped world_from_base; + try { + world_from_base = tf_buffer_.lookupTransform( + world_frame_, base_frame_, header.stamp, tf2::durationFromSec(0.05)); + } catch (const tf2::TransformException & error) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), + *this->get_clock(), 2000, "world view skipped: %s", error.what()); + return; + } + const auto world_mapping = [this, &world_from_base](const GroundPoint & point) { + return worldPixel(baseToWorld(point, world_from_base)); + }; + auto world = render( + projection_input, world_mapping, world_canvas_.width, world_canvas_.height); + drawVehicle(world, world_mapping); + publish(world, header, world_frame_, world_publisher_); + } + + void onRawImage(const sensor_msgs::msg::Image::SharedPtr message) + { + try { + const auto image = cv_bridge::toCvCopy(message, sensor_msgs::image_encodings::BGR8); + processImage(image->image, message->header); + } catch (const cv_bridge::Exception & error) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "raw image decode failed: %s", error.what()); + } catch (const cv::Exception & error) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "raw image processing failed: %s", error.what()); + } + } + + void onCompressedImage(const sensor_msgs::msg::CompressedImage::SharedPtr message) + { + if (message->data.empty()) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "compressed image decode failed: empty data"); + return; + } + try { + const cv::Mat image = cv::imdecode(message->data, cv::IMREAD_COLOR); + if (image.empty()) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "compressed image decode failed: empty decoded frame"); + return; + } + processImage(image, message->header); + } catch (const cv::Exception & error) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "compressed image processing failed: %s", error.what()); + } + } + + std::string base_frame_; + std::string world_frame_; + bool enable_world_view_{true}; + bool input_is_rectified_{true}; + bool logged_input_size_{false}; + CameraIntrinsics intrinsics_{}; + RigidTransform camera_from_base_{{}, {}}; + std::unique_ptr projection_; + BirdseyeCanvas vehicle_canvas_{}; + BirdseyeCanvas world_canvas_{}; + std::unique_ptr vehicle_geometry_; + double world_origin_x_{0.0}; + double world_origin_y_{0.0}; + cv::Mat camera_matrix_; + cv::Mat distortion_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::Subscription::SharedPtr raw_image_subscription_; + rclcpp::Subscription::SharedPtr + compressed_image_subscription_; + rclcpp::Publisher::SharedPtr vehicle_publisher_; + rclcpp::Publisher::SharedPtr world_publisher_; +}; + +} // namespace origincar_birdseye + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/origincar_birdseye/src/synthetic_camera_node.cpp b/src/origincar_birdseye/src/synthetic_camera_node.cpp new file mode 100644 index 0000000..c6ce9c4 --- /dev/null +++ b/src/origincar_birdseye/src/synthetic_camera_node.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include + +#include +#include +#include + +namespace origincar_birdseye +{ + +class SyntheticCameraNode : public rclcpp::Node +{ +public: + SyntheticCameraNode() + : Node("synthetic_camera_node") + { + const auto image_topic = this->declare_parameter("image_topic", "/image"); + width_ = this->declare_parameter("input_width", 0); + height_ = this->declare_parameter("input_height", 0); + if (width_ <= 0 || height_ <= 0) { + throw std::runtime_error("synthetic input_width and input_height must be positive"); + } + publisher_ = this->create_publisher(image_topic, 10); + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), [this]() {publishFrame();}); + } + +private: + void publishFrame() + { + const auto width = static_cast(width_); + const auto height = static_cast(height_); + sensor_msgs::msg::Image message; + message.header.stamp = this->now(); + message.header.frame_id = "front_camera_optical_frame"; + message.height = height; + message.width = width; + message.encoding = sensor_msgs::image_encodings::RGB8; + message.step = width * 3; + message.data.resize(static_cast(message.step) * height); + for (uint32_t row = 0; row < height; ++row) { + for (uint32_t column = 0; column < width; ++column) { + const bool line = row % 40 < 2 || column % 40 < 2; + const size_t index = static_cast(row) * message.step + column * 3; + message.data[index] = line ? 240 : 50; + message.data[index + 1] = line ? 240 : 100; + message.data[index + 2] = line ? 240 : 50; + } + } + publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + int width_{0}; + int height_{0}; +}; + +} // namespace origincar_birdseye + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/origincar_birdseye/test/test_ground_projection.cpp b/src/origincar_birdseye/test/test_ground_projection.cpp new file mode 100644 index 0000000..870f64d --- /dev/null +++ b/src/origincar_birdseye/test/test_ground_projection.cpp @@ -0,0 +1,114 @@ +#include + +#include +#include + +#include "origincar_birdseye/ground_projection.hpp" +#include "origincar_birdseye/birdseye_geometry.hpp" +#include "origincar_birdseye/configuration_validation.hpp" +#include "origincar_birdseye/image_validation.hpp" + +namespace origincar_birdseye +{ + +TEST(GroundProjection, ProjectsGroundPointFromVehicleCoordinates) +{ + CameraIntrinsics intrinsics{800.0, 800.0, 400.0, 225.0, 800, 450}; + RigidTransform camera_from_base( + {0.0, -1.0, 0.0, + 0.0, 0.0, -1.0, + 1.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}); + GroundProjection projection(intrinsics, camera_from_base); + + const auto pixel = projection.projectBaseGroundPoint({1.0, 0.0}); + + ASSERT_TRUE(pixel.has_value()); + EXPECT_NEAR(pixel->x, 400.0, 1e-6); + EXPECT_NEAR(pixel->y, 225.0, 1e-6); +} + +} // namespace origincar_birdseye + +namespace origincar_birdseye +{ + +TEST(BirdseyeGeometry, KeepsVehicleAtConfiguredCanvasPosition) +{ + BirdseyeGeometry geometry({600, 800, 0.01, 2.0, 3.0}); + + const auto vehicle = geometry.basePointToCanvasPixel({0.0, 0.0}); + const auto one_meter_ahead = geometry.basePointToCanvasPixel({1.0, 0.0}); + + ASSERT_TRUE(vehicle.has_value()); + ASSERT_TRUE(one_meter_ahead.has_value()); + EXPECT_NEAR(vehicle->x, 300.0, 1e-6); + EXPECT_NEAR(vehicle->y, 200.0, 1e-6); + EXPECT_NEAR(one_meter_ahead->x, 300.0, 1e-6); + EXPECT_NEAR(one_meter_ahead->y, 100.0, 1e-6); +} + +TEST(BirdseyeGeometry, PreservesPointsOutsideCanvasForHomography) +{ + BirdseyeGeometry geometry({600, 800, 0.01, 2.0, 3.0}); + + const auto four_meters_ahead = geometry.basePointToCanvasPixel({4.0, 2.0}); + + ASSERT_TRUE(four_meters_ahead.has_value()); + EXPECT_NEAR(four_meters_ahead->x, 100.0, 1e-6); + EXPECT_NEAR(four_meters_ahead->y, -200.0, 1e-6); +} + +TEST(ConfigurationValidation, RejectsInvalidCanvas) +{ + const auto zero_scale = validateCanvas({600, 800, 0.0, 2.0, 3.0}, "vehicle"); + const auto invalid_width = validateCanvas({0, 800, 0.01, 2.0, 3.0}, "vehicle"); + + EXPECT_TRUE(zero_scale.has_value()); + EXPECT_TRUE(invalid_width.has_value()); +} + +TEST(ConfigurationValidation, RejectsNonFiniteCameraParameters) +{ + CameraIntrinsics intrinsics{800.0, 800.0, 400.0, 225.0, 800, 450}; + std::vector rotation{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + std::numeric_limits::quiet_NaN()}; + const std::vector translation{0.0, 0.0, 0.2}; + + EXPECT_TRUE(validateCameraConfiguration(intrinsics, rotation, translation).has_value()); +} + +} // namespace origincar_birdseye + +namespace origincar_birdseye +{ + +TEST(ImageValidation, AcceptsConfiguredInputSize) +{ + EXPECT_FALSE(validateImageSize(1280, 720, 1280, 720).has_value()); +} + +TEST(ImageValidation, ReportsConfiguredAndActualInputSizes) +{ + const auto error = validateImageSize(1280, 720, 800, 450); + + ASSERT_TRUE(error.has_value()); + EXPECT_NE(error->find("expected 1280x720"), std::string::npos); + EXPECT_NE(error->find("received 800x450"), std::string::npos); +} + +TEST(ImageValidation, AcceptsRawAndCompressedMessageTypes) +{ + EXPECT_FALSE(validateImageMessageType("raw").has_value()); + EXPECT_FALSE(validateImageMessageType("compressed").has_value()); +} + +TEST(ImageValidation, RejectsUnknownMessageType) +{ + const auto error = validateImageMessageType("mjpeg"); + + ASSERT_TRUE(error.has_value()); + EXPECT_NE(error->find("raw or compressed"), std::string::npos); +} + +} // namespace origincar_birdseye