Skip to content

Commit

Permalink
doxygen: update docs and create groups for utils folder
Browse files Browse the repository at this point in the history
  • Loading branch information
GuillaumeLaine committed Feb 26, 2024
1 parent ababec0 commit 9a43ef3
Show file tree
Hide file tree
Showing 6 changed files with 115 additions and 47 deletions.
2 changes: 1 addition & 1 deletion Doxyfile
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
10 changes: 10 additions & 0 deletions px4_ros2_cpp/include/px4_ros2/utils/frame_conversion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,22 @@
* 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 <Eigen/Eigen>
#include <px4_ros2/utils/geometry.hpp>

namespace px4_ros2
{
/** \ingroup frame_conversion
* @{
*/

/**
* @brief Converts attitude from NED to ENU frame.
Expand Down Expand Up @@ -176,4 +185,5 @@ static inline Eigen::Matrix<T, 3, 1> varianceEnuToNed(const Eigen::Matrix<T, 3,
return {v_enu.y(), v_enu.x(), v_enu.z()};
}

/** @}*/
} // namespace px4_ros2
132 changes: 91 additions & 41 deletions px4_ros2_cpp/include/px4_ros2/utils/geodesic.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,20 @@
* SPDX-License-Identifier: BSD-3-Clause
****************************************************************************/

/**
* @defgroup geodesic Geodesic
* @ingroup utils
*
* This group contains helper functions to convert points between
* the geographical coordinate system ("global") and the local azimuthal
* equidistant plane ("local"). Additionaly it provides methods for commonly
* used geodesic operations.
*
* Latitude and longitude are in degrees (degrees: 8.1234567°, not 81234567°).
* Altitude is in meters in the above mean sea level (AMSL) frame.
* Heading is in radians starting North going clockwise.
*/

#pragma once

#include <cmath>
Expand All @@ -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:
Expand All @@ -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;

Expand All @@ -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
*/
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -178,39 +217,45 @@ 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,
const Eigen::Vector2d & global_position_line_end,
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,
Expand All @@ -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,
Expand All @@ -254,4 +303,5 @@ static inline Eigen::Vector3d addVectorToGlobalPosition(
global_position.z() + d_alt};
}

/** @}*/
} // namespace px4_ros2
7 changes: 6 additions & 1 deletion px4_ros2_cpp/include/px4_ros2/utils/geometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,8 @@
****************************************************************************/

/**
* @file geometry.hpp
* @defgroup geometry Geometry
* @ingroup utils
*
* All rotations and axis systems follow the right-hand rule
*
Expand All @@ -27,6 +28,9 @@

namespace px4_ros2
{
/** \ingroup geometry
* @{
*/

/**
* @brief Converts radians to degrees
Expand Down Expand Up @@ -185,4 +189,5 @@ Type quaternionToYaw(const Eigen::Quaternion<Type> & q)
return std::atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);
}

/** @}*/
} // namespace px4_ros2
3 changes: 3 additions & 0 deletions px4_ros2_cpp/include/px4_ros2/utils/groups.dox
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
/*!
\defgroup utils Utils
*/
Loading

0 comments on commit 9a43ef3

Please sign in to comment.