diff --git a/.github/workflows/gtest.yml b/.github/workflows/gtest.yml index d18c4144..85289b6f 100644 --- a/.github/workflows/gtest.yml +++ b/.github/workflows/gtest.yml @@ -16,8 +16,7 @@ jobs: env: WS_DIR: ${{ github.workspace }}/ws SRC_DIR: ${{ github.workspace }}/ws/src - IMAGE_URL: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg - IMAGE_PATH: /tmp/model_infer_example.jpg + TEST_ASSETS_ROOT: /tmp/auto_aim steps: - name: Checkout repository into ROS2 workspace @@ -26,9 +25,16 @@ jobs: repository: Alliance-Algorithm/rmcs_auto_aim_v2 path: ws/src/rmcs_auto_aim_v2 - - name: Download test frame + - name: Install yq dependency run: | - curl -fsSL "$IMAGE_URL" -o "$IMAGE_PATH" + apt-get update + apt-get install -y yq + + - name: Download test assets + shell: bash + run: | + cd "$SRC_DIR/rmcs_auto_aim_v2/test" + TEST_ASSETS_ROOT="$TEST_ASSETS_ROOT" ./download_assets.sh - name: Configure test project shell: bash @@ -49,4 +55,4 @@ jobs: run: | source /opt/ros/jazzy/setup.bash cd "$SRC_DIR/rmcs_auto_aim_v2/test" - IMAGE="$IMAGE_PATH" ctest --test-dir build --output-on-failure + ctest --test-dir build --output-on-failure diff --git a/README.md b/README.md index 60d63340..f8108395 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,18 @@ 本项目以工程化为最终目的，为机器人提供一个测试与工作流完备，配置友好，重构开销小，错误提示拟人的自瞄系统，方便队员的后续维护和持续开发，为迭代提供舒适的代码基础 +## 核心概念 + +依赖隐藏： + +非侵入式： + +提前编写期检查： + +推迟运行时多态： + +自动化与测试： + ## 部署步骤 先确保海康相机的 SDK 正确构建，再保证 `rmcs_exetutor` 正确构建，如果要运行 RMCS 控制系统的话 @@ -145,7 +157,7 @@ ros2 launch rmcs_auto_aim_v2 launch.py [...] [INFO] [...] [visualization]: Sdp has been written to: /tmp/auto_aim.sdp ``` -随后在本机下载 `VLC`，此外，还需要下载插件：`vlc-plugin-live555` 和 `vlc-plugin-ffmpeg` 以支持播放推流 +对于 `VLC`，需要下载插件：`vlc-plugin-live555` 和 `vlc-plugin-ffmpeg` 接下来只需要将 `/tmp/auto_aim.sdp` 文件拷贝到自己电脑上，使用能够打开`SDP`文件的视频播放器打开即可，也可以使用指令： diff --git a/config/config.yaml b/config/config.yaml index ec76c174..ea3d446f 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -5,14 +5,14 @@ capturer: show_loss_framerate: false show_loss_framerate_interval: 500 reconnect_wait_interval: 100 - source: "hikcamera" + source: "local_video" hikcamera: # int timeout_ms: 500 # float - exposure_us: 3000.0 + exposure_us: 2000.0 # float - framerate: 120 + framerate: 60 # float gain: 16.9807 @@ -21,8 +21,14 @@ capturer: trigger_mode: false fixed_framerate: true local_video: - location: "" + # 替换为你具体的路径 + location: "/workspaces/alliance/test_videos/solve_pnp_v2.mp4" + # double 帧率 + frame_rate: 60 + # bool 是否循环播放 loop_play: true + # bool 是否允许跳帧以满足实时性 + allow_skipping: false identifier: binarization_threshold: 0.5 @@ -41,8 +47,8 @@ identifier: nms_threshold: 0.3 pose_estimator: - camera_matrix: [2414.9359264386621, 0, 717.26243105567414, 0, 2418.0489262208148, 582.68540529942845, 0, 0, 1] - distort_coeff: [-0.0209453389287673, 0.15028138841073832, -0.0006517722113234505, -0.0016861906197686788, 0] + camera_matrix: [1.722231837421459e+03, 0, 7.013056440882832e+02, 0, 1.724876404292754e+03,5.645821718351237e+02 , 0, 0, 1] + distort_coeff: [-0.064232403853946, -0.087667493884102, 0,0, 0.792381808294582] transforms: - parent: "imu_link" diff --git a/doc/utility.md b/doc/utility.md new file mode 100644 index 00000000..1e0d6f5e --- /dev/null +++ b/doc/utility.md @@ -0,0 +1,98 @@ +# Utility 工具库文档 + +本文档描述了 `utility` 工具库中各个模块的功能和用途。 + +## 设计理念 + +本工具库中的许多组件是为了**隐藏第三方库的头文件**而设计的包装器。第三方库（如 ROS2 的 rclcpp、OpenCV、Eigen、PCL 等）的头文件会增加编译成本，影响编译体验。通过使用包装器提供有限确定的接口，可以： + +- **减少编译时间**：避免在头文件中包含大型第三方库头文件 +- **降低编译依赖**：使用 PIMPL 模式将实现细节隐藏在 `.cpp` 文件中 +- **保持接口稳定**：提供简洁、稳定的接口，减少对第三方库内部变化的依赖 +- **改善编译体验**：只暴露必要的接口，减少不必要的类型暴露 + +因此，建议优先使用这些包装器而不是直接包含原始第三方库头文件。 + +## 核心工具 + +### 线性代数 +- **[linear.hpp](../src/utility/math/linear.hpp)**: 提供 `Translation` 和 `Orientation` 结构体，用于表示三维空间中的平移和旋转。支持从符合特定 trait 的对象进行转换，并提供 `copy_to` 方法将数据复制到目标对象。 + +### 错误处理 +- **[panic.hpp](../src/utility/panic.hpp)** / **[panic.cpp](../src/utility/panic.cpp)**: 提供 `panic` 函数，用于程序异常终止。会输出详细的错误信息，包括消息、文件位置、函数名、行号、线程 ID、时间戳和堆栈跟踪。 + +### 设计模式 +- **[pimpl.hpp](../src/utility/pimpl.hpp)**: 提供 PIMPL（Pointer to Implementation）模式的宏定义 `RMCS_PIMPL_DEFINITION`，用于隐藏实现细节。 +- **[details.hpp](../src/utility/details.hpp)**: 提供 Details 模式的宏定义 `RMCS_DETAILS_DEFINITION`，用于分离接口和实现细节。 + +### 序列化 +- **[serializable.hpp](../src/utility/serializable.hpp)**: 提供序列化框架，支持从 YAML 节点或 rclcpp 节点读取参数。使用 `Serializable` 结构体和 `MemberMeta` 来定义可序列化的成员，支持类型安全的参数读取。 + +### 次数限制 +- **[times_limit.hpp](../src/utility/times_limit.hpp)**: 提供 `TimesLimit` 类，用于限制某个操作的执行次数。支持启用/禁用、重置计数等功能。 + +### 图像处理（OpenCV 包装器） +- **[image.hpp](../src/utility/image/image.hpp)** / **[image.cpp](../src/utility/image/image.cpp)**: 提供 `Image` 类，封装图像数据和时间戳。使用 PIMPL 模式隐藏 OpenCV 头文件，避免在头文件中暴露 `cv::Mat` 等 OpenCV 类型。 +- **[image.details.hpp](../src/utility/image/image.details.hpp)**: 定义 `Image::Details` 结构体，包含 OpenCV 的 `cv::Mat` 和相关的访问方法。此文件包含 OpenCV 头文件，应仅在实现文件中使用。 +- **[image/painter.hpp](../src/utility/image/image/painter.hpp)**: 图像绘制工具（具体实现需查看源码）。 + +### ROS2 节点（rclcpp 包装器） +- **[node.hpp](../src/utility/node.hpp)**: 提供 `Node` 类，继承自 `rclcpp::Node`，扩展了日志功能，支持使用 `std::format` 进行格式化输出。提供 `info`、`warn`、`error` 等方法。**注意**：此文件直接继承 rclcpp::Node，如需完全隐藏 rclcpp 头文件，请使用 `rclcpp/node.hpp` 中的 `RclcppNode`。 + +### 鸭子类型 +- **[duck_type.hpp](../src/utility/duck_type.hpp)**: 提供 `duck_array` 模板类，实现鸭子类型检查。支持在编译时验证类型是否满足特定接口要求，并提供类型安全的元素访问。 + +## 子模块 + +### 协程 (coroutine) +- **[coroutine/channel.hpp](../src/utility/coroutine/channel.hpp)**: 提供协程通道 `Channel`，用于协程之间的数据传递。支持发送和接收操作，当通道为空时自动挂起等待。 +- **[coroutine/common.hpp](../src/utility/coroutine/common.hpp)**: 提供协程任务 `task` 模板，用于管理协程的生命周期。支持立即启动、延迟销毁，并能处理异常。 +- **[coroutine/context.hpp](../src/utility/coroutine/context.hpp)**: 定义协程上下文类（当前为空实现）。 + +### 数学工具 (math) +- **[math/sigmoid.hpp](../src/utility/math/sigmoid.hpp)**: 提供 sigmoid 函数实现，用于数值稳定计算。 +- **[math/solve_armors.hpp](../src/utility/math/solve_armors.hpp)**: 提供装甲板正解和逆解算法。`ArmorsForwardSolution` 用于根据机器人位姿计算四个装甲板的位置和姿态。 +- **[math/solve_armors.cpp](../src/utility/math/solve_armors.cpp)**: 正解算法的实现。 +- **[math/solve_pnp.hpp](../src/utility/math/solve_pnp.hpp)**: 提供将相机内外参的转换函数供pnp解算使用。 + +### ROS2 扩展 (rclcpp 包装器) +- **[rclcpp/node.hpp](../src/utility/rclcpp/node.hpp)**: 提供 `RclcppNode` 类，封装 ROS2 节点的基本功能，包括日志、发布主题前缀管理等。**使用 PIMPL 模式完全隐藏 rclcpp 头文件**，推荐在需要避免编译依赖时使用。 +- **[rclcpp/node.cpp](../src/utility/rclcpp/node.cpp)**: `RclcppNode` 的实现，包含所有 rclcpp 相关的头文件。 +- **[rclcpp/node.details.hpp](../src/utility/rclcpp/node.details.hpp)**: `RclcppNode::Details` 的定义，包含 rclcpp 类型，应仅在实现文件中使用。 +- **[rclcpp/parameters.hpp](../src/utility/rclcpp/parameters.hpp)**: 提供参数接口 `IParams` 和参数管理类 `Parameters`，用于统一参数访问接口。**隐藏 rclcpp 参数访问的细节**。 +- **[rclcpp/parameters.cpp](../src/utility/rclcpp/parameters.cpp)**: `Parameters` 的实现，包含 rclcpp 头文件。 +- **[rclcpp/rclcpp_param.hpp](../src/utility/rclcpp/rclcpp_param.hpp)**: 提供 `make_params` 函数，用于从 rclcpp 节点创建参数接口实现。**此文件包含 rclcpp 头文件**，应在实现文件中使用。 +- **[rclcpp/configuration.hpp](../src/utility/rclcpp/configuration.hpp)**: 提供 `configuration` 函数，用于从 YAML 文件加载配置。**此文件包含 YAML-CPP 头文件**。 +- **[rclcpp/visual/armor.hpp](../src/utility/rclcpp/visual/armor.hpp)** / **[rclcpp/visual/armor.cpp](../src/utility/rclcpp/visual/armor.cpp)**: ROS2 可视化消息相关（装甲板可视化），封装 ROS2 可视化消息类型。 + +### 机器人相关 (robot) +- **[robot/armor.hpp](../src/utility/robot/armor.hpp)**: 定义装甲板相关类型，包括 `ArmorColor`（颜色枚举）、`ArmorShape`（形状枚举）、`ArmorType`（类型）、`LightStrip`（灯条）和 `Armor` 结构体。 +- **[robot/color.hpp](../src/utility/robot/color.hpp)**: 定义 `CampColor` 枚举，表示阵营颜色（未知、红色、蓝色）。 +- **[robot/id.hpp](../src/utility/robot/id.hpp)**: 定义 `DeviceId` 枚举和 `DeviceIds` 类，用于标识不同类型的机器人设备（英雄、工程、步兵、哨兵等）。提供设备 ID 的位操作和查询功能。 + +### 日志 (logging) +- **[logging/printer.hpp](../src/utility/logging/printer.hpp)**: 提供 `Printer` 类，用于日志输出。支持不同级别的日志（INFO、WARN、ERROR），使用 `std::format` 进行格式化。 +- **[logging/printer.cpp](../src/utility/logging/printer.cpp)**: `Printer` 的实现。 + +### 共享内存 (shared) +- **[shared/context.hpp](../src/utility/shared/context.hpp)**: 定义共享上下文结构体，包含时间戳和字节数组，用于进程间数据共享。 +- **[shared/interprocess.hpp](../src/utility/shared/interprocess.hpp)**: 提供进程间通信的共享内存客户端。`Client` 模板类包含 `Send` 和 `Recv` 两个类，分别用于发送和接收数据。使用版本号机制确保数据一致性。 + +### 单例模式 (singleton) +- **[singleton/running.hpp](../src/utility/singleton/running.hpp)**: 提供运行状态管理的全局函数 `get_running` 和 `set_running`。 +- **[singleton/running.cpp](../src/utility/singleton/running.cpp)**: 运行状态管理的实现。 + +### 线程工具 (thread) +- **[thread/spsc_queue.hpp](../src/utility/thread/spsc_queue.hpp)**: 提供单生产者单消费者（SPSC）无锁队列的别名定义，基于 Boost.Lockfree 库。 +- **[thread/workers.hpp](../src/utility/thread/workers.hpp)**: 提供 `WorkersContext` 类，用于管理工作线程池。支持提交任务并返回 future，任务必须是 noexcept 可调用的。 +- **[thread/workers.cpp](../src/utility/thread/workers.cpp)**: `WorkersContext` 的实现。 + +### 模型 (model) +- **[model/armor_detection.hpp](../src/utility/model/armor_detection.hpp)**: 定义 `ArmorDetection` 结构体，用于表示检测到的装甲板信息。包含角点坐标、置信度、颜色信息和角色信息。支持从原始数据直接反序列化，并提供边界框计算和角点缩放功能。**注意**：此文件包含 OpenCV 头文件（`opencv2/core/types.hpp`），如需完全隐藏 OpenCV 依赖，建议使用包装器模式。 + + +## 一点个性 + +### ASCII 艺术 +- **[acsii_art.hpp](../src/utility/acsii_art.hpp)**: 提供 ASCII 艺术横幅，用于程序启动时显示 RMCS 标识。 + diff --git a/package.xml b/package.xml index f7e5b742..61f86b26 100644 --- a/package.xml +++ b/package.xml @@ -14,8 +14,9 @@ rmcs_executor + hikcamera ament_cmake - \ No newline at end of file + diff --git a/src/component.cpp b/src/component.cpp index 7cea0b7a..5c6d8f93 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -21,6 +21,21 @@ class AutoAimComponent final : public rmcs_executor::Component { } auto update() -> void override { + using namespace rmcs_description; + if (rmcs_tf.ready()) [[likely]] { + auto camera_odom = + fast_tf::lookup_transform( + *rmcs_tf); + + control_state.timestamp = Clock::now(); + + control_state.camera_to_odom_transform.posture = camera_odom.translation(); + control_state.camera_to_odom_transform.orientation = + Eigen::Quaterniond(camera_odom.rotation()); + + //... + } + recv_state(); send_state(); } @@ -33,6 +48,8 @@ class AutoAimComponent final : public rmcs_executor::Component { ControlClient::Send shm_send; ControlClient::Recv shm_recv; + ControlState control_state; + FramerateCounter framerate; private: @@ -46,6 +63,7 @@ class AutoAimComponent final : public rmcs_executor::Component { if (shm_recv.is_updated()) { auto timestamp = Stamp {}; + shm_recv.with_read([&](const auto& state) { timestamp = state.timestamp; }); if (shm_recv.is_updated()) { @@ -67,8 +85,9 @@ class AutoAimComponent final : public rmcs_executor::Component { return; } - shm_send.with_write([](ControlState& state) { - state.timestamp = Clock::now(); + shm_send.with_write([&](ControlState& state) { + state = control_state; + // ... }); } diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index cf4558d3..90d93efd 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -1,7 +1,9 @@ #include "pose_estimator.hpp" + #include "kernel/transform_tree.hpp" #include "utility/logging/printer.hpp" #include "utility/math/solve_pnp.hpp" +#include "utility/math/solve_pnp/pnp_solution.hpp" #include "utility/serializable.hpp" #include "utility/yaml/tf.hpp" @@ -16,25 +18,24 @@ struct PoseEstimator::Impl { std::array camera_matrix; std::array distort_coeff; - // clang-format off constexpr static std::tuple metas { - &Config::camera_matrix, "camera_matrix", - &Config::distort_coeff, "distort_coeff", + &Config::camera_matrix, + "camera_matrix", + &Config::distort_coeff, + "distort_coeff", }; - // clang-format on }; + Config config; + PnpSolution pnp_solution {}; Printer log { "PoseEstimator" }; - PnpSolution pnp_solution; - auto initialize(const YAML::Node& yaml) noexcept -> std::expected try { auto result = config.serialize(yaml); if (!result.has_value()) { return std::unexpected { result.error() }; } - { auto result = serialize_from(yaml["transforms"]); if (!result.has_value() @@ -43,13 +44,61 @@ struct PoseEstimator::Impl { + util::to_string(result.error()) }; } } + { + pnp_solution.input.camera_matrix = + reshape_array(config.camera_matrix); + pnp_solution.input.distort_coeff = + reshape_array(config.distort_coeff); + } return {}; } catch (const std::exception& e) { return std::unexpected { e.what() }; } - auto transform() { } + auto solve_pnp(std::optional> const& armors) noexcept + -> std::optional> { + if (!armors.has_value()) return std::nullopt; + auto const& _armors = *armors; + + auto armor_shape = [](ArmorShape shape) { + if (shape == ArmorShape::SMALL) { + return rmcs::kSmallArmorShapeOpenCV; + } else { + return rmcs::kLargeArmorShapeOpenCV; + } + }; + + auto armors_in_camera = std::vector {}; + + std::ranges::for_each(_armors | std::views::enumerate, + [&armors_in_camera, &armor_shape, this](auto const& item) { + auto [i, armor] = item; + + pnp_solution.input.armor_shape = armor_shape(armor.shape); + pnp_solution.input.genre = armor.genre; + pnp_solution.input.color = armor_color2camp_color(armor.color); + std::ranges::copy(armor.corners(), pnp_solution.input.armor_detection.begin()); + + auto solved = pnp_solution.solve(); + if (!solved) { + log.warn("solvePnP failed for armor {} ({} {})", i, + get_enum_name(armor.genre), get_enum_name(armor.color)); + return; + } + + auto armor_3d = Armor3D {}; + armor_3d.genre = pnp_solution.result.genre; + armor_3d.color = camp_color2armor_color(pnp_solution.result.color); + armor_3d.id = i; + pnp_solution.result.translation.copy_to(armor_3d.translation); + pnp_solution.result.orientation.copy_to(armor_3d.orientation); + + armors_in_camera.emplace_back(armor_3d); + }); + + return armors_in_camera; + } }; auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept @@ -57,6 +106,11 @@ auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept return pimpl->initialize(yaml); } +auto PoseEstimator::solve_pnp(std::optional> const& armors) const noexcept + -> std::optional> { + return pimpl->solve_pnp(armors); +} + PoseEstimator::PoseEstimator() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index 2c30f633..76f20e49 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -1,6 +1,9 @@ #pragma once + #include "utility/math/linear.hpp" #include "utility/pimpl.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/robot/armor.hpp" #include #include @@ -10,8 +13,14 @@ class PoseEstimator { RMCS_PIMPL_DEFINITION(PoseEstimator) public: + using RclcppNode = util::RclcppNode; + auto initialize(const YAML::Node&) noexcept -> std::expected; + auto visualize(RclcppNode& visual_node) -> void; + + auto solve_pnp(std::optional> const&) const noexcept + -> std::optional>; auto update_imu_link(const Orientation&) noexcept -> void; }; diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 297bc629..f10b8d08 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -1,13 +1,15 @@ #include "visualization.hpp" -#include "module/debug/visualization/stream_session.hpp" +#include + +#include "module/debug/visualization/armor_visualizer.hpp" +#include "module/debug/visualization/stream_session.hpp" #include "utility/image/image.details.hpp" #include "utility/logging/printer.hpp" #include "utility/serializable.hpp" -#include - using namespace rmcs::kernel; +using namespace rmcs::util; constexpr std::array kVideoTypes { "RTP_JEPG", @@ -47,9 +49,14 @@ struct Visualization::Impl { bool is_initialized = false; bool size_determined = false; - Impl() noexcept { session = std::make_unique(); } + std::unique_ptr armor_visualizer; + + Impl() noexcept { + session = std::make_unique(); + armor_visualizer = std::make_unique(); + } - auto initialize(const YAML::Node& yaml) noexcept -> NormalResult { + auto initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept -> NormalResult { auto config = Config {}; auto result = config.serialize(yaml); if (!result.has_value()) { @@ -67,6 +74,9 @@ struct Visualization::Impl { } else { return std::unexpected { "Unknown video type: " + config.stream_type }; } + + armor_visualizer->initialize(visual_node); + is_initialized = true; return {}; } @@ -114,11 +124,15 @@ struct Visualization::Impl { return session->push_frame(mat); } + auto visualize_armors(std::span const& armors) const -> bool { + if (!is_initialized) return false; + return armor_visualizer->visualize(armors); + } }; -auto Visualization::initialize(const YAML::Node& yaml) noexcept +auto Visualization::initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept -> std::expected { - return pimpl->initialize(yaml); + return pimpl->initialize(yaml, visual_node); } auto Visualization::initialized() const noexcept -> bool { return pimpl->initialized(); } @@ -127,6 +141,10 @@ auto Visualization::send_image(const Image& image) noexcept -> bool { return pimpl->send_image(image); } +auto Visualization::visualize_armors(std::span const& armors) const -> bool { + return pimpl->visualize_armors(armors); +} + Visualization::Visualization() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index 419e2a97..efd00d9b 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -1,5 +1,7 @@ #pragma once #include "utility/image/image.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/robot/armor.hpp" #include #include @@ -17,11 +19,14 @@ class Visualization { } public: - auto initialize(const YAML::Node&) noexcept -> std::expected; + auto initialize(const YAML::Node& yaml, util::RclcppNode& visual_node) noexcept + -> std::expected; auto initialized() const noexcept -> bool; auto send_image(const Image&) noexcept -> bool; + + auto visualize_armors(std::span const& armors) const -> bool; }; } diff --git a/src/module/capturer/local_video.cpp b/src/module/capturer/local_video.cpp index 64bc1bf7..af63632c 100644 --- a/src/module/capturer/local_video.cpp +++ b/src/module/capturer/local_video.cpp @@ -1,16 +1,126 @@ #include "local_video.hpp" + +#include +#include + +#include + +#include "utility/image/image.details.hpp" + using namespace rmcs::cap; -struct LocalVideo::Impl { }; +struct LocalVideo::Impl { + Config config; + + using Clock = std::chrono::steady_clock; + + std::optional capturer; + + std::chrono::nanoseconds interval_duration { 0 }; + Clock::time_point last_read_time { Clock::now() }; + + auto set_framerate_interval(double hz) noexcept -> void { + if (hz > 0) { + interval_duration = + std::chrono::nanoseconds(static_cast(std::round(1.0 / hz * 1e9))); + } else { + interval_duration = std::chrono::nanoseconds { 0 }; + } + }; + + auto configure(Config const& _config) -> std::expected { + if (_config.location.empty() || !std::filesystem::exists(_config.location)) { + return std::unexpected { "Local video is not found or location is empty" }; + } + + config = _config; + + try { + capturer.emplace(config.location); + } catch (std::exception const& e) { + return std::unexpected { "Failed to construct VideoCapture: " + std::string(e.what()) }; + } catch (...) { + return std::unexpected { "Failed to construct VideoCapture due to an unknown error." }; + } + + double source_fps = capturer->get(cv::CAP_PROP_FPS); + double target_fps = source_fps > 0 ? source_fps : 30.0; + + if (config.frame_rate > 0) { + target_fps = config.frame_rate; + } + + set_framerate_interval(target_fps); + + last_read_time = Clock::now(); + + return {}; + } + + auto connect() -> std::expected { return configure(config); } + + auto connected() const noexcept -> bool { return capturer.has_value() && capturer->isOpened(); } + + auto disconnect() noexcept -> void { + if (capturer.has_value()) { + capturer.reset(); + } + interval_duration = std::chrono::nanoseconds { 0 }; + } + + auto wait_image() noexcept -> std::expected, std::string> { + if (!capturer.has_value() || !capturer->isOpened()) { + return std::unexpected { "Video stream is not opened." }; + } + + const auto time_before_read = Clock::now(); + const auto next_read_time_expected = last_read_time + interval_duration; + auto wait_duration = next_read_time_expected - time_before_read; + + if (wait_duration.count() > 0) { + std::this_thread::sleep_for(wait_duration); + last_read_time = next_read_time_expected; + } else { + last_read_time = config.allow_skipping ? Clock::now() : next_read_time_expected; + } + + auto frame = cv::Mat {}; + auto image = std::make_unique(); + if (!capturer->read(frame)) { + if (config.loop_play) { + if (capturer->set(cv::CAP_PROP_POS_FRAMES, 0) && capturer->read(frame)) { + last_read_time = Clock::now(); + } else { + return std::unexpected { "End of file reached and failed to " + "loop/reset." }; + } + } else { + return std::unexpected { "End of file reached." }; + } + } + + if (frame.empty()) { + return std::unexpected { "Read frame is empty, possibly due to IO error." }; + } + image->details().set_mat(frame); + image->set_timestamp(last_read_time); + + return image; + }; +}; -auto LocalVideo::configure(const ConfigDetail& config) noexcept - -> std::expected { } +auto LocalVideo::configure(Config const& config) -> std::expected { + return pimpl->configure(config); +} +auto LocalVideo::wait_image() noexcept -> std::expected, std::string> { + return pimpl->wait_image(); +} -auto LocalVideo::connect() noexcept -> std::expected { } +auto LocalVideo::connect() noexcept -> std::expected { return pimpl->connect(); } -auto LocalVideo::connected() const noexcept -> bool { } +auto LocalVideo::connected() const noexcept -> bool { return pimpl->connected(); } -auto LocalVideo::wait_image() -> std::expected, std::string> { } +auto LocalVideo::disconnect() noexcept -> void { return pimpl->disconnect(); } LocalVideo::LocalVideo() noexcept : pimpl { std::make_unique() } { } diff --git a/src/module/capturer/local_video.hpp b/src/module/capturer/local_video.hpp index 69ae1818..bf7515cf 100644 --- a/src/module/capturer/local_video.hpp +++ b/src/module/capturer/local_video.hpp @@ -6,30 +6,39 @@ namespace rmcs::cap { -class LocalVideo { +struct LocalVideo { RMCS_PIMPL_DEFINITION(LocalVideo) -public: +private: struct ConfigDetail { std::string location; + double frame_rate; + bool loop_play; + bool allow_skipping; }; +public: struct Config : ConfigDetail, util::Serializable { constexpr static std::tuple metas { - &Config::location, - "location", + &ConfigDetail::location, + "location", // 视频文件路径 + &ConfigDetail::frame_rate, + "frame_rate", // 帧率 + &ConfigDetail::loop_play, + "loop_play", // 循环播放 + &ConfigDetail::allow_skipping, + "allow_skipping" // 允许跳帧以保证实时性 }; }; - auto configure(const ConfigDetail&) noexcept -> std::expected; + auto configure(Config const&) -> std::expected; auto connect() noexcept -> std::expected; - auto disconnect() noexcept { } - auto connected() const noexcept -> bool; - auto wait_image() -> std::expected, std::string>; -}; + auto disconnect() noexcept -> void; + auto wait_image() noexcept -> std::expected, std::string>; +}; } diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp new file mode 100644 index 00000000..afc95912 --- /dev/null +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -0,0 +1,90 @@ +#include "armor_visualizer.hpp" + +#include "utility/rclcpp/visual/armor.hpp" +#include "utility/robot/armor.hpp" + +using namespace rmcs::debug; +using VisualArmor = rmcs::util::visual::Armor; + +struct ArmorShadow { + decltype(rmcs::Armor3D::genre) genre; + decltype(rmcs::Armor3D::color) color; + decltype(rmcs::Armor3D::id) id; + + bool operator==(ArmorShadow const& other) const = default; + bool operator!=(ArmorShadow const& other) const { return !(*this == other); } +}; + +struct ArmorVisualizer::Impl final { + auto initialize(util::RclcppNode& visual_node) noexcept -> void { + node = std::ref(visual_node); + } + + auto visualize(std::span const& _armors) -> bool { + if (!node.has_value()) { + return false; + } + + auto new_size = _armors.size(); + visual_armors.reserve(new_size); + current_armors.reserve(new_size); + visual_armors.resize(new_size); + current_armors.resize(new_size); + + for (size_t i = 0; i < new_size; i++) { + auto const& input = _armors[i]; + auto& armor_ptr = visual_armors[i]; + auto& shadow = current_armors[i]; + + bool changed = !armor_ptr || needs_rebuild(shadow, input); + + if (changed) { + auto const config = VisualArmor::Config { + .rclcpp = node.value().get(), + .device = input.genre, + .camp = armor_color2camp_color(input.color), + .id = input.id, + .name = "solved_pnp_armor", + .tf = "camera_link", + }; + + armor_ptr = std::make_unique(config); + + shadow.genre = input.genre; + shadow.color = input.color; + shadow.id = input.id; + } + + armor_ptr->move(input.translation, input.orientation); + armor_ptr->update(); + } + + return true; + } + + // static auto camp(ArmorColor const& color) -> CampColor { + // if (color == ArmorColor::BLUE) return CampColor::BLUE; + // if (color == ArmorColor::RED) return CampColor::RED; + // return CampColor::UNKNOWN; + // }; + + static bool needs_rebuild(ArmorShadow shadow, Armor3D const& input) { + return shadow.genre != input.genre || shadow.color != input.color || shadow.id != input.id; + } + + std::optional> node; + std::vector current_armors; + std::vector> visual_armors; +}; + +auto ArmorVisualizer::initialize(util::RclcppNode& visual_node) noexcept -> void { + return pimpl->initialize(visual_node); +} + +auto ArmorVisualizer::visualize(std::span const& armors) -> bool { + return pimpl->visualize(armors); +} + +ArmorVisualizer::ArmorVisualizer() noexcept + : pimpl { std::make_unique() } { }; +ArmorVisualizer::~ArmorVisualizer() noexcept = default; diff --git a/src/module/debug/visualization/armor_visualizer.hpp b/src/module/debug/visualization/armor_visualizer.hpp new file mode 100644 index 00000000..bcc8bea4 --- /dev/null +++ b/src/module/debug/visualization/armor_visualizer.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "utility/pimpl.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/robot/armor.hpp" + +namespace rmcs::debug { + +class ArmorVisualizer { + + RMCS_PIMPL_DEFINITION(ArmorVisualizer) + +public: + auto initialize(util::RclcppNode&) noexcept -> void; + + auto visualize(std::span const&) -> bool; +}; +} diff --git a/src/module/identifier/model.cpp b/src/module/identifier/model.cpp index dff83fcc..285ecddc 100644 --- a/src/module/identifier/model.cpp +++ b/src/module/identifier/model.cpp @@ -234,6 +234,7 @@ struct OpenVinoNet::Impl { for (std::size_t row = 0; row < rows; row++) { auto line = armor_type {}; + //(0,1) 顶左，(2,3) 底左，(4,5) 底右，(6,7) 顶右 line.unsafe_from(std::span { data + row * cols, cols }); line.confidence = util::sigmoid(line.confidence); diff --git a/src/runtime.cpp b/src/runtime.cpp index a1f53770..4ece5e70 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -23,6 +23,7 @@ auto main() -> int { std::signal(SIGINT, [](int) { util::set_running(false); }); auto rclcpp_node = util::RclcppNode { "AutoAim" }; + rclcpp_node.set_pub_topic_prefix("/rmcs/auto_aim/"); auto handle_result = [&](auto runtime_name, const auto& result) { if (!result.has_value()) { @@ -60,9 +61,9 @@ auto main() -> int { { auto config = configuration["identifier"]; - const auto path = std::filesystem::path { util::Parameters::share_location() } + const auto model_location = std::filesystem::path { util::Parameters::share_location() } / std::filesystem::path { config["model_location"].as() }; - config["model_location"] = path.string(); + config["model_location"] = model_location.string(); auto result = identifier.initialize(config); handle_result("identifier", result); @@ -76,7 +77,7 @@ auto main() -> int { // VISUALIZATION if (use_visualization) { auto config = configuration["visualization"]; - auto result = visualization.initialize(config); + auto result = visualization.initialize(config, rclcpp_node); handle_result("visualization", result); } @@ -95,26 +96,29 @@ auto main() -> int { for (const auto& armor_2d : *armors_2d) util::draw(*image, armor_2d); } + if (visualization.initialized()) { visualization.send_image(*image); } - auto armor_3d = std::ignore; - - auto future_state = std::ignore; - using namespace rmcs::util; + control_system.update_state({ .timestamp = Clock::now(), }); - if (framerate.tick()) { - rclcpp_node.info("Framerate: {}hz", framerate.fps()); + auto armors_3d = pose_estimator.solve_pnp(armors_2d); + + if (!armors_3d.has_value()) continue; + + if (visualization.initialized()) { + visualization.visualize_armors(*armors_3d); } + // TODO: pose estimator + // TODO: predictor + // TODO: control + rclcpp_node.spin_once(); } - - rclcpp_node.spin_once(); } - rclcpp_node.shutdown(); } diff --git a/src/utility/image/armor.cpp b/src/utility/image/armor.cpp index 3472f1de..876e5978 100644 --- a/src/utility/image/armor.cpp +++ b/src/utility/image/armor.cpp @@ -47,6 +47,7 @@ auto draw(Image& canvas, const Armor2D& armor) noexcept -> void { cv::putText(opencv_mat, "TR", armor.tr, font, scale, white, thickness, cv::LINE_AA); cv::putText(opencv_mat, "BL", armor.bl, font, scale, white, thickness, cv::LINE_AA); cv::putText(opencv_mat, "BR", armor.br, font, scale, white, thickness, cv::LINE_AA); + cv::putText(opencv_mat, "TL", armor.tl, font, scale, white, thickness, cv::LINE_AA); auto info = std::format("{:.2f} {} {}", armor.confidence, genre, shape); cv::putText(opencv_mat, info, armor.tl + cv::Point2f { 0, -5 }, font, scale, white, thickness, diff --git a/src/utility/math/conversion.hpp b/src/utility/math/conversion.hpp new file mode 100644 index 00000000..a00820b6 --- /dev/null +++ b/src/utility/math/conversion.hpp @@ -0,0 +1,35 @@ + +#pragma once + +#include + +namespace rmcs::util { + +// OpenCV 与 ROS 坐标系之间的变换矩阵 +static const Eigen::Matrix3d kCoordTransformMatrix = + // clang-format off + (Eigen::Matrix3d() << 0, 0, 1, + -1, 0, 0, + 0,-1, 0).finished(); +// clang-format on + +static inline auto opencv2ros_position(const Eigen::Vector3d& position) -> Eigen::Vector3d { + auto result = Eigen::Vector3d(position.z(), -position.x(), -position.y()); + return result; +} + +static inline Eigen::Matrix3d opencv2ros_rotation(const Eigen::Matrix3d& rotation_matrix) { + return kCoordTransformMatrix * rotation_matrix * kCoordTransformMatrix.transpose(); +} + +static inline Eigen::Vector3d ros2opencv_position(const Eigen::Vector3d& position) { + auto result = Eigen::Vector3d(-position.y(), -position.z(), position.x()); + return result; +} + +static inline Eigen::Matrix3d ros2opencv_rotation(const Eigen::Matrix3d& rotation_matrix) { + + return kCoordTransformMatrix.transpose() * rotation_matrix * kCoordTransformMatrix; +} + +} diff --git a/src/utility/math/solve_pnp.cpp b/src/utility/math/solve_pnp.cpp deleted file mode 100644 index 1f0e898b..00000000 --- a/src/utility/math/solve_pnp.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include "solve_pnp.hpp" -#include -#include -#include -#include - -using namespace rmcs::util; - -template -static auto cast_opencv_matrix(std::array& source) { - auto mat_type = int {}; - /* */ if constexpr (std::same_as) { - mat_type = CV_64FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32SC1; - } else { - static_assert(false, "Unsupport mat scale type"); - } - - return cv::Mat { 1, cols, mat_type, source.data() }; -} - -template -static auto cast_opencv_matrix(std::array, rows>& source) { - auto mat_type = int {}; - /* */ if constexpr (std::same_as) { - mat_type = CV_64FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32SC1; - } else { - static_assert(false, "Unsupport mat scale type"); - } - - return cv::Mat { rows, cols, mat_type, source[0].data() }; -} - -auto PnpSolution::solve() noexcept -> void { - const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); - const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); - - const auto armor_shape = std::ranges::to(input.armor_shape - | std::views::transform([](const Point3d& point) { return point.make(); })); - - const auto armor_detection = std::ranges::to(input.armor_detection - | std::views::transform([](const Point2d& point) { return point.make(); })); - - auto rota_vec = cv::Vec3d {}; - auto tran_vec = cv::Vec3d {}; - cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, - false, cv::SOLVEPNP_IPPE); - - { - result.translation.x = tran_vec[0]; - result.translation.y = tran_vec[1]; - result.translation.z = tran_vec[2]; - } - { - auto rotation_opencv = cv::Mat {}; - cv::Rodrigues(rota_vec, rotation_opencv); - - auto rotation_eigen = Eigen::Matrix3d {}; - rotation_eigen << // Col Major - rotation_opencv.at(0, 0), // [0,0] - rotation_opencv.at(0, 1), // [0,1] - rotation_opencv.at(0, 2), // [0,2] - rotation_opencv.at(1, 0), // [1,0] - rotation_opencv.at(1, 1), // [1,1] - rotation_opencv.at(1, 2), // [1,2] - rotation_opencv.at(2, 0), // [2,0] - rotation_opencv.at(2, 1), // [2,1] - rotation_opencv.at(2, 2); // [2,2] - result.orientation = Eigen::Quaterniond { rotation_eigen }; - } -} diff --git a/src/utility/math/solve_pnp.hpp b/src/utility/math/solve_pnp.hpp index 806b098a..257626d6 100644 --- a/src/utility/math/solve_pnp.hpp +++ b/src/utility/math/solve_pnp.hpp @@ -1,26 +1,73 @@ #pragma once -#include "utility/math/linear.hpp" -#include "utility/math/point.hpp" -#include - -namespace rmcs::util { - -struct PnpSolution { - struct Input { - // Row Major - std::array, 3> camera_matrix; - std::array distort_coeff; - std::array armor_shape; - std::array armor_detection; - } input; - struct Result { - Translation translation; - Orientation orientation; - } result; - - PnpSolution() noexcept = default; - - auto solve() noexcept -> void; -}; +#include +#include +#include + +template +static auto cast_opencv_matrix(std::array& source) { + auto mat_type = int {}; + /* */ if constexpr (std::same_as) { + mat_type = CV_64FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32SC1; + } else { + static_assert(false, "Unsupport mat scale type"); + } + + return cv::Mat { 1, cols, mat_type, source.data() }; +} + +template +static auto cast_opencv_matrix(std::array, rows>& source) { + auto mat_type = int {}; + /* */ if constexpr (std::same_as) { + mat_type = CV_64FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32SC1; + } else { + static_assert(false, "Unsupport mat scale type"); + } + + return cv::Mat { rows, cols, mat_type, source[0].data() }; +} + +template +concept ConvertibleTo = std::is_convertible_v; + +/* @Note: Row-Major */ +template + requires ConvertibleTo +static auto reshape_array(std::array const& input_array) + -> std::array, rows> { + static_assert(N == rows * cols, "input_array的元素总数N必须等于rows*cols"); + using ResultArray = std::array, rows>; + + ResultArray result_array; + for (std::size_t i = 0; i < N; ++i) { + std::size_t row_idx = i / cols; + std::size_t col_idx = i % cols; + + result_array[row_idx][col_idx] = input_array[i]; + } + return result_array; +} + +/* @Note: Row-Major */ +template + requires ConvertibleTo +static auto reshape_array(std::array const& input_array) + -> std::array { + using ResultArray = std::array; + + ResultArray result_array; + std::transform(input_array.begin(), input_array.end(), result_array.begin(), + [](const input_type& val) { return static_cast(val); }); + + return result_array; } diff --git a/src/utility/math/solve_pnp/pnp_solution.cpp b/src/utility/math/solve_pnp/pnp_solution.cpp new file mode 100644 index 00000000..9b4c76ab --- /dev/null +++ b/src/utility/math/solve_pnp/pnp_solution.cpp @@ -0,0 +1,45 @@ +#include "pnp_solution.hpp" + +#include "utility/math/conversion.hpp" +#include "utility/math/solve_pnp.hpp" +#include +#include +#include + +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT +#include + +using namespace rmcs::util; +auto PnpSolution::solve() noexcept -> bool { + const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); + const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); + + const auto armor_shape = std::ranges::to(input.armor_shape + | std::views::transform([](const Point3d& point) { return point.make(); })); + + const auto armor_detection = std::ranges::to(input.armor_detection + | std::views::transform([](const Point2d& point) { return point.make(); })); + + auto rota_vec = cv::Vec3d {}; + auto tran_vec = cv::Vec3d {}; + auto success = cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, + rota_vec, tran_vec, false, cv::SOLVEPNP_IPPE); + + if (!success) return false; + + auto tran_vec_eigen_opencv = Eigen::Vector3d {}; + cv::cv2eigen(tran_vec, tran_vec_eigen_opencv); + + auto rotation_opencv = cv::Mat {}; + cv::Rodrigues(rota_vec, rotation_opencv); + auto rotation_eigen_opencv = Eigen::Matrix3d {}; + cv::cv2eigen(rotation_opencv, rotation_eigen_opencv); + + result.genre = input.genre; + result.color = input.color; + result.translation = opencv2ros_position(tran_vec_eigen_opencv); + result.orientation = + Eigen::Quaterniond(opencv2ros_rotation(rotation_eigen_opencv)).normalized(); + + return true; +} diff --git a/src/utility/math/solve_pnp/pnp_solution.hpp b/src/utility/math/solve_pnp/pnp_solution.hpp new file mode 100644 index 00000000..1499dcb0 --- /dev/null +++ b/src/utility/math/solve_pnp/pnp_solution.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include + +#include "utility/math/linear.hpp" +#include "utility/math/point.hpp" +#include "utility/robot/color.hpp" +#include "utility/robot/id.hpp" + +namespace rmcs::util { + +struct PnpSolution { + struct Input { + // Row Major + std::array, 3> camera_matrix; + std::array distort_coeff; + std::array armor_shape; + std::array armor_detection; + DeviceId genre; + CampColor color; + } input; + struct Result { + Translation translation; + Orientation orientation; + DeviceId genre; + CampColor color; + } result; + + PnpSolution() noexcept = default; + + auto solve() noexcept -> bool; +}; +} diff --git a/src/utility/model/armor_detection.hpp b/src/utility/model/armor_detection.hpp index 7c6ccd95..c2c7c917 100644 --- a/src/utility/model/armor_detection.hpp +++ b/src/utility/model/armor_detection.hpp @@ -9,15 +9,16 @@ struct ArmorInferResult { using Point = cv::Point_; using Rect = cv::Rect_; + //(0,1) 顶左，(2,3) 底左，(4,5) 底右，(6,7) 顶右 struct Corners { precision_type lt_x; precision_type lt_y; + precision_type lb_x; + precision_type lb_y; precision_type rb_x; precision_type rb_y; precision_type rt_x; precision_type rt_y; - precision_type lb_x; - precision_type lb_y; auto lt() const noexcept { return Point { lt_x, lt_y }; } auto rb() const noexcept { return Point { rb_x, rb_y }; } auto rt() const noexcept { return Point { rt_x, rt_y }; } diff --git a/src/utility/rclcpp/visual/armor.cpp b/src/utility/rclcpp/visual/armor.cpp index f60be4bd..03f71020 100644 --- a/src/utility/rclcpp/visual/armor.cpp +++ b/src/utility/rclcpp/visual/armor.cpp @@ -1,59 +1,106 @@ #include "armor.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/node.details.hpp" -#include -#include + +#include using namespace rmcs::util::visual; -using Marker = visualization_msgs::msg::Marker; +using Marker = visualization_msgs::msg::Marker; +using MarkerArray = visualization_msgs::msg::MarkerArray; struct Armor::Impl { static inline rclcpp::Clock rclcpp_clock { RCL_SYSTEM_TIME }; + std::unique_ptr config; + Marker marker; - std::shared_ptr> rclcpp_pub; + Marker arrow_marker; + std::shared_ptr> rclcpp_pub; - explicit Impl(const Config& config) { - auto& rclcpp = config.rclcpp; - auto& details = config.rclcpp.details; + explicit Impl(const Config& config) + : config(std::make_unique(config)) { + initialize(); + } - if (!prefix::check_naming(config.id) || !prefix::check_naming(config.tf)) { - util::panic( - std::format("Not a valid naming for armor id or tf: {}", prefix::naming_standard)); - } + static auto create_rclcpp_publisher(Config const& config) + -> std::shared_ptr> { + const auto topic_name { config.rclcpp.get_pub_topic_prefix() + config.name }; - const auto topic_name { rclcpp.get_pub_topic_prefix() + config.id }; - rclcpp_pub = details->make_pub(topic_name, qos::debug); + if (!config.rclcpp.details) + util::panic("Rclcpp node details are required in config to create publisher."); - marker.header.frame_id = config.tf; + return config.rclcpp.details->make_pub(topic_name, qos::debug); + } + + auto initialize() -> void { + if (!prefix::check_naming(config->name) || !prefix::check_naming(config->tf)) { + util::panic(std::format( + "Not a valid naming for armor name or tf: {}", prefix::naming_standard)); + } - marker.ns = config.id; - marker.id = 0; - marker.type = Marker::CUBE; - marker.action = Marker::ADD; + marker.header.frame_id = config->tf; + marker.ns = config->name; + marker.id = config->id; + marker.type = Marker::CUBE; + marker.action = Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); // ref: "https://www.robomaster.com/zh-CN/products/components/detail/149" - /* */ if (DeviceIds::kSmallArmorDevices().contains(config.device)) { + /* */ if (DeviceIds::kSmallArmorDevices().contains(config->device)) { marker.scale.x = 0.003, marker.scale.y = 0.140, marker.scale.z = 0.125; - } else if (DeviceIds::kLargeArmorDevices().contains(config.device)) { + } else if (DeviceIds::kLargeArmorDevices().contains(config->device)) { marker.scale.x = 0.003, marker.scale.y = 0.235, marker.scale.z = 0.127; } else { util::panic("Wrong device id for a visualized armor"); }; - /* */ if (config.camp == CampColor::RED) { + /* */ if (config->camp == CampColor::RED) { marker.color.r = 1., marker.color.g = 0., marker.color.b = 0., marker.color.a = 1.; - } else if (config.camp == CampColor::BLUE) { + } else if (config->camp == CampColor::BLUE) { marker.color.r = 0., marker.color.g = 0., marker.color.b = 1., marker.color.a = 1.; } else { - util::panic("Please specify a valid armor color"); + marker.color.r = 1., marker.color.g = 0., marker.color.b = 1., marker.color.a = 1.; + } + + arrow_marker.header.frame_id = config->tf; + arrow_marker.ns = config->name + std::string("_arrow"); + arrow_marker.id = config->id; + arrow_marker.type = Marker::ARROW; + arrow_marker.action = Marker::ADD; + arrow_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + + arrow_marker.scale.x = 0.2; + arrow_marker.scale.y = 0.01; + arrow_marker.scale.z = 0.01; + + /* */ if (config->camp == CampColor::RED) { + arrow_marker.color.r = 1., arrow_marker.color.g = 0., arrow_marker.color.b = 0., + arrow_marker.color.a = 1.; + } else if (config->camp == CampColor::BLUE) { + arrow_marker.color.r = 0., arrow_marker.color.g = 0., arrow_marker.color.b = 1., + arrow_marker.color.a = 1.; + } else { + arrow_marker.color.r = 1., arrow_marker.color.g = 0., arrow_marker.color.b = 1., + arrow_marker.color.a = 1.; } } auto update() noexcept -> void { - marker.header.stamp = rclcpp_clock.now(); - rclcpp_pub->publish(marker); + if (!rclcpp_pub) { + rclcpp_pub = create_rclcpp_publisher(*config); + } + + MarkerArray visual_marker; + const auto current_stamp = rclcpp_clock.now(); + marker.header.stamp = current_stamp; + arrow_marker.header.stamp = current_stamp; + + arrow_marker.pose = marker.pose; + visual_marker.markers.emplace_back(marker); + visual_marker.markers.emplace_back(arrow_marker); + + rclcpp_pub->publish(visual_marker); } auto move(const Translation& t, const Orientation& q) noexcept { diff --git a/src/utility/rclcpp/visual/armor.hpp b/src/utility/rclcpp/visual/armor.hpp index 5b6bf36a..e09f1a4f 100644 --- a/src/utility/rclcpp/visual/armor.hpp +++ b/src/utility/rclcpp/visual/armor.hpp @@ -3,6 +3,7 @@ #include "utility/rclcpp/visual/movable.hpp" #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" +#include namespace rmcs::util::visual { @@ -14,11 +15,13 @@ struct Armor : public Movable { DeviceId device; CampColor camp; - std::string id; + int id; + std::string name; std::string tf; }; explicit Armor(const Config&) noexcept; + ~Armor() noexcept; Armor(const Armor&) = delete; diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index 7d7aaed7..a2bd7114 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -1,6 +1,8 @@ #pragma once #include "utility/math/point.hpp" +#include "utility/robot/color.hpp" #include "utility/robot/id.hpp" +#include #include #include @@ -11,6 +13,17 @@ constexpr auto get_enum_name(ArmorColor color) noexcept { constexpr std::array details { "DARK", "RED", "BLUE", "MIX" }; return details[std::to_underlying(color)]; } +inline constexpr auto armor_color2camp_color(ArmorColor const& color) -> CampColor { + if (color == ArmorColor::BLUE) return CampColor::BLUE; + if (color == ArmorColor::RED) return CampColor::RED; + return CampColor::UNKNOWN; +}; + +inline constexpr auto camp_color2armor_color(CampColor const& color) -> ArmorColor { + if (color == CampColor::BLUE) return ArmorColor::BLUE; + if (color == CampColor::RED) return ArmorColor::RED; + return ArmorColor::MIX; +}; enum class ArmorShape : bool { LARGE, SMALL }; constexpr auto get_enum_name(ArmorShape shape) noexcept { @@ -43,22 +56,46 @@ struct Armor2D { } }; -struct Armor3D { }; +struct Armor3D { + ArmorGenre genre; + ArmorColor color; + int id; + + Eigen::Vector3d translation; + Eigen::Quaterniond orientation; +}; struct Armor { }; using Armors = std::vector; -constexpr std::array kLargeArmorShape { +constexpr double kLightBarHeight = 0.056; +constexpr double kLargeArmorWidth = 0.23; +constexpr double kSmallArmorWidth = 0.135; + +constexpr std::array kLargeArmorShapeOpenCV { + Point3d { -0.5 * kLargeArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-left + Point3d { 0.5 * kLargeArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-right + Point3d { 0.5 * kLargeArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right + Point3d { -0.5 * kLargeArmorWidth, 0.5 * kLightBarHeight, -0.0 } // Bottom-left +}; + +constexpr std::array kSmallArmorShapeOpenCV { + Point3d { -0.5 * kSmallArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-left + Point3d { 0.5 * kSmallArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-right + Point3d { 0.5 * kSmallArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right + Point3d { -0.5 * kSmallArmorWidth, 0.5 * kLightBarHeight, -0.0 } // Bottom-left +}; + +constexpr std::array kLargeArmorShapeRos { Point3d { 0.0, 0.115, 0.028 }, // Top-left - Point3d { 0.0, -0.115, 0.028 }, // Top-right Point3d { 0.0, -0.115, -0.028 }, // Bottom-right + Point3d { 0.0, -0.115, 0.028 }, // Top-right Point3d { 0.0, 0.115, -0.028 } // Bottom-left }; -constexpr std::array kSmallArmorShape { +constexpr std::array kSmallArmorShapeRos { Point3d { 0.0, 0.0675, 0.028 }, // Top-left - Point3d { 0.0, -0.0675, 0.028 }, // Top-right Point3d { 0.0, -0.0675, -0.028 }, // Bottom-right + Point3d { 0.0, -0.0675, 0.028 }, // Top-right Point3d { 0.0, 0.0675, -0.028 } // Bottom-left }; - } diff --git a/src/utility/robot/id.hpp b/src/utility/robot/id.hpp index 5ca91c67..9452c450 100644 --- a/src/utility/robot/id.hpp +++ b/src/utility/robot/id.hpp @@ -86,8 +86,9 @@ struct DeviceIds { static constexpr auto None() { return DeviceIds {}; } static constexpr auto Full() { return DeviceIds { (1 << 11) - 1 }; } - constexpr DeviceIds() = default; - constexpr DeviceIds(const DeviceIds&) = default; + constexpr DeviceIds() = default; + constexpr DeviceIds(const DeviceIds&) = default; + constexpr DeviceIds& operator=(const DeviceIds&) = default; constexpr explicit DeviceIds(uint16_t data) noexcept : data { data } { }; diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index 8b615b4b..3152ad47 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -16,6 +16,11 @@ enum class ShootMode { BUFF_LARGE, }; +struct Transform { + Direction3d posture {}; + Orientation orientation {}; +}; + struct AutoAimState { Stamp timestamp {}; @@ -34,6 +39,13 @@ struct ControlState { double bullet_speed {}; Orientation imu_state {}; + /*Note: + * 对应关系： + * odom<->fast_tf::OdomImu, + * camera<->fast_tf::CameraLink + * */ + Transform camera_to_odom_transform {}; + DeviceIds targets { DeviceIds::Full() }; }; static_assert(std::is_trivially_copyable_v); diff --git a/src/utility/tf/static_tf.hpp b/src/utility/tf/static_tf.hpp index a02b406d..f46f2683 100644 --- a/src/utility/tf/static_tf.hpp +++ b/src/utility/tf/static_tf.hpp @@ -94,9 +94,6 @@ struct Joint { using Result = typename FindInTuple::Result; }; - /// Path - /// TODO: - /// /// Function Based /// @@ -127,7 +124,7 @@ struct Joint { static constexpr auto find() noexcept { using Result = typename Find::Result; static_assert(!std::same_as, "没有找到你想要的变换节点"); - return Result { }; + return Result {}; } template diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 782f3577..dd33da70 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -68,7 +68,7 @@ target_link_libraries( test_model_infer yaml-cpp::yaml-cpp openvino::runtime - ${OpenCV_LIBS} + ${OpenCV_LIBRARIES} ) # Device Id @@ -81,11 +81,21 @@ ament_add_gtest( ament_add_gtest( test_solve_pnp ${TEST_DIR}/solve_pnp.cpp - ${RMCS_SRC_DIR}/utility/math/solve_pnp.cpp + ${RMCS_SRC_DIR}/utility/math/solve_pnp/pnp_solution.cpp + ${RMCS_SRC_DIR}/module/identifier/model.cpp + ${RMCS_SRC_DIR}/utility/image/image.cpp +) +ament_target_dependencies( + test_solve_pnp + rclcpp + visualization_msgs + geometry_msgs ) target_link_libraries( test_solve_pnp ${OpenCV_LIBRARIES} + openvino::runtime + yaml-cpp::yaml-cpp ) # Static TF @@ -98,3 +108,41 @@ target_link_libraries( test_static_tf yaml-cpp::yaml-cpp ) + +# Transform Communication +ament_add_gtest( + test_transform_communication + ${TEST_DIR}/transform_communication.cpp +) + +if(HIKCAMERA_AVAILABLE) + # Hikcamera + add_executable( + example_hikcamera + ${TEST_DIR}/hikcamera.cpp + ) + target_link_libraries( + example_hikcamera + ${OpenCV_LIBRARIES} + ${hikcamera_LIBRARIES} + ) + + # RTP Streaming + add_executable( + example_streaming + ${TEST_DIR}/streaming.cpp + ${RMCS_SRC_DIR}/module/capturer/hikcamera.cpp + ${RMCS_SRC_DIR}/module/debug/visualization/stream_session.cpp + ${RMCS_SRC_DIR}/module/debug/visualization/stream_context.cpp + ${RMCS_SRC_DIR}/utility/image/image.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp + ) + target_link_libraries( + example_streaming + rclcpp::rclcpp + ${OpenCV_LIBRARIES} + ${hikcamera_LIBRARIES} + ) +else() + message(STATUS "hikcamera 未检测到，跳过相关构建") +endif() diff --git a/test/asset.yml b/test/asset.yml index 3259ccac..c7e5578c 100644 --- a/test/asset.yml +++ b/test/asset.yml @@ -1,3 +1,13 @@ -b4_front_2_armors: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg -b1_orientation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/orientation.mp4 -b1_translation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/translation.mp4 +resources: + #model_infer + model_infer_img: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg + + #solve_pnp + blue_0_5m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m.jpg" + blue_0_5m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m-45degree.jpg" + blue_1_0m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m.jpg" + blue_1_0m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m-45degree.jpg" + blue_2_0m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m.jpg" + blue_2_0m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m-45degree.jpg" + blue_3_0m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m.jpg" + blue_3_0m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m-45degree.jpg" diff --git a/test/download_assets.sh b/test/download_assets.sh new file mode 100755 index 00000000..ab12af2a --- /dev/null +++ b/test/download_assets.sh @@ -0,0 +1,117 @@ +#!/bin/bash + +# 设置：如果任何命令失败，立即退出，对管道也生效 +set -eo pipefail + +# --- 配置 --- +# 获取脚本自身的目录，用于定位配置文件 +SCRIPT_DIR=$(cd "$(dirname "$0")" && pwd) +CONFIG_YML="$SCRIPT_DIR/asset.yml" +# 资源存放目录，优先使用 TEST_ASSETS_ROOT，默认为 /tmp/auto_aim（与测试代码一致） +DOWNLOAD_DIR="${TEST_ASSETS_ROOT:-/tmp/auto_aim}" +# 初始化为空字符串，表示当前没有文件在下载** +CURRENT_DOWNLOAD_PATH="" +# ----------------- + +# --- 信号处理函数 --- +cleanup() { + echo "" + echo "[INTERRUPT] 捕获到 Ctrl+C。开始清理..." >&2 + + if [ -n "$CURRENT_DOWNLOAD_PATH" ]; then + if [ -f "$CURRENT_DOWNLOAD_PATH" ]; then + echo "[CLEANUP] 删除不完整的文件: $CURRENT_DOWNLOAD_PATH" >&2 + rm -f "$CURRENT_DOWNLOAD_PATH" + fi + fi + echo "[EXIT] 脚本已终止。" >&2 + exit 1 +} + +trap cleanup INT TERM + +check_dependencies() { + if ! command -v yq &> /dev/null || ! command -v curl &> /dev/null; then + echo "[FATAL] 错误: 缺少必要的依赖工具 (yq 或 curl)。" >&2 + echo "请安装 yq (用于解析 YAML) 和 curl (用于下载)。" >&2 + exit 1 + fi +} + +download_asset() { + local asset_id="$1" + local url="$2" + local filename + filename=$(basename "$url") + local local_path="$DOWNLOAD_DIR/$filename" + + if [ -f "$local_path" ]; then + echo " [SKIP] $asset_id 已存在。" + return 0 + fi + + echo " [INFO] 正在下载 $asset_id -> $filename..." + + CURRENT_DOWNLOAD_PATH="$local_path" + + if curl -fsSL -o "$local_path" "$url" &> /dev/null; then + echo " [SUCCESS] -> $local_path" + CURRENT_DOWNLOAD_PATH="" + return 0 + else + echo " [ERROR] 下载失败: $asset_id (URL: $url)" >&2 + rm -f "$local_path" + CURRENT_DOWNLOAD_PATH="" + return 1 + fi +} + +main() { + local RESOURCE_MAPPING + local FAILED_COUNT=0 + + check_dependencies + + echo "--- 资源下载脚本启动 ---" + + if [ ! -f "$CONFIG_YML" ]; then + echo "[FATAL] 配置文件未找到于 $CONFIG_YML" >&2 + trap - INT TERM + exit 1 + fi + + mkdir -p "$DOWNLOAD_DIR" + + RESOURCE_MAPPING=$(yq '.resources | to_entries | .[] | "\(.key)|\(.value)"' "$CONFIG_YML") + + if [ -z "$RESOURCE_MAPPING" ]; then + echo "[WARNING] YAML 文件中未定义任何 'resources'，退出。" + trap - INT TERM + exit 0 + fi + + echo "找到 $(echo "$RESOURCE_MAPPING" | wc -l | tr -d '[:space:]') 个资源需要处理。" + + while IFS='|' read -r asset_id url; do + asset_id=$(echo "$asset_id" | tr -d '"') + url=$(echo "$url" | tr -d '"') + + download_asset "$asset_id" "$url" || FAILED_COUNT=$((FAILED_COUNT + 1)) + + done <<< "$RESOURCE_MAPPING" + + echo "" + echo "--- 资源下载完成 ---" + + trap - INT TERM + + if [ "$FAILED_COUNT" -eq 0 ]; then + echo "[INFO] 所有资源下载成功或已跳过。" + exit 0 + else + echo "[ERROR] 警告：有 $FAILED_COUNT 个资源下载失败。" >&2 + exit 1 + fi +} + +main diff --git a/test/model_infer.cpp b/test/model_infer.cpp index cca80c9b..11a53927 100644 --- a/test/model_infer.cpp +++ b/test/model_infer.cpp @@ -29,6 +29,21 @@ constexpr auto config = R"( nms_threshold: 0.3 )"; +// --- 资源路径 --- +static std::filesystem::path asset_root() { + if (const char* env = std::getenv("TEST_ASSETS_ROOT"); env && *env) { + return std::filesystem::path { env }; + } + + const char* default_path = "/tmp/auto_aim"; + + return std::filesystem::path { default_path }; +} + +static std::filesystem::path asset_path(std::string_view filename) { + return asset_root() / filename; +} + TEST(model, sync_infer) { using namespace rmcs::identifier; @@ -41,13 +56,12 @@ TEST(model, sync_infer) { auto result = net.configure(yaml); ASSERT_TRUE(result.has_value()) << error_head << result.error(); - auto image_location = std::getenv("IMAGE"); - ASSERT_NE(image_location, nullptr) << error_head << "Set env 'IMAGE' to pass infer source"; + const auto image_location = asset_path("model_infer_example.jpg"); auto image { Image {} }; image.details().mat = cv::imread(image_location); ASSERT_FALSE(image.details().mat.empty()) - << error_head << std::format("Failed to read image from '{}'", image_location); + << error_head << "Failed to read image from " + std::string(image_location); const auto use_roi_segment = yaml["use_roi_segment"].as(); const auto roi_cols = yaml["roi_cols"].as(); diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index d62da49b..28a99a37 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -1,424 +1,291 @@ -#include "utility/math/solve_pnp.hpp" -#include "utility/math/linear.hpp" -#include "utility/math/point.hpp" +#include // for std::clamp, std::replace +#include // for std::getenv +#include +#include +#include // for std::setprecision +#include // for structured output +#include +#include +#include -#include -#include #include -#include +#include +#include + +#include "module/identifier/model.hpp" +#include "utility/image/image.details.hpp" +#include "utility/math/point.hpp" +#include "utility/math/solve_pnp/pnp_solution.hpp" +#include "utility/robot/armor.hpp" -using namespace rmcs; using namespace rmcs::util; +using namespace rmcs; +using Eigen::Quaterniond; using Eigen::Vector2d; using Eigen::Vector3d; -// 辅助函数：创建测试用的相机内参 -PnpSolution::Input create_test_input(double focal_length = 800.0, double cx = 320.0, - double cy = 240.0, const std::array& distort = { 0.0, 0.0, 0.0, 0.0, 0.0 }) { - PnpSolution::Input input; - input.camera_matrix = { { - { focal_length, 0.0, cx }, - { 0.0, focal_length, cy }, - { 0.0, 0.0, 1.0 }, - } }; - input.distort_coeff = distort; - return input; -} - -// 辅助函数：创建小装甲板的 3D 点 -std::array create_small_armor_shape() { - constexpr double ARMOR_WIDTH = 0.135; // 135mm - constexpr double ARMOR_HEIGHT = 0.056; // 56mm - - const auto eigen_points = std::array { - Point3d { 0.0, ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, // 右上 - Point3d { 0.0, -ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, // 右下 - Point3d { 0.0, -ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 }, // 左下 - Point3d { 0.0, ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 } // 左上 - }; - - auto points = std::array(); - for (size_t i = 0; i < 4; i++) { - points[i] = Point3d(eigen_points[i]); +// --- 资源路径 --- +// 测试资源路径配置: +// - 优先使用环境变量 TEST_ASSETS_ROOT +// - 未设置时默认使用 /tmp/auto_aim +// - 运行前需执行: cd test && ./download_assets.sh +static std::filesystem::path asset_root() { + if (const char* env = std::getenv("TEST_ASSETS_ROOT"); env && *env) { + return std::filesystem::path { env }; } - return points; -} - -// 辅助函数：创建大装甲板的 3D 点 -std::array create_big_armor_shape() { - constexpr double BIG_ARMOR_WIDTH = 0.230; // 230mm - constexpr double ARMOR_HEIGHT = 0.056; // 56mm - - const auto eigen_points = std::array { - Point3d { 0.0, BIG_ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, - Point3d { 0.0, -BIG_ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, - Point3d { 0.0, -BIG_ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 }, - Point3d { 0.0, BIG_ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 }, - }; - auto points = std::array(); - for (size_t i = 0; i < 4; i++) { - points[i] = Point3d(eigen_points[i]); - } - return points; -} + const char* default_path = "/tmp/auto_aim"; -// 辅助函数：从 Eigen::Vector2d 创建 Point2d 数组 -std::array create_armor_detection(const std::array& eigen_points) { - auto points = std::array(); - for (size_t i = 0; i < 4; i++) { - points[i] = Point2d(eigen_points[i]); - } - return points; + return std::filesystem::path { default_path }; } -// 辅助函数：验证四元数是否归一化 -bool is_quaternion_normalized(const Orientation& q, double tolerance = 0.01) { - const auto norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); - return std::abs(norm - 1.0) < tolerance; +static std::filesystem::path asset_path(std::string_view filename) { + return asset_root() / filename; } -// 辅助函数：计算两点之间的距离 -double distance(const Translation& a, const Translation& b) { - const auto dx = a.x - b.x; - const auto dy = a.y - b.y; - const auto dz = a.z - b.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); -} - -// ========== 测试用例 ========== - -class PnpSolverTest : public ::testing::Test { -protected: - void SetUp() override { - // 每个测试前的设置 - } - - void TearDown() override { - // 每个测试后的清理 - } +// --- 测试数据 --- +struct PnpTestCase { + std::string filename; + double expected_distance_m; // 预期的目标距离（米） + double expected_angle_deg; // 预期的偏航角（度，0 或 45） }; -TEST_F(PnpSolverTest, BasicSmallArmor) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; - - const auto trans_norm = std::sqrt(solution.result.translation.x * solution.result.translation.x - + solution.result.translation.y * solution.result.translation.y - + solution.result.translation.z * solution.result.translation.z); - - EXPECT_GT(trans_norm, 0.0) << "Translation should not be zero"; -} - -TEST_F(PnpSolverTest, BigArmor) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_big_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 380.0, 200.0 }, - Vector2d { 260.0, 200.0 }, - Vector2d { 260.0, 280.0 }, - Vector2d { 380.0, 280.0 }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); +// 所有本地测试数据 +const std::vector kPnpTestCases = { + { "blue-0.5m.jpg", 0.5, 0.0 }, + { "blue-0.5m-45degree.jpg", 0.5, 45.0 }, + { "blue-1.0m.jpg", 1.0, 0.0 }, + { "blue-1.0m-45degree.jpg", 1.0, 45.0 }, + { "blue-2.0m.jpg", 2.0, 0.0 }, + { "blue-2.0m-45degree.jpg", 2.0, 45.0 }, + { "blue-3.0m.jpg", 3.0, 0.0 }, + { "blue-3.0m-45degree.jpg", 3.0, 45.0 }, +}; - // 执行求解 - EXPECT_NO_THROW(solution.solve()); +// --- 资源读取辅助函数 --- +// --- 核心辅助函数：相机/装甲板参数 --- +// 辅助函数：创建测试用的相机内参 +PnpSolution::Input create_test_input(double fx = 1.722231837421459e+03, + double fy = 1.724876404292754e+03, double cx = 7.013056440882832e+02, + double cy = 5.645821718351237e+02, double k1 = -0.064232403853946, + double k2 = -0.087667493884102, double k3 = 0.792381808294582) { - // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; + auto distort_coeff = std::array { k1, k2, 0, 0, k3 }; + PnpSolution::Input input {}; + input.camera_matrix = { { + { fx, 0.0, cx }, + { 0.0, fy, cy }, + { 0.0, 0.0, 1.0 }, + } }; + input.distort_coeff = distort_coeff; - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; + return input; } -TEST_F(PnpSolverTest, DifferentDistances) { - constexpr double FOCAL_LENGTH = 800.0; - constexpr double ARMOR_WIDTH = 0.135; - - const auto test_distances = std::vector { 1.0, 2.0, 3.0, 5.0 }; - - for (const auto expected_distance : test_distances) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(FOCAL_LENGTH); - solution.input.armor_shape = create_small_armor_shape(); - - // 根据距离计算图像中的大小 - const auto pixel_size = (FOCAL_LENGTH * ARMOR_WIDTH) / expected_distance; - const auto center = Vector2d { 320.0, 240.0 }; - - const auto eigen_detection = std::array { - center + Vector2d { pixel_size / 2, -pixel_size * 0.2 }, - center + Vector2d { -pixel_size / 2, -pixel_size * 0.2 }, - center + Vector2d { -pixel_size / 2, pixel_size * 0.2 }, - center + Vector2d { pixel_size / 2, pixel_size * 0.2 }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - const auto distance_error = std::abs(solution.result.translation.z - expected_distance); - - EXPECT_LT(distance_error, expected_distance * 0.5) // - << "Distance error too large for expected distance " << expected_distance; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; +// 辅助函数：使用 OpenCV 坐标系定义的装甲板 3D 点 +// [Top Left, Top Right, Bottom Right, Bottom Left] +std::array create_small_armor_shape() { return rmcs::kSmallArmorShapeOpenCV; } + +std::array infer_armor_detection_from_file(std::string_view filename) { + using namespace rmcs::identifier; + + constexpr auto config_yaml = R"( + model_location: "assets/yolov5.xml" + infer_device: "AUTO" + use_roi_segment: false + use_corner_correction: false + roi_rows: 640 + roi_cols: 640 + input_rows: 640 + input_cols: 640 + min_confidence: 0.8 + score_threshold: 0.7 + nms_threshold: 0.3 + )"; + + auto net = OpenVinoNet {}; + auto yaml = YAML::Load(config_yaml); + + const auto location = std::filesystem::path { __FILE__ }.parent_path(); + auto model_location = location / "../models/yolov5.xml"; + yaml["model_location"] = model_location.string(); + + auto cfg_result = net.configure(yaml); + if (!cfg_result.has_value()) { + throw std::runtime_error("Failed to configure OpenVinoNet: " + cfg_result.error()); } -} - -TEST_F(PnpSolverTest, EdgeCaseSmallDetection) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点（小检测框，远距离） - const auto eigen_detection = std::array { - Vector2d { 322.0, 238.0 }, - Vector2d { 318.0, 238.0 }, - Vector2d { 318.0, 242.0 }, - Vector2d { 322.0, 242.0 }, - }; - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; -} + const auto full_path = asset_path(filename); + auto cv_mat = cv::imread(full_path.string(), cv::IMREAD_COLOR); + if (cv_mat.empty()) { + throw std::runtime_error("Failed to read image: " + full_path.string()); + } -TEST_F(PnpSolverTest, WithDistortion) { - auto solution = PnpSolution {}; + auto image = rmcs::Image {}; + image.details().mat = cv_mat; - solution.input = create_test_input(800.0, 320.0, 240.0, { 0.1, -0.2, 0.0, 0.0, 0.0 }); - solution.input.armor_shape = create_small_armor_shape(); + auto infer_result = net.sync_infer(image); + if (!infer_result.has_value()) { + throw std::runtime_error("OpenVino inference failed: " + infer_result.error()); + } + const auto& armors = infer_result.value(); + if (armors.empty()) { + throw std::runtime_error("No armor detected from image: " + full_path.string()); + } - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, + const auto& armor = armors.front(); + // [Top Left, Top Right, Bottom Right, Bottom Left] 与 3D 坐标定义一致 + return std::array { + Point2d { armor.tl() }, // 0 + Point2d { armor.tr() }, // 1 + Point2d { armor.br() }, // 2 + Point2d { armor.bl() }, // 3 }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; } -TEST_F(PnpSolverTest, QuaternionValidity) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); +// --- PnP 结果处理辅助函数 --- - // 执行求解 - solution.solve(); +/** + * @brief 将弧度值规范化到 [-PI/2, PI/2] 范围内 (对应于 [-90度, 90度])。 + * @return 规范化后的角度，范围在 [-PI/2, PI/2] 之间 (弧度)。 + */ +double normalize_angle_90(double angle_rad) { + constexpr double PI = M_PI; + constexpr double HALF_PI = PI / 2.0; - // 验证四元数的有效性 - const auto& q = solution.result.orientation; - - EXPECT_TRUE(std::isfinite(q.x)) << "Quaternion x should be finite"; - EXPECT_TRUE(std::isfinite(q.y)) << "Quaternion y should be finite"; - EXPECT_TRUE(std::isfinite(q.z)) << "Quaternion z should be finite"; - EXPECT_TRUE(std::isfinite(q.w)) << "Quaternion w should be finite"; + // 1. 将角度限制在 [-PI, PI] 范围内 + double normalized = std::fmod(angle_rad + PI, 2.0 * PI); + if (normalized < 0) { + normalized += 2.0 * PI; + } + normalized -= PI; - const auto norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + // 2. 将角度从 [-PI, PI] 映射到 [-PI/2, PI/2] (180度对称处理) + if (normalized > HALF_PI) { + normalized = PI - normalized; + } else if (normalized < -HALF_PI) { + normalized = -PI - normalized; + } - EXPECT_NEAR(norm, 1.0, 0.01) << "Quaternion should be normalized"; + return std::clamp(normalized, -HALF_PI, HALF_PI); } -TEST_F(PnpSolverTest, TranslationValidity) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - solution.solve(); - - // 验证平移向量的有效性 - const auto& t = solution.result.translation; - - EXPECT_TRUE(std::isfinite(t.x)) << "Translation x should be finite"; - EXPECT_TRUE(std::isfinite(t.y)) << "Translation y should be finite"; - EXPECT_TRUE(std::isfinite(t.z)) << "Translation z should be finite"; - - EXPECT_GT(t.z, 0.0) << "Translation z should be positive"; +// 四元数转 ZYX 欧拉角 (Yaw, Pitch, Roll)，单位：弧度 +static Eigen::Vector3d quaternion_to_euler_rad(const Orientation& q) { + Quaterniond quat(q.w, q.x, q.y, q.z); // Eigen 构造函数期望 (w,x,y,z) + const auto euler = quat.toRotationMatrix().eulerAngles(2, 1, 0); // yaw(Z), pitch(Y), roll(X) + return { euler[0], euler[1], euler[2] }; } -TEST_F(PnpSolverTest, DifferentFocalLengths) { - const auto focal_lengths = std::vector { 400.0, 800.0, 1200.0, 1600.0 }; - - for (const auto focal : focal_lengths) { - auto solution = PnpSolution {}; +// --- 参数化测试类 --- +class PnpSolverParameterizedTest : public ::testing::TestWithParam { +protected: + PnpSolution solution; + PnpTestCase test_case; + double actual_distance; + double folded_yaw_deg; + double distance_error; + double yaw_error; + const double max_allowed_distance_error_ratio = 0.08; // 8% + const double max_allowed_yaw_error_deg = 15.0; // 15 degrees - solution.input = create_test_input(focal); + void SetUp() override { + test_case = GetParam(); + solution.input = create_test_input(); solution.input.armor_shape = create_small_armor_shape(); - const auto scale = focal / 800.0; - const auto center = Vector2d { 320.0, 240.0 }; - const auto offset = Vector2d { 30.0, 20.0 }; - - const auto eigen_detection = std::array { - center + Vector2d { offset.x() * scale, -offset.y() * scale }, - center + Vector2d { -offset.x() * scale, -offset.y() * scale }, - center + Vector2d { -offset.x() * scale, offset.y() * scale }, - center + Vector2d { offset.x() * scale, offset.y() * scale }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; - } -} - -TEST_F(PnpSolverTest, Consistency) { - auto solution1 = PnpSolution {}; - auto solution2 = PnpSolution {}; + const auto detection = infer_armor_detection_from_file(test_case.filename); + solution.input.armor_detection = detection; - solution1.input = create_test_input(); - solution1.input.armor_shape = create_small_armor_shape(); + std::cerr << std::fixed << std::setprecision(1); + std::cerr << "[2D_POINTS] | FILE: " << test_case.filename << " | TL(" << detection[0].x + << "," << detection[0].y << ")" + << " TR(" << detection[1].x << "," << detection[1].y << ")" + << " BR(" << detection[2].x << "," << detection[2].y << ")" + << " BL(" << detection[3].x << "," << detection[3].y << ")" << std::endl; + std::cerr << std::defaultfloat; // 恢复默认浮点格式 - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, - }; - - solution1.input.armor_detection = create_armor_detection(eigen_detection); - solution2.input = solution1.input; // 相同输入 - - // 执行求解 - solution1.solve(); - solution2.solve(); - - // 验证一致性 - const auto trans_diff = distance(solution1.result.translation, solution2.result.translation); + SCOPED_TRACE(test_case.filename); - EXPECT_LT(trans_diff, 0.001) << "Results should be consistent"; + EXPECT_NO_THROW(solution.solve()) + << "Pnp solve failed for file: " << asset_path(test_case.filename); - const auto quat_diff = - std::sqrt(std::pow(solution1.result.orientation.x - solution2.result.orientation.x, 2) - + std::pow(solution1.result.orientation.y - solution2.result.orientation.y, 2) - + std::pow(solution1.result.orientation.z - solution2.result.orientation.z, 2) - + std::pow(solution1.result.orientation.w - solution2.result.orientation.w, 2)); + // --- 解算结果处理 --- + const auto& result = solution.result; + actual_distance = result.translation.x; + distance_error = std::abs(actual_distance - test_case.expected_distance_m); - EXPECT_TRUE(quat_diff < 0.001 || quat_diff > 1.9) // - << "Quaternions should be consistent"; -} - -TEST_F(PnpSolverTest, Performance) { - auto solution = PnpSolution {}; + const auto euler_rad = quaternion_to_euler_rad(result.orientation); + const double actual_yaw_rad = euler_rad[0]; - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - constexpr int iterations = 1000; - const auto start = std::chrono::high_resolution_clock::now(); + const double folded_yaw_rad = normalize_angle_90(actual_yaw_rad); + folded_yaw_deg = folded_yaw_rad * 180.0 / M_PI; + yaw_error = std::abs(folded_yaw_deg - test_case.expected_angle_deg); + } - for (int i = 0; i < iterations; i++) { - solution.solve(); + // 打印结构化报告 + void PrintStructuredReport() const { + std::cerr << std::fixed << std::setprecision(4); + std::cerr << "[TEST_REPORT] |" << std::left << std::setw(50) << test_case.filename << "|"; + + // 距离信息 + std::cerr << " DISTANCE: " << std::setw(6) << actual_distance << "m (Exp: " << std::setw(4) + << test_case.expected_distance_m << "m) | Error: " << std::setw(6) + << distance_error << "m | Status: "; + if (distance_error < test_case.expected_distance_m * max_allowed_distance_error_ratio) { + std::cerr << "PASS |"; + } else { + std::cerr << "FAIL |"; + } + + // 角度信息 + std::cerr << " YAW: " << std::setw(6) << folded_yaw_deg << "deg (Exp: " << std::setw(4) + << test_case.expected_angle_deg << "deg) | Error: " << std::setw(6) << yaw_error + << "deg | Status: "; + if (yaw_error < max_allowed_yaw_error_deg) { + std::cerr << "PASS |" << std::endl; + } else { + std::cerr << "FAIL |" << std::endl; + } } +}; - const auto end = std::chrono::high_resolution_clock::now(); - const auto duration = std::chrono::duration_cast(end - start); +// 核心测试：距离和角度精度 +TEST_P(PnpSolverParameterizedTest, DistanceAndAngleAccuracy) { + this->PrintStructuredReport(); - const auto avg_time_us = static_cast(duration.count()) / iterations; + // 1. 距离断言 (8% 误差) + const double max_dist_error = + GetParam().expected_distance_m * this->max_allowed_distance_error_ratio; - std::cout << "Average solve time: " << avg_time_us << " microseconds\n"; + EXPECT_LT(this->distance_error, max_dist_error) + << "Distance error (" << this->distance_error << "m) exceeded " << max_dist_error + << "m for " << GetParam().filename; - EXPECT_LT(avg_time_us, 1000.0) << "Solve should be fast enough"; + // 2. 角度断言 (15度误差) + EXPECT_LT(this->yaw_error, this->max_allowed_yaw_error_deg) + << "Yaw error (" << this->yaw_error << "deg) exceeded " << this->max_allowed_yaw_error_deg + << "deg for " << GetParam().filename; } +// 注册参数化测试 +INSTANTIATE_TEST_SUITE_P(AllImageTests, PnpSolverParameterizedTest, + ::testing::ValuesIn(kPnpTestCases), [](const ::testing::TestParamInfo& info) { + std::string name = info.param.filename; + std::replace(name.begin(), name.end(), '.', '_'); + std::replace(name.begin(), name.end(), '-', '_'); + return name; + }); + // 主函数 int main(int argc, char** argv) { + std::cout << "\n--- Starting PnP Accuracy Tests ---\n"; + std::cout << "[TEST_REPORT] | URL (Simplified Name) | DISTANCE: " + "Actual (Exp) | Error | Status | YAW: Actual (Exp) | Error | Status |\n"; + ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); + + std::cout << "--- PnP Accuracy Tests Finished ---\n"; + return result; } diff --git a/test/static_tf.cpp b/test/static_tf.cpp index d95284e1..c2223731 100644 --- a/test/static_tf.cpp +++ b/test/static_tf.cpp @@ -32,6 +32,10 @@ TEST(static_tf, construct) { SentryTf::foreach_df_with_parent( [](auto parent) { std::println("{} -> {}", parent, T::name); }); + using Result = SentryTf::Find<"0.0.0">::Result; + + constexpr auto result = SentryTf::find<"0.0.0">(); + static_assert(SentryTf::name == "0"); static_assert(SentryTf::child_amount > 0); static_assert(SentryTf::total_amount > 0); diff --git a/test/transform_communication.cpp b/test/transform_communication.cpp new file mode 100644 index 00000000..6e8f68cb --- /dev/null +++ b/test/transform_communication.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include + +#include "utility/shared/context.hpp" +#include "utility/shared/interprocess.hpp" +#include + +using Transform = rmcs::util::Transform; + +namespace { + +constexpr auto create_input_transform() -> Transform { + using rmcs::Direction3d; + using rmcs::Orientation; + return { + .posture = Direction3d { 1.0, 2.0, 3.0 }, + .orientation = Orientation { 0.0, 0.0, 0.0, 1.0 }, + }; +} + +// 非期望值，用于验证接收后被正确覆盖 +inline auto create_failed_input_transform() -> Transform { + using rmcs::Direction3d; + using rmcs::Orientation; + return { + .posture = Direction3d { 2.0, 3.0, 3.0 }, + .orientation = Orientation { Eigen::Quaterniond::Identity() }, + }; +} + +inline auto expect_transform_equal(const Transform& lhs, const Transform& rhs) -> void { + EXPECT_DOUBLE_EQ(lhs.posture.x, rhs.posture.x); + EXPECT_DOUBLE_EQ(lhs.posture.y, rhs.posture.y); + EXPECT_DOUBLE_EQ(lhs.posture.z, rhs.posture.z); + EXPECT_DOUBLE_EQ(lhs.orientation.x, rhs.orientation.x); + EXPECT_DOUBLE_EQ(lhs.orientation.y, rhs.orientation.y); + EXPECT_DOUBLE_EQ(lhs.orientation.z, rhs.orientation.z); + EXPECT_DOUBLE_EQ(lhs.orientation.w, rhs.orientation.w); +} + +} // namespace + +// 父子进程通信：确保写入的 Transform 能被完整读取 +TEST(TransformShm, SendRecvSequence) { + using Send = rmcs::shm::Client::Send; + using Recv = rmcs::shm::Client::Recv; + + using namespace std::chrono_literals; + + constexpr auto shm_name = "/rmcs_auto_aim_transform_shm_test"; + constexpr auto test_value = create_input_transform(); + constexpr auto max_attempts = 100; + constexpr auto poll_interval = 10ms; + constexpr auto init_delay = 50ms; + + // 父进程先创建共享内存对象 + auto send = Send {}; + ASSERT_TRUE(send.open(shm_name)); + ASSERT_TRUE(send.opened()); + + auto pid = fork(); + ASSERT_GE(pid, 0); + + if (pid == 0) { + // 子进程：接收数据 + auto recv = Recv {}; + ASSERT_TRUE(recv.open(shm_name)); + ASSERT_TRUE(recv.opened()); + + // 等待接收数据 + auto received_value = create_failed_input_transform(); + auto received = false; + + // 尝试接收数据，最多等待一段时间 + for (auto i = 0; i < max_attempts; ++i) { + if (recv.is_updated()) { + recv.with_read([&](const auto& data) { received_value = data; }); + received = true; + break; + } + std::this_thread::sleep_for(poll_interval); + } + + ASSERT_TRUE(received && "failed to receive"); + + expect_transform_equal(received_value, test_value); + + std::exit(0); + } else { + // 父进程：发送数据 + // 等待一下子进程准备好 + std::this_thread::sleep_for(init_delay); + + // 发送数据 + send.with_write([&](auto& data) { data = test_value; }); + + // 等待子进程完成 + auto status = int { 0 }; + waitpid(pid, &status, 0); + ASSERT_TRUE(WIFEXITED(status)); + ASSERT_EQ(WEXITSTATUS(status), 0); + } +} + +// 单进程读写：验证版本控制与快照读取的正确性，模拟 component 中动态变换的发布/消费 +TEST(TransformShm, SnapshotAndUpdate) { + using Client = rmcs::shm::Client; + Client::Send send; + Client::Recv recv; + + constexpr auto shm_name = "/rmcs_auto_aim_snapshot_test"; + + ASSERT_TRUE(send.open(shm_name)); + ASSERT_TRUE(recv.open(shm_name)); + + const auto first = create_input_transform(); + const auto second = create_failed_input_transform(); + + // 初次写入，接收端应检测到更新 + send.send(first); + EXPECT_TRUE(recv.is_updated()); + + Transform snapshot {}; + recv.with_read([&](const auto& data) { snapshot = data; }); + expect_transform_equal(snapshot, first); + EXPECT_FALSE(recv.is_updated()); // 读取后版本同步 + + // 再写入一次不同数据，接收端应再次检测到更新 + send.with_write([&](auto& data) { data = second; }); + EXPECT_TRUE(recv.is_updated()); + + recv.recv(snapshot); // 使用 recv 接口读取一次 + expect_transform_equal(snapshot, second); + EXPECT_FALSE(recv.is_updated()); +} diff --git a/tool/CMakeLists.txt b/tool/CMakeLists.txt index 37e5875a..e54354a7 100644 --- a/tool/CMakeLists.txt +++ b/tool/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(rclcpp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(yaml-cpp REQUIRED) find_package(OpenCV 4 REQUIRED) include_directories( diff --git a/tool/visualization.cpp b/tool/visualization.cpp index df746d9c..2b0a7bc0 100644 --- a/tool/visualization.cpp +++ b/tool/visualization.cpp @@ -35,15 +35,15 @@ auto main() -> int { .rclcpp = visual, .device = DeviceId::SENTRY, .camp = CampColor::BLUE, - .id = "", + .id = 0, + .name = "visual_test_armor", .tf = "camera_link", }; - auto& name = config.id; - auto index = char { 'a' }; - std::ranges::for_each(armors, [&](auto& armor) { - name = std::string { "sentry/" } + index++; - armor = std::make_unique(config); - }); + + for (auto i = 0; i < (int)armors.size(); ++i) { + config.id = i; + armors[i] = std::make_unique(config); + } } auto posture = std::make_unique( //