From 6411262049712f226621bfec5682bcadbf748008 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Fri, 18 Sep 2026 14:26:15 +0200 Subject: [PATCH] fix(replay): keep one message id per instance of multi-instance topics Signed-off-by: Marco Hauswirth --- src/modules/replay/ReplayEkf2.cpp | 53 ++++++++++++++++++++++++------- src/modules/replay/ReplayEkf2.hpp | 11 +++++-- 2 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 2900d282533..0dba4386852 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -113,10 +113,10 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) _airspeed_validated_msg_id = msg_id; } else if (sub.orb_meta == ORB_ID(distance_sensor)) { - _distance_sensor_msg_id = msg_id; + _distance_sensor_msg_ids.push_back(msg_id); } else if (sub.orb_meta == ORB_ID(vehicle_optical_flow)) { - _optical_flow_msg_id = msg_id; + _optical_flow_msg_ids.push_back(msg_id); } else if (sub.orb_meta == ORB_ID(vehicle_air_data)) { _vehicle_air_data_msg_id = msg_id; @@ -128,7 +128,7 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) _vehicle_visual_odometry_msg_id = msg_id; } else if (sub.orb_meta == ORB_ID(aux_global_position)) { - _aux_global_position_msg_id = msg_id; + _aux_global_position_msg_ids.push_back(msg_id); } else if (sub.orb_meta == ORB_ID(vehicle_local_position_groundtruth)) { _vehicle_local_position_groundtruth_msg_id = msg_id; @@ -177,12 +177,12 @@ bool ReplayEkf2::publishEkf2Topics(sensor_combined_s &sensor_combined, std::ifstream &replay_file) { findTimestampAndPublish(sensor_combined.timestamp, _airspeed_msg_id, replay_file); - findTimestampAndPublish(sensor_combined.timestamp, _distance_sensor_msg_id, replay_file); - findTimestampAndPublish(sensor_combined.timestamp, _optical_flow_msg_id, replay_file); + findTimestampAndPublish(sensor_combined.timestamp, _distance_sensor_msg_ids, replay_file); + findTimestampAndPublish(sensor_combined.timestamp, _optical_flow_msg_ids, replay_file); findTimestampAndPublish(sensor_combined.timestamp, _vehicle_air_data_msg_id, replay_file); findTimestampAndPublish(sensor_combined.timestamp, _vehicle_magnetometer_msg_id, replay_file); findTimestampAndPublish(sensor_combined.timestamp, _vehicle_visual_odometry_msg_id, replay_file); - findTimestampAndPublish(sensor_combined.timestamp, _aux_global_position_msg_id, replay_file); + findTimestampAndPublish(sensor_combined.timestamp, _aux_global_position_msg_ids, replay_file); findTimestampAndPublish(sensor_combined.timestamp, _ranging_beacon_msg_id, replay_file); findTimestampAndPublish(sensor_combined.timestamp, _vehicle_gps_position_msg_id, replay_file); findTimestampAndPublish(sensor_combined.timestamp, _vehicle_land_detected_msg_id, replay_file); @@ -216,14 +216,20 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs } }; + auto handle_multi_sensor_publication = [&](int16_t timestamp_relative, const std::vector &msg_ids) { + for (uint16_t msg_id : msg_ids) { + handle_sensor_publication(timestamp_relative, msg_id); + } + }; + handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id); handle_sensor_publication(ekf2_timestamps.airspeed_validated_timestamp_rel, _airspeed_validated_msg_id); - handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id); - handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id); + handle_multi_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_ids); + handle_multi_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_ids); handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id); handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id); handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id); - handle_sensor_publication(0, _aux_global_position_msg_id); + handle_multi_sensor_publication(0, _aux_global_position_msg_ids); handle_sensor_publication(0, _ranging_beacon_msg_id); handle_sensor_publication(0, _vehicle_local_position_groundtruth_msg_id); handle_sensor_publication(0, _vehicle_global_position_groundtruth_msg_id); @@ -303,6 +309,19 @@ ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::if return topic_published; } +bool +ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, const std::vector &msg_ids, + std::ifstream &replay_file) +{ + bool topic_published = false; + + for (uint16_t msg_id : msg_ids) { + topic_published |= findTimestampAndPublish(timestamp, msg_id, replay_file); + } + + return topic_published; +} + void ReplayEkf2::publishSensorCombined(Subscription &sub, void *data) { @@ -407,6 +426,16 @@ ReplayEkf2::onExitMainLoop() } }; + auto print_multi_sensor_statistics = [this](const std::vector &msg_ids, const char *name) { + for (uint16_t msg_id : msg_ids) { + Subscription &sub = *_subscriptions[msg_id]; + + if (sub.publication_counter > 0 || sub.approx_timestamp_counter > 0) { + PX4_INFO("%s[%u]: %i (%i)", name, sub.multi_id, sub.publication_counter, sub.approx_timestamp_counter); + } + } + }; + PX4_INFO(""); if (!_ekf2_timestamps_exists) { @@ -417,13 +446,13 @@ ReplayEkf2::onExitMainLoop() print_sensor_statistics(_airspeed_msg_id, "airspeed"); print_sensor_statistics(_airspeed_validated_msg_id, "airspeed_validated"); - print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); - print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow"); + print_multi_sensor_statistics(_distance_sensor_msg_ids, "distance_sensor"); + print_multi_sensor_statistics(_optical_flow_msg_ids, "vehicle_optical_flow"); print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data"); print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer"); print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry"); - print_sensor_statistics(_aux_global_position_msg_id, "aux_global_position"); + print_multi_sensor_statistics(_aux_global_position_msg_ids, "aux_global_position"); print_sensor_statistics(_ranging_beacon_msg_id, "ranging_beacon"); if (_ekf2_update_timeouts > 0) { diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index c891bf8eb09..562a9b6ac86 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -118,6 +118,9 @@ private: */ bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file); + /** same for a topic with multiple instances */ + bool findTimestampAndPublish(uint64_t timestamp, const std::vector &msg_ids, std::ifstream &replay_file); + /** * publish the sensor_combined samples preceding @a timestamp, each in its own lockstep cycle * @param timestamp of the ekf2 update these samples precede, in microseconds @@ -129,13 +132,10 @@ private: uint16_t _airspeed_msg_id = msg_id_invalid; uint16_t _airspeed_validated_msg_id = msg_id_invalid; - uint16_t _distance_sensor_msg_id = msg_id_invalid; - uint16_t _optical_flow_msg_id = msg_id_invalid; uint16_t _sensor_combined_msg_id = msg_id_invalid; uint16_t _vehicle_air_data_msg_id = msg_id_invalid; uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid; uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid; - uint16_t _aux_global_position_msg_id = msg_id_invalid; uint16_t _ranging_beacon_msg_id = msg_id_invalid; uint16_t _vehicle_local_position_groundtruth_msg_id = msg_id_invalid; uint16_t _vehicle_global_position_groundtruth_msg_id = msg_id_invalid; @@ -146,6 +146,11 @@ private: uint16_t _sensor_selection_msg_id = msg_id_invalid; uint16_t _launch_detection_status_msg_id = msg_id_invalid; + // multi-instance topics: one msg_id per instance + std::vector _distance_sensor_msg_ids; + std::vector _optical_flow_msg_ids; + std::vector _aux_global_position_msg_ids; + bool _ekf2_timestamps_exists{false}; uint64_t _last_sensor_combined_timestamp{0};