diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 00000000..54bb8280 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,84 @@ +## Agent 协作开发规范(Auto Aim) + +本文件用于约束 Agent 在本仓库中的行为,目标是: + +- 保持算法开发由人类主导 +- 降低无边界代码生成带来的维护风险 +- 让 Agent 在可控范围内稳定提供辅助价值 + +## 1. 指令优先级与冲突处理 + +当多条指令冲突时,按以下优先级执行(高到低): + +1. System 指令 +2. Developer 指令 +3. 本文档(AGENTS.md) +4. 用户普通实现请求 + +如发生冲突,必须遵循更高优先级指令,并在回复中简要说明原因。 + +## 2. 核心原则:算法开发由人类主导 + +Agent 不应主导大规模算法功能实现。面对“完整实现大需求”时,先进行风险判断。 + +### 2.1 高风险判定(命中任意两条即视为高风险) + +- 缺少充分且可执行的实现计划(Plan) +- 缺少开发者提供的明确框架/边界/修改目标 +- 缺少验收标准、测试标准或回归范围 + +### 2.2 高风险场景下的行为 + +- 不直接生成大规模 Feature 代码 +- 输出结构化建议:风险点、拆解方案、最小可验证下一步 +- 将主导权交还开发者,等待进一步明确指令 + +## 3. 允许与禁止的工作边界 + +### 3.1 允许(默认可执行) + +- 小范围修复(bug fix) +- 已有方案下的局部实现 +- 测试补充、文档整理、日志与可观测性改进 +- 不改变核心算法路径的重构 + +### 3.2 禁止(默认不执行) + +- 在缺少明确方案时主导完整算法功能开发 +- 未经约束地跨模块大改 +- 以“兜底默认值”掩盖配置或调用错误 + +## 4. 慎重提取辅助函数 + +提取公共辅助函数前必须评估: + +- 复用范围是否足够大且稳定 +- 是否能以内联或局部函数解决 +- 命名是否会增加全局命名空间噪声 + +若仅为单点使用或短期逻辑,优先保持局部实现。 + +## 5. 关于非法条件检查与失败策略 + +默认策略:尽早失败(fail fast),避免 silent fallback。 + +- 构造/初始化阶段应一次性完成可验证检查 +- 配置或参数非法时应明确报错,不用默认值掩盖 +- 对已由类型系统约束的函数参数,不重复做同类防御性检查 +- 仅在越界、索引失效、外部系统不稳定等场景增加必要检查 + +目标是在错误发生时暴露真实原因,降低后期调试成本。 + +## 6. 标准响应模板(高风险请求) + +当判定为高风险请求时,按以下结构回复: + +1. 风险判断:说明命中的高风险条件 +2. 不直接实现原因:说明可维护性/可验证性风险 +3. 推荐最小下一步:给出 1 个可执行且可验证的下一步 + +示例: + +- 风险判断:当前需求缺少明确实现边界和验收标准 +- 不直接实现原因:直接大范围生成会提高回归风险,难以定位问题 +- 推荐最小下一步:先确定模块边界与接口草案,我基于该草案实现第一阶段并附带测试 diff --git a/config/config.yaml b/config/config.yaml index c3b67eeb..a54280c3 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -41,7 +41,7 @@ identifier: - "shenzhen-0526.onnx" - "shenzhen-0708.onnx" model_location: "tongji-yolov5.xml" - infer_device: "GPU" + infer_device: "AUTO" use_roi_segment: false roi_rows: 640 roi_cols: 640 @@ -59,8 +59,20 @@ tracker: tracking_confirm_frames: 2 pose_estimator: - 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] + 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" @@ -82,10 +94,10 @@ fire_control: yaw_offset: 0.0 # degree pitch_offset: 0.0 # degree - coming_angle: 70.0 # degree - leaving_angle: 20.0 # degree - outpost_coming_angle: 70.0 # degree - outpost_leaving_angle: 30.0 # degree + coming_angle: 70.0 # degree + leaving_angle: 20.0 # degree + outpost_coming_angle: 70.0 # degree + outpost_leaving_angle: 30.0 # degree angular_velocity_threshold: 120 # degree/s first_tolerance: 3 # 近距离射击容差,degree diff --git a/src/component.cpp b/src/component.cpp index 3850cb5e..a1574386 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -8,6 +8,7 @@ #include #include #include + #include #include @@ -31,7 +32,7 @@ class AutoAimComponent final : public rmcs_executor::Component { using namespace std::chrono_literals; framerate.set_interval(2s); - visual::Transform::Config config { + const auto config = visual::Transform::Config { .rclcpp = rclcpp, // 当前组件持有的 RclcppNode .topic = "odom_to_camera_transform", // 发布的 topic 名 .parent_frame = "odom_imu_link", // 父坐标系 @@ -54,7 +55,7 @@ class AutoAimComponent final : public rmcs_executor::Component { } private: - static constexpr auto auto_aim_state_timeout { std::chrono::milliseconds { 100 } }; + static constexpr auto kAutoAimTimeout = std::chrono::milliseconds { 100 }; InputInterface rmcs_tf; @@ -64,7 +65,7 @@ class AutoAimComponent final : public rmcs_executor::Component { RclcppNode rclcpp; std::unique_ptr visual_odom_to_camera; - Feishu feishu; + Feishu feishu; ControlState control_state; AutoAimState auto_aim_state; bool auto_aim_state_received_ { false }; @@ -76,20 +77,37 @@ class AutoAimComponent final : public rmcs_executor::Component { FramerateCounter framerate; ActionThrottler action_throttler { std::chrono::seconds(1), 233 }; - auto has_fresh_auto_aim_state() const -> bool { - return auto_aim_state_received_ - && Clock::now() - auto_aim_state.timestamp <= auto_aim_state_timeout; + /// FIXME: + /// 很多细碎的辅助函数和逻辑 + /// 显然是不需要的,记得重构掉 + static auto make_invalid_auto_aim_state() -> AutoAimState { + return AutoAimState::kInvalid(); } - static auto make_invalid_auto_aim_state() -> AutoAimState { - auto state = AutoAimState {}; - state.reset(); - return state; + static auto compute_target_direction(const AutoAimState& state) -> Eigen::Vector3d { + if (!state.gimbal_takeover || !std::isfinite(state.yaw) || !std::isfinite(state.pitch)) { + return Eigen::Vector3d::Zero(); + } + + const auto& [yaw, pitch] = std::tie(state.yaw, state.pitch); + + return { + std::cos(pitch) * std::cos(yaw), + std::cos(pitch) * std::sin(yaw), + std::sin(pitch), + }; + } + + auto has_fresh_auto_aim_state() const -> bool { + return auto_aim_state_received_ + && Clock::now() - auto_aim_state.timestamp <= kAutoAimTimeout; } auto resolve_auto_aim_state() -> AutoAimState { - if (feishu.updated()) { - auto_aim_state = feishu.fetch(); + if (feishu.heartbeat()) { + if (auto latest = feishu.latest()) { + auto_aim_state = *latest; + } auto_aim_state_received_ = true; } @@ -106,27 +124,11 @@ class AutoAimComponent final : public rmcs_executor::Component { *target_direction = compute_target_direction(state); } - static auto compute_target_direction(const AutoAimState& state) -> Eigen::Vector3d { - if (!state.has_control_direction()) { - return Eigen::Vector3d::Zero(); - } - - const auto& [yaw, pitch] = std::tie(state.yaw, state.pitch); - - // clang-format off - return Eigen::Vector3d { - std::cos(pitch) * std::cos(yaw), - std::cos(pitch) * std::sin(yaw), - std::sin(pitch) - }; - // clang-format on - } - auto forward_auto_aim_outputs() -> void { publish_auto_aim_outputs(resolve_auto_aim_state()); } auto handle_tf_not_ready() -> void { action_throttler.dispatch("tf_not_ready", [&] { rclcpp.warn("rmcs_tf is not ready"); }); - control_state.reset(); + control_state = ControlState::kInvalid(); publish_auto_aim_outputs(make_invalid_auto_aim_state()); } @@ -134,13 +136,8 @@ class AutoAimComponent final : public rmcs_executor::Component { update_gimbal_direction(); update_control_state(); - auto success = feishu.commit(control_state); - if (!success) { - action_throttler.dispatch("commit_control_state_failed", - [&] { rclcpp.info("commit control state failed!"); }); - } else { - action_throttler.reset("commit_control_state_failed"); - } + feishu.with_write([&](auto& data) { data = control_state; }); + action_throttler.reset("commit_control_state_failed"); } auto update_control_state() -> void { diff --git a/src/kernel/common.hpp b/src/kernel/common.hpp index 4429fa91..52d5a396 100644 --- a/src/kernel/common.hpp +++ b/src/kernel/common.hpp @@ -12,14 +12,12 @@ using result_type = std::expected; using handle_type = std::coroutine_handle::promise_type>; namespace details { - using Expansion = rmcs::util::Serializable; - template concept has_config_trait = requires { typename T::Config; }; template concept serialable_config_trait = - requires { requires std::derived_from; }; + requires { requires std::derived_from; }; template concept can_initialize_trait = requires(T& kernel) { diff --git a/src/kernel/feishu.hpp b/src/kernel/feishu.hpp index de74f918..a6863f1d 100644 --- a/src/kernel/feishu.hpp +++ b/src/kernel/feishu.hpp @@ -1,60 +1,122 @@ #pragma once - -#include "utility/shared/context.hpp" +#include "utility/clock.hpp" #include "utility/shared/interprocess.hpp" +#include +#include +#include +#include namespace rmcs::kernel { -template -constexpr const char* shm_name = nullptr; +template +concept timestamp_trait = requires(const T& data) { + { data.timestamp } -> std::convertible_to; +}; -template <> -constexpr auto shm_name = "/shm_autoaim_state"; +template +concept context_trait = requires { + { T::kLabel } -> std::convertible_to; + { T::kLength } -> std::convertible_to; +}; -template <> -constexpr auto shm_name = "/shm_control_state"; +template +class Feishu { +private: + using Timestamp = TimePoint; + using Duration = Timestamp::duration; -enum class RuntimeRole { AutoAim, Control }; + using SendClient = shm::Client::Send; + SendClient send_client { }; -template -class Feishu { -public: - using AutoAimState = util::AutoAimState; - using ControlState = util::ControlState; + using RecvClient = shm::Client::Recv; + RecvClient recv_client { }; - using SendData = std::conditional_t; - using RecvData = std::conditional_t; + mutable Timestamp latest_timestamp = Timestamp::min(); + std::deque recv_buffer { }; - using SendClient = rmcs::shm::Client::Send; - using RecvClient = rmcs::shm::Client::Recv; + template F> + auto recv(F&& f) const noexcept { + auto with = [this, f = std::forward(f)](const RecvT& data) { + latest_timestamp = data.timestamp; + f(data); + }; + recv_client.with_read(with); + } + auto recv(RecvT& data) const noexcept { + recv([&](const RecvT& source) { data = source; }); + } - auto commit(SendData const& data) noexcept -> bool { - if (!ensure_open(send_client, shm_name)) [[unlikely]] - return false; - send_client.with_write([&](SendData& shared) { shared = data; }); - return true; + auto start() noexcept -> bool { + auto send_opened = send_client.opened() || send_client.open(SendT::kLabel); + auto recv_opened = recv_client.opened() || recv_client.open(RecvT::kLabel); + return send_opened && recv_opened; } - auto fetch() noexcept -> const RecvData& { - // Note:直接读取当前共享内存中的数据;如需检测是否有新数据,请先调用 updated() - if (!ensure_open(recv_client, shm_name)) return recv_buffer; - recv_client.with_read([&](RecvData const& shared) { recv_buffer = shared; }); - return recv_buffer; + auto updated() const noexcept { return recv_client.is_updated(); } + +public: + /// 发送消息 + template F> + auto with_write(F&& f) noexcept { + send_client.with_write(std::forward(f)); + } + auto send(const SendT& data) noexcept { + with_write([&](SendT& buffer) { buffer = data; }); } - auto updated() noexcept -> bool { - return ensure_open(recv_client, shm_name) && recv_client.is_updated(); + /// 双向通道是否建立 + auto online() const noexcept { return send_client.opened() && recv_client.opened(); } + + /// @return bool 是否收到数据 + auto heartbeat() noexcept -> bool { + if (start() && updated()) { + recv([this](const RecvT& data) { + if (recv_buffer.size() >= RecvT::kLength) { + recv_buffer.pop_front(); + } + recv_buffer.push_back(data); + }); + return true; + } + return false; } -private: - SendClient send_client {}; - RecvClient recv_client {}; + /// 获取最新的数据切片 + auto latest() const noexcept -> std::optional { + if (recv_buffer.empty()) { + return std::nullopt; + } + return std::optional { recv_buffer.back() }; + } + + /// 搜索离传入时间戳最近的消息 + auto search(Timestamp target, Duration max = std::chrono::seconds { 5 }) const + -> std::optional { + static_assert(timestamp_trait); + + if (target > latest_timestamp) return std::nullopt; + + auto to_return = std::optional { }; + auto shortest = max; + for (const RecvT& data : recv_buffer | std::views::reverse) { + const auto timestamp = Timestamp { data.timestamp }; + + // 区间范围外的,跳过 + if (timestamp > target + max) continue; + if (timestamp < target - max) break; + + // 更新最近值 + const auto interval = timestamp - target; + if (shortest > std::chrono::abs(interval)) { + shortest = std::chrono::abs(interval); - RecvData recv_buffer {}; + to_return = data; + } - template - auto ensure_open(Client& client, const char* name) noexcept -> bool { - return client.opened() || (name && client.open(name)); + // 间隔为负值时,已经过 target + if (interval < Duration::zero()) break; + } + return to_return; } }; diff --git a/src/kernel/fire_control.hpp b/src/kernel/fire_control.hpp index 4284bcc3..efe88147 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -10,8 +10,6 @@ namespace rmcs::kernel { class FireControl { - using Clock = util::Clock; - RMCS_PIMPL_DEFINITION(FireControl) public: diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index 0978bcc7..b38a7cca 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -23,7 +23,7 @@ class PoseEstimator { auto solve_pnp(std::vector const&) const -> std::optional>; - auto set_odom_to_camera_transform(util::Transform const& transform) -> void; + auto set_odom_to_camera_transform(Transform const& transform) -> void; auto odom_to_camera(std::span armors) const -> std::vector; auto odom_to_camera(Armor3D const& armor) const -> Armor3D; diff --git a/src/kernel/tracker.cpp b/src/kernel/tracker.cpp index 35914b15..16a12a91 100644 --- a/src/kernel/tracker.cpp +++ b/src/kernel/tracker.cpp @@ -37,7 +37,7 @@ struct Tracker::Impl { return std::unexpected { result.error() }; } - return {}; + return { }; } auto set_invincible_armors(DeviceIds devices) -> void { @@ -49,7 +49,7 @@ struct Tracker::Impl { return result; } - auto decide(std::span armors, Clock::time_point t) -> Decider::Output { + auto decide(std::span armors, TimePoint t) -> Decider::Output { auto decider_output = decider.update(armors, t); return decider_output; } @@ -73,6 +73,6 @@ auto Tracker::filter_armors(std::span armors) const -> std::vectorfilter_armors(armors); } -auto Tracker::decide(std::span armors, Clock::time_point t) -> Decider::Output { +auto Tracker::decide(std::span armors, TimePoint t) -> Decider::Output { return pimpl->decide(armors, t); } diff --git a/src/kernel/tracker.hpp b/src/kernel/tracker.hpp index 22afea0f..4b9ac3a1 100644 --- a/src/kernel/tracker.hpp +++ b/src/kernel/tracker.hpp @@ -12,8 +12,6 @@ namespace rmcs::kernel { struct Tracker { - using Clock = util::Clock; - RMCS_PIMPL_DEFINITION(Tracker) public: auto initialize(const YAML::Node& yaml) noexcept -> std::expected; @@ -22,6 +20,6 @@ struct Tracker { auto filter_armors(std::span armors) const -> std::vector; - auto decide(std::span armors, Clock::time_point t) -> tracker::Decider::Output; + auto decide(std::span armors, TimePoint t) -> tracker::Decider::Output; }; } diff --git a/src/module/capturer/local_video.cpp b/src/module/capturer/local_video.cpp index af63632c..de3da003 100644 --- a/src/module/capturer/local_video.cpp +++ b/src/module/capturer/local_video.cpp @@ -12,12 +12,13 @@ using namespace rmcs::cap; struct LocalVideo::Impl { Config config; - using Clock = std::chrono::steady_clock; + using Clock = std::chrono::steady_clock; + using TimePoint = std::chrono::steady_clock::time_point; std::optional capturer; std::chrono::nanoseconds interval_duration { 0 }; - Clock::time_point last_read_time { Clock::now() }; + TimePoint last_read_time { Clock::now() }; auto set_framerate_interval(double hz) noexcept -> void { if (hz > 0) { @@ -54,7 +55,7 @@ struct LocalVideo::Impl { last_read_time = Clock::now(); - return {}; + return { }; } auto connect() -> std::expected { return configure(config); } @@ -84,7 +85,7 @@ struct LocalVideo::Impl { last_read_time = config.allow_skipping ? Clock::now() : next_read_time_expected; } - auto frame = cv::Mat {}; + auto frame = cv::Mat { }; auto image = std::make_unique(); if (!capturer->read(frame)) { if (config.loop_play) { diff --git a/src/module/debug/framerate.hpp b/src/module/debug/framerate.hpp index 8dacb32a..35c18b56 100644 --- a/src/module/debug/framerate.hpp +++ b/src/module/debug/framerate.hpp @@ -3,13 +3,12 @@ #include #include +#include "utility/clock.hpp" + namespace rmcs { class FramerateCounter { public: - using Clock = std::chrono::steady_clock; - using TimePoint = Clock::time_point; - std::deque frame_times; TimePoint last_reach_interval_timestamp; diff --git a/src/module/identifier/armor_detection.cpp b/src/module/identifier/armor_detection.cpp index aba3fb0d..639a0a16 100644 --- a/src/module/identifier/armor_detection.cpp +++ b/src/module/identifier/armor_detection.cpp @@ -19,7 +19,6 @@ #include using namespace rmcs::identifier; -using do_not_warning = rmcs::Image::Details; struct ArmorDetection::Impl { @@ -88,16 +87,16 @@ struct ArmorDetection::Impl { const auto rows = static_cast(shape.at(1)); const auto cols = static_cast(shape.at(2)); if (cols != result_type::length()) { - return {}; + return { }; } - auto parsed_results = std::vector {}; - auto scores = std::vector {}; - auto boxes = std::vector {}; + auto parsed_results = std::vector { }; + auto scores = std::vector { }; + auto boxes = std::vector { }; const auto* data = tensor.data(); for (std::size_t row = 0; row < rows; row++) { - auto line = result_type {}; + auto line = result_type { }; line.unsafe_from(std::span { data + row * cols, cols }); line.confidence() = util::sigmoid(line.confidence()); @@ -108,10 +107,10 @@ struct ArmorDetection::Impl { } } - auto kept_points = std::vector {}; + auto kept_points = std::vector { }; cv::dnn::NMSBoxes(boxes, scores, score_threshold, nms_threshold, kept_points); - auto final_result = Armor2Ds {}; + auto final_result = Armor2Ds { }; final_result.reserve(kept_points.size()); for (auto idx : kept_points) { @@ -162,7 +161,7 @@ struct ArmorDetection::Impl { } else { return std::unexpected { "Unsupported model type: " + model_name }; } - return {}; + return { }; } catch (const std::runtime_error& e) { return std::unexpected { std::string { "Failed to load model | " } + e.what() }; @@ -174,7 +173,7 @@ struct ArmorDetection::Impl { template auto compile_model_with() -> void { - auto model = model_type {}; + auto model = model_type { }; if (!config.infer_device.empty()) { model.infer_device = config.infer_device; } @@ -257,7 +256,7 @@ struct ArmorDetection::Impl { template static auto cast_to_armor_result(raw_type raw) noexcept -> Armor2D { - auto armor = Armor2D {}; + auto armor = Armor2D { }; armor.genre = raw.armor_genre(); armor.color = raw.armor_color(); diff --git a/src/module/predictor/backend/robot_state_backend.cpp b/src/module/predictor/backend/robot_state_backend.cpp index d66b80a4..cfdbe876 100644 --- a/src/module/predictor/backend/robot_state_backend.cpp +++ b/src/module/predictor/backend/robot_state_backend.cpp @@ -11,14 +11,14 @@ namespace rmcs::predictor { template class RobotStateBackendAdapter final : public IRobotStateBackend { public: - explicit RobotStateBackendAdapter(Clock::time_point stamp) noexcept + explicit RobotStateBackendAdapter(TimePoint stamp) noexcept : state { stamp } { } - auto initialize(Armor3D const& armor, Clock::time_point t) -> void override { + auto initialize(Armor3D const& armor, TimePoint t) -> void override { state.initialize(armor, t); } - auto predict(Clock::time_point t) -> void override { state.predict(t); } + auto predict(TimePoint t) -> void override { state.predict(t); } auto update(std::span armors) -> bool override { return state.update(armors); } @@ -32,8 +32,8 @@ class RobotStateBackendAdapter final : public IRobotStateBackend { State state; }; -[[nodiscard]] auto make_robot_state_backend(RobotStateBackendKind kind, - IRobotStateBackend::Clock::time_point stamp) -> std::unique_ptr { +[[nodiscard]] auto make_robot_state_backend(RobotStateBackendKind kind, TimePoint stamp) + -> std::unique_ptr { switch (kind) { case RobotStateBackendKind::Outpost: return std::make_unique>(stamp); diff --git a/src/module/predictor/backend/robot_state_backend.hpp b/src/module/predictor/backend/robot_state_backend.hpp index 956bebe9..6b6a3338 100644 --- a/src/module/predictor/backend/robot_state_backend.hpp +++ b/src/module/predictor/backend/robot_state_backend.hpp @@ -15,12 +15,10 @@ enum class RobotStateBackendKind : std::uint8_t { class IRobotStateBackend { public: - using Clock = util::Clock; - virtual ~IRobotStateBackend() noexcept = default; - virtual auto initialize(Armor3D const& armor, Clock::time_point t) -> void = 0; - virtual auto predict(Clock::time_point t) -> void = 0; + virtual auto initialize(Armor3D const& armor, TimePoint t) -> void = 0; + virtual auto predict(TimePoint t) -> void = 0; virtual auto update(std::span armors) -> bool = 0; @@ -39,7 +37,7 @@ class IRobotStateBackend { } } -[[nodiscard]] auto make_robot_state_backend(RobotStateBackendKind kind, - IRobotStateBackend::Clock::time_point stamp) -> std::unique_ptr; +[[nodiscard]] auto make_robot_state_backend(RobotStateBackendKind kind, TimePoint stamp) + -> std::unique_ptr; } // namespace rmcs::predictor diff --git a/src/module/predictor/backend/snapshot_backend.hpp b/src/module/predictor/backend/snapshot_backend.hpp index 9f909c8c..fac9b451 100644 --- a/src/module/predictor/backend/snapshot_backend.hpp +++ b/src/module/predictor/backend/snapshot_backend.hpp @@ -10,10 +10,9 @@ struct ISnapshotBackend { DeviceId device; CampColor color; int armor_num; - Snapshot::Clock::time_point stamp; + TimePoint stamp; - ISnapshotBackend(DeviceId device, CampColor color, int armor_num, - Snapshot::Clock::time_point stamp) noexcept + ISnapshotBackend(DeviceId device, CampColor color, int armor_num, TimePoint stamp) noexcept : device { device } , color { color } , armor_num { armor_num } @@ -21,12 +20,10 @@ struct ISnapshotBackend { virtual ~ISnapshotBackend() noexcept = default; - [[nodiscard]] virtual auto kinematics_at(Snapshot::Clock::time_point t) const - -> Snapshot::Kinematics = 0; - [[nodiscard]] virtual auto predicted_armors(Snapshot::Clock::time_point t) const - -> std::vector = 0; + [[nodiscard]] virtual auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics = 0; + [[nodiscard]] virtual auto predicted_armors(TimePoint t) const -> std::vector = 0; - auto time_stamp() const -> Snapshot::Clock::time_point { return stamp; } + auto time_stamp() const -> TimePoint { return stamp; } }; } // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 2af4aa5f..19b634ed 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -43,7 +43,7 @@ auto make_observation(rmcs::Armor3D const& armor) -> OutpostObservation { auto const ypr = rmcs::util::eulers(orientation); auto const ypd = rmcs::util::xyz2ypd(xyz); - auto z = OutpostEKF::ZVec {}; + auto z = OutpostEKF::ZVec { }; z << ypd[0], ypd[1], ypd[2], ypr[0]; return { z, OutpostEKFParameters::R(xyz, ypr, ypd), xyz, ypr, ypd }; @@ -86,7 +86,7 @@ struct TrackingConfig { std::chrono::duration reset_interval { 1.5 }; int spin_confirm_switches { 2 }; int min_converged_updates { 6 }; - MatchingConfig matching {}; + MatchingConfig matching { }; }; struct SpinTracker { @@ -95,7 +95,7 @@ struct SpinTracker { int candidate_count { 0 }; bool locked { false }; - auto reset() -> void { *this = {}; } + auto reset() -> void { *this = { }; } auto current_sign() const -> int { if (locked) return locked_sign; @@ -177,9 +177,9 @@ class AssociationEngine { , config_ { config } { } auto decide(OutpostObservation const& observation) const -> AssociationDecision { - if (!has_assigned_slot(layout_, current_armor_id_)) return {}; + if (!has_assigned_slot(layout_, current_armor_id_)) return { }; - auto best_decision = AssociationDecision {}; + auto best_decision = AssociationDecision { }; auto const current_phase = layout_.slots[current_armor_id_].phase_offset; auto const current_height = layout_.slots[current_armor_id_].height_offset; @@ -216,7 +216,7 @@ class AssociationEngine { // 这里没有加yaw约束,一是因为yaw的抖动太大,二是因为大部分图像中 一帧只有一块装甲板 if (azimuth_error > config_.azimuth_gate || z_error > config_.z_gate) { - return {}; + return { }; } auto const H = OutpostEKFParameters::H(x_, phase_offset, height_offset); @@ -225,7 +225,7 @@ class AssociationEngine { auto const S = H * P_ * H.transpose() + observation.R; auto const mahalanobis = rmcs::util::mahalanobis_distance(innovation, S); if (!mahalanobis.has_value() || *mahalanobis > config_.mahalanobis_gate) { - return {}; + return { }; } auto error = *mahalanobis; @@ -296,16 +296,16 @@ class AssociationEngine { } // namespace struct OutpostRobotState::Impl { - explicit Impl(Clock::time_point stamp) noexcept + explicit Impl(TimePoint stamp) noexcept : time_stamp { stamp } { } - auto initialize(Armor3D const& armor, Clock::time_point t) -> void { + auto initialize(Armor3D const& armor, TimePoint t) -> void { color = armor_color2camp_color(armor.color); ekf = EKF { OutpostEKFParameters::x(armor), - OutpostEKFParameters::P_initial_dig().asDiagonal() }; + OutpostEKFParameters::P_initial_dig().asDiagonal() }; time_stamp = t; - layout = OutpostArmorLayout {}; + layout = OutpostArmorLayout { }; layout.slots[0].assigned = true; spin.reset(); @@ -314,7 +314,7 @@ struct OutpostRobotState::Impl { initialized = true; } - auto predict(Clock::time_point t) -> void { + auto predict(TimePoint t) -> void { if (initialized) { auto dt = rmcs::util::delta_time(t, time_stamp); if (dt > config.reset_interval) { @@ -361,10 +361,10 @@ struct OutpostRobotState::Impl { auto distance() const -> double { return std::sqrt(ekf.x[0] * ekf.x[0] + ekf.x[2] * ekf.x[2]); } private: - auto reset_runtime_state(Clock::time_point t) -> void { + auto reset_runtime_state(TimePoint t) -> void { color = CampColor::UNKNOWN; - ekf = EKF {}; - layout = OutpostArmorLayout {}; + ekf = EKF { }; + layout = OutpostArmorLayout { }; time_stamp = t; initialized = false; current_armor_id = kUnknownArmorId; @@ -373,7 +373,7 @@ struct OutpostRobotState::Impl { } auto select_best_match(std::span armors) const -> std::optional { - auto best_match = std::optional {}; + auto best_match = std::optional { }; auto matcher = AssociationEngine { ekf.x, ekf.P(), layout, current_armor_id, spin, config.matching }; @@ -410,30 +410,30 @@ struct OutpostRobotState::Impl { } CampColor color { CampColor::UNKNOWN }; - EKF ekf { EKF {} }; - OutpostArmorLayout layout {}; - Clock::time_point time_stamp; + EKF ekf { EKF { } }; + OutpostArmorLayout layout { }; + TimePoint time_stamp; bool initialized { false }; int current_armor_id { kUnknownArmorId }; - SpinTracker spin {}; + SpinTracker spin { }; int update_count { 0 }; - TrackingConfig config {}; + TrackingConfig config { }; }; OutpostRobotState::OutpostRobotState() noexcept : OutpostRobotState(Clock::now()) { } -OutpostRobotState::OutpostRobotState(Clock::time_point stamp) noexcept +OutpostRobotState::OutpostRobotState(TimePoint stamp) noexcept : pimpl { std::make_unique(stamp) } { } OutpostRobotState::~OutpostRobotState() noexcept = default; -auto OutpostRobotState::initialize(Armor3D const& armor, Clock::time_point t) -> void { +auto OutpostRobotState::initialize(Armor3D const& armor, TimePoint t) -> void { return pimpl->initialize(armor, t); } -auto OutpostRobotState::predict(Clock::time_point t) -> void { return pimpl->predict(t); } +auto OutpostRobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } auto OutpostRobotState::update(std::span armors) -> bool { return pimpl->update(armors); diff --git a/src/module/predictor/outpost/robot_state.hpp b/src/module/predictor/outpost/robot_state.hpp index b3168b1f..3c0f1372 100644 --- a/src/module/predictor/outpost/robot_state.hpp +++ b/src/module/predictor/outpost/robot_state.hpp @@ -12,13 +12,12 @@ namespace rmcs::predictor { class OutpostRobotState { public: - using Clock = util::Clock; - using EKF = OutpostEKFParameters::EKF; + using EKF = OutpostEKFParameters::EKF; - explicit OutpostRobotState(Clock::time_point stamp) noexcept; + explicit OutpostRobotState(TimePoint stamp) noexcept; - auto initialize(Armor3D const& armor, Clock::time_point t) -> void; - auto predict(Clock::time_point t) -> void; + auto initialize(Armor3D const& armor, TimePoint t) -> void; + auto predict(TimePoint t) -> void; auto update(std::span armors) -> bool; diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index 0da020c8..a748ba42 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -3,8 +3,8 @@ #include #include -#include "module/predictor/outpost/ekf_parameter.hpp" #include "module/predictor/backend/snapshot_backend.hpp" +#include "module/predictor/outpost/ekf_parameter.hpp" #include "utility/math/conversion.hpp" #include "utility/time.hpp" @@ -19,7 +19,7 @@ namespace { } auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { - auto armor = Armor3D {}; + auto armor = Armor3D { }; armor.genre = device; armor.color = camp_color2armor_color(color); armor.id = id; @@ -27,26 +27,23 @@ namespace { } struct OutpostSnapshotBackend final : ISnapshotBackend { - explicit OutpostSnapshotBackend( - Snapshot::OutpostEKF::XVec x, CampColor color, int armor_num, - Snapshot::Clock::time_point stamp, int spin_sign, OutpostArmorLayout layout) noexcept + explicit OutpostSnapshotBackend(Snapshot::OutpostEKF::XVec x, CampColor color, + int armor_num, TimePoint stamp, int spin_sign, OutpostArmorLayout layout) noexcept : ISnapshotBackend { DeviceId::OUTPOST, color, armor_num, stamp } , x { std::move(x) } , spin_sign { normalize_spin_sign(spin_sign) } , layout { layout } { } - [[nodiscard]] auto kinematics_at(Snapshot::Clock::time_point t) const - -> Snapshot::Kinematics override { + [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { return kinematics_of(predict_state_at(t)); } - [[nodiscard]] auto predicted_armors(Snapshot::Clock::time_point t) const - -> std::vector override { + [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { auto const predicted_x = predict_state_at(t); auto const max_armors = std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount); - auto armors = std::vector {}; + auto armors = std::vector { }; armors.reserve(max_armors); for (int id = 0; id < max_armors; ++id) { @@ -83,7 +80,7 @@ namespace { return { Eigen::Vector3d { x[0], x[2], center_z }, angular_velocity }; } - auto predict_state_at(Snapshot::Clock::time_point t) const -> Snapshot::OutpostEKF::XVec { + auto predict_state_at(TimePoint t) const -> Snapshot::OutpostEKF::XVec { auto const dt = util::delta_time(t, stamp).count(); return OutpostEKFParameters::f(dt, spin_sign)(x); } @@ -96,8 +93,8 @@ namespace { } // namespace auto detail::make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - Snapshot::Clock::time_point stamp, int outpost_spin_sign, - OutpostArmorLayout outpost_layout) noexcept -> Snapshot { + TimePoint stamp, int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept + -> Snapshot { return detail::make_snapshot(std::make_unique( std::move(ekf_x), color, armor_num, stamp, outpost_spin_sign, outpost_layout)); } diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp index d65d652c..7d4f84e9 100644 --- a/src/module/predictor/outpost/snapshot.hpp +++ b/src/module/predictor/outpost/snapshot.hpp @@ -7,7 +7,6 @@ namespace rmcs::predictor::detail { auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - Snapshot::Clock::time_point stamp, int outpost_spin_sign, - OutpostArmorLayout outpost_layout) noexcept -> Snapshot; + TimePoint stamp, int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; } // namespace rmcs::predictor::detail diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index e23a42a8..868d8439 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -17,10 +17,10 @@ struct RegularRobotState::Impl { bool is_valid; }; - explicit Impl(Clock::time_point stamp) noexcept + explicit Impl(TimePoint stamp) noexcept : time_stamp { stamp } { } - auto initialize(Armor3D const& armor, Clock::time_point t) -> void { + auto initialize(Armor3D const& armor, TimePoint t) -> void { device = armor.genre; color = armor_color2camp_color(armor.color); armor_num = EKFParameters::armor_num(armor.genre); @@ -30,7 +30,7 @@ struct RegularRobotState::Impl { initialized = true; } - auto predict(Clock::time_point t) -> void { + auto predict(TimePoint t) -> void { if (initialized) { auto dt = util::delta_time(t, time_stamp); if (dt > reset_interval) { @@ -134,7 +134,7 @@ struct RegularRobotState::Impl { auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; auto const ypr = util::eulers(orientation); - auto z = EKF::ZVec {}; + auto z = EKF::ZVec { }; z << ypd[0], ypd[1], ypd[2], ypr[0]; ekf.update( @@ -149,7 +149,7 @@ struct RegularRobotState::Impl { } auto calculate_armors(EKF::XVec const& x) const -> std::vector { - auto armors = std::vector {}; + auto armors = std::vector { }; armors.reserve(armor_num); for (int i = 0; i < armor_num; ++i) { auto angle = EKFParameters::armor_yaw(device, x, i); @@ -163,8 +163,8 @@ struct RegularRobotState::Impl { CampColor color { CampColor::UNKNOWN }; int armor_num { 0 }; - EKF ekf { EKF {} }; - Clock::time_point time_stamp; + EKF ekf { EKF { } }; + TimePoint time_stamp; bool initialized { false }; int update_count { 0 }; @@ -176,18 +176,18 @@ struct RegularRobotState::Impl { RegularRobotState::RegularRobotState() noexcept : RegularRobotState(Clock::now()) { } -RegularRobotState::RegularRobotState(Clock::time_point stamp) noexcept +RegularRobotState::RegularRobotState(TimePoint stamp) noexcept : pimpl { std::make_unique(stamp) } { } RegularRobotState::~RegularRobotState() noexcept = default; RegularRobotState::RegularRobotState(RegularRobotState&&) noexcept = default; auto RegularRobotState::operator=(RegularRobotState&&) noexcept -> RegularRobotState& = default; -auto RegularRobotState::initialize(Armor3D const& armor, Clock::time_point t) -> void { +auto RegularRobotState::initialize(Armor3D const& armor, TimePoint t) -> void { return pimpl->initialize(armor, t); } -auto RegularRobotState::predict(Clock::time_point t) -> void { return pimpl->predict(t); } +auto RegularRobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } auto RegularRobotState::update(std::span armors) -> bool { return pimpl->update(armors); diff --git a/src/module/predictor/regular/robot_state.hpp b/src/module/predictor/regular/robot_state.hpp index c8ee4078..8e48defe 100644 --- a/src/module/predictor/regular/robot_state.hpp +++ b/src/module/predictor/regular/robot_state.hpp @@ -11,13 +11,12 @@ namespace rmcs::predictor { class RegularRobotState { public: - using Clock = util::Clock; - using EKF = EKFParameters::EKF; + using EKF = EKFParameters::EKF; - explicit RegularRobotState(Clock::time_point stamp) noexcept; + explicit RegularRobotState(TimePoint stamp) noexcept; - auto initialize(Armor3D const& armor, Clock::time_point t) -> void; - auto predict(Clock::time_point t) -> void; + auto initialize(Armor3D const& armor, TimePoint t) -> void; + auto predict(TimePoint t) -> void; auto update(std::span armors) -> bool; diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp index 9a61d916..77207669 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -2,8 +2,8 @@ #include -#include "module/predictor/regular/ekf_parameter.hpp" #include "module/predictor/backend/snapshot_backend.hpp" +#include "module/predictor/regular/ekf_parameter.hpp" #include "utility/math/conversion.hpp" #include "utility/time.hpp" @@ -12,7 +12,7 @@ namespace rmcs::predictor { namespace { auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { - auto armor = Armor3D {}; + auto armor = Armor3D { }; armor.genre = device; armor.color = camp_color2armor_color(color); armor.id = id; @@ -20,22 +20,19 @@ namespace { } struct RegularSnapshotBackend final : ISnapshotBackend { - explicit RegularSnapshotBackend( - Snapshot::NormalEKF::XVec x, DeviceId device, CampColor color, - int armor_num, Snapshot::Clock::time_point stamp) noexcept + explicit RegularSnapshotBackend(Snapshot::NormalEKF::XVec x, DeviceId device, + CampColor color, int armor_num, TimePoint stamp) noexcept : ISnapshotBackend { device, color, armor_num, stamp } , x { std::move(x) } { } - [[nodiscard]] auto kinematics_at(Snapshot::Clock::time_point t) const - -> Snapshot::Kinematics override { + [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { return kinematics_of(predict_state_at(t)); } - [[nodiscard]] auto predicted_armors(Snapshot::Clock::time_point t) const - -> std::vector override { + [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { auto const predicted_x = predict_state_at(t); - auto armors = std::vector {}; + auto armors = std::vector { }; armors.reserve(armor_num); for (int id = 0; id < armor_num; ++id) { @@ -57,7 +54,7 @@ namespace { return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; } - auto predict_state_at(Snapshot::Clock::time_point t) const -> Snapshot::NormalEKF::XVec { + auto predict_state_at(TimePoint t) const -> Snapshot::NormalEKF::XVec { auto const dt = util::delta_time(t, stamp).count(); return EKFParameters::f(dt)(x); } @@ -68,10 +65,9 @@ namespace { } // namespace auto detail::make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, - CampColor color, int armor_num, Snapshot::Clock::time_point stamp) noexcept -> Snapshot { - return detail::make_snapshot( - std::make_unique( - std::move(ekf_x), device, color, armor_num, stamp)); + CampColor color, int armor_num, TimePoint stamp) noexcept -> Snapshot { + return detail::make_snapshot(std::make_unique( + std::move(ekf_x), device, color, armor_num, stamp)); } } // namespace rmcs::predictor diff --git a/src/module/predictor/regular/snapshot.hpp b/src/module/predictor/regular/snapshot.hpp index b934e4fb..da77793d 100644 --- a/src/module/predictor/regular/snapshot.hpp +++ b/src/module/predictor/regular/snapshot.hpp @@ -7,6 +7,6 @@ namespace rmcs::predictor::detail { auto make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, CampColor color, - int armor_num, Snapshot::Clock::time_point stamp) noexcept -> Snapshot; + int armor_num, TimePoint stamp) noexcept -> Snapshot; } // namespace rmcs::predictor::detail diff --git a/src/module/predictor/robot_state.cpp b/src/module/predictor/robot_state.cpp index fb0088a5..3a38fc7f 100644 --- a/src/module/predictor/robot_state.cpp +++ b/src/module/predictor/robot_state.cpp @@ -4,25 +4,17 @@ using namespace rmcs::predictor; -namespace { - -auto empty_snapshot(RobotState::Clock::time_point stamp) -> Snapshot { - return Snapshot::empty(stamp); -} - -} // namespace - struct RobotState::Impl { - std::unique_ptr backend {}; - Clock::time_point pending_time_stamp { Clock::now() }; + std::unique_ptr backend { }; + TimePoint pending_time_stamp { Clock::now() }; - [[nodiscard]] static auto make_backend(DeviceId device, Clock::time_point stamp) + [[nodiscard]] static auto make_backend(DeviceId device, TimePoint stamp) -> std::unique_ptr { auto const kind = classify_robot_state_backend(device); return make_robot_state_backend(kind, stamp); } - auto reset_backend(Armor3D const& armor, Clock::time_point stamp) -> void { + auto reset_backend(Armor3D const& armor, TimePoint stamp) -> void { backend = make_backend(armor.genre, stamp); pending_time_stamp = stamp; } @@ -32,12 +24,12 @@ struct RobotState::Impl { backend = make_backend(armor.genre, pending_time_stamp); } - auto initialize(Armor3D const& armor, Clock::time_point t) -> void { + auto initialize(Armor3D const& armor, TimePoint t) -> void { reset_backend(armor, t); backend->initialize(armor, t); } - auto predict(Clock::time_point t) -> void { + auto predict(TimePoint t) -> void { pending_time_stamp = t; if (backend) backend->predict(t); } @@ -51,7 +43,7 @@ struct RobotState::Impl { auto is_converged() const -> bool { return backend ? backend->is_converged() : false; } auto get_snapshot() const -> Snapshot { - return backend ? backend->get_snapshot() : empty_snapshot(pending_time_stamp); + return backend ? backend->get_snapshot() : Snapshot::empty(pending_time_stamp); } auto distance() const -> double { return backend ? backend->distance() : 0.0; } @@ -61,11 +53,11 @@ RobotState::RobotState() noexcept : pimpl { std::make_unique() } { } RobotState::~RobotState() noexcept = default; -auto RobotState::initialize(rmcs::Armor3D const& armor, Clock::time_point t) -> void { +auto RobotState::initialize(rmcs::Armor3D const& armor, TimePoint t) -> void { return pimpl->initialize(armor, t); } -auto RobotState::predict(Clock::time_point t) -> void { return pimpl->predict(t); } +auto RobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } auto RobotState::update(std::span armors) -> bool { return pimpl->update(armors); } diff --git a/src/module/predictor/robot_state.hpp b/src/module/predictor/robot_state.hpp index 46ac98f3..dd727a10 100644 --- a/src/module/predictor/robot_state.hpp +++ b/src/module/predictor/robot_state.hpp @@ -9,13 +9,11 @@ namespace rmcs::predictor { struct RobotState { - using Clock = util::Clock; - RMCS_PIMPL_DEFINITION(RobotState) public: - auto initialize(Armor3D const&, Clock::time_point) -> void; + auto initialize(Armor3D const&, TimePoint) -> void; - auto predict(Clock::time_point t) -> void; + auto predict(TimePoint t) -> void; auto update(std::span armors) -> bool; diff --git a/src/module/predictor/snapshot.cpp b/src/module/predictor/snapshot.cpp index 56c522ff..00569466 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -9,20 +9,18 @@ namespace rmcs::predictor { namespace { -struct EmptySnapshotBackend final : ISnapshotBackend { - explicit EmptySnapshotBackend(Snapshot::Clock::time_point stamp) noexcept - : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } + struct EmptySnapshotBackend final : ISnapshotBackend { + explicit EmptySnapshotBackend(TimePoint stamp) noexcept + : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } - [[nodiscard]] auto kinematics_at(Snapshot::Clock::time_point) const - -> Snapshot::Kinematics override { - return { Eigen::Vector3d::Zero(), 0.0 }; - } + [[nodiscard]] auto kinematics_at(TimePoint) const -> Snapshot::Kinematics override { + return { Eigen::Vector3d::Zero(), 0.0 }; + } - [[nodiscard]] auto predicted_armors(Snapshot::Clock::time_point) const - -> std::vector override { - return {}; - } -}; + [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { + return { }; + } + }; } // namespace @@ -30,27 +28,25 @@ auto detail::make_snapshot(std::unique_ptr backend) noexcept - return Snapshot { std::move(backend) }; } -auto Snapshot::empty(Clock::time_point stamp) noexcept -> Snapshot { +auto Snapshot::empty(TimePoint stamp) noexcept -> Snapshot { return detail::make_snapshot(std::make_unique(stamp)); } Snapshot::Snapshot(std::unique_ptr backend) noexcept : backend { std::move(backend) } { } -Snapshot::Snapshot(Snapshot&&) noexcept = default; +Snapshot::Snapshot(Snapshot&&) noexcept = default; auto Snapshot::operator=(Snapshot&&) noexcept -> Snapshot& = default; Snapshot::~Snapshot() noexcept = default; -auto Snapshot::time_stamp() const -> Clock::time_point { return backend->time_stamp(); } +auto Snapshot::time_stamp() const -> TimePoint { return backend->time_stamp(); } auto Snapshot::kinematics() const -> Kinematics { return backend->kinematics_at(time_stamp()); } -auto Snapshot::kinematics_at(Clock::time_point t) const -> Kinematics { - return backend->kinematics_at(t); -} +auto Snapshot::kinematics_at(TimePoint t) const -> Kinematics { return backend->kinematics_at(t); } -auto Snapshot::predicted_armors(Clock::time_point t) const -> std::vector { +auto Snapshot::predicted_armors(TimePoint t) const -> std::vector { return backend->predicted_armors(t); } diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index c2db3ee0..620be080 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -15,7 +15,7 @@ class Snapshot; namespace detail { -auto make_snapshot(std::unique_ptr backend) noexcept -> Snapshot; + auto make_snapshot(std::unique_ptr backend) noexcept -> Snapshot; } // namespace detail @@ -23,14 +23,13 @@ class Snapshot { public: using NormalEKF = util::EKF<11, 4>; using OutpostEKF = util::EKF<6, 4>; - using Clock = util::Clock; struct Kinematics { Eigen::Vector3d center_position; double angular_velocity; }; - static auto empty(Clock::time_point stamp) noexcept -> Snapshot; + static auto empty(TimePoint stamp) noexcept -> Snapshot; Snapshot(Snapshot const&) = delete; Snapshot(Snapshot&&) noexcept; @@ -38,11 +37,11 @@ class Snapshot { Snapshot& operator=(Snapshot&&) noexcept; ~Snapshot() noexcept; - auto time_stamp() const -> Clock::time_point; + auto time_stamp() const -> TimePoint; auto kinematics() const -> Kinematics; - auto kinematics_at(Clock::time_point t) const -> Kinematics; + auto kinematics_at(TimePoint t) const -> Kinematics; - auto predicted_armors(Clock::time_point t) const -> std::vector; + auto predicted_armors(TimePoint t) const -> std::vector; private: explicit Snapshot(std::unique_ptr backend) noexcept; diff --git a/src/module/tracker/decider.cpp b/src/module/tracker/decider.cpp index 0f3148f2..d1daca5c 100644 --- a/src/module/tracker/decider.cpp +++ b/src/module/tracker/decider.cpp @@ -25,7 +25,7 @@ struct Decider::Impl { static constexpr double kPrimaryTargetScoreBonus = 2.0; struct TargetMemory { - std::optional last_seen_time {}; + std::optional last_seen_time { }; std::size_t consecutive_missing_frames { 0 }; std::size_t consecutive_stable_frames { 0 }; bool temporary_lost_armed { false }; @@ -62,7 +62,7 @@ struct Decider::Impl { return std::unexpected { "tracker.tracking_confirm_frames must be > 0" }; } - return {}; + return { }; } auto set_priority_mode(PriorityMode const& mode) -> void { priority_mode = mode; } @@ -76,14 +76,14 @@ struct Decider::Impl { } } - auto update(std::span armors, Clock::time_point t) -> Output { + auto update(std::span armors, TimePoint t) -> Output { // 推进所有现有追踪器的时间轴 for (auto& [id, tracker] : trackers) { tracker->predict(t); } - auto observed_ids = std::unordered_set {}; - auto grouped_armors = std::unordered_map> {}; + auto observed_ids = std::unordered_set { }; + auto grouped_armors = std::unordered_map> { }; for (const auto& armor : armors) { grouped_armors[armor.genre].emplace_back(armor); @@ -172,7 +172,7 @@ struct Decider::Impl { if (std::ranges::empty(candidates)) return DeviceId::UNKNOWN; - auto it = std::ranges::max_element(candidates, {}, + auto it = std::ranges::max_element(candidates, { }, [&](const auto& pair) { return calculate_score(pair.first, *pair.second); }); return it->first; @@ -257,7 +257,7 @@ struct Decider::Impl { std::unordered_map> trackers; std::unordered_map target_memories; - Config config {}; + Config config { }; PriorityMode priority_mode; const PriorityMode mode1 = { @@ -297,6 +297,6 @@ auto Decider::set_priority_mode(PriorityMode const& mode) -> void { return pimpl->set_priority_mode(mode); } -auto Decider::update(std::span armors, Clock::time_point t) -> Output { +auto Decider::update(std::span armors, TimePoint t) -> Output { return pimpl->update(armors, t); } diff --git a/src/module/tracker/decider.hpp b/src/module/tracker/decider.hpp index 3b214a1a..23d6f846 100644 --- a/src/module/tracker/decider.hpp +++ b/src/module/tracker/decider.hpp @@ -15,8 +15,6 @@ namespace rmcs::tracker { struct Decider { - using Clock = util::Clock; - RMCS_PIMPL_DEFINITION(Decider) public: @@ -31,6 +29,6 @@ struct Decider { auto set_priority_mode(PriorityMode const& mode) -> void; - auto update(std::span armors, Clock::time_point t) -> Output; + auto update(std::span armors, TimePoint t) -> Output; }; } diff --git a/src/runtime.cpp b/src/runtime.cpp index d2d2f2d2..a455f0cf 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -6,8 +6,8 @@ #include "kernel/tracker.hpp" #include "kernel/visualization.hpp" -#include "module/debug/action_throttler.hpp" #include "utility/image/armor.hpp" +#include "utility/logging_util.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/configuration.hpp" #include "utility/rclcpp/node.hpp" @@ -17,7 +17,6 @@ #include #include #include -#include #include using namespace rmcs; @@ -29,205 +28,166 @@ 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 node = RclcppNode { "AutoAim" }; + node.set_pub_topic_prefix("/rmcs/auto_aim/"); + + auto logging = LoggingUtil { node }; + logging.reset("receive", 5); + logging.reset("detection", 5); + + /// Runtime + auto feishu = kernel::Feishu { }; + auto capturer = kernel::Capturer { }; + auto identifier = kernel::Identifier { }; + auto tracker = kernel::Tracker { }; + auto pose_estimator = kernel::PoseEstimator { }; + auto fire_control = kernel::FireControl { }; + auto visualization = kernel::Visualization { }; + + /// Configure + auto configuration = util::configuration(); + auto use_visualization = configuration["use_visualization"].as(); + auto use_painted_image = configuration["use_painted_image"].as(); + auto without_rmcs = configuration["is_local_runtime"].as(); + + auto handle_result = [&](auto runtime_name, const auto& result) { + if (!result.has_value()) { + node.error("Failed to init '{}'", runtime_name); + node.error(" {}", result.error()); + util::panic(std::format("Failed to initialize {}", runtime_name)); + } + }; + + // CAPTURER + { + auto config = configuration["capturer"]; + auto result = capturer.initialize(config); + handle_result("capturer", result); + } + // IDENTIFIER + { + auto config = configuration["identifier"]; + const auto model_location = std::filesystem::path { util::Parameters::share_location() } + / std::filesystem::path { config["model_location"].as() }; + config["model_location"] = model_location.string(); + + auto result = identifier.initialize(config); + handle_result("identifier", result); + } + // TRACKER + { + auto config = configuration["tracker"]; + auto result = tracker.initialize(config); + handle_result("tracker", result); + } + // POSE ESTIMATOR + { + auto config = configuration["pose_estimator"]; + auto result = pose_estimator.initialize(config); + handle_result("pose_estimator", result); + } + // FIRE CONTROL { - /// Runtime - auto feishu = kernel::Feishu {}; - auto capturer = kernel::Capturer {}; - auto identifier = kernel::Identifier {}; - auto tracker = kernel::Tracker {}; - auto pose_estimator = kernel::PoseEstimator {}; - auto fire_control = kernel::FireControl {}; - auto visualization = kernel::Visualization {}; - - auto action_throttler = util::ActionThrottler { 1s, 233 }; - - /// Configure - auto configuration = util::configuration(); - auto use_visualization = configuration["use_visualization"].as(); - auto use_painted_image = configuration["use_painted_image"].as(); - auto is_local_runtime = configuration["is_local_runtime"].as(); - - auto handle_result = [&](auto runtime_name, const auto& result) { - if (!result.has_value()) { - rclcpp_node.error("Failed to init '{}'", runtime_name); - rclcpp_node.error(" {}", result.error()); - util::panic(std::format("Failed to initialize {}", runtime_name)); + auto config = configuration["fire_control"]; + auto result = fire_control.initialize(config); + handle_result("fire_control", result); + } + // VISUALIZATION + if (use_visualization) { + auto config = configuration["visualization"]; + auto result = visualization.initialize(config, node); + handle_result("visualization", result); + } + + while (util::get_running()) { + node.spin_once(); + + auto updated = feishu.heartbeat(); + + auto image = capturer.fetch_image(); + if (!image) continue; + + [[maybe_unused]] auto _ = std::experimental::scope_exit { [&] { + if (visualization.initialized()) { + visualization.send_image(*image); } - }; + } }; - // CAPTURER - { - auto config = configuration["capturer"]; - auto result = capturer.initialize(config); - handle_result("capturer", result); + auto received = ControlState::kInvalid(); + if (!without_rmcs && updated) { + received = *feishu.latest(); } - // IDENTIFIER - { - auto config = configuration["identifier"]; - const auto model_location = std::filesystem::path { util::Parameters::share_location() } - / std::filesystem::path { config["model_location"].as() }; - config["model_location"] = model_location.string(); + /// 1. Identify Armor + /// + auto armors_2d = Armor2Ds { }; + auto result = identifier.sync_identify(*image); + if (!result.has_value()) { + logging.error("detection", "Armor detection failed"); + } else { + logging.reset("detection", 5); + + tracker.set_invincible_armors(received.invincible_devices); + auto filtered = tracker.filter_armors(*result); + if (use_painted_image) { + for (const auto& armor_2d : filtered) + util::draw(*image, armor_2d); + } - auto result = identifier.initialize(config); - handle_result("identifier", result); - } - // TRACKER - { - auto config = configuration["tracker"]; - auto result = tracker.initialize(config); - handle_result("tracker", result); + armors_2d = std::move(filtered); } - // POSE ESTIMATOR - { - auto config = configuration["pose_estimator"]; - auto result = pose_estimator.initialize(config); - handle_result("pose_estimator", result); + + /// 2. Transform 2d to 3d + /// + auto armors_3d = Armor3Ds { }; + if (!armors_2d.empty()) { + auto solved_armors_3d = pose_estimator.solve_pnp(armors_2d); + if (solved_armors_3d && visualization.initialized()) { + std::ignore = visualization.solved_pnp_armors(*solved_armors_3d); + } + + if (solved_armors_3d) { + pose_estimator.set_odom_to_camera_transform(received.odom_to_camera_transform); + armors_3d = pose_estimator.odom_to_camera(*solved_armors_3d); + } } - // FIRE CONTROL - { - auto config = configuration["fire_control"]; - auto result = fire_control.initialize(config); - handle_result("fire_control", result); + + /// 3. Apply Tracker + /// + auto target = tracker.decide(armors_3d, image->get_timestamp()); + auto target_id = target.target_id; + auto snapshot = std::move(target.snapshot); + + auto command = AutoAimState::kInvalid(); + if (target.allow_takeover) { + command.timestamp = Clock::now(); + command.gimbal_takeover = true; + command.shoot_permitted = false; + command.yaw = received.yaw; + command.pitch = received.pitch; + command.target = target_id; } - // VISUALIZATION - if (use_visualization) { - auto config = configuration["visualization"]; - auto result = visualization.initialize(config, rclcpp_node); - handle_result("visualization", result); + + if (target.allow_takeover && snapshot) { + if (auto result = + fire_control.solve(*snapshot, target.tracking_confirmed, received.yaw)) { + command.shoot_permitted = result->shoot_permitted; + command.yaw = result->yaw; + command.pitch = result->pitch; + } } - // DEBUG - constexpr auto control_state_label { "control_state_not_updated" }; - constexpr auto identifier_failed_label { "identifier_failed" }; - constexpr auto feishu_commit_label { "feishu_commit_failed" }; - { - action_throttler.register_action(control_state_label, 1); - action_throttler.register_action(identifier_failed_label, 1); - action_throttler.register_action(feishu_commit_label, 1); + + if (visualization.initialized() && snapshot) { + visualization.predicted_armors(snapshot->predicted_armors(Clock::now())); } + /// 4. Transmit State /// - /// Steps - /// - const auto fetch_control_state = [&] -> ControlState { - if (is_local_runtime) { - auto state = ControlState {}; - state.reset(); - return state; - } - if (!feishu.updated()) { - action_throttler.dispatch(control_state_label, - [&] { rclcpp_node.warn("Control state 尚未更新,使用上一次缓存值."); }); - } else { - action_throttler.reset(control_state_label); - } - return feishu.fetch(); - }; - - const auto commit_state = [&](const AutoAimState& state) { - if (!feishu.commit(state)) { - action_throttler.dispatch(feishu_commit_label, [&] { - rclcpp_node.warn( - "Commit auto_aim_state failed (target={})", rmcs::to_string(state.target)); - }); - return false; - } + feishu.send(command); - action_throttler.reset(feishu_commit_label); - return true; - }; - - for (;;) { - if (!util::get_running()) [[unlikely]] - break; - - rclcpp_node.spin_once(); - - if (auto image = capturer.fetch_image()) { - auto stream_guard = std::experimental::scope_exit { [&] { - if (visualization.initialized()) { - visualization.send_image(*image); - } - } }; - std::ignore = stream_guard; - - auto control_state = fetch_control_state(); - auto next_state = AutoAimState {}; - next_state.reset(); - - /// 1. Identify Armor - /// - auto armors_2d = Armor2Ds {}; - { - auto result = identifier.sync_identify(*image); - if (!result.has_value()) { - action_throttler.dispatch(identifier_failed_label, - [&] { rclcpp_node.error("Armor detection failed"); }); - } else { - action_throttler.reset(identifier_failed_label); - - tracker.set_invincible_armors(control_state.invincible_devices); - auto filtered = tracker.filter_armors(*result); - if (use_painted_image) { - for (const auto& armor_2d : filtered) - util::draw(*image, armor_2d); - } - - armors_2d = std::move(filtered); - } - } - - /// 2. Transform 2d to 3d - /// - auto armors_3d = Armor3Ds {}; - if (!armors_2d.empty()) { - auto solved_armors_3d = pose_estimator.solve_pnp(armors_2d); - if (solved_armors_3d && visualization.initialized()) { - std::ignore = visualization.solved_pnp_armors(*solved_armors_3d); - } - - if (solved_armors_3d) { - pose_estimator.set_odom_to_camera_transform( - control_state.odom_to_camera_transform); - armors_3d = pose_estimator.odom_to_camera(*solved_armors_3d); - } - } - - /// 3. Apply Tracker - /// - { - auto tracker_output = tracker.decide(armors_3d, image->get_timestamp()); - auto tracked_target = tracker_output.target_id; - auto snapshot = std::move(tracker_output.snapshot); - - if (tracker_output.allow_takeover) { - next_state.set_hold_state( - control_state.yaw, control_state.pitch, tracked_target); - } - - if (tracker_output.allow_takeover && snapshot) { - if (auto control_cmd = fire_control.solve( - *snapshot, tracker_output.tracking_confirmed, control_state.yaw)) { - next_state.set_tracking_state(control_cmd->yaw, control_cmd->pitch, - tracked_target, control_cmd->shoot_permitted); - } - } - - if (visualization.initialized() && snapshot) { - visualization.predicted_armors(snapshot->predicted_armors(Clock::now())); - } - } - - /// 4. Transmit State - /// - commit_state(next_state); - } - } // runtime loop scope - } // runtime objects scope + } // runtime loop scope - rclcpp_node.shutdown(); + node.shutdown(); return 0; } diff --git a/src/utility/clock.hpp b/src/utility/clock.hpp index 547bf205..3b437cbd 100644 --- a/src/utility/clock.hpp +++ b/src/utility/clock.hpp @@ -1,8 +1,11 @@ #pragma once #include -namespace rmcs::util { +namespace rmcs { -using Clock = std::chrono::steady_clock; +using Clock = std::chrono::steady_clock; +using TimePoint = Clock::time_point; +using Duration = TimePoint::duration; +using Timestamp = TimePoint; -} +} // namespace rmcs diff --git a/src/utility/image/image.cpp b/src/utility/image/image.cpp index 7061b7c2..8e816754 100644 --- a/src/utility/image/image.cpp +++ b/src/utility/image/image.cpp @@ -3,18 +3,18 @@ using namespace rmcs; struct Image::Impl { - Clock::time_point timestamp; + TimePoint timestamp; Details details; }; auto Image::details() noexcept -> Details& { return pimpl->details; } auto Image::details() const noexcept -> const Details& { return pimpl->details; } -auto Image::get_timestamp() const noexcept -> Clock::time_point // +auto Image::get_timestamp() const noexcept -> TimePoint // { return pimpl->timestamp; } -auto Image::set_timestamp(Clock::time_point timestamp) noexcept -> void // +auto Image::set_timestamp(TimePoint timestamp) noexcept -> void // { pimpl->timestamp = timestamp; } diff --git a/src/utility/image/image.hpp b/src/utility/image/image.hpp index 29ab9644..40e6a782 100644 --- a/src/utility/image/image.hpp +++ b/src/utility/image/image.hpp @@ -8,14 +8,12 @@ class Image { RMCS_PIMPL_DEFINITION(Image) public: - using Clock = util::Clock; - struct Details; auto details() noexcept -> Details&; auto details() const noexcept -> Details const&; - auto get_timestamp() const noexcept -> Clock::time_point; - auto set_timestamp(Clock::time_point) noexcept -> void; + auto get_timestamp() const noexcept -> TimePoint; + auto set_timestamp(TimePoint) noexcept -> void; }; } diff --git a/src/utility/logging_util.hpp b/src/utility/logging_util.hpp new file mode 100644 index 00000000..572a15cc --- /dev/null +++ b/src/utility/logging_util.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include "utility/rclcpp/node.hpp" + +#include +#include +#include +#include +#include +#include + +namespace rmcs::util { + +class LoggingUtil { +private: + RclcppNode& rclcpp; + std::unordered_map store; + + template + auto exec(std::string_view name, Fn&& f) { + if (!store.contains(name)) return; + + if (auto& limit = store.at(name); limit > 0) { + limit--; + std::forward(f)(); + } + } + +public: + explicit LoggingUtil(RclcppNode& rclcpp) noexcept + : rclcpp { rclcpp } { } + + auto reset(std::string_view name, std::uint8_t limit) { store[name] = limit; } + + template + auto info(std::string_view name, std::format_string fmt, Args&&... args) noexcept { + exec(name, [=, this] { rclcpp.info(fmt, std::forward(args)...); }); + } + + template + auto warn(std::string_view name, std::format_string fmt, Args&&... args) noexcept { + exec(name, [=, this] { rclcpp.warn(fmt, std::forward(args)...); }); + } + + template + auto error(std::string_view name, std::format_string fmt, Args&&... args) noexcept { + exec(name, [=, this] { rclcpp.error(fmt, std::forward(args)...); }); + } +}; + +} // namespace rmcs::util diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index a0654089..df9fe02f 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -6,7 +6,10 @@ #include #include -namespace rmcs::util { +namespace rmcs { + +template +concept context_trait = std::is_trivially_copyable_v; enum class ShootMode { STOPPING, @@ -17,12 +20,31 @@ enum class ShootMode { }; struct Transform { - Translation position {}; - Orientation orientation {}; + Translation position { }; + Orientation orientation { }; + + static constexpr auto kNaN() { + return Transform { + Translation { + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + }, + Orientation { + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + }, + }; + }; }; struct AutoAimState { - Clock::time_point timestamp {}; + static constexpr auto kLabel = "/shm_autoaim_state"; + static constexpr auto kLength = 512; + + TimePoint timestamp { }; bool gimbal_takeover { false }; bool shoot_permitted = { false }; @@ -32,63 +54,54 @@ struct AutoAimState { DeviceId target { DeviceId::UNKNOWN }; - auto reset() noexcept -> void { - timestamp = Clock::now(); - - gimbal_takeover = false; - shoot_permitted = false; - yaw = std::numeric_limits::quiet_NaN(); - pitch = std::numeric_limits::quiet_NaN(); - target = DeviceId::UNKNOWN; - } - - auto set_hold_state(double current_yaw, double current_pitch, DeviceId current_target) noexcept - -> void { - timestamp = Clock::now(); - - gimbal_takeover = true; - shoot_permitted = false; - yaw = current_yaw; - pitch = current_pitch; - target = current_target; - } - - auto set_tracking_state(double target_yaw, double target_pitch, DeviceId tracked_target, - bool allow_shoot) noexcept -> void { - timestamp = Clock::now(); - - gimbal_takeover = true; - shoot_permitted = allow_shoot; - yaw = target_yaw; - pitch = target_pitch; - target = tracked_target; - } - - [[nodiscard]] auto has_control_direction() const noexcept -> bool { - return gimbal_takeover && std::isfinite(yaw) && std::isfinite(pitch); + static auto kInvalid() { + return AutoAimState { + .timestamp = Clock::now(), + .gimbal_takeover = false, + .shoot_permitted = false, + .yaw = std::numeric_limits::quiet_NaN(), + .pitch = std::numeric_limits::quiet_NaN(), + .target = DeviceId::UNKNOWN, + }; } }; -static_assert(std::is_trivially_copyable_v); +static_assert(context_trait); struct ControlState { - Clock::time_point timestamp {}; + static constexpr auto kLabel = "/shm_control_state"; + static constexpr auto kLength = 512; + + /// Dynamic Context + /// + TimePoint timestamp { }; ShootMode shoot_mode { ShootMode::BATTLE }; double yaw { std::numeric_limits::quiet_NaN() }; double pitch { std::numeric_limits::quiet_NaN() }; - DeviceIds invincible_devices { DeviceIds::None() }; + Transform odom_to_camera_transform { }; + + struct { + TimePoint timestamp = Clock::now(); + std::uint64_t index = 0; + } capture_signals; - Transform odom_to_camera_transform {}; + /// Lazy Context + /// + DeviceIds invincible_devices { DeviceIds::None() }; - auto reset() noexcept -> void { - timestamp = Clock::now(); - shoot_mode = ShootMode::STOPPING; - yaw = std::numeric_limits::quiet_NaN(); - pitch = std::numeric_limits::quiet_NaN(); - invincible_devices = DeviceIds::None(); - odom_to_camera_transform = {}; + static auto kInvalid() { + return ControlState { + .timestamp = Clock::now(), + .shoot_mode = ShootMode::STOPPING, + .yaw = std::numeric_limits::quiet_NaN(), + .pitch = std::numeric_limits::quiet_NaN(), + .odom_to_camera_transform = Transform::kNaN(), + .capture_signals = { }, + .invincible_devices = DeviceIds::None(), + }; } }; -static_assert(std::is_trivially_copyable_v); -} +static_assert(context_trait); + +} // namespace rmcs diff --git a/src/utility/tf/static_tf.hpp b/src/utility/tf/static_tf.hpp index f46f2683..4bb458cb 100644 --- a/src/utility/tf/static_tf.hpp +++ b/src/utility/tf/static_tf.hpp @@ -124,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 @@ -353,8 +353,9 @@ struct Joint { template static constexpr auto impl_traversal_child(auto&& on_recursion) noexcept -> std::size_t { - auto result = std::size_t { 0 }; - auto recursion = [&]() { + auto result = std::size_t { 0 }; + + [[maybe_unused]] auto recursion = [&]() { if (result) return; /* */ if (child == T::name) { result = 1; // End Point diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f2c3fdb0..e114ac3e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -121,9 +121,3 @@ ament_add_gtest( ${TEST_DIR}/feishu_test.cpp ) -# Action throttler -ament_add_gtest( - test_action_throttler - ${TEST_DIR}/action_throttler.cpp -) - diff --git a/test/action_throttler.cpp b/test/action_throttler.cpp deleted file mode 100644 index 25d2659c..00000000 --- a/test/action_throttler.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include -#include - -#include "module/debug/action_throttler.hpp" - -using namespace std::chrono_literals; -using rmcs::util::ActionThrottler; - -TEST(ActionThrottler, DispatchByIntervalAndQuota) { - ActionThrottler throttler { 10ms, 2 }; // 10ms 节拍,配额 2 次 - throttler.register_action("foo"); - - int count = 0; - // 第一次 tick -> 执行 - EXPECT_TRUE(throttler.dispatch("foo", [&] { ++count; })); - // 同一节拍内第二次调用 metronome 未到达,直接 false - EXPECT_FALSE(throttler.dispatch("foo", [&] { ++count; })); - - // 等待到下一个节拍 - std::this_thread::sleep_for(12ms); - EXPECT_TRUE(throttler.dispatch("foo", [&] { ++count; })); - - // 配额用完,再到下个节拍也不会执行 - std::this_thread::sleep_for(12ms); - EXPECT_FALSE(throttler.dispatch("foo", [&] { ++count; })); - - EXPECT_EQ(count, 2); -} - -TEST(ActionThrottler, DifferentTagsDoNotShareInterval) { - ActionThrottler throttler { 10ms, 1 }; - throttler.register_action("foo"); - throttler.register_action("bar"); - - int foo_count = 0; - int bar_count = 0; - - EXPECT_TRUE(throttler.dispatch("foo", [&] { ++foo_count; })); - EXPECT_TRUE(throttler.dispatch("bar", [&] { ++bar_count; })); - - EXPECT_EQ(foo_count, 1); - EXPECT_EQ(bar_count, 1); -} - -TEST(ActionThrottler, ResetRestoreQuota) { - ActionThrottler throttler { 1ms, 1 }; - throttler.register_action("bar"); - - int count = 0; - EXPECT_TRUE(throttler.dispatch("bar", [&] { ++count; })); - // 配额耗尽 - std::this_thread::sleep_for(2ms); - EXPECT_FALSE(throttler.dispatch("bar", [&] { ++count; })); - - throttler.reset("bar"); - std::this_thread::sleep_for(2ms); - EXPECT_TRUE(throttler.dispatch("bar", [&] { ++count; })); - EXPECT_EQ(count, 2); -} diff --git a/test/feishu_test.cpp b/test/feishu_test.cpp index 31d5d9ce..19bc430d 100644 --- a/test/feishu_test.cpp +++ b/test/feishu_test.cpp @@ -6,55 +6,57 @@ #include #include "kernel/feishu.hpp" +#include "utility/shared/context.hpp" using namespace std::chrono_literals; +using rmcs::AutoAimState; +using rmcs::Clock; +using rmcs::ControlState; using rmcs::kernel::Feishu; -using rmcs::kernel::RuntimeRole; -using rmcs::util::AutoAimState; -using rmcs::util::Clock; -using rmcs::util::ControlState; TEST(FeishuIntegration, BidirectionalCommunication) { const auto pid = ::fork(); ASSERT_NE(pid, -1); if (pid == 0) { - auto feishu_child = Feishu {}; - std::this_thread::sleep_for(50ms); // ensure shm is ready + auto feishu_child = Feishu { }; + std::this_thread::sleep_for(50ms); auto deadline = Clock::now() + 500ms; - auto ctrl = std::optional {}; + auto ctrl = std::optional { }; while (!ctrl && Clock::now() < deadline) { - if (feishu_child.updated()) { - ctrl = feishu_child.fetch(); + if (feishu_child.heartbeat()) { + ctrl = feishu_child.latest(); } else { std::this_thread::sleep_for(10ms); } } ASSERT_TRUE(ctrl.has_value()); - auto auto_state = AutoAimState {}; + auto auto_state = AutoAimState { }; auto_state.gimbal_takeover = true; auto_state.shoot_permitted = true; auto_state.yaw = 1.23; - ASSERT_TRUE(feishu_child.commit(auto_state)); + feishu_child.send(auto_state); exit(0); } - auto feishu_parent = Feishu {}; - std::this_thread::sleep_for(50ms); // ensure shm is ready + auto feishu_parent = Feishu { }; + std::this_thread::sleep_for(50ms); - auto ctrl = ControlState {}; + feishu_parent.heartbeat(); + + auto ctrl = ControlState { }; ctrl.timestamp = Clock::now(); - ASSERT_TRUE(feishu_parent.commit(ctrl)); + feishu_parent.send(ctrl); auto deadline = Clock::now() + 500ms; - auto auto_state = std::optional {}; + auto auto_state = std::optional { }; while (!auto_state && Clock::now() < deadline) { - if (feishu_parent.updated()) { - auto_state = feishu_parent.fetch(); + if (feishu_parent.heartbeat()) { + auto_state = feishu_parent.latest(); } else { std::this_thread::sleep_for(10ms); } diff --git a/test/static_tf.cpp b/test/static_tf.cpp index c2223731..d95284e1 100644 --- a/test/static_tf.cpp +++ b/test/static_tf.cpp @@ -32,10 +32,6 @@ 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 index 94557c72..20b58bbe 100644 --- a/test/transform_communication.cpp +++ b/test/transform_communication.cpp @@ -7,7 +7,7 @@ #include "utility/shared/interprocess.hpp" #include -using Transform = rmcs::util::Transform; +using Transform = rmcs::Transform; namespace { @@ -56,7 +56,7 @@ TEST(TransformShm, SendRecvSequence) { constexpr auto init_delay = 50ms; // 父进程先创建共享内存对象 - auto send = Send {}; + auto send = Send { }; ASSERT_TRUE(send.open(shm_name)); ASSERT_TRUE(send.opened()); @@ -65,7 +65,7 @@ TEST(TransformShm, SendRecvSequence) { if (pid == 0) { // 子进程:接收数据 - auto recv = Recv {}; + auto recv = Recv { }; ASSERT_TRUE(recv.open(shm_name)); ASSERT_TRUE(recv.opened()); @@ -122,7 +122,7 @@ TEST(TransformShm, SnapshotAndUpdate) { send.send(first); EXPECT_TRUE(recv.is_updated()); - Transform snapshot {}; + Transform snapshot { }; recv.with_read([&](const auto& data) { snapshot = data; }); expect_transform_equal(snapshot, first); EXPECT_FALSE(recv.is_updated()); // 读取后版本同步