From f0ed40de3c88e8840831ec56dc90a2e516e4a71a Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Fri, 16 Feb 2024 15:33:01 +0100 Subject: [PATCH 1/5] utils: geodesic conversions and utils --- px4_ros2_cpp/CMakeLists.txt | 3 + .../include/px4_ros2/utils/geodesic.hpp | 250 ++++++++++++++++++ px4_ros2_cpp/src/utils/geodesic.cpp | 185 +++++++++++++ .../src/utils/map_projection_impl.cpp | 88 ++++++ .../src/utils/map_projection_impl.hpp | 128 +++++++++ 5 files changed, 654 insertions(+) create mode 100644 px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp create mode 100644 px4_ros2_cpp/src/utils/geodesic.cpp create mode 100644 px4_ros2_cpp/src/utils/map_projection_impl.cpp create mode 100644 px4_ros2_cpp/src/utils/map_projection_impl.hpp diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt index b6128527..6f0dbd51 100644 --- a/px4_ros2_cpp/CMakeLists.txt +++ b/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) diff --git a/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp b/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp new file mode 100644 index 00000000..276c3769 --- /dev/null +++ b/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 +#include +#include +#include +#include + +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 _map_projection_math; + rclcpp::Subscription::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(global_position_now.head(2)), + static_cast(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(global_position_now.head(2)), + static_cast(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(global_position_now.head(2)), + static_cast(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(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(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(global_position.head(2)), + static_cast(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 diff --git a/px4_ros2_cpp/src/utils/geodesic.cpp b/px4_ros2_cpp/src/utils/geodesic.cpp new file mode 100644 index 00000000..bc1b83de --- /dev/null +++ b/px4_ros2_cpp/src/utils/geodesic.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include + +#include "map_projection_impl.hpp" + +namespace px4_ros2 +{ + +MapProjection::MapProjection(Context & context) +: _node(context.node()) +{ + _map_projection_math = std::make_shared(); + _vehicle_local_position_sub = _node.create_subscription( + "/fmu/out/vehicle_local_position", rclcpp::QoS(10).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); + } + } + ); +} + +bool MapProjection::isInitialized() const +{ + return _map_projection_math->isInitialized(); +} + +void MapProjection::assertInitalized() const +{ + if (!isInitialized()) { + throw std::runtime_error("Map projection impossible: uninitialized."); + } +} + +Eigen::Vector2f MapProjection::globalToLocal(const Eigen::Vector2d & global_position) const +{ + assertInitalized(); + return _map_projection_math->globalToLocal(global_position); +} + +Eigen::Vector3f MapProjection::globalToLocal(const Eigen::Vector3d & global_position) const +{ + assertInitalized(); + return _map_projection_math->globalToLocal(global_position); +} + +Eigen::Vector2d MapProjection::localToGlobal(const Eigen::Vector2f & local_position) const +{ + assertInitalized(); + return _map_projection_math->localToGlobal(local_position); +} + +Eigen::Vector3d MapProjection::localToGlobal(const Eigen::Vector3f & local_position) const +{ + assertInitalized(); + return _map_projection_math->localToGlobal(local_position); +} + +float horizontalDistanceToGlobalPosition( + const Eigen::Vector2d & global_position_now, + const Eigen::Vector2d & global_position_next) +{ + const double lat_now_rad = degToRad(global_position_now.x()); + const double lat_next_rad = degToRad(global_position_next.x()); + + const double d_lat = lat_next_rad - lat_now_rad; + const double d_lon = degToRad(global_position_next.y()) - degToRad(global_position_now.y()); + + const double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos( + lat_now_rad) * cos(lat_next_rad); + + const double c = atan2(sqrt(a), sqrt(1.0 - a)); + + return static_cast(kRadiusOfEarth * 2.0 * c); +} + +float distanceToGlobalPosition( + const Eigen::Vector3d & global_position_now, + const Eigen::Vector3d & global_position_next) +{ + const float dxy = horizontalDistanceToGlobalPosition(global_position_now, global_position_next); + const float dz = static_cast(global_position_now.z() - global_position_next.z()); + + return sqrtf(dxy * dxy + dz * dz); +} + +float headingToGlobalPosition( + const Eigen::Vector2d & global_position_now, + const Eigen::Vector2d & global_position_next) +{ + const double lat_now_rad = degToRad(global_position_now.x()); + const double lat_next_rad = degToRad(global_position_next.x()); + + const double cos_lat_next = cos(lat_next_rad); + const double d_lon = degToRad(global_position_next.y() - global_position_now.y()); + + const float y = static_cast(sin(d_lon) * cos_lat_next); + const float x = + static_cast(cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos_lat_next * + cos(d_lon)); + + return wrapPi(atan2f(y, x)); +} + +Eigen::Vector2f vectorToGlobalPosition( + const Eigen::Vector2d & global_position_now, + const Eigen::Vector2d & global_position_next) +{ + Eigen::Vector2f vector_ne; + + const double lat_now_rad = degToRad(global_position_now.x()); + const double lat_next_rad = degToRad(global_position_next.x()); + const double d_lon = degToRad(global_position_next.y()) - degToRad(global_position_now.y()); + + vector_ne.x() = + static_cast(kRadiusOfEarth * + (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos( + lat_next_rad) * cos(d_lon))); + vector_ne.y() = static_cast(kRadiusOfEarth * sin(d_lon) * cos(lat_next_rad)); + + return vector_ne; +} + +Eigen::Vector2d globalPositionFromLineAndDist( + const Eigen::Vector2d & global_position_line_start, + const Eigen::Vector2d & global_position_line_end, + float dist_from_start) +{ + float heading = headingToGlobalPosition(global_position_line_start, global_position_line_end); + return globalPositionFromHeadingAndDist(global_position_line_start, heading, dist_from_start); +} + +Eigen::Vector2d globalPositionFromHeadingAndDist( + const Eigen::Vector2d & global_position_now, + float heading, float dist) +{ + Eigen::Vector2d global_position_target; + + heading = wrapPi(heading); + + const double radius_ratio = static_cast(dist) / kRadiusOfEarth; + + const double lat_now_rad = degToRad(global_position_now.x()); + const double lon_now_rad = degToRad(global_position_now.y()); + + const double lat_target_rad = asin( + sin(lat_now_rad) * cos(radius_ratio) + cos(lat_now_rad) * sin(radius_ratio) * cos( + static_cast(heading))); + const double lon_target_rad = lon_now_rad + atan2( + sin(static_cast(heading)) * sin(radius_ratio) * cos(lat_now_rad), + cos(radius_ratio) - sin(lat_now_rad) * sin(lat_target_rad)); + + global_position_target.x() = radToDeg(lat_target_rad); + global_position_target.y() = radToDeg(lon_target_rad); + + return global_position_target; +} + +Eigen::Vector2d addVectorToGlobalPosition( + const Eigen::Vector2d & global_position, + const Eigen::Vector2f & vector_ne) +{ + Eigen::Vector2d global_position_res; + + double lat_now_rad = degToRad(global_position.x()); + double lon_now_rad = degToRad(global_position.y()); + + global_position_res.x() = + radToDeg(lat_now_rad + static_cast(vector_ne.x()) / kRadiusOfEarth); + global_position_res.y() = + radToDeg( + lon_now_rad + static_cast(vector_ne.y()) / + (kRadiusOfEarth * cos(lat_now_rad))); + + return global_position_res; +} + +} // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/utils/map_projection_impl.cpp b/px4_ros2_cpp/src/utils/map_projection_impl.cpp new file mode 100644 index 00000000..4e91e60c --- /dev/null +++ b/px4_ros2_cpp/src/utils/map_projection_impl.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +/* + * Azimuthal Equidistant Projection + * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html + */ + +#include "map_projection_impl.hpp" + + +namespace px4_ros2 +{ + +void MapProjectionImpl::initReference(double lat_0, double lon_0, double alt_0, uint64_t timestamp) +{ + _ref_timestamp = timestamp; + _ref_lat = degToRad(lat_0); + _ref_lon = degToRad(lon_0); + _ref_alt_amsl = alt_0; + _ref_sin_lat = std::sin(_ref_lat); + _ref_cos_lat = std::cos(_ref_lat); + _ref_init_done = true; +} + +Eigen::Vector2f MapProjectionImpl::globalToLocal(const Eigen::Vector2d & global_position) const +{ + Eigen::Vector2f local_position; + + const double lat_rad = degToRad(global_position.x()); + const double lon_rad = degToRad(global_position.y()); + + const double sin_lat = std::sin(lat_rad); + const double cos_lat = std::cos(lat_rad); + + const double cos_d_lon = std::cos(lon_rad - _ref_lon); + + const double arg = std::clamp( + _ref_sin_lat * sin_lat + _ref_cos_lat * cos_lat * cos_d_lon, -1.0, + 1.0); + const double c = std::acos(arg); + + double k = 1.0; + + if (std::fabs(c) > 0) { + k = (c / std::sin(c)); + } + + local_position.x() = + static_cast(k * (_ref_cos_lat * sin_lat - _ref_sin_lat * cos_lat * cos_d_lon) * + kRadiusOfEarth); + local_position.y() = + static_cast(k * cos_lat * std::sin(lon_rad - _ref_lon) * kRadiusOfEarth); + + return local_position; +} + +Eigen::Vector2d MapProjectionImpl::localToGlobal(const Eigen::Vector2f & local_position) const +{ + Eigen::Vector2d global_position; + + const double x_rad = static_cast(local_position.x()) / kRadiusOfEarth; + const double y_rad = static_cast(local_position.y()) / kRadiusOfEarth; + const double c = std::sqrt(x_rad * x_rad + y_rad * y_rad); + + if (std::fabs(c) > 0) { + const double sin_c = std::sin(c); + const double cos_c = std::cos(c); + + const double lat_rad = std::asin(cos_c * _ref_sin_lat + (x_rad * sin_c * _ref_cos_lat) / c); + const double lon_rad = + (_ref_lon + + std::atan2(y_rad * sin_c, c * _ref_cos_lat * cos_c - x_rad * _ref_sin_lat * sin_c)); + + global_position.x() = radToDeg(lat_rad); + global_position.y() = radToDeg(lon_rad); + + } else { + global_position.x() = radToDeg(_ref_lat); + global_position.y() = radToDeg(_ref_lon); + } + + return global_position; +} + +} // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/utils/map_projection_impl.hpp b/px4_ros2_cpp/src/utils/map_projection_impl.hpp new file mode 100644 index 00000000..50a0dd7b --- /dev/null +++ b/px4_ros2_cpp/src/utils/map_projection_impl.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace px4_ros2 +{ + +static constexpr double kRadiusOfEarth = 6371000; // meters (m) + +/** + * @brief C++ class for mapping lat/lon coordinates to local coordinates using a reference position + */ +class MapProjectionImpl final +{ +private: + uint64_t _ref_timestamp{0}; /**< Reference time (microseconds), since system start */ + double _ref_lat{0.0}; /**< Reference point latitude (degrees) */ + double _ref_lon{0.0}; /**< Reference point longitude (degrees) */ + double _ref_alt_amsl{0.0}; /**< Reference point altitude AMSL (meters) */ + double _ref_sin_lat{0.0}; + double _ref_cos_lat{0.0}; + bool _ref_init_done{false}; + +public: + /** + * @brief Construct a new Map Projection object + * The generated object will be uninitialized. + * To initialize, use the `initReference` function + */ + explicit MapProjectionImpl() = default; + + /** + * @brief Construct and initialize a new Map Projection object + */ + explicit MapProjectionImpl(double lat_0, double lon_0, double alt_0, uint64_t timestamp) + { + initReference(lat_0, lon_0, alt_0, timestamp); + } + + /** + * @brief Initialize the map transformation + * + * with reference coordinates on the geographic coordinate system + * where the azimuthal equidistant plane's origin is located + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ + void initReference(double lat_0, double lon_0, double alt_0, uint64_t timestamp); + + /** + * @return true, if the map reference has been initialized before + */ + inline bool isInitialized() const {return _ref_init_done;} + + /** + * @return the timestamp of the reference which the map projection was initialized with + */ + inline uint64_t getProjectionReferenceTimestamp() const {return _ref_timestamp;} + + /** + * @return the projection reference latitude in degrees + */ + inline double getProjectionReferenceLat() const {return radToDeg(_ref_lat);} + + /** + * @return the projection reference longitude in degrees + */ + inline double getProjectionReferenceLon() const {return radToDeg(_ref_lon);} + + /** + * 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] + */ + inline Eigen::Vector3f globalToLocal(const Eigen::Vector3d & global_position) const + { + Eigen::Vector3f local_position; + + local_position.head(2) = globalToLocal(static_cast(global_position.head(2))); + local_position.z() = static_cast(_ref_alt_amsl - global_position.z()); + return local_position; + } + + /** + * 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°) + */ + inline Eigen::Vector3d localToGlobal(const Eigen::Vector3f & local_position) const + { + Eigen::Vector3d global_position; + + global_position.head(2) = localToGlobal(static_cast(local_position.head(2))); + global_position.z() = static_cast(_ref_alt_amsl - local_position.z()); + return global_position; + } +}; + +} // namespace px4_ros2 From adc25b60b94f48f66146d93987d84ab03a4aba1c Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Fri, 16 Feb 2024 15:33:28 +0100 Subject: [PATCH 2/5] control: add global goto setpoint --- .../px4_ros2/control/setpoint_types/goto.hpp | 33 +++++++++++++++++++ .../src/control/setpoint_types/goto.cpp | 30 +++++++++++++++++ 2 files changed, 63 insertions(+) diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp index c9a0b8e9..44bdf4dc 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp @@ -10,6 +10,7 @@ #include #include +#include namespace px4_ros2 { @@ -53,5 +54,37 @@ class GotoSetpointType : public SetpointBase _goto_setpoint_pub; }; +class GotoGlobalSetpointType +{ +public: + explicit GotoGlobalSetpointType(Context & context); + + ~GotoGlobalSetpointType() = default; + + /** + * @brief Go-to global setpoint update + * + * Unset optional values are not controlled + * + * @param global_position latitude [deg], longitude [deg], altitude AMSL [m] + * @param heading [rad] from North + * @param max_horizontal_speed [m/s] in NE-datum of NED earth-fixed frame + * @param max_vertical_speed [m/s] in D-axis of NED earth-fixed frame + * @param max_heading_rate [rad/s] about D-axis of NED earth-fixed frame + */ + void update( + const Eigen::Vector3d & global_position, + const std::optional & heading = {}, + const std::optional & max_horizontal_speed = {}, + const std::optional & max_vertical_speed = {}, + const std::optional & max_heading_rate = {} + ); + +private: + rclcpp::Node & _node; + std::shared_ptr _map_projection; + std::shared_ptr _goto_setpoint; +}; + /** @}*/ } /* namespace px4_ros2 */ diff --git a/px4_ros2_cpp/src/control/setpoint_types/goto.cpp b/px4_ros2_cpp/src/control/setpoint_types/goto.cpp index 7398fa8a..488bf6ad 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/goto.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/goto.cpp @@ -64,4 +64,34 @@ SetpointBase::Configuration GotoSetpointType::getConfiguration() config.climb_rate_enabled = true; return config; } + +GotoGlobalSetpointType::GotoGlobalSetpointType(Context & context) +: _node(context.node()), _map_projection(std::make_shared(context)), + _goto_setpoint(std::make_shared(context)) +{ + RequirementFlags requirements{}; + requirements.global_position = true; + context.setRequirement(requirements); +} + +void GotoGlobalSetpointType::update( + const Eigen::Vector3d & global_position, + const std::optional & heading, + const std::optional & max_horizontal_speed, + const std::optional & max_vertical_speed, + const std::optional & max_heading_rate) +{ + if (!_map_projection->isInitialized()) { + RCLCPP_ERROR( + _node.get_logger(), + "Goto global setpoint update failed: map projection is uninitialized. Is /fmu/out/vehicle_local_position published?"); + return; + } + + Eigen::Vector3f local_position = _map_projection->globalToLocal(global_position); + _goto_setpoint->update( + local_position, heading, max_horizontal_speed, max_vertical_speed, + max_heading_rate); +} + } // namespace px4_ros2 From 1f4bb0672b59d524fa016133859fe7d798a4fe5d Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Fri, 16 Feb 2024 15:35:51 +0100 Subject: [PATCH 3/5] examples: add global goto --- examples/cpp/modes/goto/include/mode.hpp | 2 +- examples/cpp/modes/goto_global/CMakeLists.txt | 34 +++ .../cpp/modes/goto_global/include/mode.hpp | 224 ++++++++++++++++++ examples/cpp/modes/goto_global/package.xml | 26 ++ examples/cpp/modes/goto_global/src/main.cpp | 17 ++ 5 files changed, 302 insertions(+), 1 deletion(-) create mode 100644 examples/cpp/modes/goto_global/CMakeLists.txt create mode 100644 examples/cpp/modes/goto_global/include/mode.hpp create mode 100644 examples/cpp/modes/goto_global/package.xml create mode 100644 examples/cpp/modes/goto_global/src/main.cpp diff --git a/examples/cpp/modes/goto/include/mode.hpp b/examples/cpp/modes/goto/include/mode.hpp index 049ef8fc..ed7570a9 100644 --- a/examples/cpp/modes/goto/include/mode.hpp +++ b/examples/cpp/modes/goto/include/mode.hpp @@ -125,7 +125,7 @@ class FlightModeTest : public px4_ros2::ModeBase break; case State::GoingSouthwest: { - // go to southwest corner while facing the northeastern corner + // go to southwest corner while facing the northwestern corner const Eigen::Vector2f position_of_interest_m = _start_position_m.head(2) + Eigen::Vector2f{kTriangleHeight, 0.f}; const Eigen::Vector2f vehicle_to_poi_xy = position_of_interest_m - diff --git a/examples/cpp/modes/goto_global/CMakeLists.txt b/examples/cpp/modes/goto_global/CMakeLists.txt new file mode 100644 index 00000000..34a3e8e9 --- /dev/null +++ b/examples/cpp/modes/goto_global/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(example_mode_goto_global_cpp) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter) +endif() + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_ros2_cpp REQUIRED) + +include_directories(include ${Eigen3_INCLUDE_DIRS}) +add_executable(example_mode_goto_global + src/main.cpp) +ament_target_dependencies(example_mode_goto_global Eigen3 px4_ros2_cpp rclcpp) + +install(TARGETS + example_mode_goto_global + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/examples/cpp/modes/goto_global/include/mode.hpp b/examples/cpp/modes/goto_global/include/mode.hpp new file mode 100644 index 00000000..8b9833ef --- /dev/null +++ b/examples/cpp/modes/goto_global/include/mode.hpp @@ -0,0 +1,224 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +static const std::string kName = "Go-to Global Example"; +static const std::string kNodeName = "example_mode_goto_global"; + +using namespace px4_ros2::literals; // NOLINT + +class FlightModeTest : public px4_ros2::ModeBase +{ +public: + explicit FlightModeTest(rclcpp::Node & node) + : ModeBase(node, kName) + { + _goto_setpoint = std::make_shared(*this); + + _vehicle_global_position = std::make_shared(*this); + + _vehicle_attitude_sub = node.create_subscription( + "/fmu/out/vehicle_attitude", rclcpp::QoS(1).best_effort(), + [this](px4_msgs::msg::VehicleAttitude::UniquePtr msg) { + updateVehicleHeading(msg); + } + ); + } + + void onActivate() override + { + _state = State::SettlingAtStart; + _start_position_set = false; + _start_heading_set = false; + } + + void onDeactivate() override {} + + void updateSetpoint(float dt_s) override + { + if (!_start_position_set) { + _start_position_m = _vehicle_global_position->position(); + _start_position_set = true; + } + + switch (_state) { + case State::SettlingAtStart: { + // just settling at the starting vehicle position + _goto_setpoint->update(_start_position_m); + if (positionReached(_start_position_m)) { + _state = State::GoingNorth; + } + } + break; + + case State::GoingNorth: { + // go north to the northwest corner facing in direction of travel + const Eigen::Vector3d target_position_m = px4_ros2::addVectorToGlobalPosition( + _start_position_m, Eigen::Vector3f{kTriangleHeight, 0.f, 0.f}); + + const float heading_target_rad = px4_ros2::headingToGlobalPosition( + _vehicle_global_position->position(), target_position_m); + + if (px4_ros2::horizontalDistanceToGlobalPosition( + _vehicle_global_position->position(), + target_position_m) < 0.1f) + { + // stop caring about heading (the arctangent becomes undefined) + _goto_setpoint->update(target_position_m); + } else { + _goto_setpoint->update(target_position_m, heading_target_rad); + } + + if (positionReached(target_position_m)) { + _state = State::GoingEast; + } + } + break; + + case State::GoingEast: { + // go to the northeast corner while spinning + const Eigen::Vector3d target_position_m = px4_ros2::addVectorToGlobalPosition( + _start_position_m, Eigen::Vector3f{kTriangleHeight, kTriangleWidth, 0.f}); + + // scale the speed limits by distance to the target + const float distance_to_target = px4_ros2::horizontalDistanceToGlobalPosition( + _vehicle_global_position->position(), target_position_m); + const float speed_scale = std::min(distance_to_target / kTriangleWidth, 1.f); + + const float max_horizontal_velocity_m_s = 5.f * speed_scale + (1.f - speed_scale) * 1.f; + const float max_vertical_velocity_m_s = 3.f * speed_scale + (1.f - speed_scale) * 0.5f; + const float max_heading_rate_rad_s = + px4_ros2::degToRad(45.f * speed_scale + (1.f - speed_scale) * 25.f); + const float heading_setpoint_rate_of_change = + px4_ros2::degToRad(40.f * speed_scale + (1.f - speed_scale) * 20.f); + + if (!_start_heading_set) { + _spinning_heading_rad = _vehicle_heading_rad; + _start_heading_set = true; + } + + if (!positionReached(target_position_m)) { + _spinning_heading_rad += heading_setpoint_rate_of_change * dt_s; + } + + _goto_setpoint->update( + target_position_m, + _spinning_heading_rad, + max_horizontal_velocity_m_s, + max_vertical_velocity_m_s, + max_heading_rate_rad_s); + + if (positionReached(target_position_m)) { + _state = State::GoingSouthwest; + } + } + break; + + case State::GoingSouthwest: { + // go to southwest corner while facing the northwestern corner + const Eigen::Vector2d position_of_interest_m = px4_ros2::addVectorToGlobalPosition( + _start_position_m.head( + 2), Eigen::Vector2f{kTriangleHeight, 0.f}); + const float heading_target_rad = px4_ros2::headingToGlobalPosition( + _vehicle_global_position->position().head(2), position_of_interest_m); + + _goto_setpoint->update(_start_position_m, heading_target_rad); + if (positionReached(_start_position_m)) { + completed(px4_ros2::Result::Success); + return; + } + } + break; + } + } + +private: + static constexpr float kTriangleHeight = 20.f; // [m] + static constexpr float kTriangleWidth = 30.f; // [m] + + enum class State + { + SettlingAtStart = 0, + GoingNorth, + GoingEast, + GoingSouthwest + } _state; + + // NED earth-fixed frame. box pattern starting corner (first position the mode sees on activation) + Eigen::Vector3d _start_position_m; + bool _start_position_set{false}; + + // [-pi, pi] current vehicle heading from VehicleAttitude subscription + float _vehicle_heading_rad{0.f}; + + // [-pi, pi] current heading setpoint during spinning phase + float _spinning_heading_rad{0.f}; + + // used for heading initialization when dynamically updating heading setpoints + bool _start_heading_set{false}; + + std::shared_ptr _goto_setpoint; + std::shared_ptr _vehicle_global_position; + rclcpp::Subscription::SharedPtr _vehicle_attitude_sub; + + void updateVehicleHeading(const px4_msgs::msg::VehicleAttitude::UniquePtr & msg) + { + const Eigen::Quaternionf q = Eigen::Quaternionf(msg->q[0], msg->q[1], msg->q[2], msg->q[3]); + const Eigen::Vector3f rpy = px4_ros2::quaternionToEulerRpy(q); + _vehicle_heading_rad = rpy(2); + } + + bool positionReached(const Eigen::Vector3d & target_position_m) const + { + static constexpr float kPositionErrorThreshold = 0.5f; // [m] + const float position_error = px4_ros2::distanceToGlobalPosition( + _vehicle_global_position->position(), target_position_m); + return position_error < kPositionErrorThreshold; + } + + bool headingReached(float target_heading_rad) const + { + static constexpr float kHeadingErrorThreshold = 7.0_deg; + const float heading_error_wrapped = px4_ros2::wrapPi(target_heading_rad - _vehicle_heading_rad); + return fabsf(heading_error_wrapped) < kHeadingErrorThreshold; + } +}; + +class TestNode : public rclcpp::Node +{ +public: + TestNode() + : Node(kNodeName) + { + // Enable debug output + auto ret = + rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str); + rcutils_reset_error(); + } + + _mode = std::make_unique(*this); + + if (!_mode->doRegister()) { + throw std::runtime_error("Registration failed"); + } + } + +private: + std::unique_ptr _mode; +}; diff --git a/examples/cpp/modes/goto_global/package.xml b/examples/cpp/modes/goto_global/package.xml new file mode 100644 index 00000000..8fadf30e --- /dev/null +++ b/examples/cpp/modes/goto_global/package.xml @@ -0,0 +1,26 @@ + + + + example_mode_goto_global_cpp + 0.0.1 + Example mode: Go-to global + Beat Kueng + BSD-3-Clause + + eigen3_cmake_module + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + eigen + + px4_ros2_cpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/examples/cpp/modes/goto_global/src/main.cpp b/examples/cpp/modes/goto_global/src/main.cpp new file mode 100644 index 00000000..60b60a57 --- /dev/null +++ b/examples/cpp/modes/goto_global/src/main.cpp @@ -0,0 +1,17 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "rclcpp/rclcpp.hpp" + +#include + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 3d82ec1a53ccff5c16a87d79e9b1df12b3b84ac1 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 21 Feb 2024 17:59:24 +0100 Subject: [PATCH 4/5] test: add geodesic unit tests --- px4_ros2_cpp/CMakeLists.txt | 2 + .../include/px4_ros2/utils/geodesic.hpp | 4 +- .../src/utils/map_projection_impl.hpp | 17 ++- px4_ros2_cpp/test/unit/utils/geodesic.cpp | 127 ++++++++++++++++++ .../test/unit/utils/map_projection_impl.cpp | 74 ++++++++++ px4_ros2_cpp/test/unit/utils/util.cpp | 14 +- px4_ros2_cpp/test/unit/utils/util.hpp | 4 + 7 files changed, 236 insertions(+), 6 deletions(-) create mode 100644 px4_ros2_cpp/test/unit/utils/geodesic.cpp create mode 100644 px4_ros2_cpp/test/unit/utils/map_projection_impl.cpp diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt index 6f0dbd51..6e4b0165 100644 --- a/px4_ros2_cpp/CMakeLists.txt +++ b/px4_ros2_cpp/CMakeLists.txt @@ -132,7 +132,9 @@ if(BUILD_TESTING) test/unit/main.cpp test/unit/modes.cpp test/unit/utils/frame_conversion.cpp + test/unit/utils/geodesic.cpp test/unit/utils/geometry.cpp + test/unit/utils/map_projection_impl.cpp ) target_include_directories(${PROJECT_NAME}_unit_tests PRIVATE ${CMAKE_CURRENT_LIST_DIR}) target_link_libraries(${PROJECT_NAME}_unit_tests ${PROJECT_NAME} unit_utils) diff --git a/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp b/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp index 276c3769..e3159b61 100644 --- a/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp +++ b/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp @@ -174,8 +174,8 @@ inline Eigen::Vector3f vectorToGlobalPosition( * 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 global_position_line_start line start lat [deg], lon [deg] (47.1234567°, not 471234567°) + * @param global_position_line_end line end lat [deg], lon [deg] (47.1234567°, not 471234567°) * @param dist distance [m] of target global position from position 'start' towards position 'end' (can be negative) */ Eigen::Vector2d globalPositionFromLineAndDist( diff --git a/px4_ros2_cpp/src/utils/map_projection_impl.hpp b/px4_ros2_cpp/src/utils/map_projection_impl.hpp index 50a0dd7b..2bc4e444 100644 --- a/px4_ros2_cpp/src/utils/map_projection_impl.hpp +++ b/px4_ros2_cpp/src/utils/map_projection_impl.hpp @@ -21,8 +21,8 @@ class MapProjectionImpl final { private: uint64_t _ref_timestamp{0}; /**< Reference time (microseconds), since system start */ - double _ref_lat{0.0}; /**< Reference point latitude (degrees) */ - double _ref_lon{0.0}; /**< Reference point longitude (degrees) */ + double _ref_lat{0.0}; /**< Reference point latitude (radians) */ + double _ref_lon{0.0}; /**< Reference point longitude (radians) */ double _ref_alt_amsl{0.0}; /**< Reference point altitude AMSL (meters) */ double _ref_sin_lat{0.0}; double _ref_cos_lat{0.0}; @@ -74,6 +74,19 @@ class MapProjectionImpl final */ inline double getProjectionReferenceLon() const {return radToDeg(_ref_lon);} + /** + * @return the projection reference altitude AMSL in meters + */ + inline double getProjectionReferenceAlt() const {return _ref_alt_amsl;} + + /** + * @return the projection reference position: lat [deg], lon [deg], alt AMSL [m] + */ + inline Eigen::Vector3d getProjectionReferencePosition() const + { + return Eigen::Vector3d{radToDeg(_ref_lat), radToDeg(_ref_lon), _ref_alt_amsl}; + } + /** * Transform a point in the geographic coordinate system to the local * azimuthal equidistant plane using the projection diff --git a/px4_ros2_cpp/test/unit/utils/geodesic.cpp b/px4_ros2_cpp/test/unit/utils/geodesic.cpp new file mode 100644 index 00000000..2e650e79 --- /dev/null +++ b/px4_ros2_cpp/test/unit/utils/geodesic.cpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include + +#include "util.hpp" + +using namespace px4_ros2::literals; // NOLINT + +/** + * Expected computation results from https://www.movable-type.co.uk/scripts/latlong.html +*/ + +TEST(Geodesic, horizontalDistanceToGlobalPosition) { + const Eigen::Vector3d global_position_start{47.3566094, 8.5190237, 151.3}; + const Eigen::Vector3d global_position_end{47.35839, 8.52894, 581.3}; + const float distance_exp = 772.749f; + + // Check against expected value + const float distance_res = px4_ros2::horizontalDistanceToGlobalPosition( + global_position_start, + global_position_end); + EXPECT_NEAR(distance_exp, distance_res, 1e-3); +} + +TEST(Geodesic, distanceToGlobalPosition) { + const Eigen::Vector3d global_position_start{47.3566094, 8.5190237, 151.3}; + const Eigen::Vector3d global_position_end{47.35839, 8.52894, 581.3}; + const float distance_exp = 884.331f; + + // Check against expected value + const float distance_res = px4_ros2::distanceToGlobalPosition( + global_position_start, + global_position_end); + EXPECT_NEAR(distance_exp, distance_res, 1e-3); +} + +TEST(Geodesic, headingToGlobalPosition) { + const Eigen::Vector3d global_position_start{47.3566094, 8.5190237, 151.3}; + const Eigen::Vector3d global_position_end{47.35839, 8.52894, 581.3}; + const float heading_exp = 75.1505_deg; + + // Check against expected value + const float heading_res = px4_ros2::headingToGlobalPosition( + global_position_start, + global_position_end); + EXPECT_NEAR(heading_exp, heading_res, 1e-3); +} + +TEST(Geodesic, vectorToGlobalPosition) { + const Eigen::Vector3d global_position_start{47.3566094, 8.5190237, 151.3}; + const Eigen::Vector3d global_position_end{47.35839, 8.52894, 581.3}; + const Eigen::Vector3f vector_exp{198.041f, 746.941f, -430.f}; + + // Check against expected value + const Eigen::Vector3f vector_res = px4_ros2::vectorToGlobalPosition( + global_position_start, + global_position_end); + vectorsApproxEqualTest(vector_exp, vector_res); + + // Check resulting vector matches initial start -> end position displacement + const Eigen::Vector3d global_position_end_new = px4_ros2::addVectorToGlobalPosition( + global_position_start, vector_res); + vectorsApproxEqualTest(global_position_end, global_position_end_new); +} + +TEST(Geodesic, globalPositionFromLineAndDist) { + const Eigen::Vector2d global_position_start{47.3566094, 8.5190237}; + const Eigen::Vector2d global_position_end{47.35839, 8.52894}; + const float dist_from_start = 123.4f; + const Eigen::Vector2d global_position_exp{47.35689, 8.52061}; + + // Check against expected value + const Eigen::Vector2d global_position_res = px4_ros2::globalPositionFromLineAndDist( + global_position_start, global_position_end, dist_from_start); + EXPECT_TRUE(global_position_exp.isApprox(global_position_res, 1e-3)); + + // Check resulting position distance from start position matches provided distance + const float dist_new = px4_ros2::horizontalDistanceToGlobalPosition( + global_position_start, + global_position_res); + EXPECT_NEAR(dist_from_start, dist_new, 1e-3); +} + +TEST(Geodesic, globalPositionFromHeadingAndDist) { + const Eigen::Vector3d global_position_start{47.3566094, 8.5190237, 151.3}; + const float heading = 19.4_deg; + const float dist = 123.4f; + const Eigen::Vector3d global_position_exp{47.3577, 8.5196, 151.3}; + + // Check against expected value + const Eigen::Vector3d global_position_res = px4_ros2::globalPositionFromHeadingAndDist( + global_position_start, heading, dist); + vectorsApproxEqualTest(global_position_exp, global_position_res); + + // Check resulting position heading and distance from start position match provided values + const float heading_new = px4_ros2::headingToGlobalPosition( + global_position_start, + global_position_res); + const float dist_new = px4_ros2::distanceToGlobalPosition( + global_position_start, + global_position_res); + EXPECT_NEAR(heading, heading_new, 1e-3); + EXPECT_NEAR(dist, dist_new, 1e-3); +} + +TEST(Geodesic, addVectorToGlobalPosition) { + const Eigen::Vector3d global_position{47.3566094, 8.5190237, 151.3}; + const Eigen::Vector3f vector{198.041f, 746.941f, -430.f}; + const Eigen::Vector3d global_position_exp{47.35839, 8.52894, 581.3}; + + // Check against expected value + const Eigen::Vector3d global_position_res = px4_ros2::addVectorToGlobalPosition( + global_position, + vector); + vectorsApproxEqualTest(global_position_exp, global_position_res); + + // Check resulting position displacement from the start position matches the provided vector + const Eigen::Vector3f vector_new = px4_ros2::vectorToGlobalPosition( + global_position, + global_position_res); + vectorsApproxEqualTest(vector, vector_new); +} diff --git a/px4_ros2_cpp/test/unit/utils/map_projection_impl.cpp b/px4_ros2_cpp/test/unit/utils/map_projection_impl.cpp new file mode 100644 index 00000000..3f9dc61a --- /dev/null +++ b/px4_ros2_cpp/test/unit/utils/map_projection_impl.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include + +#include +#include "util.hpp" + +class MapProjectionImplTest : public ::testing::Test +{ +public: + void SetUp() override + { + _map_projection.initReference(kLatRef, kLonRef, kAltAmslRef, kTimestampRef); + } + +protected: + static constexpr double kLatRef = 47.3566094; + static constexpr double kLonRef = 8.5190237; + static constexpr double kAltAmslRef = 151.3; + static constexpr uint64_t kTimestampRef = 0; + + px4_ros2::MapProjectionImpl _map_projection; +}; + + +TEST_F(MapProjectionImplTest, isInitialized) { + ASSERT_TRUE(_map_projection.isInitialized()); +} + +TEST_F(MapProjectionImplTest, getters) { + EXPECT_EQ(kLatRef, _map_projection.getProjectionReferenceLat()); + EXPECT_EQ(kLonRef, _map_projection.getProjectionReferenceLon()); + EXPECT_EQ(kAltAmslRef, _map_projection.getProjectionReferenceAlt()); + EXPECT_EQ(kTimestampRef, _map_projection.getProjectionReferenceTimestamp()); + vectorsApproxEqualTest( + Eigen::Vector3d{kLatRef, kLonRef, kAltAmslRef}, + _map_projection.getProjectionReferencePosition()); +} + +TEST_F(MapProjectionImplTest, localToGlobal) { + + const Eigen::Vector3f local_position = {0.5f, 1.f, -2.f}; + const Eigen::Vector3d global_position = _map_projection.localToGlobal(local_position); + const Eigen::Vector3f local_position_new = _map_projection.globalToLocal(global_position); + const Eigen::Vector3d global_position_new = _map_projection.localToGlobal(local_position_new); + + // Check against expected value + const Eigen::Vector3d global_position_exp{47.3566138966073, 8.51903697542915, 153.3}; + vectorsApproxEqualTest(global_position_exp, global_position); + + // Check results against inverse operation + vectorsApproxEqualTest(local_position, local_position_new); + vectorsApproxEqualTest(global_position, global_position_new); +} + +TEST_F(MapProjectionImplTest, globalToLocal) { + + const Eigen::Vector3d global_position = {47.356616973876953, 8.5190505981445313, 163.7}; + const Eigen::Vector3f local_position = _map_projection.globalToLocal(global_position); + const Eigen::Vector3d global_position_new = _map_projection.localToGlobal(local_position); + const Eigen::Vector3f local_position_new = _map_projection.globalToLocal(global_position_new); + + // Check against expected value + const Eigen::Vector3f local_position_exp{0.842f, 2.026f, -12.4f}; + vectorsApproxEqualTest(local_position_exp, local_position); + + // Check results against inverse operation + vectorsApproxEqualTest(global_position, global_position_new); + vectorsApproxEqualTest(local_position, local_position_new); +} diff --git a/px4_ros2_cpp/test/unit/utils/util.cpp b/px4_ros2_cpp/test/unit/utils/util.cpp index f6e11f46..b09ad924 100644 --- a/px4_ros2_cpp/test/unit/utils/util.cpp +++ b/px4_ros2_cpp/test/unit/utils/util.cpp @@ -24,8 +24,8 @@ void quaternionsApproxEqualTest( } void vectorsApproxEqualTest( - const Eigen::Vector3f & v_expected, const Eigen::Vector3f & v_actual, - const std::string & msg, const float precision) + const Eigen::Vector3d & v_expected, const Eigen::Vector3d & v_actual, + const std::string & msg, const double precision) { EXPECT_TRUE(v_expected.isApprox(v_actual, precision)) << "test: " << msg << std::endl @@ -33,6 +33,16 @@ void vectorsApproxEqualTest( << "v_expected: " << v_expected.transpose() << std::endl; } +void vectorsApproxEqualTest( + const Eigen::Vector3f & v_expected, const Eigen::Vector3f & v_actual, + const std::string & msg, const float precision) +{ + vectorsApproxEqualTest( + v_expected.cast(), + v_actual.cast(), + msg, static_cast(precision)); +} + void quaternionToEulerReconstructionTest(const Eigen::Quaternionf & q, const std::string & msg) { Eigen::Vector3f v_euler = px4_ros2::quaternionToEulerRpy(q); diff --git a/px4_ros2_cpp/test/unit/utils/util.hpp b/px4_ros2_cpp/test/unit/utils/util.hpp index 1e61d658..8032ef7c 100644 --- a/px4_ros2_cpp/test/unit/utils/util.hpp +++ b/px4_ros2_cpp/test/unit/utils/util.hpp @@ -18,6 +18,10 @@ void vectorsApproxEqualTest( const Eigen::Vector3f & v_expected, const Eigen::Vector3f & v_actual, const std::string & msg = "", float precision = 1e-3); +void vectorsApproxEqualTest( + const Eigen::Vector3d & v_expected, const Eigen::Vector3d & v_actual, + const std::string & msg = "", double precision = 1e-6); + void quaternionToEulerReconstructionTest( const Eigen::Quaternionf & q, const std::string & msg = ""); From b17daee9cf833ffc058764059491e57e1fd5dff3 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 21 Feb 2024 22:01:18 +0100 Subject: [PATCH 5/5] implemented some review suggestions --- .../px4_ros2/control/setpoint_types/goto.hpp | 2 +- .../include/px4_ros2/utils/geodesic.hpp | 21 +++++++---- .../src/control/setpoint_types/goto.cpp | 4 +-- px4_ros2_cpp/src/utils/geodesic.cpp | 36 ++++++++++++++----- 4 files changed, 44 insertions(+), 19 deletions(-) diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp index 44bdf4dc..b12140fc 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp @@ -82,7 +82,7 @@ class GotoGlobalSetpointType private: rclcpp::Node & _node; - std::shared_ptr _map_projection; + std::unique_ptr _map_projection; std::shared_ptr _goto_setpoint; }; diff --git a/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp b/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp index e3159b61..91f8ef70 100644 --- a/px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp +++ b/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 _map_projection_math; + std::unique_ptr _map_projection_math; rclcpp::Subscription::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) { diff --git a/px4_ros2_cpp/src/control/setpoint_types/goto.cpp b/px4_ros2_cpp/src/control/setpoint_types/goto.cpp index 488bf6ad..9d45f577 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/goto.cpp +++ b/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(context)), +: _node(context.node()), _map_projection(std::make_unique(context)), _goto_setpoint(std::make_shared(context)) { RequirementFlags requirements{}; @@ -82,7 +82,7 @@ void GotoGlobalSetpointType::update( const std::optional & 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; diff --git a/px4_ros2_cpp/src/utils/geodesic.cpp b/px4_ros2_cpp/src/utils/geodesic.cpp index bc1b83de..89bebad3 100644 --- a/px4_ros2_cpp/src/utils/geodesic.cpp +++ b/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(); + _map_projection_math = std::make_unique(); _vehicle_local_position_sub = _node.create_subscription( - "/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