implemented some review suggestions

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp

@@ -82,7 +82,7 @@ class GotoGlobalSetpointType
 
 private:
   rclcpp::Node & _node;
-  std::shared_ptr<MapProjection> _map_projection;
+  std::unique_ptr<MapProjection> _map_projection;
   std::shared_ptr<GotoSetpointType> _goto_setpoint;
 };
 

px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp

@@ -21,7 +21,7 @@ class MapProjection
 public:
   explicit MapProjection(Context & context);
 
-  ~MapProjection() = default;
+  ~MapProjection();
 
   /**
    * @return true, if the map reference has been initialized before
@@ -70,8 +70,15 @@ class MapProjection
    */
   void assertInitalized() const;
 
+  /**
+   * Callback for VehicleLocalPosition messages which intializes and updates the map projection reference point from PX4
+   *
+   * @param msg the VehicleLocalPosition message
+  */
+  void vehicleLocalPositionCallback(px4_msgs::msg::VehicleLocalPosition::UniquePtr msg);
+
   rclcpp::Node & _node;
-  std::shared_ptr<MapProjectionImpl> _map_projection_math;
+  std::unique_ptr<MapProjectionImpl> _map_projection_math;
   rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr _vehicle_local_position_sub;
 };
 
@@ -93,7 +100,7 @@ float horizontalDistanceToGlobalPosition(
  * @param global_position_next next lat [deg], lon [deg], alt AMSL [m] (8.1234567°, not 81234567°)
  * @return the horizontal distance [m] between both positions
  */
-inline float horizontalDistanceToGlobalPosition(
+static inline float horizontalDistanceToGlobalPosition(
   const Eigen::Vector3d & global_position_now,
   const Eigen::Vector3d & global_position_next)
 {
@@ -131,7 +138,7 @@ float headingToGlobalPosition(
  * @param global_position_next next lat [deg], lon [deg], alt AMSL [m] (8.1234567°, not 81234567°)
  * @return the heading [rad] to the next global position (clockwise)
 */
-inline float headingToGlobalPosition(
+static inline float headingToGlobalPosition(
   const Eigen::Vector3d & global_position_now,
   const Eigen::Vector3d & global_position_next)
 {
@@ -158,7 +165,7 @@ Eigen::Vector2f vectorToGlobalPosition(
  * @param global_position_next next lat [deg], lon [deg], alt AMSL [m] (8.1234567°, not 81234567°)
  * @return the vector [m^3] in local frame to the next global position (NED)
 */
-inline Eigen::Vector3f vectorToGlobalPosition(
+static inline Eigen::Vector3f vectorToGlobalPosition(
   const Eigen::Vector3d & global_position_now,
   const Eigen::Vector3d & global_position_next)
 {
@@ -205,7 +212,7 @@ Eigen::Vector2d globalPositionFromHeadingAndDist(
  * @param distance distance from the current position [m]
  * @return the target global position
  */
-inline Eigen::Vector3d globalPositionFromHeadingAndDist(
+static inline Eigen::Vector3d globalPositionFromHeadingAndDist(
   const Eigen::Vector3d & global_position_now,
   float heading, float dist)
 {
@@ -235,7 +242,7 @@ Eigen::Vector2d addVectorToGlobalPosition(
  * @param vector_ne local vector to add [m^3] (NED)
  * @return the resulting global position from the addition
 */
-inline Eigen::Vector3d addVectorToGlobalPosition(
+static inline Eigen::Vector3d addVectorToGlobalPosition(
   const Eigen::Vector3d & global_position,
   const Eigen::Vector3f & vector_ned)
 {

px4_ros2_cpp/src/control/setpoint_types/goto.cpp

@@ -66,7 +66,7 @@ SetpointBase::Configuration GotoSetpointType::getConfiguration()
 }
 
 GotoGlobalSetpointType::GotoGlobalSetpointType(Context & context)
-: _node(context.node()), _map_projection(std::make_shared<MapProjection>(context)),
+: _node(context.node()), _map_projection(std::make_unique<MapProjection>(context)),
   _goto_setpoint(std::make_shared<GotoSetpointType>(context))
 {
   RequirementFlags requirements{};
@@ -82,7 +82,7 @@ void GotoGlobalSetpointType::update(
   const std::optional<float> & max_heading_rate)
 {
   if (!_map_projection->isInitialized()) {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Goto global setpoint update failed: map projection is uninitialized. Is /fmu/out/vehicle_local_position published?");
     return;

px4_ros2_cpp/src/utils/geodesic.cpp

@@ -13,18 +13,36 @@ namespace px4_ros2
 MapProjection::MapProjection(Context & context)
 : _node(context.node())
 {
-  _map_projection_math = std::make_shared<MapProjectionImpl>();
+  _map_projection_math = std::make_unique<MapProjectionImpl>();
   _vehicle_local_position_sub = _node.create_subscription<px4_msgs::msg::VehicleLocalPosition>(
-    "/fmu/out/vehicle_local_position", rclcpp::QoS(10).best_effort(),
+    "/fmu/out/vehicle_local_position", rclcpp::QoS(1).best_effort(),
     [this](px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) {
-      const uint64_t timestamp = msg->ref_timestamp;
-      if (!isInitialized() && std::isfinite(timestamp) && timestamp != 0) {
-        _map_projection_math->initReference(
-          msg->ref_lat, msg->ref_lon, msg->ref_alt,
-          msg->ref_timestamp);
-      }
+      vehicleLocalPositionCallback(std::move(msg));
+    });
+}
+
+MapProjection::~MapProjection() = default;
+
+void MapProjection::vehicleLocalPositionCallback(px4_msgs::msg::VehicleLocalPosition::UniquePtr msg)
+{
+  const uint64_t timestamp_cur = msg->ref_timestamp;
+  const uint64_t timestamp_ref = _map_projection_math->getProjectionReferenceTimestamp();
+  if (!isInitialized()) {
+    if (timestamp_cur != 0) {
+      // Initialize map projection reference point
+      _map_projection_math->initReference(
+        msg->ref_lat, msg->ref_lon, msg->ref_alt,
+        timestamp_cur
+      );
     }
-  );
+  } else if (timestamp_cur != timestamp_ref) {
+    // Update reference point if it has changed
+    _map_projection_math->initReference(
+      msg->ref_lat, msg->ref_lon, msg->ref_alt,
+      timestamp_cur
+    );
+    RCLCPP_WARN(_node.get_logger(), "Map projection reference point has been reset.");
+  }
 }
 
 bool MapProjection::isInitialized() const