背景IMU传感器以高频如500Hz向外发布包含seqid序列号、timestamp时间戳以及姿态数据Roll, Pitch, Yaw的数据包。系统中有两个消费者节点导航节点Navigation Node负责定位。它追求实时性只关心当前最新的姿态允许中间有少量丢包不需要处理历史数据。感知节点Perception Node负责视觉惯性里程计VIO。它追求数据完整性要求IMU数据必须严格连续seqid连续。如果检测到丢包必须拒绝该帧数据并触发重同步否则会导致视觉追踪失败。需求请设计一个线程安全的ImuManager类实现以下功能能够接收并缓存最新的IMU数据包模拟从底层驱动获取。为导航节点提供获取最新数据的接口。为感知节点提供带有seqid连续性校验的获取接口。保证多线程环境下的数据安全且尽量降低对高频数据更新的阻塞。 实现设计思路读写锁std::shared_mutexIMU更新频率极高而消费者读取频率相对较低。使用读写锁可以让多个消费者同时读取只有在底层更新数据时才加独占锁极大地提升了并发性能。差异化返回导航节点直接拿数据感知节点通过std::optional返回如果校验失败丢包则返回空值。#include iostream#include thread#include mutex#include shared_mutex // C17 读写锁#include optional // C17 可选值#include chrono// 1. IMU 数据结构struct ImuData {int seqid; // 序列号double timestamp; // 时间戳double roll, pitch, yaw; // 姿态角};// 2. IMU 数据管理器class ImuManager {public:ImuManager() : last_seq_id_(-1) {}// 【生产者调用】模拟底层驱动推送最新的IMU数据void updateData(const ImuData new_data) {// 使用独占锁写锁因为要修改共享数据std::unique_lockstd::shared_mutex lock(mutex_);latest_data_ new_data;}// 【导航节点调用】获取最新数据容忍丢包追求实时ImuData getLatestData() {// 使用共享锁读锁允许多个消费者同时读取std::shared_lockstd::shared_mutex lock(mutex_);return latest_data_;}// 【感知节点调用】获取连续数据校验 seqid如果丢包则返回空std::optionalImuData getValidatedData() {std::shared_lockstd::shared_mutex lock(mutex_);// 处理初始状态if (-1 last_seq_id_) {last_seq_id_ latest_data_.seqid;return latest_data_;}// 强转为 unsigned int 处理 int 溢出if (static_castunsigned int(latest_data_.seqid - last_seq_id_) ! 1) {std::cerr [感知节点警告] 检测到丢包! 期望seq: last_seq_id_ 1 , 实际收到seq: latest_data_.seqid 拒绝该帧数据\n;return std::nullopt;}// 数据连续更新本地记录的 seqid 并返回数据last_seq_id_ latest_data_.seqid;return latest_data_;}private:mutable std::shared_mutex mutex_; // 读写锁保护 latest_data_ImuData latest_data_; // 缓存的最新IMU数据int last_seq_id_; // 记录感知节点上一次成功消费的 seqid};// 3. 模拟导航节点要求最新容忍丢包void navigationNode(ImuManager manager) {while (true) {ImuData data manager.getLatestData();// 导航算法直接接收最新观测值即使中间丢了几个包也能通过EKF协方差自适应std::cout [导航节点] 融合最新IMU - seq: data.seqid , Yaw: data.yaw \n;std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 模拟100Hz运行}}// 4. 模拟感知节点要求严格连续丢包则报警/重同步void perceptionNode(ImuManager manager) {while (true) {auto data_opt manager.getValidatedData();if (data_opt) {// 数据连续进行VIO视觉惯性里程计计算ImuData data data_opt.value();std::cout [感知节点] VIO计算正常 - seq: data.seqid \n;}else {// 数据不连续触发重同步逻辑例如重置光流追踪器或等待下一个关键帧std::cout [感知节点] 触发重同步机制...\n;}std::this_thread::sleep_for(std::chrono::milliseconds(33)); // 模拟30Hz运行与相机同步}}// 5. 模拟底层驱动生产者void mockImuDriver(ImuManager manager) {int seq 0;while (true) {ImuData data;data.seqid seq;data.timestamp std::chrono::duration_caststd::chrono::microseconds(std::chrono::steady_clock::now().time_since_epoch()).count();data.roll 0.1;data.pitch 0.2;data.yaw (seq % 360) * 1.0;manager.updateData(data);// 模拟偶尔丢包例如每15帧丢一次实际串口读取中可能发生if (0 seq % 15) {std::cout --- [底层驱动模拟] 发生一次丢包 ---\n;seq; // 跳过一個 seqid}std::this_thread::sleep_for(std::chrono::milliseconds(2)); // 模拟500Hz高频输出}}int main() {ImuManager imu;// 启动生产者模拟驱动和两个消费者线程std::thread driver_thread(mockImuDriver, std::ref(imu));std::thread nav_thread(navigationNode, std::ref(imu));std::thread perc_thread(perceptionNode, std::ref(imu));driver_thread.join();nav_thread.join();perc_thread.join();return 0;} 总结读写锁std::shared_mutex的应用相比于普通的std::mutex读写锁非常适合这种“单生产者-多消费者”且“读多写少”IMU更新极快但消费者读取相对较慢的场景。它能让导航节点和感知节点同时读取数据互不阻塞只有在驱动层更新latest_data_的瞬间才会加锁。std::optional的使用在 C17 中使用std::optional来返回可能失败的结果是非常现代且安全的做法。它明确地告诉调用者“这个函数不一定有返回值”比传统的“返回 bool 引用传参”或者“返回 -1 等错误码”更加清晰。业务逻辑的解耦代码清晰地展示了导航和感知对数据质量的不同容忍度。