odometry: use subscription class for global and local position APIs

Auterion · Dec 14, 2023 · 2463ae8 · 2463ae8
1 parent e46b33c
commit 2463ae8
Show file tree

Hide file tree

Showing 5 changed files with 55 additions and 39 deletions.
diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp
@@ -5,11 +5,10 @@
 
 #pragma once
 
-#include <px4_msgs/msg/vehicle_global_position.hpp>
-#include <rclcpp/rclcpp.hpp>
 #include <Eigen/Eigen>
+#include <px4_msgs/msg/vehicle_global_position.hpp>
 #include <px4_ros2/common/context.hpp>
-
+#include <px4_ros2/odometry/subscription.hpp>
 
 using namespace std::chrono_literals; // NOLINT
 
@@ -40,7 +39,7 @@ class OdometryGlobalPosition
 
 private:
   rclcpp::Node & _node;
-  rclcpp::Subscription<px4_msgs::msg::VehicleGlobalPosition>::SharedPtr _vehicle_global_position_sub;
+  Subscription<px4_msgs::msg::VehicleGlobalPosition> _vehicle_global_position_sub;
   px4_msgs::msg::VehicleGlobalPosition _vehicle_global_position;
   rclcpp::Time _last_vehicle_global_position;
 };

diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp
@@ -5,13 +5,11 @@
 
 #pragma once
 
-#include <px4_msgs/msg/vehicle_local_position.hpp>
-#include <rclcpp/rclcpp.hpp>
 #include <Eigen/Eigen>
+#include <px4_msgs/msg/vehicle_local_position.hpp>
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/odometry/subscription.hpp>
 
-
 namespace px4_ros2
 {
 /** \ingroup odometry
@@ -61,7 +59,7 @@ class OdometryLocalPosition
   }
 
 private:
-  rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr _vehicle_local_position_sub;
+  Subscription<px4_msgs::msg::VehicleLocalPosition> _vehicle_local_position_sub;
   px4_msgs::msg::VehicleLocalPosition _vehicle_local_position;
 };
 

diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp
@@ -7,45 +7,68 @@
 
 #include <px4_ros2/common/context.hpp>
 
-namespace px4_ros2 {
+namespace px4_ros2
+{
+/** \ingroup odometry
+ *  @{
+ */
 
-template <typename RosMessageType> class Subscription {
+/**
+ * Provides a subscription to arbitrary ROS topics.
+ */
+template<typename RosMessageType>
+class Subscription
+{
 public:
-  Subscription(Context &context, const std::string &topic)
-      : _node(context.node()), _topic(context.topicNamespacePrefix() + topic) {
+  Subscription(Context & context, const std::string & topic)
+  : _node(context.node()), _topic(context.topicNamespacePrefix() + topic)
+  {
     _subscription = _node.create_subscription<RosMessageType>(
-        _topic, rclcpp::QoS(1).best_effort(),
-        [this](const typename RosMessageType::UniquePtr msg) {
-          _last = *msg;
-          _last_message_time = _node.get_clock()->now();
-          for (const auto& callback : _callbacks) {
-            callback(_last);
-          }
-        });
+      _topic, rclcpp::QoS(1).best_effort(),
+      [this](const typename RosMessageType::UniquePtr msg) {
+        _last = *msg;
+        _last_message_time = _node.get_clock()->now();
+        for (const auto & callback : _callbacks) {
+          callback(_last);
+        }
+      });
   }
 
-  void subscribe(std::function<void(RosMessageType)> callback) {
+  /**
+   * @brief Register a callback to be executed for each message.
+   *
+   * @param callback the callback function
+   */
+  void subscribe(std::function<void(RosMessageType)> callback)
+  {
     _callbacks.push_back(callback);
   }
 
-  RosMessageType last() const {
+  /**
+   * @brief Get the last-received message.
+   *
+   * @returns the last-received ROS message
+   * @throws std::runtime_error when no messages have been received
+   */
+  RosMessageType last() const
+  {
     if (_last_message_time.seconds() == 0) {
       throw std::runtime_error("No messages received.");
     }
     return _last;
   }
 
 private:
-  rclcpp::Node &_node;
+  rclcpp::Node & _node;
   std::string _topic;
 
-  typename rclcpp::Subscription<RosMessageType>::SharedPtr _subscription{
-      nullptr};
+  typename rclcpp::Subscription<RosMessageType>::SharedPtr _subscription{nullptr};
 
   RosMessageType _last;
   rclcpp::Time _last_message_time;
 
   std::vector<std::function<void(RosMessageType)>> _callbacks{};
 };
 
+/** @}*/
 } // namespace px4_ros2
diff --git a/px4_ros2_cpp/src/odometry/global_position.cpp b/px4_ros2_cpp/src/odometry/global_position.cpp
@@ -5,25 +5,23 @@
 
 #include <px4_ros2/odometry/global_position.hpp>
 
-
 namespace px4_ros2
 {
 
 OdometryGlobalPosition::OdometryGlobalPosition(Context & context)
 : _node(context.node()),
+  _vehicle_global_position_sub(context, "/fmu/out/vehicle_global_position"),
   _last_vehicle_global_position(_node.get_clock()->now() - 1s)
 {
-  _vehicle_global_position_sub =
-    context.node().create_subscription<px4_msgs::msg::VehicleGlobalPosition>(
-    context.topicNamespacePrefix() + "/fmu/out/vehicle_global_position", rclcpp::QoS(
-      1).best_effort(),
-    [this, context](px4_msgs::msg::VehicleGlobalPosition::UniquePtr msg) {
-      _vehicle_global_position = *msg;
+  _vehicle_global_position_sub.subscribe(
+    [this](const px4_msgs::msg::VehicleGlobalPosition msg) {
+      _vehicle_global_position = msg;
       _last_vehicle_global_position = _node.get_clock()->now();
     });
 
   RequirementFlags requirements{};
   requirements.global_position = true;
   context.setRequirement(requirements);
 }
+
 } // namespace px4_ros2
diff --git a/px4_ros2_cpp/src/odometry/local_position.cpp b/px4_ros2_cpp/src/odometry/local_position.cpp
@@ -5,23 +5,21 @@
 
 #include <px4_ros2/odometry/local_position.hpp>
 
-
 namespace px4_ros2
 {
 
 OdometryLocalPosition::OdometryLocalPosition(Context & context)
+: _vehicle_local_position_sub(context, "/fmu/out/vehicle_local_position")
 {
-  _vehicle_local_position_sub =
-    context.node().create_subscription<px4_msgs::msg::VehicleLocalPosition>(
-    context.topicNamespacePrefix() + "/fmu/out/vehicle_local_position", rclcpp::QoS(
-      1).best_effort(),
-    [this](px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) {
-      _vehicle_local_position = *msg;
+  _vehicle_local_position_sub.subscribe(
+    [this](const px4_msgs::msg::VehicleLocalPosition msg) {
+      _vehicle_local_position = msg;
     });
 
   RequirementFlags requirements{};
   requirements.local_position = true;
   requirements.local_alt = true;
   context.setRequirement(requirements);
 }
+
 } // namespace px4_ros2