odometry: improve validity checks and more tidying up

Auterion · Dec 15, 2023 · 6c15e25 · 6c15e25
1 parent 0548a02
commit 6c15e25
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 12 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
@@ -24,9 +24,14 @@ class OdometryGlobalPosition : public Subscription<px4_msgs::msg::VehicleGlobalP
 public:
   explicit OdometryGlobalPosition(Context & context);
 
+  /**
+   * @brief Get the vehicle's global position.
+   *
+   * @returns a vector of (latitude [°], longitude [°], altitude [m AMSL])
+   */
   Eigen::Vector3d position() const
   {
-    const px4_msgs::msg::VehicleGlobalPosition pos = last();
+    const px4_msgs::msg::VehicleGlobalPosition & pos = last();
     return {pos.lat, pos.lon, pos.alt};
   }
 };

diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp
@@ -26,38 +26,38 @@ class OdometryLocalPosition : public Subscription<px4_msgs::msg::VehicleLocalPos
 
   bool positionXYValid() const
   {
-    return last().xy_valid;
+    return lastValid() && last().xy_valid;
   }
 
   bool positionZValid() const
   {
-    return last().z_valid;
+    return lastValid() && last().z_valid;
   }
 
-  Eigen::Vector3f position() const
+  Eigen::Vector3f positionNed() const
   {
-    const px4_msgs::msg::VehicleLocalPosition pos = last();
+    const px4_msgs::msg::VehicleLocalPosition & pos = last();
     return {pos.x, pos.y, pos.z};
   }
 
   bool velocityXYValid() const
   {
-    return last().v_xy_valid;
+    return lastValid() && last().v_xy_valid;
   }
 
   bool velocityZValid() const
   {
-    return last().v_z_valid;
+    return lastValid() && last().v_z_valid;
   }
-  Eigen::Vector3f velocity() const
+  Eigen::Vector3f velocityNed() const
   {
-    const px4_msgs::msg::VehicleLocalPosition pos = last();
+    const px4_msgs::msg::VehicleLocalPosition & pos = last();
     return {pos.vx, pos.vy, pos.vz};
   }
 
-  Eigen::Vector3f acceleration() const
+  Eigen::Vector3f accelerationNed() const
   {
-    const px4_msgs::msg::VehicleLocalPosition pos = last();
+    const px4_msgs::msg::VehicleLocalPosition & pos = last();
     return {pos.ax, pos.ay, pos.az};
   }
 };

diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp
@@ -21,6 +21,8 @@ namespace px4_ros2
 template<typename RosMessageType>
 class Subscription
 {
+  using UpdateCallback = std::function<void (const RosMessageType &)>;
+
 public:
   Subscription(Context & context, const std::string & topic)
   : _node(context.node())
@@ -42,7 +44,7 @@ class Subscription
    *
    * @param callback the callback function
    */
-  void onUpdate(std::function<void(RosMessageType)> callback)
+  void onUpdate(const UpdateCallback & callback)
   {
     _callbacks.push_back(callback);
   }