odometry: naming updates and tidying up

px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp

@@ -10,8 +10,6 @@
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/odometry/subscription.hpp>
 
-using namespace std::chrono_literals; // NOLINT
-
 namespace px4_ros2
 {
 /** \ingroup odometry
@@ -21,27 +19,16 @@ namespace px4_ros2
 /**
  * Provides access to the vehicle's global position estimate
  */
-class OdometryGlobalPosition
+class OdometryGlobalPosition : public Subscription<px4_msgs::msg::VehicleGlobalPosition>
 {
 public:
   explicit OdometryGlobalPosition(Context & context);
 
-  bool valid()
-  {
-    return _node.get_clock()->now() - _last_vehicle_global_position < 500ms;
-  }
-
   Eigen::Vector3d position() const
   {
-    return {_vehicle_global_position.lat, _vehicle_global_position.lon,
-      _vehicle_global_position.alt};
+    const px4_msgs::msg::VehicleGlobalPosition pos = last();
+    return {pos.lat, pos.lon, pos.alt};
   }
-
-private:
-  rclcpp::Node & _node;
-  Subscription<px4_msgs::msg::VehicleGlobalPosition> _vehicle_global_position_sub;
-  px4_msgs::msg::VehicleGlobalPosition _vehicle_global_position;
-  rclcpp::Time _last_vehicle_global_position;
 };
 
 /** @}*/

px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp

@@ -19,48 +19,47 @@ namespace px4_ros2
 /**
  * Provides access to the vehicle's local position estimate
  */
-class OdometryLocalPosition
+class OdometryLocalPosition : public Subscription<px4_msgs::msg::VehicleLocalPosition>
 {
 public:
   explicit OdometryLocalPosition(Context & context);
 
-  bool positionXyValid() const
+  bool positionXYValid() const
   {
-    return _vehicle_local_position.xy_valid;
+    return last().xy_valid;
   }
 
   bool positionZValid() const
   {
-    return _vehicle_local_position.z_valid;
+    return last().z_valid;
   }
 
   Eigen::Vector3f position() const
   {
-    return {_vehicle_local_position.x, _vehicle_local_position.y, _vehicle_local_position.z};
+    const px4_msgs::msg::VehicleLocalPosition pos = last();
+    return {pos.x, pos.y, pos.z};
   }
 
   bool velocityXYValid() const
   {
-    return _vehicle_local_position.v_xy_valid;
+    return last().v_xy_valid;
   }
 
   bool velocityZValid() const
   {
-    return _vehicle_local_position.v_z_valid;
+    return last().v_z_valid;
   }
   Eigen::Vector3f velocity() const
   {
-    return {_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz};
+    const px4_msgs::msg::VehicleLocalPosition pos = last();
+    return {pos.vx, pos.vy, pos.vz};
   }
 
   Eigen::Vector3f acceleration() const
   {
-    return {_vehicle_local_position.ax, _vehicle_local_position.ay, _vehicle_local_position.az};
+    const px4_msgs::msg::VehicleLocalPosition pos = last();
+    return {pos.ax, pos.ay, pos.az};
   }
-
-private:
-  Subscription<px4_msgs::msg::VehicleLocalPosition> _vehicle_local_position_sub;
-  px4_msgs::msg::VehicleLocalPosition _vehicle_local_position;
 };
 
 /** @}*/

px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp

@@ -7,6 +7,8 @@
 
 #include <px4_ros2/common/context.hpp>
 
+using namespace std::chrono_literals; // NOLINT
+
 namespace px4_ros2
 {
 /** \ingroup odometry
@@ -21,10 +23,11 @@ class Subscription
 {
 public:
   Subscription(Context & context, const std::string & topic)
-  : _node(context.node()), _topic(context.topicNamespacePrefix() + topic)
+  : _node(context.node())
   {
+    const std::string namespaced_topic = context.topicNamespacePrefix() + topic;
     _subscription = _node.create_subscription<RosMessageType>(
-      _topic, rclcpp::QoS(1).best_effort(),
+      namespaced_topic, rclcpp::QoS(1).best_effort(),
       [this](const typename RosMessageType::UniquePtr msg) {
         _last = *msg;
         _last_message_time = _node.get_clock()->now();
@@ -35,11 +38,11 @@ class Subscription
   }
 
   /**
-   * @brief Register a callback to be executed for each message.
+   * @brief Add a callback to execute when receiving a new message.
    *
    * @param callback the callback function
    */
-  void subscribe(std::function<void(RosMessageType)> callback)
+  void onUpdate(std::function<void(RosMessageType)> callback)
   {
     _callbacks.push_back(callback);
   }
@@ -50,24 +53,46 @@ class Subscription
    * @returns the last-received ROS message
    * @throws std::runtime_error when no messages have been received
    */
-  RosMessageType last() const
+  const RosMessageType & last() const
   {
     if (_last_message_time.seconds() == 0) {
       throw std::runtime_error("No messages received.");
     }
     return _last;
   }
 
+  /**
+   * @brief Get the receive-time of the last message.
+   *
+   * @returns the time at which the last ROS message was received
+   */
+  const rclcpp::Time & lastTime() const
+  {
+    return _last_message_time;
+  }
+
+  /**
+   * @brief Check whether the last message is still valid.
+   * To be valid, the message must have been received within a given time of the current time.
+   *
+   * @param max_delay the maximum delay between the current time and when the message was received
+   * @return true if the last message was received within the maximum delay
+   */
+  template<typename DurationT = std::milli>
+  bool lastValid(const std::chrono::duration<int64_t, DurationT> max_delay = 500ms) const
+  {
+    return _node.get_clock()->now() - _last_message_time < max_delay;
+  }
+
 private:
   rclcpp::Node & _node;
-  std::string _topic;
 
   typename rclcpp::Subscription<RosMessageType>::SharedPtr _subscription{nullptr};
 
   RosMessageType _last;
   rclcpp::Time _last_message_time;
 
-  std::vector<std::function<void(RosMessageType)>> _callbacks{};
+  std::vector<std::function<void(const RosMessageType &)>> _callbacks{};
 };
 
 /** @}*/

px4_ros2_cpp/src/odometry/global_position.cpp

@@ -9,16 +9,8 @@ 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)
+: Subscription<px4_msgs::msg::VehicleGlobalPosition>(context, "/fmu/out/vehicle_global_position")
 {
-  _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);

px4_ros2_cpp/src/odometry/local_position.cpp

@@ -9,13 +9,8 @@ namespace px4_ros2
 {
 
 OdometryLocalPosition::OdometryLocalPosition(Context & context)
-: _vehicle_local_position_sub(context, "/fmu/out/vehicle_local_position")
+: Subscription<px4_msgs::msg::VehicleLocalPosition>(context, "/fmu/out/vehicle_local_position")
 {
-  _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;