ekf2/odometry: add VehicleOdometry acceleration and ROS 2 translation

- extend VehicleOdometry.msg to v1 with float32[3] acceleration in NED, defined as the derivative of NED velocity averaged over the odometry publish interval
- read _ekf.getVelocityDerivative() once in EKF2::Run(), pass it to PublishLocalPosition() and PublishOdometry(), and reset the accumulation once after both publishers consume it
- archive VehicleOdometryV0.msg in px4_msgs_old and add VehicleOdometryV1Translation to translation_node
- preserve ROS 2 interoperability between v0 and v1 clients: v0 -> v1 fills acceleration with NaN, and v1 -> v0 drops it

This follows the review direction to extend VehicleOdometry instead of adding VehicleFullState, and avoids mixed-rate aliasing or double consumption of the velocity derivative.

Tested:
- make px4_sitl_default -j4
- colcon build --packages-select px4_msgs
- colcon build --packages-select px4_msgs_old translation_node
- colcon test --packages-select translation_node --ctest-args -R translation_node_unit_tests

Signed-off-by: evannsmc <evannsmcuadrado@gmail.com>

Author: evannsmc

msg/px4_msgs_old/msg/VehicleOdometryV0.msg

@@ -0,0 +1,35 @@
+# Vehicle odometry data
+#
+# Fits ROS REP 147 for aerial vehicles
+
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp         # [us] Time since system start
+uint64 timestamp_sample  # [us] Timestamp sample
+
+uint8 pose_frame              # [@enum POSE_FRAME] Position and orientation frame of reference
+uint8 POSE_FRAME_UNKNOWN = 0  # Unknown frame
+uint8 POSE_FRAME_NED     = 1  # North-East-Down (NED) navigation frame. Aligned with True North.
+uint8 POSE_FRAME_FRD     = 2  # Forward-Right-Down (FRD) frame. Constant arbitrary heading offset from True North. Z is down.
+
+float32[3] position  # [m] [@frame local frame] [@invalid NaN If invalid/unknown] Position. Origin is position of GC at startup.
+float32[4] q         # [-] [@invalid NaN First value if invalid/unknown] Attitude (expressed as a quaternion) relative to pose reference frame at current location. Follows the Hamiltonian convention (w, x, y, z, right-handed, passive rotations from body to world)
+
+uint8 velocity_frame               # [@enum VELOCITY_FRAME] Reference frame of the velocity data
+uint8 VELOCITY_FRAME_UNKNOWN  = 0  # Unknown frame
+uint8 VELOCITY_FRAME_NED      = 1  # NED navigation frame at current position.
+uint8 VELOCITY_FRAME_FRD      = 2  # FRD navigation frame at current position. Constant arbitrary heading offset from True North. Z is down.
+uint8 VELOCITY_FRAME_BODY_FRD = 3  # FRD body-fixed frame
+
+float32[3] velocity          # [m/s] [@frame @velocity_frame] [@invalid NaN If invalid/unknown] Velocity.
+float32[3] angular_velocity  # [rad/s] [@frame @VELOCITY_FRAME_BODY_FRD] [@invalid NaN If invalid/unknown] Angular velocity in body-fixed frame
+
+float32[3] position_variance     # [m^2] Variance of position error
+float32[3] orientation_variance  # [rad^2] Variance of orientation/attitude error (expressed in body frame)
+float32[3] velocity_variance     # [m^2/s^2] Variance of velocity error
+
+uint8 reset_counter  # [-] Reset counter. Counts reset events on attitude, velocity and position.
+int8 quality         # [-] [@invalid 0] Quality. Unused.
+
+# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
+# TOPICS estimator_odometry

msg/translation_node/translations/all_translations.h

@@ -19,6 +19,7 @@
 #include "translation_vehicle_attitude_setpoint_v1.h"
 #include "translation_vehicle_command_ack_v1.h"
 #include "translation_vehicle_local_position_v1.h"
+#include "translation_vehicle_odometry_v1.h"
 #include "translation_vehicle_status_v1.h"
 #include "translation_vehicle_status_v2.h"
 #include "translation_vehicle_status_v3.h"

msg/translation_node/translations/translation_vehicle_odometry_v1.h

