From 9a43ef37d4d3efc655186d8af862703f9d253e35 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Mon, 26 Feb 2024 11:31:38 +0100 Subject: [PATCH] doxygen: update docs and create groups for utils folder --- Doxyfile | 2 +- .../px4_ros2/utils/frame_conversion.hpp | 10 ++ .../include/px4_ros2/utils/geodesic.hpp | 132 ++++++++++++------ .../include/px4_ros2/utils/geometry.hpp | 7 +- .../include/px4_ros2/utils/groups.dox | 3 + .../src/utils/map_projection_impl.hpp | 8 +- 6 files changed, 115 insertions(+), 47 deletions(-) create mode 100644 px4_ros2_cpp/include/px4_ros2/utils/groups.dox diff --git a/Doxyfile b/Doxyfile index 7fbb0d22..e59e40d5 100644 --- a/Doxyfile +++ b/Doxyfile @@ -523,7 +523,7 @@ EXTRACT_PACKAGE = NO # included in the documentation. # The default value is: NO. -EXTRACT_STATIC = NO +EXTRACT_STATIC = YES # If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined # locally in source files will be included in the documentation. If set to NO, diff --git a/px4_ros2_cpp/include/px4_ros2/utils/frame_conversion.hpp b/px4_ros2_cpp/include/px4_ros2/utils/frame_conversion.hpp index 1dd8c61c..04c8d683 100644 --- a/px4_ros2_cpp/include/px4_ros2/utils/frame_conversion.hpp +++ b/px4_ros2_cpp/include/px4_ros2/utils/frame_conversion.hpp @@ -3,6 +3,12 @@ * SPDX-License-Identifier: BSD-3-Clause ****************************************************************************/ +/** + * @defgroup frame_conversion Frame Conversion + * @ingroup utils + * This group contains helper functions to switch between reference frames. + */ + #pragma once #include @@ -10,6 +16,9 @@ namespace px4_ros2 { +/** \ingroup frame_conversion + * @{ + */ /** * @brief Converts attitude from NED to ENU frame. @@ -176,4 +185,5 @@ static inline Eigen::Matrix varianceEnuToNed(const Eigen::Matrix @@ -13,9 +27,20 @@ namespace px4_ros2 { +/** \ingroup geodesic + * @{ + */ class MapProjectionImpl; +/** + * @brief Provides methods to convert between the geographical coordinate system ("global") + * and the local azimuthal equidistant plane ("local"). + * + * This class handles ROS2 subscription and initializing the map projection reference point from PX4. + * @see MapProjectionImpl + * @ingroup geodesic + */ class MapProjection { public: @@ -29,38 +54,38 @@ class MapProjection bool isInitialized() const; /** - * Transform a point in the geographic coordinate system to the local + * @brief 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°) + * @param global_position lat [deg], lon [deg] * @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 + * @brief 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°) + * @param global_position lat [deg], lon [deg], alt AMSL [m] * @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 + * @brief 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°) + * @return the point in geographic coordinates as lat [deg], lon [deg] */ Eigen::Vector2d localToGlobal(const Eigen::Vector2f & local_position) const; /** - * Transform a point in the local azimuthal equidistant plane to the + * @brief 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°) + * @return the point in geographic coordinates as lat [deg], lon [deg], alt AMSL [m] */ Eigen::Vector3d localToGlobal(const Eigen::Vector3f & local_position) const; @@ -71,7 +96,7 @@ class MapProjection void assertInitalized() const; /** - * Callback for VehicleLocalPosition messages which intializes and updates the map projection reference point from PX4 + * @brief Callback for VehicleLocalPosition messages which intializes and updates the map projection reference point from PX4 * * @param msg the VehicleLocalPosition message */ @@ -83,22 +108,26 @@ class MapProjection }; /** - * Compute the horizontal distance between two global positions in meters. + * @brief 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°) + * @param global_position_now current lat [deg], lon [deg] + * @param global_position_next next lat [deg], lon [deg] * @return the horizontal distance [m] between both positions + * + * @ingroup geodesic */ float horizontalDistanceToGlobalPosition( const Eigen::Vector2d & global_position_now, const Eigen::Vector2d & global_position_next); /** - * Compute the horizontal distance between two global positions in meters. + * @brief 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°) + * @param global_position_now current lat [deg], lon [deg], alt AMSL [m] + * @param global_position_next next lat [deg], lon [deg], alt AMSL [m] * @return the horizontal distance [m] between both positions + + * @ingroup geodesic */ static inline float horizontalDistanceToGlobalPosition( const Eigen::Vector3d & global_position_now, @@ -110,33 +139,39 @@ static inline float horizontalDistanceToGlobalPosition( } /** - * Compute the distance between two global positions in meters. + * @brief 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°) + * @param global_position_now current lat [deg], lon [deg] + * @param global_position_next next lat [deg], lon [deg] * @return the distance [m] between both positions + + * @ingroup geodesic */ float distanceToGlobalPosition( const Eigen::Vector3d & global_position_now, const Eigen::Vector3d & global_position_next); /** - * Compute the heading to the next global position in radians. + * @brief 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°) + * @param global_position_now current lat [deg], lon [deg] + * @param global_position_next next lat [deg], lon [deg] * @return the heading [rad] to the next global position (clockwise) + * + * @ingroup geodesic */ float headingToGlobalPosition( const Eigen::Vector2d & global_position_now, const Eigen::Vector2d & global_position_next); /** - * Compute the heading to the next global position in radians. + * @brief 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°) + * @param global_position_now current lat [deg], lon [deg], alt AMSL [m] + * @param global_position_next next lat [deg], lon [deg], alt AMSL [m] * @return the heading [rad] to the next global position (clockwise) + * + * @ingroup geodesic */ static inline float headingToGlobalPosition( const Eigen::Vector3d & global_position_now, @@ -148,22 +183,26 @@ static inline float headingToGlobalPosition( } /** - * Compute the vector to the next global position in meters. + * @brief 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°) + * @param global_position_now current lat [deg], lon [deg] + * @param global_position_next next lat [deg], lon [deg] * @return the vector [m^2] in local frame to the next global position (NE) + * + * @ingroup geodesic */ 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. + * @brief 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°) + * @param global_position_now current lat [deg], lon [deg], alt AMSL [m] + * @param global_position_next next lat [deg], lon [deg], alt AMSL [m] * @return the vector [m^3] in local frame to the next global position (NED) + * + * @ingroup geodesic */ static inline Eigen::Vector3f vectorToGlobalPosition( const Eigen::Vector3d & global_position_now, @@ -178,12 +217,14 @@ static inline Eigen::Vector3f vectorToGlobalPosition( } /** - * Compute the global position on the line vector defined by two positions (start -> end) at a certain distance + * @brief 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] (47.1234567°, not 471234567°) - * @param global_position_line_end line end lat [deg], lon [deg] (47.1234567°, not 471234567°) + * @param global_position_line_start line start lat [deg], lon [deg] + * @param global_position_line_end line end lat [deg], lon [deg] * @param dist distance [m] of target global position from position 'start' towards position 'end' (can be negative) + * + * @ingroup geodesic */ Eigen::Vector2d globalPositionFromLineAndDist( const Eigen::Vector2d & global_position_line_start, @@ -191,26 +232,30 @@ Eigen::Vector2d globalPositionFromLineAndDist( float dist_from_start); /** - * Compute the global position given another global position, distance and heading + * @brief 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 global_position_now current lat [deg], lon [deg] * @param heading heading from the current position [rad] (clockwise) * @param distance distance from the current position [m] * @return the target global position + * + * @ingroup geodesic */ Eigen::Vector2d globalPositionFromHeadingAndDist( const Eigen::Vector2d & global_position_now, float heading, float dist); /** - * Compute the global position given another global position, distance and heading + * @brief 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 global_position_now current lat [deg], lon [deg] * @param heading heading from the current position [rad] (clockwise) * @param distance distance from the current position [m] * @return the target global position + * + * @ingroup geodesic */ static inline Eigen::Vector3d globalPositionFromHeadingAndDist( const Eigen::Vector3d & global_position_now, @@ -225,22 +270,26 @@ static inline Eigen::Vector3d globalPositionFromHeadingAndDist( } /** - * Compute the global position from adding a local frame vector to the current global position. + * @brief 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 global_position current lat [deg], lon [deg] * @param vector_ne local vector to add [m^2] (NE) * @return the resulting global position from the addition + * + * @ingroup geodesic */ 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. + * @brief 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 global_position current lat [deg], lon [deg], alt AMSL [m] * @param vector_ne local vector to add [m^3] (NED) * @return the resulting global position from the addition + * + * @ingroup geodesic */ static inline Eigen::Vector3d addVectorToGlobalPosition( const Eigen::Vector3d & global_position, @@ -254,4 +303,5 @@ static inline Eigen::Vector3d addVectorToGlobalPosition( global_position.z() + d_alt}; } +/** @}*/ } // namespace px4_ros2 diff --git a/px4_ros2_cpp/include/px4_ros2/utils/geometry.hpp b/px4_ros2_cpp/include/px4_ros2/utils/geometry.hpp index e1bcb3cf..2f364ce2 100644 --- a/px4_ros2_cpp/include/px4_ros2/utils/geometry.hpp +++ b/px4_ros2_cpp/include/px4_ros2/utils/geometry.hpp @@ -4,7 +4,8 @@ ****************************************************************************/ /** - * @file geometry.hpp + * @defgroup geometry Geometry + * @ingroup utils * * All rotations and axis systems follow the right-hand rule * @@ -27,6 +28,9 @@ namespace px4_ros2 { +/** \ingroup geometry + * @{ + */ /** * @brief Converts radians to degrees @@ -185,4 +189,5 @@ Type quaternionToYaw(const Eigen::Quaternion & q) return std::atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z); } +/** @}*/ } // namespace px4_ros2 diff --git a/px4_ros2_cpp/include/px4_ros2/utils/groups.dox b/px4_ros2_cpp/include/px4_ros2/utils/groups.dox new file mode 100644 index 00000000..e5ce8145 --- /dev/null +++ b/px4_ros2_cpp/include/px4_ros2/utils/groups.dox @@ -0,0 +1,3 @@ +/*! +\defgroup utils Utils +*/ diff --git a/px4_ros2_cpp/src/utils/map_projection_impl.hpp b/px4_ros2_cpp/src/utils/map_projection_impl.hpp index 2bc4e444..91a86f63 100644 --- a/px4_ros2_cpp/src/utils/map_projection_impl.hpp +++ b/px4_ros2_cpp/src/utils/map_projection_impl.hpp @@ -91,7 +91,7 @@ class MapProjectionImpl final * 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°) + * @param global_position lat [deg], lon [deg] * @return the point in local coordinates as north, east [m] */ Eigen::Vector2f globalToLocal(const Eigen::Vector2d & global_position) const; @@ -100,7 +100,7 @@ class MapProjectionImpl final * 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°) + * @param global_position lat [deg], lon [deg], alt AMSL [m] * @return the point in local coordinates as north, east, down [m] */ inline Eigen::Vector3f globalToLocal(const Eigen::Vector3d & global_position) const @@ -117,7 +117,7 @@ class MapProjectionImpl final * 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°) + * @return the point in geographic coordinates as lat [deg], lon [deg] */ Eigen::Vector2d localToGlobal(const Eigen::Vector2f & local_position) const; @@ -126,7 +126,7 @@ class MapProjectionImpl final * 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°) + * @return the point in geographic coordinates as lat [deg], lon [deg], alt AMSL [m] */ inline Eigen::Vector3d localToGlobal(const Eigen::Vector3f & local_position) const {