Add local global conversion

px4_ros2_cpp/CMakeLists.txt

@@ -44,6 +44,7 @@ set(HEADER_FILES
         include/px4_ros2/odometry/global_position.hpp
         include/px4_ros2/odometry/local_position.hpp
         include/px4_ros2/utils/frame_conversion.hpp
+        include/px4_ros2/utils/geodesic.hpp
         include/px4_ros2/utils/geometry.hpp
 )
 
@@ -67,6 +68,8 @@ add_library(px4_ros2_cpp
         src/navigation/experimental/local_position_measurement_interface.cpp
         src/odometry/global_position.cpp
         src/odometry/local_position.cpp
+        src/utils/geodesic.cpp
+        src/utils/map_projection_impl.cpp
 )
 ament_target_dependencies(px4_ros2_cpp ament_index_cpp Eigen3 rclcpp px4_msgs)
 

px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp

@@ -0,0 +1,250 @@
+/****************************************************************************
+ * Copyright (c) 2024 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+#include <Eigen/Eigen>
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_local_position.hpp>
+#include <px4_ros2/common/context.hpp>
+
+namespace px4_ros2
+{
+
+class MapProjectionImpl;
+
+class MapProjection
+{
+public:
+  explicit MapProjection(Context & context);
+
+  ~MapProjection() = default;
+
+  /**
+   * @return true, if the map reference has been initialized before
+   */
+  bool isInitialized() const;
+
+  /**
+   * Transform a point in the geographic coordinate system to the local
+   * azimuthal equidistant plane using the projection
+   *
+   * @param global_position lat [deg], lon [deg] (degrees: 8.1234567°, not 81234567°)
+   * @return the point in local coordinates as north, east [m]
+   */
+  Eigen::Vector2f globalToLocal(const Eigen::Vector2d & global_position) const;
+
+  /**
+   * Transform a point in the geographic coordinate system to the local
+   * azimuthal equidistant plane using the projection
+   *
+   * @param global_position lat [deg], lon [deg], alt AMSL [m] (degrees: 8.1234567°, not 81234567°)
+   * @return the point in local coordinates as north, east, down [m]
+   */
+  Eigen::Vector3f globalToLocal(const Eigen::Vector3d & global_position) const;
+
+  /**
+   * Transform a point in the local azimuthal equidistant plane to the
+   * geographic coordinate system using the projection
+   *
+   * @param local_position north, east [m]
+   * @return the point in geographic coordinates as lat [deg], lon [deg] (degrees: 8.1234567°, not 81234567°)
+   */
+  Eigen::Vector2d localToGlobal(const Eigen::Vector2f & local_position) const;
+
+  /**
+   * Transform a point in the local azimuthal equidistant plane to the
+   * geographic coordinate system using the projection
+   *
+   * @param local_position north, east, down [m]
+   * @return the point in geographic coordinates as lat [deg], lon [deg], alt AMSL [m] (degrees: 8.1234567°, not 81234567°)
+   */
+  Eigen::Vector3d localToGlobal(const Eigen::Vector3f & local_position) const;
+
+private:
+  /**
+   * @throw runtime error if class instance is not initalized
+   */
+  void assertInitalized() const;
+
+  rclcpp::Node & _node;
+  std::shared_ptr<MapProjectionImpl> _map_projection_math;
+  rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr _vehicle_local_position_sub;
+};
+
+/**
+ * Compute the horizontal distance between two global positions in meters.
+ *
+ * @param global_position_now current lat [deg], lon [deg] (47.1234567°, not 471234567°)
+ * @param global_position_next next lat [deg], lon [deg] (8.1234567°, not 81234567°)
+ * @return the horizontal distance [m] between both positions
+ */
+float horizontalDistanceToGlobalPosition(
+  const Eigen::Vector2d & global_position_now,
+  const Eigen::Vector2d & global_position_next);
+
+/**
+ * Compute the horizontal distance between two global positions in meters.
+ *
+ * @param global_position_now current lat [deg], lon [deg], alt AMSL [m] (47.1234567°, not 471234567°)
+ * @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(
+  const Eigen::Vector3d & global_position_now,
+  const Eigen::Vector3d & global_position_next)
+{
+  return horizontalDistanceToGlobalPosition(
+    static_cast<Eigen::Vector2d>(global_position_now.head(2)),
+    static_cast<Eigen::Vector2d>(global_position_next.head(2)));
+}
+
+/**
+ * Compute the distance between two global positions in meters.
+ *
+ * @param global_position_now current lat [deg], lon [deg] (47.1234567°, not 471234567°)
+ * @param global_position_next next lat [deg], lon [deg] (8.1234567°, not 81234567°)
+ * @return the distance [m] between both positions
+ */
+float distanceToGlobalPosition(
+  const Eigen::Vector3d & global_position_now,
+  const Eigen::Vector3d & global_position_next);
+
+/**
+ * Compute the heading to the next global position in radians.
+ *
+ * @param global_position_now current lat [deg], lon [deg] (47.1234567°, not 471234567°)
+ * @param global_position_next next lat [deg], lon [deg] (8.1234567°, not 81234567°)
+ * @return the heading [rad] to the next global position (clockwise)
+*/
+float headingToGlobalPosition(
+  const Eigen::Vector2d & global_position_now,
+  const Eigen::Vector2d & global_position_next);
+
+/**
+ * Compute the heading to the next global position in radians.
+ *
+ * @param global_position_now current lat [deg], lon [deg], alt AMSL [m] (47.1234567°, not 471234567°)
+ * @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(
+  const Eigen::Vector3d & global_position_now,
+  const Eigen::Vector3d & global_position_next)
+{
+  return headingToGlobalPosition(
+    static_cast<Eigen::Vector2d>(global_position_now.head(2)),
+    static_cast<Eigen::Vector2d>(global_position_next.head(2)));
+}
+
+/**
+ * Compute the vector to the next global position in meters.
+ *
+ * @param global_position_now current lat [deg], lon [deg] (47.1234567°, not 471234567°)
+ * @param global_position_next next lat [deg], lon [deg] (8.1234567°, not 81234567°)
+ * @return the vector [m^2] in local frame to the next global position (NE)
+*/
+Eigen::Vector2f vectorToGlobalPosition(
+  const Eigen::Vector2d & global_position_now,
+  const Eigen::Vector2d & global_position_next);
+
+/**
+ * Compute the vector to the next global position in meters.
+ *
+ * @param global_position_now current lat [deg], lon [deg], alt AMSL [m] (47.1234567°, not 471234567°)
+ * @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(
+  const Eigen::Vector3d & global_position_now,
+  const Eigen::Vector3d & global_position_next)
+{
+  const Eigen::Vector2f vector_res =
+    vectorToGlobalPosition(
+    static_cast<Eigen::Vector2d>(global_position_now.head(2)),
+    static_cast<Eigen::Vector2d>(global_position_next.head(2)));
+  const double d_alt = -(global_position_next.z() - global_position_now.z());
+  return Eigen::Vector3f {vector_res.x(), vector_res.y(), static_cast<float>(d_alt)};
+}
+
+/**
+ * Compute the global position on the line vector defined by two positions (start -> end) at a certain distance
+ * from global position 'start'.
+ *
+ * @param global_position_line_start line start lat [deg], lon [deg], alt AMSL [m] (47.1234567°, not 471234567°)
+ * @param global_position_line_end line end lat [deg], lon [deg], alt AMSL [m] (47.1234567°, not 471234567°)
+ * @param dist distance [m] of target global position from position 'start' towards position 'end' (can be negative)
+ */
+Eigen::Vector2d globalPositionFromLineAndDist(
+  const Eigen::Vector2d & global_position_line_start,
+  const Eigen::Vector2d & global_position_line_end,
+  float dist_from_start);
+
+/**
+ * Compute the global position given another global position, distance and heading
+ * see http://www.movable-type.co.uk/scripts/latlong.html
+ *
+ * @param global_position_now current lat [deg], lon [deg] (47.1234567°, not 471234567°)
+ * @param heading heading from the current position [rad] (clockwise)
+ * @param distance distance from the current position [m]
+ * @return the target global position
+ */
+Eigen::Vector2d globalPositionFromHeadingAndDist(
+  const Eigen::Vector2d & global_position_now,
+  float heading, float dist);
+
+/**
+ * Compute the global position given another global position, distance and heading
+ * see http://www.movable-type.co.uk/scripts/latlong.html
+ *
+ * @param global_position_now current lat [deg], lon [deg] (47.1234567°, not 471234567°)
+ * @param heading heading from the current position [rad] (clockwise)
+ * @param distance distance from the current position [m]
+ * @return the target global position
+ */
+inline Eigen::Vector3d globalPositionFromHeadingAndDist(
+  const Eigen::Vector3d & global_position_now,
+  float heading, float dist)
+{
+  const Eigen::Vector2d global_position_res = globalPositionFromHeadingAndDist(
+    static_cast<Eigen::Vector2d>(global_position_now.head(2)), heading, dist);
+  return Eigen::Vector3d {
+    global_position_res.x(),
+    global_position_res.y(),
+    global_position_now.z()};
+}
+
+/**
+ * Compute the global position from adding a local frame vector to the current global position.
+ *
+ * @param global_position current lat [deg], lon [deg] (47.1234567°, not 471234567°)
+ * @param vector_ne local vector to add [m^2] (NE)
+ * @return the resulting global position from the addition
+*/
+Eigen::Vector2d addVectorToGlobalPosition(
+  const Eigen::Vector2d & global_position,
+  const Eigen::Vector2f & vector_ne);
+
+/**
+ * Compute the global position from adding a local frame vector to the current global position.
+ *
+ * @param global_position current lat [deg], lon [deg], alt AMSL [m] (47.1234567°, not 471234567°)
+ * @param vector_ne local vector to add [m^3] (NED)
+ * @return the resulting global position from the addition
+*/
+inline Eigen::Vector3d addVectorToGlobalPosition(
+  const Eigen::Vector3d & global_position,
+  const Eigen::Vector3f & vector_ned)
+{
+  const Eigen::Vector2d global_position_res = addVectorToGlobalPosition(
+    static_cast<Eigen::Vector2d>(global_position.head(2)),
+    static_cast<Eigen::Vector2f>(vector_ned.head(2)));
+  const double d_alt = -vector_ned.z();
+  return Eigen::Vector3d {global_position_res.x(), global_position_res.y(),
+    global_position.z() + d_alt};
+}
+
+}  // namespace px4_ros2