@@ -0,0 +1,54 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate VehicleOdometry v0 <--> v1
+#include <limits>
+#include <px4_msgs_old/msg/vehicle_odometry_v0.hpp>
+#include <px4_msgs/msg/vehicle_odometry.hpp>
+
+class VehicleOdometryV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::VehicleOdometryV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::VehicleOdometry;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/out/vehicle_odometry";
+
+#define COMMON_VEHICLE_ODOMETRY_FIELDS(FUNCTION) \
+	FUNCTION(timestamp) \
+	FUNCTION(timestamp_sample) \
+	FUNCTION(pose_frame) \
+	FUNCTION(position) \
+	FUNCTION(q) \
+	FUNCTION(velocity_frame) \
+	FUNCTION(velocity) \
+	FUNCTION(angular_velocity) \
+	FUNCTION(position_variance) \
+	FUNCTION(orientation_variance) \
+	FUNCTION(velocity_variance) \
+	FUNCTION(reset_counter) \
+	FUNCTION(quality)
+
+#define COPY_FIELD_FROM_OLDER(field) msg_newer.field = msg_older.field;
+#define COPY_FIELD_TO_OLDER(field) msg_older.field = msg_newer.field;
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		COMMON_VEHICLE_ODOMETRY_FIELDS(COPY_FIELD_FROM_OLDER)
+		msg_newer.acceleration.fill(std::numeric_limits<float>::quiet_NaN());
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		COMMON_VEHICLE_ODOMETRY_FIELDS(COPY_FIELD_TO_OLDER)
+	}
+
+#undef COPY_FIELD_FROM_OLDER
+#undef COPY_FIELD_TO_OLDER
+#undef COMMON_VEHICLE_ODOMETRY_FIELDS
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleOdometryV1Translation);

msg/versioned/VehicleOdometry.msg

@@ -2,7 +2,7 @@
 #
 # Fits ROS REP 147 for aerial vehicles
 
-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1
 
 uint64 timestamp         # [us] Time since system start
 uint64 timestamp_sample  # [us] Timestamp sample
@@ -22,6 +22,7 @@ uint8 VELOCITY_FRAME_FRD      = 2  # FRD navigation frame at current position. C
 uint8 VELOCITY_FRAME_BODY_FRD = 3  # FRD body-fixed frame
 
 float32[3] velocity          # [m/s] [@frame @velocity_frame] [@invalid NaN If invalid/unknown] Velocity.
+float32[3] acceleration      # [m/s^2] [@frame local frame] [@invalid NaN If invalid/unknown] Acceleration of body origin in local NED frame.
 float32[3] angular_velocity  # [rad/s] [@frame @VELOCITY_FRAME_BODY_FRD] [@invalid NaN If invalid/unknown] Angular velocity in body-fixed frame
 
 float32[3] position_variance     # [m^2] Variance of position error

src/modules/ekf2/EKF2.cpp

@@ -815,8 +815,10 @@ void EKF2::Run()
 		if (_ekf.update()) {
 			perf_set_elapsed(_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
 
-			PublishLocalPosition(now);
-			PublishOdometry(now, imu_sample_new);
+			const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
+			PublishLocalPosition(now, vel_deriv);
+			PublishOdometry(now, imu_sample_new, vel_deriv);
+			_ekf.resetVelocityDerivativeAccumulation();
 			PublishGlobalPosition(now);
 			PublishSensorBias(now);
 
@@ -1580,7 +1582,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_estimator_innovation_variances_pub.publish(variances);
 }
 
-void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
+void EKF2::PublishLocalPosition(const hrt_abstime &timestamp, const Vector3f &vel_deriv)
 {
 	vehicle_local_position_s lpos{};
 	// generate vehicle local position data
@@ -1602,8 +1604,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.z_deriv = _ekf.getVerticalPositionDerivative();
 
 	// Acceleration of body origin in local frame
-	const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
-	_ekf.resetVelocityDerivativeAccumulation();
 	lpos.ax = vel_deriv(0);
 	lpos.ay = vel_deriv(1);
 	lpos.az = vel_deriv(2);
@@ -1703,7 +1703,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	_local_position_pub.publish(lpos);
 }
 
-void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample)
+void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample, const Vector3f &vel_deriv)
 {
 	// generate vehicle odometry data
 	vehicle_odometry_s odom;
@@ -1720,6 +1720,9 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
 	odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
 	_ekf.getVelocity().copyTo(odom.velocity);
 
+	// acceleration
+	vel_deriv.copyTo(odom.acceleration);
+
 	// angular_velocity
 	_ekf.getAngularVelocityAndResetAccumulator().copyTo(odom.angular_velocity);
 

src/modules/ekf2/EKF2.hpp

@@ -192,8 +192,8 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	void PublishInnovations(const hrt_abstime &timestamp);
 	void PublishInnovationTestRatios(const hrt_abstime &timestamp);
 	void PublishInnovationVariances(const hrt_abstime &timestamp);
-	void PublishLocalPosition(const hrt_abstime &timestamp);
-	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample);
+	void PublishLocalPosition(const hrt_abstime &timestamp, const matrix::Vector3f &vel_deriv);
+	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample, const matrix::Vector3f &vel_deriv);
 	void PublishSensorBias(const hrt_abstime &timestamp);
 	void PublishStates(const hrt_abstime &timestamp);
 	void PublishStatus(const hrt_abstime &timestamp);