From 3f3ff64eb54c097f9d68affe47c2b24515643023 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 21 Feb 2024 22:01:18 +0100 Subject: [PATCH] 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 44bdf4d..b12140f 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 e3159b6..91f8ef7 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 488bf6a..9d45f57 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 bc1b83d..89bebad 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