feat: Add event interfaces and board-clocked IMU pipeline#83
Merged
Conversation
This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary.
Walkthrough此 PR 引入事件驱动组件接口、四元数 IMU EKF、板卡时钟抬升工具、futex 与内存池基础设施，并将 OmniInfantry 硬件接入改为基于 EKF 的采样融合与事件输出链路。 ChangesEvent-Driven Component Framework
Core Utilities and Message Types
IMU Filtering and Time Management
Hardware Component Integration
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Poem
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (5)
rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp (1)
21-21: ⚡ Quick win字段命名与类型不一致。
字段名为
imu_snapshot暗示完整的 IMU 快照（对应rmcs_msgs::ImuSnapshot结构体），但实际类型为Eigen::Quaterniond（仅姿态四元数）。建议将字段名改为imu_orientation以准确反映其语义，或将类型改为rmcs_msgs::ImuSnapshot以包含完整的 IMU 状态（姿态 + 角速度 + 时间戳）。♻️ 建议的命名修正
- Eigen::Quaterniond imu_snapshot; + Eigen::Quaterniond imu_orientation;🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp` at line 21, The field imu_snapshot is misnamed for its type Eigen::Quaterniond; either rename the field to imu_orientation to reflect it holds only the orientation quaternion, or change its type to rmcs_msgs::ImuSnapshot to represent a full IMU snapshot (orientation, angular velocity, timestamp). Update all uses of imu_snapshot accordingly (e.g., constructors, serializers, accessors) to the new name or type to keep semantics consistent with rmcs_msgs::ImuSnapshot and Eigen::Quaterniond.rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp (1)
40-46: ⚡ Quick win析构器中未检查
free()的返回值。自定义析构器在第 45 行调用
pool->free(ptr)但未检查返回值。虽然在正确使用下free()应总是返回true，但如果由于内部状态损坏或其他异常情况导致free()返回false，该错误会被静默忽略。建议添加断言或日志记录以便调试时发现此类问题。🛡️ 建议的防御性检查
[pool = pool_](T* ptr) { std::destroy_at(ptr); auto guard = std::scoped_lock{pool->mutex}; - pool->free(ptr); + if (!pool->free(ptr)) { + std::fprintf(stderr, + "PooledSharedFactory: failed to free pointer %p\n", + static_cast<void*>(ptr)); + std::fflush(stderr); + } });🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp` around lines 40 - 46, The custom deleter used in the std::shared_ptr allocation (the lambda inside pooled_shared_factory.hpp that calls std::destroy_at(ptr) and then pool->free(ptr)) currently ignores the boolean result of pool->free(ptr); update the deleter to capture and check the return value and react on failure (e.g., assert, throw, or call a logger) so failures are not silently swallowed—ensure you reference the deleter lambda and pool->free(ptr) when making the change and choose consistent behavior with the project’s error handling (assert in debug builds or log/error-report in production).rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp (1)
40-50: 析构函数强制终止程序的严格约束。当存在未归还的槽时，析构函数会调用
std::terminate()终止程序。这是非常严格的生命周期契约，文档已明确说明（第 37-38 行），但在某些场景下可能导致意外的程序终止。请确保所有使用方都清楚理解这一约束，并在池销毁前正确释放所有分配的槽。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp` around lines 40 - 50, The destructor ~MemoryPool() currently calls std::terminate() when !empty(), which is too strict; change it to avoid unconditional termination by either (a) replacing std::terminate() with a non-fatal behavior (log the error and return) or (b) making the strict behavior configurable and only invoked when a new flag (e.g., strict_lifetime_enforced_) is true or in debug builds (NDEBUG check), and add a clear API to enforce cleanup (e.g., MemoryPool::shutdown() or MemoryPool::release_all()) so callers can explicitly release slots before destruction; keep the existing stderr message and fflush and reference empty(), size_, capacity_, and ~MemoryPool() when implementing the change.rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp (1)
29-39: ⚡ Quick win潜在的重复 TF 广播。
当前实现在两个位置广播 TF：
before_updating()中无条件广播（第 29 行）update()中基于时间条件广播（第 36 行）当时间条件满足时，同一更新周期内会进行两次广播。虽然根据 AI 摘要这是有意设计（确保其他组件更新前有最新 TF 数据），但这可能导致不必要的开销。建议：
- 如果
before_updating()已经广播，可在update()中跳过该周期的广播- 或在代码注释中明确说明双重广播的必要性
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp` around lines 29 - 39, The two calls to fast_tf::rcl::broadcast_all(*tf_) in before_updating() and update() can cause duplicate broadcasts in the same cycle; modify before_updating() to set a small per-cycle flag (e.g. tf_broadcasted_this_cycle_) when it calls broadcast_all, and change update() to check tf_broadcasted_this_cycle_ before calling fast_tf::rcl::broadcast_all(*tf_) (skip the broadcast if the flag is set), and clear/reset tf_broadcasted_this_cycle_ at the start of the next cycle (use update_count_ or timestamp_ progression to detect cycle boundary); alternatively, if duplicate broadcasts are intentional, add a brief comment in before_updating() and update() referencing broadcast_all to explain why both broadcasts are required.rmcs_ws/src/rmcs_executor/src/executor.hpp (1)
374-374: 💤 Low value命名空间闭合大括号后的多余分号。
虽然不会导致编译错误，但这是一个不必要的分号。
♻️ 建议修复
-}; // namespace rmcs_executor +} // namespace rmcs_executor🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_executor/src/executor.hpp` at line 374, 删除命名空间结束处多余的分号：在文件中定位命名空间闭合符号“}”对应的命名空间标识符 rmcs_executor（即行包含 “}; // namespace rmcs_executor”）并去掉紧跟在闭括号后的分号，只保留 “} // namespace rmcs_executor”。确保不改动注释文本以保持注释说明唯一标识符不变。
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp`:
- Around line 38-50: The initialization path writes latest_snapshot_ without
holding mutex_ which can race with snapshot(); modify the block in the
initialization branch (the code around ekf_.reset_from_accel, ekf_state_time_,
latest_snapshot_, initialized_) so that you acquire the lock (e.g.,
std::scoped_lock guard{mutex_}) before writing latest_snapshot_ and before
setting initialized_, perform ekf_state_time_ and latest_snapshot_ assignments
while holding the lock, then set initialized_ and release the guard; keep
ekf_.reset_from_accel(...) call outside the lock if desired to avoid holding the
mutex during that operation.
---
Nitpick comments:
In `@rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp`:
- Around line 29-39: The two calls to fast_tf::rcl::broadcast_all(*tf_) in
before_updating() and update() can cause duplicate broadcasts in the same cycle;
modify before_updating() to set a small per-cycle flag (e.g.
tf_broadcasted_this_cycle_) when it calls broadcast_all, and change update() to
check tf_broadcasted_this_cycle_ before calling
fast_tf::rcl::broadcast_all(*tf_) (skip the broadcast if the flag is set), and
clear/reset tf_broadcasted_this_cycle_ at the start of the next cycle (use
update_count_ or timestamp_ progression to detect cycle boundary);
alternatively, if duplicate broadcasts are intentional, add a brief comment in
before_updating() and update() referencing broadcast_all to explain why both
broadcasts are required.
In `@rmcs_ws/src/rmcs_executor/src/executor.hpp`:
- Line 374: 删除命名空间结束处多余的分号：在文件中定位命名空间闭合符号“}”对应的命名空间标识符 rmcs_executor（即行包含 “}; //
namespace rmcs_executor”）并去掉紧跟在闭括号后的分号，只保留 “} // namespace
rmcs_executor”。确保不改动注释文本以保持注释说明唯一标识符不变。
In `@rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp`:
- Line 21: The field imu_snapshot is misnamed for its type Eigen::Quaterniond;
either rename the field to imu_orientation to reflect it holds only the
orientation quaternion, or change its type to rmcs_msgs::ImuSnapshot to
represent a full IMU snapshot (orientation, angular velocity, timestamp). Update
all uses of imu_snapshot accordingly (e.g., constructors, serializers,
accessors) to the new name or type to keep semantics consistent with
rmcs_msgs::ImuSnapshot and Eigen::Quaterniond.
In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp`:
- Around line 40-50: The destructor ~MemoryPool() currently calls
std::terminate() when !empty(), which is too strict; change it to avoid
unconditional termination by either (a) replacing std::terminate() with a
non-fatal behavior (log the error and return) or (b) making the strict behavior
configurable and only invoked when a new flag (e.g., strict_lifetime_enforced_)
is true or in debug builds (NDEBUG check), and add a clear API to enforce
cleanup (e.g., MemoryPool::shutdown() or MemoryPool::release_all()) so callers
can explicitly release slots before destruction; keep the existing stderr
message and fflush and reference empty(), size_, capacity_, and ~MemoryPool()
when implementing the change.
In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp`:
- Around line 40-46: The custom deleter used in the std::shared_ptr allocation
(the lambda inside pooled_shared_factory.hpp that calls std::destroy_at(ptr) and
then pool->free(ptr)) currently ignores the boolean result of pool->free(ptr);
update the deleter to capture and check the return value and react on failure
(e.g., assert, throw, or call a logger) so failures are not silently
swallowed—ensure you reference the deleter lambda and pool->free(ptr) when
making the change and choose consistent behavior with the project’s error
handling (assert in debug builds or log/error-report in production).
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 143c2a30-3681-4552-b6c4-b5670116acfb
📒 Files selected for processing (19)
rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpprmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpprmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpprmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpprmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpprmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpprmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpprmcs_ws/src/rmcs_executor/package.xmlrmcs_ws/src/rmcs_executor/src/component.cpprmcs_ws/src/rmcs_executor/src/executor.hpprmcs_ws/src/rmcs_executor/src/main.cpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpprmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpprmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpprmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpprmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpprmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp
Comment on lines
+38
to
+50
| if (!initialized_) { | ||
| if (ekf_.reset_from_accel(accel_g)) { | ||
| ekf_state_time_ = sample_time; | ||
| latest_snapshot_ = { | ||
| ekf_.quaternion(), | ||
| Eigen::Vector3d::Zero(), | ||
| ekf_state_time_, | ||
| }; | ||
| const auto guard = std::scoped_lock{mutex_}; | ||
| initialized_ = true; | ||
| } | ||
| return; | ||
| } |
There was a problem hiding this comment.
初始化路径存在数据竞争
latest_snapshot_ 在第 41-45 行写入时没有持有互斥锁，但 snapshot() 方法会在持锁状态下读取它。如果 snapshot() 在 initialized_ 被设置后但 latest_snapshot_ 写入完成前被调用，会导致数据竞争。
应该在写入 latest_snapshot_ 之前获取锁：
🔒 建议修复
if (!initialized_) {
if (ekf_.reset_from_accel(accel_g)) {
ekf_state_time_ = sample_time;
- latest_snapshot_ = {
- ekf_.quaternion(),
- Eigen::Vector3d::Zero(),
- ekf_state_time_,
- };
const auto guard = std::scoped_lock{mutex_};
+ latest_snapshot_ = {
+ ekf_.quaternion(),
+ Eigen::Vector3d::Zero(),
+ ekf_state_time_,
+ };
initialized_ = true;
}
return;
}🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp` around lines 38 -
50, The initialization path writes latest_snapshot_ without holding mutex_ which
can race with snapshot(); modify the block in the initialization branch (the
code around ekf_.reset_from_accel, ekf_state_time_, latest_snapshot_,
initialized_) so that you acquire the lock (e.g., std::scoped_lock
guard{mutex_}) before writing latest_snapshot_ and before setting initialized_,
perform ekf_state_time_ and latest_snapshot_ assignments while holding the lock,
then set initialized_ and release the guard; keep ekf_.reset_from_accel(...)
call outside the lock if desired to avoid holding the mutex during that
operation.
Member
Author
There was a problem hiding this comment.
latest_snapshot_ 这里原实现没有实际竞争问题，本次不计划修改。当前初始化路径的同步关系是：先写入 latest_snapshot_，再在持有 mutex_ 的情况下将 initialized_ 置为 true；而消费者 snapshot() 也是在持有同一把 mutex_ 时先检查 initialized_，只有为 true 才读取 latest_snapshot_。因此当读方观察到 initialized_ == true 时，latest_snapshot_ 已经完成发布，不会出现“已初始化标志可见但快照内容未就绪”的情况。换言之，这里依赖的是受同一把锁保护的发布条件，而不是无同步读取未受保护数据；现有实现可以满足正确性要求。考虑到本次更改不会提升行为正确性，只是改写同步表达方式，因此暂不采纳。
There was a problem hiding this comment.
@qzhhhi 感谢详细的解释，您的分析是正确的。
C++ 内存模型保证了此处的正确性：
- 线程 A：写入
latest_snapshot_→ 获取mutex_（acquire） → 写入initialized_ = true→ 释放mutex_（release） - 线程 B：获取
mutex_（acquire，与线程 A 的 release 同步） → 读取initialized_（为true） → 读取latest_snapshot_
由于线程 A 中写入 latest_snapshot_ sequenced-before mutex 的 release，而该 release synchronizes-with 线程 B 的 acquire，因此整条 happens-before 链成立——当读方观察到 initialized_ == true 时，latest_snapshot_ 的写入已对其可见。这是一种合法的"锁外写入、锁内发布"同步模式，我最初的判断有误，撤回该建议。
🐇 hops away thoughtfully
✏️ Learnings added
Learnt from: qzhhhi
Repo: Alliance-Algorithm/RMCS PR: 83
File: rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp:38-50
Timestamp: 2026-06-10T14:10:46.014Z
Learning: In `rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp`, the initialization path writes `latest_snapshot_` outside the `mutex_` lock, then acquires the lock and sets `initialized_ = true`. This is a valid "publish-before-locking" pattern under the C++ memory model: the mutex release (after setting `initialized_`) synchronizes-with the consumer's mutex acquire, which transitively makes the pre-lock write to `latest_snapshot_` visible to any thread that observes `initialized_ == true` under the same lock. There is no data race here.
Note: Learnings are effective only in the context of similar code segments. To apply general coding guidelines, please configure review instructions. You can manage existing learnings in the app.
There was a problem hiding this comment.
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (2)
rmcs_ws/src/rmcs_executor/src/executor.hpp (2)
74-83:⚠️ Potential issue | 🟠 Major | ⚡ Quick win致命异常后应立即停掉事件输出。
这里直接
rclcpp::shutdown()，但disable_event_outputs()/wait_event_outputs_idle()只在析构里执行。按rmcs_ws/src/rmcs_executor/src/main.cpp:33-72的生命周期，shutdown()只会让spin()退出，Executor要到作用域结束才析构；这段窗口里硬件事件回调仍可能继续打进组件，和失败后的清理并发。建议在两个catch分支里先停输出并等待空闲，再触发 shutdown。建议修改
} catch (const std::exception& exception) { + disable_event_outputs(); + wait_event_outputs_idle(); RCLCPP_FATAL( get_logger(), "Executor update thread terminated by exception: %s", exception.what()); rclcpp::shutdown(); } catch (...) { + disable_event_outputs(); + wait_event_outputs_idle(); RCLCPP_FATAL( get_logger(), "Executor update thread terminated by unknown exception"); rclcpp::shutdown(); }🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_executor/src/executor.hpp` around lines 74 - 83, The catch blocks in the Executor update thread currently call rclcpp::shutdown() directly, but event outputs are only stopped in the destructor which may race with in-flight hardware callbacks; modify both catch branches so they first call disable_event_outputs() and then wait_event_outputs_idle() on this Executor instance to stop and drain event outputs before invoking rclcpp::shutdown(), keeping the existing log calls (use get_logger()) and preserving behaviour if those helper calls are no-ops when already disabled.
55-63:⚠️ Potential issue | 🟠 Major | ⚡ Quick win在计算周期前校验
update_rate。Line 62 直接做
1'000'000'000.0 / update_rate。0、负数、NaN/inf，以及会被舍入成0ns的极大值，都会把更新线程带到非法周期或 busy loop。这里应该在启动前拒绝非有限值和<= 0，并保证最终周期至少为1ns。建议修改
+ `#include` <cmath> + double update_rate; if (!get_parameter("update_rate", update_rate)) throw std::runtime_error{"Unable to get parameter update_rate<double>"}; + if (!std::isfinite(update_rate) || update_rate <= 0.0) + throw std::runtime_error{"Parameter update_rate must be finite and > 0"}; predefined_msg_provider_->set_update_rate(update_rate); enable_event_outputs(); thread_ = std::thread{[update_rate, this]() { - const auto period = std::chrono::nanoseconds( - static_cast<long>(std::round(1'000'000'000.0 / update_rate))); + const auto period = std::max( + std::chrono::nanoseconds{1}, + std::chrono::duration_cast<std::chrono::nanoseconds>( + std::chrono::duration<double>(1.0 / update_rate))); auto next_iteration_time = std::chrono::steady_clock::now();🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_executor/src/executor.hpp` around lines 55 - 63, Validate update_rate before using it to compute the period: after obtaining update_rate (used in predefined_msg_provider_->set_update_rate) but before spawning thread_ and computing period in the thread_ lambda, check that update_rate is finite and > 0 (reject NaN/inf and non-positive values) and clamp or enforce that the computed period is at least 1 nanosecond (e.g., ensure the rounded nanoseconds value is >= 1) to avoid 0ns/busy-loop or invalid divisions; if validation fails, throw a clear std::runtime_error describing the invalid update_rate.
🧹 Nitpick comments (1)
rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp (1)
40-55: 🏗️ Heavy lift
try_make()仍然不是纯池化路径。这里池化的只有
T本体；std::shared_ptr(ptr, deleter)仍会为控制块单独做一次堆分配，所以池还有空槽时这里也可能抛出std::bad_alloc。如果上层把nullptr当成唯一失败信号，热路径里的异常语义就会和接口名预期不一致。建议确认调用侧是否显式接住这类异常；如果目标是完全避免额外堆分配，需要把控制块也一起池化。🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp` around lines 40 - 55, try_make currently only pools the T instance but still uses std::shared_ptr(ptr, deleter) which may allocate a separate control block and throw std::bad_alloc; either make the API non-throwing by catching allocation failures and returning nullptr, or implement true pooled control blocks so no heap allocation occurs. Locate PooledSharedFactory::try_make and its use of std::shared_ptr(std::construct_at(static_cast<T*>(storage), ...), [pool=pool_](T* ptr){...}) and choose one fix: (A) wrap the shared_ptr construction in a try/catch for std::bad_alloc (or std::exception) and on catch destroy the constructed T and return nullptr, ensuring pool_->free is called appropriately; or (B) change the allocation to create the shared_ptr control block from the pool too (e.g., provide a pooled allocator or custom control-block allocation path so the control block and T are allocated from the pool together) and update the deleter to return both T and control-block resources to pool. Ensure consistency with callers expecting nullptr vs exceptions.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Outside diff comments:
In `@rmcs_ws/src/rmcs_executor/src/executor.hpp`:
- Around line 74-83: The catch blocks in the Executor update thread currently
call rclcpp::shutdown() directly, but event outputs are only stopped in the
destructor which may race with in-flight hardware callbacks; modify both catch
branches so they first call disable_event_outputs() and then
wait_event_outputs_idle() on this Executor instance to stop and drain event
outputs before invoking rclcpp::shutdown(), keeping the existing log calls (use
get_logger()) and preserving behaviour if those helper calls are no-ops when
already disabled.
- Around line 55-63: Validate update_rate before using it to compute the period:
after obtaining update_rate (used in predefined_msg_provider_->set_update_rate)
but before spawning thread_ and computing period in the thread_ lambda, check
that update_rate is finite and > 0 (reject NaN/inf and non-positive values) and
clamp or enforce that the computed period is at least 1 nanosecond (e.g., ensure
the rounded nanoseconds value is >= 1) to avoid 0ns/busy-loop or invalid
divisions; if validation fails, throw a clear std::runtime_error describing the
invalid update_rate.
---
Nitpick comments:
In `@rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp`:
- Around line 40-55: try_make currently only pools the T instance but still uses
std::shared_ptr(ptr, deleter) which may allocate a separate control block and
throw std::bad_alloc; either make the API non-throwing by catching allocation
failures and returning nullptr, or implement true pooled control blocks so no
heap allocation occurs. Locate PooledSharedFactory::try_make and its use of
std::shared_ptr(std::construct_at(static_cast<T*>(storage), ...),
[pool=pool_](T* ptr){...}) and choose one fix: (A) wrap the shared_ptr
construction in a try/catch for std::bad_alloc (or std::exception) and on catch
destroy the constructed T and return nullptr, ensuring pool_->free is called
appropriately; or (B) change the allocation to create the shared_ptr control
block from the pool too (e.g., provide a pooled allocator or custom
control-block allocation path so the control block and T are allocated from the
pool together) and update the deleter to return both T and control-block
resources to pool. Ensure consistency with callers expecting nullptr vs
exceptions.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: c40d7ebe-35f9-40e4-a5d7-f424338f625b
📒 Files selected for processing (2)
rmcs_ws/src/rmcs_executor/src/executor.hpprmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp
noskillzheng
pushed a commit
that referenced
this pull request
Jun 18, 2026
This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary.
noskillzheng
pushed a commit
that referenced
this pull request
Jul 4, 2026
This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary.
ZGZ713912
added a commit
that referenced
this pull request
Jul 7, 2026
commit 1e99fd9 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab658 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect <chenchengyue201@126.com> commit b5ae6cb Author: Zihan Qin <zihanqin2048@gmail.com> Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon <floatpigeon@proton.me> Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d Author: qzhhhi <zihanqin2048@gmail.com> Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files
ZGZ713912
added a commit
that referenced
this pull request
Jul 7, 2026
commit 1e99fd9 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab658 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect <chenchengyue201@126.com> commit b5ae6cb Author: Zihan Qin <zihanqin2048@gmail.com> Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon <floatpigeon@proton.me> Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d Author: qzhhhi <zihanqin2048@gmail.com> Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files
ZGZ713912
added a commit
that referenced
this pull request
Jul 7, 2026
commit 2c63c7d Author: creeper5820 <131014151+creeper5820@users.noreply.github.com> Date: Tue Jul 7 00:03:30 2026 +0800 feat: Integrate flight support (#85) - Add flight.yaml parameter config for Flight hardware, gimbal, friction wheels, feeder, referee interaction, auto-aim, and odin_ros_driver. - Add rmcs_core::hardware::Flight hardware plugin supporting flight controller, IMU, remote control, CAN/UART comms, TF, and referee serial. - Add rmcs_core::referee::app::ui::Flight UI plugin for ammo, friction wheel status, and auto-aim mode display. - Replace/add Flight-related hardware, controller, and UI components in plugin registry; remove some legacy chassis/controller registrations. - Add auto-aim toggle, yaw limit support, and direction clamping based on current angle to gimbal control. - Switch shooting control to /auto_aim/should_shoot input and refine auto/manual shooting logic. - Add kMHF7015 motor type support. - Add supercap/battery display toggle to StatusRing. - Minor CMake, description file, and export formatting/cleanup. Co-authored-by: heyeuu <2829004293@qq.com> Co-authored-by: zlq040222 <1542498005@qq.com> commit 1e99fd9 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab658 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect <chenchengyue201@126.com> commit b5ae6cb Author: Zihan Qin <zihanqin2048@gmail.com> Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon <floatpigeon@proton.me> Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d Author: qzhhhi <zihanqin2048@gmail.com> Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files
ZGZ713912
added a commit
that referenced
this pull request
Jul 7, 2026
commit 2c63c7d Author: creeper5820 <131014151+creeper5820@users.noreply.github.com> Date: Tue Jul 7 00:03:30 2026 +0800 feat: Integrate flight support (#85) - Add flight.yaml parameter config for Flight hardware, gimbal, friction wheels, feeder, referee interaction, auto-aim, and odin_ros_driver. - Add rmcs_core::hardware::Flight hardware plugin supporting flight controller, IMU, remote control, CAN/UART comms, TF, and referee serial. - Add rmcs_core::referee::app::ui::Flight UI plugin for ammo, friction wheel status, and auto-aim mode display. - Replace/add Flight-related hardware, controller, and UI components in plugin registry; remove some legacy chassis/controller registrations. - Add auto-aim toggle, yaw limit support, and direction clamping based on current angle to gimbal control. - Switch shooting control to /auto_aim/should_shoot input and refine auto/manual shooting logic. - Add kMHF7015 motor type support. - Add supercap/battery display toggle to StatusRing. - Minor CMake, description file, and export formatting/cleanup. Co-authored-by: heyeuu <2829004293@qq.com> Co-authored-by: zlq040222 <1542498005@qq.com> commit 1e99fd9 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab658 Author: Zihan Qin <zihanqin2048@gmail.com> Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect <chenchengyue201@126.com> commit b5ae6cb Author: Zihan Qin <zihanqin2048@gmail.com> Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon <floatpigeon@proton.me> Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d Author: qzhhhi <zihanqin2048@gmail.com> Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files
qzhhhi
added a commit
that referenced
this pull request
Jul 7, 2026
This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This commit lays the groundwork for upcoming IMU-camera hardware synchronization.
Known limitation:
概述
本PR为IMU-摄像机硬件同步管道奠定基础，引入事件接口系统，新增板卡时钟和IMU快照消息类型，并将基于EKF的IMU处理集成到OmniInfantry硬件平台中。
核心变更
事件接口系统（rmcs_executor）
InterfaceKind枚举（Normal和Event），允许区分常规接口与事件接口EventInputInterface<T>用于接收事件，需要预先注册回调函数QueuedEventInputInterface<T>实现基于环形队列和工作线程的事件消费，支持异常捕获和丢弃计数统计EventOutputInterface<T>提供emit()方法广播事件register_input和register_output重载，并在配对阶段按 (name, kind) 进行匹配验证消息类型（rmcs_msgs）
IMU EKF处理链路
reset_from_accelBoardClock::time_pointOmniInfantry 平台集成
librmcs::agent::CBoard切换到librmcs::agent::RmcsBoardLiteBmi088Ekf的事件驱动处理imu_snapshot_output_事件接口发出camera_signal_output_事件接口发出实用工具库（rmcs_utility）
其他调整
before_updating()钩子在更新前进行一次无条件TF广播OutputInfoMap参数，按接口种类过滤输出依赖关系
工作量评估
代码审查难度为中至高，涉及事件接口系统架构、EKF算法实现和硬件集成逻辑。