Switch aux_global_position to AuxGlobalPosition message type

px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp

@@ -14,7 +14,7 @@ using namespace std::chrono_literals;  // NOLINT
   {"fmu/in/actuator_motors"}, \
   {"fmu/in/actuator_servos"}, \
   {"fmu/in/arming_check_reply"}, \
-  {"fmu/in/aux_global_position", "VehicleGlobalPosition"}, \
+  {"fmu/in/aux_global_position"}, \
   {"fmu/in/config_control_setpoints", "VehicleControlMode"}, \
   {"fmu/in/config_overrides_request", "ConfigOverrides"}, \
   {"fmu/in/fixed_wing_lateral_setpoint"}, \

px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_position_measurement_interface.hpp

@@ -7,7 +7,7 @@
 
 #include <Eigen/Eigen>
 #include <optional>
-#include <px4_msgs/msg/vehicle_global_position.hpp>
+#include <px4_msgs/msg/aux_global_position.hpp>
 #include <px4_ros2/navigation/experimental/navigation_interface_base.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -16,6 +16,19 @@ namespace px4_ros2 {
  *  @{
  */
 
+/**
+ * @brief Source type of the global position data.
+ */
+enum class GlobalPositionSource : uint8_t {
+  Unknown = px4_msgs::msg::AuxGlobalPosition::SOURCE_UNKNOWN,
+  GNSS = px4_msgs::msg::AuxGlobalPosition::SOURCE_GNSS,
+  Vision = px4_msgs::msg::AuxGlobalPosition::SOURCE_VISION,
+  Pseudolites = px4_msgs::msg::AuxGlobalPosition::SOURCE_PSEUDOLITES,
+  Terrain = px4_msgs::msg::AuxGlobalPosition::SOURCE_TERRAIN,
+  Magnetic = px4_msgs::msg::AuxGlobalPosition::SOURCE_MAGNETIC,
+  Estimator = px4_msgs::msg::AuxGlobalPosition::SOURCE_ESTIMATOR,
+};
+
 /**
  * @struct GlobalPositionMeasurement
  * @brief Represents a global position measurement to be passed to
@@ -46,8 +59,15 @@ struct GlobalPositionMeasurement {
  */
 class GlobalPositionMeasurementInterface : public PositionMeasurementInterfaceBase {
  public:
-  explicit GlobalPositionMeasurementInterface(rclcpp::Node& node,
-                                              std::string topic_namespace_prefix = "");
+  /**
+   * @param id Unique identifier non-zero for this position source. PX4 uses this to demultiplex
+   *   measurements from multiple external positioning sources on the same topic.
+   * @param source Source type of the position data. Defaults to GlobalPositionSource::Vision.
+   */
+  explicit GlobalPositionMeasurementInterface(
+      rclcpp::Node& node, uint8_t id = 1,
+      GlobalPositionSource source = GlobalPositionSource::Vision,
+      std::string topic_namespace_prefix = "");
   ~GlobalPositionMeasurementInterface() override = default;
 
   /**
@@ -94,11 +114,12 @@ class GlobalPositionMeasurementInterface : public PositionMeasurementInterfaceBa
    */
   bool isValueNotNAN(const GlobalPositionMeasurement& measurement) const;
 
-  rclcpp::Publisher<px4_msgs::msg::VehicleGlobalPosition>::SharedPtr _aux_global_position_pub;
+  rclcpp::Publisher<px4_msgs::msg::AuxGlobalPosition>::SharedPtr _aux_global_position_pub;
 
+  const uint8_t _id;
+  const GlobalPositionSource _source;
   uint8_t _lat_lon_reset_counter{
       0}; /** Counter for reset events on horizontal position coordinates */
-  // uint8_t _altitude_frame;
 };
 
 /** @}*/

px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp

@@ -7,17 +7,19 @@
 #include <px4_ros2/utils/message_version.hpp>
 
 using Eigen::Vector2d;
-using px4_msgs::msg::VehicleGlobalPosition;
+using px4_msgs::msg::AuxGlobalPosition;
 
 namespace px4_ros2 {
 
 GlobalPositionMeasurementInterface::GlobalPositionMeasurementInterface(
-    rclcpp::Node& node, std::string topic_namespace_prefix)
-    : PositionMeasurementInterfaceBase(node, std::move(topic_namespace_prefix))
+    rclcpp::Node& node, uint8_t id, GlobalPositionSource source, std::string topic_namespace_prefix)
+    : PositionMeasurementInterfaceBase(node, std::move(topic_namespace_prefix)),
+      _id(id),
+      _source(source)
 {
-  _aux_global_position_pub = node.create_publisher<VehicleGlobalPosition>(
+  _aux_global_position_pub = node.create_publisher<AuxGlobalPosition>(
       topicNamespacePrefix() + "fmu/in/aux_global_position" +
-          px4_ros2::getMessageNameVersion<VehicleGlobalPosition>(),
+          px4_ros2::getMessageNameVersion<AuxGlobalPosition>(),
       10);
 }
 
@@ -42,7 +44,9 @@ void GlobalPositionMeasurementInterface::update(
   }
 
   // Populate aux global position
-  VehicleGlobalPosition aux_global_position;
+  AuxGlobalPosition aux_global_position;
+  aux_global_position.id = _id;
+  aux_global_position.source = static_cast<uint8_t>(_source);
   aux_global_position.lat_lon_reset_counter = _lat_lon_reset_counter;
 
   aux_global_position.timestamp_sample =

px4_ros2_cpp/test/unit/global_navigation.cpp

@@ -41,12 +41,11 @@ class GlobalPositionInterfaceTest : public testing::Test {
 
     _global_navigation_interface =
         std::make_shared<px4_ros2::GlobalPositionMeasurementInterface>(*_node);
-    _subscriber = _node->create_subscription<px4_msgs::msg::VehicleGlobalPosition>(
+    _subscriber = _node->create_subscription<px4_msgs::msg::AuxGlobalPosition>(
         "/fmu/in/aux_global_position" +
-            px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleGlobalPosition>(),
-        rclcpp::QoS(10).best_effort(), [this](px4_msgs::msg::VehicleGlobalPosition::UniquePtr msg) {
-          _update_msg = std::move(msg);
-        });
+            px4_ros2::getMessageNameVersion<px4_msgs::msg::AuxGlobalPosition>(),
+        rclcpp::QoS(10).best_effort(),
+        [this](px4_msgs::msg::AuxGlobalPosition::UniquePtr msg) { _update_msg = std::move(msg); });
   }
 
   bool waitForUpdate()
@@ -67,8 +66,8 @@ class GlobalPositionInterfaceTest : public testing::Test {
   std::shared_ptr<rclcpp::Node> _node;
   rclcpp::executors::SingleThreadedExecutor _executor;
   std::shared_ptr<px4_ros2::GlobalPositionMeasurementInterface> _global_navigation_interface;
-  rclcpp::Subscription<px4_msgs::msg::VehicleGlobalPosition>::SharedPtr _subscriber;
-  px4_msgs::msg::VehicleGlobalPosition::UniquePtr _update_msg;
+  rclcpp::Subscription<px4_msgs::msg::AuxGlobalPosition>::SharedPtr _subscriber;
+  px4_msgs::msg::AuxGlobalPosition::UniquePtr _update_msg;
 
   static constexpr std::chrono::seconds kTimeoutDuration{3s};
   static constexpr std::chrono::milliseconds kSleepInterval{10ms};