ekf2_replay: log groundtruth messages

PX4 · Dec 6, 2024 · 723fd7d · 723fd7d
1 parent 1782751
commit 723fd7d
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 0 deletions.
diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp
@@ -108,6 +108,15 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
 
 	} else if (sub.orb_meta == ORB_ID(aux_global_position)) {
 		_aux_global_position_msg_id = msg_id;
+
+	} else if (sub.orb_meta == ORB_ID(vehicle_local_position_groundtruth)) {
+		_vehicle_local_position_groundtruth_msg_id = msg_id;
+
+	} else if (sub.orb_meta == ORB_ID(vehicle_attitude_groundtruth)) {
+		_vehicle_attitude_groundtruth_msg_id = msg_id;
+
+	} else if (sub.orb_meta == ORB_ID(vehicle_global_position_groundtruth)) {
+		_vehicle_global_position_groundtruth_msg_id = msg_id;
 	}
 
 	// the main loop should only handle publication of the following topics, the sensor topics are
@@ -135,6 +144,9 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
 	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_sensor_publication(0, _vehicle_local_position_groundtruth_msg_id);
+	handle_sensor_publication(0, _vehicle_global_position_groundtruth_msg_id);
+	handle_sensor_publication(0, _vehicle_attitude_groundtruth_msg_id);
 
 	// sensor_combined: publish last because ekf2 is polling on this
 	if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {

diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp
@@ -89,6 +89,9 @@ class ReplayEkf2 : public Replay
 	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 _vehicle_local_position_groundtruth_msg_id = msg_id_invalid;
+	uint16_t _vehicle_global_position_groundtruth_msg_id = msg_id_invalid;
+	uint16_t _vehicle_attitude_groundtruth_msg_id = msg_id_invalid;
 };
 
 } //namespace px4