apply version suffix to fmu topics

px4_ros2_cpp/CMakeLists.txt

@@ -49,6 +49,7 @@ set(HEADER_FILES
         include/px4_ros2/utils/frame_conversion.hpp
         include/px4_ros2/utils/geodesic.hpp
         include/px4_ros2/utils/geometry.hpp
+        include/px4_ros2/utils/message_version.hpp
         include/px4_ros2/vehicle_state/battery.hpp
         include/px4_ros2/vehicle_state/home_position.hpp
         include/px4_ros2/vehicle_state/land_detected.hpp

px4_ros2_cpp/include/px4_ros2/components/mode.hpp

@@ -162,7 +162,10 @@ class ModeBase : public Context
   RequirementFlags & modeRequirements() {return _health_and_arming_checks.modeRequirements();}
 
 protected:
-  void setSkipMessageCompatibilityCheck() {_skip_message_compatibility_check = true;}
+  void setSkipMessageCompatibilityCheck(bool skip = true)
+  {
+    _skip_message_compatibility_check = skip;
+  }
   void overrideRegistration(const std::shared_ptr<Registration> & registration);
 
 private:
@@ -191,7 +194,7 @@ class ModeBase : public Context
   std::shared_ptr<Registration> _registration;
 
   const Settings _settings;
-  bool _skip_message_compatibility_check{false};
+  bool _skip_message_compatibility_check{true};
 
   HealthAndArmingChecks _health_and_arming_checks;
 

px4_ros2_cpp/include/px4_ros2/utils/message_version.hpp

@@ -0,0 +1,53 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+/**
+ * @defgroup message_version Message Version
+ * @ingroup utils
+ * This group contains helper functions to handle message versioning.
+ */
+
+#pragma once
+
+#include <string>
+#include <type_traits>
+
+namespace px4_ros2
+{
+
+/**
+ * @brief Trait to check if a message type `T` has a `MESSAGE_VERSION` constant.
+ *
+ * If `T` has `MESSAGE_VERSION`, `HasMessageVersion<T>::value` is `true`;
+ * otherwise it is `false`.
+ *
+ * @tparam T The message type to check.
+ */
+template<typename T, typename = void>
+struct HasMessageVersion : std::false_type {};
+
+/// Specialization for types that have a `MESSAGE_VERSION` constant.
+template<typename T>
+struct HasMessageVersion<T, std::void_t<decltype(T::MESSAGE_VERSION)>>: std::true_type {};
+
+/**
+ * @brief Retrieves the version suffix for a given message type.
+ *
+ * @tparam T The message type, which may or may not define a `MESSAGE_VERSION` constant.
+ * @return std::string The version suffix (e.g., "_v1") or an empty string if `MESSAGE_VERSION` is `0` or undefined.
+ */
+template<typename T>
+std::string getMessageNameVersion()
+{
+  if constexpr (HasMessageVersion<T>::value) {
+    if (T::MESSAGE_VERSION == 0) {return "";}
+    return "_v" + std::to_string(T::MESSAGE_VERSION);
+  } else {
+    return "";
+  }
+}
+
+
+} // namespace px4_ros2

px4_ros2_cpp/include/px4_ros2/vehicle_state/battery.hpp

@@ -9,6 +9,7 @@
 #include <px4_msgs/msg/battery_status.hpp>
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/utils/subscription.hpp>
+#include <px4_ros2/utils/message_version.hpp>
 
 namespace px4_ros2
 {
@@ -25,7 +26,8 @@ class Battery : public Subscription<px4_msgs::msg::BatteryStatus>
 {
 public:
   explicit Battery(Context & context)
-  : Subscription<px4_msgs::msg::BatteryStatus>(context, "fmu/out/battery_status") {}
+  : Subscription<px4_msgs::msg::BatteryStatus>(context,
+      "fmu/out/battery_status" + px4_ros2::getMessageNameVersion<px4_msgs::msg::BatteryStatus>()) {}
 
   /**
    * @brief Get the vehicle's battery voltage.

px4_ros2_cpp/include/px4_ros2/vehicle_state/home_position.hpp

@@ -9,6 +9,7 @@
 #include <px4_msgs/msg/home_position.hpp>
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/utils/subscription.hpp>
+#include <px4_ros2/utils/message_version.hpp>
 
 namespace px4_ros2
 {
@@ -25,7 +26,8 @@ class HomePosition : public Subscription<px4_msgs::msg::HomePosition>
 {
 public:
   explicit HomePosition(Context & context)
-  : Subscription<px4_msgs::msg::HomePosition>(context, "fmu/out/home_position") {}
+  : Subscription<px4_msgs::msg::HomePosition>(context,
+      "fmu/out/home_position" + +px4_ros2::getMessageNameVersion<px4_msgs::msg::HomePosition>()) {}
 
   /**
    * @brief Get the vehicle's home position in local coordinates.

px4_ros2_cpp/include/px4_ros2/vehicle_state/land_detected.hpp

@@ -8,6 +8,7 @@
 #include <px4_msgs/msg/vehicle_land_detected.hpp>
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/utils/subscription.hpp>
+#include <px4_ros2/utils/message_version.hpp>
 
 namespace px4_ros2
 {
@@ -24,7 +25,9 @@ class LandDetected : public Subscription<px4_msgs::msg::VehicleLandDetected>
 {
 public:
   explicit LandDetected(Context & context)
-  : Subscription<px4_msgs::msg::VehicleLandDetected>(context, "fmu/out/vehicle_land_detected") {}
+  : Subscription<px4_msgs::msg::VehicleLandDetected>(context,
+      "fmu/out/vehicle_land_detected" +
+      px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleLandDetected>()) {}
 
   /**
    * @brief Check if vehicle is landed on the ground.

px4_ros2_cpp/include/px4_ros2/vehicle_state/vehicle_status.hpp

@@ -8,6 +8,7 @@
 #include <px4_msgs/msg/vehicle_status.hpp>
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/utils/subscription.hpp>
+#include <px4_ros2/utils/message_version.hpp>
 
 namespace px4_ros2
 {
@@ -24,7 +25,8 @@ class VehicleStatus : public Subscription<px4_msgs::msg::VehicleStatus>
 {
 public:
   explicit VehicleStatus(Context & context)
-  : Subscription<px4_msgs::msg::VehicleStatus>(context, "fmu/out/vehicle_status") {}
+  : Subscription<px4_msgs::msg::VehicleStatus>(context,
+      "fmu/out/vehicle_status" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleStatus>()) {}
 
   /**
    * @brief Get the vehicle's arming status.

px4_ros2_cpp/include/px4_ros2/vehicle_state/vtol_status.hpp

@@ -8,6 +8,7 @@
 #include <px4_msgs/msg/vtol_vehicle_status.hpp>
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/utils/subscription.hpp>
+#include <px4_ros2/utils/message_version.hpp>
 
 namespace px4_ros2
 {
@@ -24,7 +25,9 @@ class VtolStatus : public Subscription<px4_msgs::msg::VtolVehicleStatus>
 {
 public:
   explicit VtolStatus(Context & context)
-  : Subscription<px4_msgs::msg::VtolVehicleStatus>(context, "fmu/out/vtol_vehicle_status") {}
+  : Subscription<px4_msgs::msg::VtolVehicleStatus>(context,
+      "fmu/out/vtol_vehicle_status" +
+      px4_ros2::getMessageNameVersion<px4_msgs::msg::VtolVehicleStatus>()) {}
 
   /**
    * @brief Check if vehicle is in an undefined state. This indicates the vehicle is not a VTOL.

px4_ros2_cpp/src/components/health_and_arming_checks.cpp

@@ -5,6 +5,7 @@
 
 #include "registration.hpp"
 #include "px4_ros2/components/health_and_arming_checks.hpp"
+#include "px4_ros2/utils/message_version.hpp"
 
 #include <cassert>
 #include <utility>
@@ -21,10 +22,11 @@ HealthAndArmingChecks::HealthAndArmingChecks(
   _check_callback(std::move(check_callback))
 {
   _arming_check_reply_pub = _node.create_publisher<px4_msgs::msg::ArmingCheckReply>(
-    topic_namespace_prefix + "fmu/in/arming_check_reply", 1);
+    topic_namespace_prefix + "fmu/in/arming_check_reply" + px4_ros2::getMessageNameVersion<px4_msgs::msg::ArmingCheckReply>(),
+    1);
 
   _arming_check_request_sub = _node.create_subscription<px4_msgs::msg::ArmingCheckRequest>(
-    topic_namespace_prefix + "fmu/out/arming_check_request",
+    topic_namespace_prefix + "fmu/out/arming_check_request" + px4_ros2::getMessageNameVersion<px4_msgs::msg::ArmingCheckRequest>(),
     rclcpp::QoS(1).best_effort(),
     [this](px4_msgs::msg::ArmingCheckRequest::UniquePtr msg) {
 

px4_ros2_cpp/src/components/manual_control_input.cpp

@@ -5,6 +5,7 @@
 
 #include "px4_ros2/components/manual_control_input.hpp"
 #include "px4_ros2/components/mode.hpp"
+#include "px4_ros2/utils/message_version.hpp"
 
 namespace px4_ros2
 {
@@ -16,7 +17,7 @@ ManualControlInput::ManualControlInput(Context & context, bool is_optional)
 
   _manual_control_setpoint_sub =
     context.node().create_subscription<px4_msgs::msg::ManualControlSetpoint>(
-    context.topicNamespacePrefix() + "fmu/out/manual_control_setpoint", rclcpp::QoS(
+    context.topicNamespacePrefix() + "fmu/out/manual_control_setpoint" + px4_ros2::getMessageNameVersion<px4_msgs::msg::ManualControlSetpoint>(), rclcpp::QoS(
       1).best_effort(),
     [this](px4_msgs::msg::ManualControlSetpoint::UniquePtr msg) {
       _manual_control_setpoint = *msg;

px4_ros2_cpp/src/components/message_compatibility_check.cpp

@@ -6,6 +6,7 @@
 #include "px4_ros2/components/message_compatibility_check.hpp"
 #include <px4_msgs/msg/message_format_request.hpp>
 #include <px4_msgs/msg/message_format_response.hpp>
+#include <px4_ros2/utils/message_version.hpp>
 
 #include <string>
 #include <vector>
@@ -229,13 +230,15 @@ bool messageCompatibilityCheck(
     message_format_response_sub
     =
     node.create_subscription<px4_msgs::msg::MessageFormatResponse>(
-      topic_namespace_prefix + "fmu/out/message_format_response", rclcpp::QoS(1).best_effort(),
+      topic_namespace_prefix + "fmu/out/message_format_response" + px4_ros2::getMessageNameVersion<px4_msgs::msg::MessageFormatResponse>(), rclcpp::QoS(
+        1).best_effort(),
       [](px4_msgs::msg::MessageFormatResponse::UniquePtr msg) {});
 
   const rclcpp::Publisher<px4_msgs::msg::MessageFormatRequest>::SharedPtr message_format_request_pub
     =
     node.create_publisher<px4_msgs::msg::MessageFormatRequest>(
-      topic_namespace_prefix + "fmu/in/message_format_request", 1);
+      topic_namespace_prefix + "fmu/in/message_format_request" + px4_ros2::getMessageNameVersion<px4_msgs::msg::MessageFormatRequest>(),
+      1);
 
   const std::string msgs_dir = ament_index_cpp::get_package_share_directory("px4_msgs");
   if (msgs_dir.empty()) {

px4_ros2_cpp/src/components/mode.cpp

@@ -6,6 +6,7 @@
 #include "px4_ros2/components/mode.hpp"
 #include "px4_ros2/components/message_compatibility_check.hpp"
 #include "px4_ros2/components/wait_for_fmu.hpp"
+#include "px4_ros2/utils/message_version.hpp"
 
 #include "registration.hpp"
 
@@ -28,16 +29,19 @@ ModeBase::ModeBase(
     topic_namespace_prefix), _config_overrides(node, topic_namespace_prefix)
 {
   _vehicle_status_sub = node.create_subscription<px4_msgs::msg::VehicleStatus>(
-    topic_namespace_prefix + "fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+    topic_namespace_prefix + "fmu/out/vehicle_status" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleStatus>(), rclcpp::QoS(
+      1).best_effort(),
     [this](px4_msgs::msg::VehicleStatus::UniquePtr msg) {
       if (_registration->registered()) {
         vehicleStatusUpdated(msg);
       }
     });
   _mode_completed_pub = node.create_publisher<px4_msgs::msg::ModeCompleted>(
-    topic_namespace_prefix + "fmu/in/mode_completed", 1);
+    topic_namespace_prefix + "fmu/in/mode_completed" + px4_ros2::getMessageNameVersion<px4_msgs::msg::ModeCompleted>(),
+    1);
   _config_control_setpoints_pub = node.create_publisher<px4_msgs::msg::VehicleControlMode>(
-    topic_namespace_prefix + "fmu/in/config_control_setpoints", 1);
+    topic_namespace_prefix + "fmu/in/config_control_setpoints" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleControlMode>(),
+    1);
 }
 
 ModeBase::ModeID ModeBase::id() const

px4_ros2_cpp/src/components/mode_executor.cpp

@@ -6,6 +6,7 @@
 #include "px4_ros2/components/mode_executor.hpp"
 #include "px4_ros2/components/message_compatibility_check.hpp"
 #include "px4_ros2/components/wait_for_fmu.hpp"
+#include "px4_ros2/utils/message_version.hpp"
 
 #include "registration.hpp"
 
@@ -26,15 +27,17 @@ ModeExecutorBase::ModeExecutorBase(
   _config_overrides(node, topic_namespace_prefix)
 {
   _vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
-    topic_namespace_prefix + "fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+    topic_namespace_prefix + "fmu/out/vehicle_status" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleStatus>(), rclcpp::QoS(
+      1).best_effort(),
     [this](px4_msgs::msg::VehicleStatus::UniquePtr msg) {
       if (_registration->registered()) {
         vehicleStatusUpdated(msg);
       }
     });
 
   _vehicle_command_pub = _node.create_publisher<px4_msgs::msg::VehicleCommand>(
-    topic_namespace_prefix + "fmu/in/vehicle_command_mode_executor", 1);
+    topic_namespace_prefix + "fmu/in/vehicle_command_mode_executor" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleCommand>(),
+    1);
 
 }
 
@@ -127,7 +130,7 @@ Result ModeExecutorBase::sendCommandSync(
   // (We could also use exchange_in_use_by_wait_set_state(), but that might cause an
   // inconsistent state)
   const auto vehicle_command_ack_sub = _node.create_subscription<px4_msgs::msg::VehicleCommandAck>(
-    _topic_namespace_prefix + "fmu/out/vehicle_command_ack", rclcpp::QoS(1).best_effort(),
+    _topic_namespace_prefix + "fmu/out/vehicle_command_ack" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleCommandAck>(), rclcpp::QoS(1).best_effort(),
     [](px4_msgs::msg::VehicleCommandAck::UniquePtr msg) {});
 
   // Wait until we have a publisher
@@ -430,7 +433,8 @@ ModeExecutorBase::ScheduledMode::ScheduledMode(
   const std::string & topic_namespace_prefix)
 {
   _mode_completed_sub = node.create_subscription<px4_msgs::msg::ModeCompleted>(
-    topic_namespace_prefix + "fmu/out/mode_completed", rclcpp::QoS(1).best_effort(),
+    topic_namespace_prefix + "fmu/out/mode_completed" + px4_ros2::getMessageNameVersion<px4_msgs::msg::ModeCompleted>(), rclcpp::QoS(
+      1).best_effort(),
     [this, &node](px4_msgs::msg::ModeCompleted::UniquePtr msg) {
       if (active() && msg->nav_state == static_cast<uint8_t>(_mode_id)) {
         RCLCPP_DEBUG(

px4_ros2_cpp/src/components/overrides.cpp

@@ -4,6 +4,7 @@
  ****************************************************************************/
 
 #include "px4_ros2/components/overrides.hpp"
+#include "px4_ros2/utils/message_version.hpp"
 
 #include <cassert>
 
@@ -14,7 +15,8 @@ ConfigOverrides::ConfigOverrides(rclcpp::Node & node, const std::string & topic_
 : _node(node)
 {
   _config_overrides_pub = _node.create_publisher<px4_msgs::msg::ConfigOverrides>(
-    topic_namespace_prefix + "fmu/in/config_overrides_request", 1);
+    topic_namespace_prefix + "fmu/in/config_overrides_request" + px4_ros2::getMessageNameVersion<px4_msgs::msg::ConfigOverrides>(),
+    1);
 }
 
 void ConfigOverrides::controlAutoDisarm(bool enabled)

px4_ros2_cpp/src/components/registration.cpp

@@ -8,6 +8,7 @@
 #include <cassert>
 #include <random>
 #include <unistd.h>
+#include <px4_ros2/utils/message_version.hpp>
 
 static constexpr uint16_t kLatestPX4ROS2ApiVersion = 1;
 
@@ -18,17 +19,19 @@ Registration::Registration(rclcpp::Node & node, const std::string & topic_namesp
 {
   _register_ext_component_reply_sub =
     node.create_subscription<px4_msgs::msg::RegisterExtComponentReply>(
-    topic_namespace_prefix + "fmu/out/register_ext_component_reply",
+    topic_namespace_prefix + "fmu/out/register_ext_component_reply" + px4_ros2::getMessageNameVersion<px4_msgs::msg::RegisterExtComponentReply>(),
     rclcpp::QoS(1).best_effort(),
     [](px4_msgs::msg::RegisterExtComponentReply::UniquePtr msg) {
     });
 
   _register_ext_component_request_pub =
     node.create_publisher<px4_msgs::msg::RegisterExtComponentRequest>(
-    topic_namespace_prefix + "fmu/in/register_ext_component_request", 1);
+    topic_namespace_prefix + "fmu/in/register_ext_component_request" + px4_ros2::getMessageNameVersion<px4_msgs::msg::RegisterExtComponentRequest>(),
+    1);
 
   _unregister_ext_component_pub = node.create_publisher<px4_msgs::msg::UnregisterExtComponent>(
-    topic_namespace_prefix + "fmu/in/unregister_ext_component", 1);
+    topic_namespace_prefix + "fmu/in/unregister_ext_component" + px4_ros2::getMessageNameVersion<px4_msgs::msg::UnregisterExtComponent>(),
+    1);
 
   _unregister_ext_component.mode_id = px4_ros2::ModeBase::kModeIDInvalid;
 }

px4_ros2_cpp/src/components/wait_for_fmu.cpp

@@ -5,6 +5,7 @@
 
 #include <px4_ros2/components/wait_for_fmu.hpp>
 #include <px4_msgs/msg/vehicle_status.hpp>
+#include <px4_ros2/utils/message_version.hpp>
 
 namespace px4_ros2
 {
@@ -16,7 +17,8 @@ bool waitForFMU(
   RCLCPP_DEBUG(node.get_logger(), "Waiting for FMU...");
   const rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub =
     node.create_subscription<px4_msgs::msg::VehicleStatus>(
-    topic_namespace_prefix + "fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+    topic_namespace_prefix + "fmu/out/vehicle_status" + px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleStatus>(), rclcpp::QoS(
+      1).best_effort(),
     [](px4_msgs::msg::VehicleStatus::UniquePtr msg) {});
 
   rclcpp::WaitSet wait_set;