PX4 ROS 2 Interface Library
Library to interface with PX4 from a companion computer using ROS 2
All Classes Functions Variables Typedefs Enumerations Enumerator Modules Pages
Geodesic

Classes

class  px4_ros2::MapProjection
 Provides methods to convert between the geographical coordinate system ("global") and the local azimuthal equidistant plane ("local"). More...
 
float px4_ros2::horizontalDistanceToGlobalPosition (const Eigen::Vector2d &global_position_now, const Eigen::Vector2d &global_position_next)
 Compute the horizontal distance between two global positions in meters. More...
 
static float px4_ros2::horizontalDistanceToGlobalPosition (const Eigen::Vector3d &global_position_now, const Eigen::Vector3d &global_position_next)
 Compute the horizontal distance between two global positions in meters. More...
 
float px4_ros2::distanceToGlobalPosition (const Eigen::Vector3d &global_position_now, const Eigen::Vector3d &global_position_next)
 Compute the distance between two global positions in meters. More...
 
float px4_ros2::headingToGlobalPosition (const Eigen::Vector2d &global_position_now, const Eigen::Vector2d &global_position_next)
 Compute the heading to the next global position in radians. More...
 
static float px4_ros2::headingToGlobalPosition (const Eigen::Vector3d &global_position_now, const Eigen::Vector3d &global_position_next)
 Compute the heading to the next global position in radians. More...
 
Eigen::Vector2f px4_ros2::vectorToGlobalPosition (const Eigen::Vector2d &global_position_now, const Eigen::Vector2d &global_position_next)
 Compute the vector to the next global position in meters. More...
 
static Eigen::Vector3f px4_ros2::vectorToGlobalPosition (const Eigen::Vector3d &global_position_now, const Eigen::Vector3d &global_position_next)
 Compute the vector to the next global position in meters. More...
 
Eigen::Vector2d px4_ros2::globalPositionFromLineAndDist (const Eigen::Vector2d &global_position_line_start, const Eigen::Vector2d &global_position_line_end, float dist_from_start)
 Compute the global position on the line vector defined by two positions (start -> end) at a certain distance from global position 'start'. More...
 
Eigen::Vector2d px4_ros2::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. More...
 
static Eigen::Vector3d px4_ros2::globalPositionFromHeadingAndDist (const Eigen::Vector3d &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. More...
 
Eigen::Vector2d px4_ros2::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. More...
 
static Eigen::Vector3d px4_ros2::addVectorToGlobalPosition (const Eigen::Vector3d &global_position, const Eigen::Vector3f &vector_ned)
 Compute the global position from adding a local frame vector to the current global position. More...
 

Detailed Description

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.

Function Documentation

◆ addVectorToGlobalPosition() [1/2]

Eigen::Vector2d px4_ros2::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.

Parameters
global_positioncurrent lat [deg], lon [deg]
vector_nelocal vector to add [m^2] (NE)
Returns
the resulting global position from the addition

◆ addVectorToGlobalPosition() [2/2]

static Eigen::Vector3d px4_ros2::addVectorToGlobalPosition ( const Eigen::Vector3d &  global_position,
const Eigen::Vector3f &  vector_ned 
)
inlinestatic

Compute the global position from adding a local frame vector to the current global position.

Parameters
global_positioncurrent lat [deg], lon [deg], alt AMSL [m]
vector_nedlocal vector to add [m^3] (NED)
Returns
the resulting global position from the addition

◆ distanceToGlobalPosition()

float px4_ros2::distanceToGlobalPosition ( const Eigen::Vector3d &  global_position_now,
const Eigen::Vector3d &  global_position_next 
)

Compute the distance between two global positions in meters.

Parameters
global_position_nowcurrent lat [deg], lon [deg]
global_position_nextnext lat [deg], lon [deg]
Returns
the distance [m] between both positions

◆ globalPositionFromHeadingAndDist() [1/2]

Eigen::Vector2d px4_ros2::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.

Parameters
global_position_nowcurrent lat [deg], lon [deg]
headingheading from the current position [rad] (clockwise)
distdistance from the current position [m]
Returns
the target global position

◆ globalPositionFromHeadingAndDist() [2/2]

static Eigen::Vector3d px4_ros2::globalPositionFromHeadingAndDist ( const Eigen::Vector3d &  global_position_now,
float  heading,
float  dist 
)
inlinestatic

Compute the global position given another global position, distance and heading see http://www.movable-type.co.uk/scripts/latlong.html.

Parameters
global_position_nowcurrent lat [deg], lon [deg]
headingheading from the current position [rad] (clockwise)
distdistance from the current position [m]
Returns
the target global position

◆ globalPositionFromLineAndDist()

Eigen::Vector2d px4_ros2::globalPositionFromLineAndDist ( const Eigen::Vector2d &  global_position_line_start,
const Eigen::Vector2d &  global_position_line_end,
float  dist_from_start 
)

Compute the global position on the line vector defined by two positions (start -> end) at a certain distance from global position 'start'.

Parameters
global_position_line_startline start lat [deg], lon [deg]
global_position_line_endline end lat [deg], lon [deg]
dist_from_startdistance [m] of target global position from position 'start' towards position 'end' (can be negative)

◆ headingToGlobalPosition() [1/2]

float px4_ros2::headingToGlobalPosition ( const Eigen::Vector2d &  global_position_now,
const Eigen::Vector2d &  global_position_next 
)

Compute the heading to the next global position in radians.

Parameters
global_position_nowcurrent lat [deg], lon [deg]
global_position_nextnext lat [deg], lon [deg]
Returns
the heading [rad] to the next global position (clockwise)

◆ headingToGlobalPosition() [2/2]

static float px4_ros2::headingToGlobalPosition ( const Eigen::Vector3d &  global_position_now,
const Eigen::Vector3d &  global_position_next 
)
inlinestatic

Compute the heading to the next global position in radians.

Parameters
global_position_nowcurrent lat [deg], lon [deg], alt AMSL [m]
global_position_nextnext lat [deg], lon [deg], alt AMSL [m]
Returns
the heading [rad] to the next global position (clockwise)

◆ horizontalDistanceToGlobalPosition() [1/2]

float px4_ros2::horizontalDistanceToGlobalPosition ( const Eigen::Vector2d &  global_position_now,
const Eigen::Vector2d &  global_position_next 
)

Compute the horizontal distance between two global positions in meters.

Parameters
global_position_nowcurrent lat [deg], lon [deg]
global_position_nextnext lat [deg], lon [deg]
Returns
the horizontal distance [m] between both positions

◆ horizontalDistanceToGlobalPosition() [2/2]

static float px4_ros2::horizontalDistanceToGlobalPosition ( const Eigen::Vector3d &  global_position_now,
const Eigen::Vector3d &  global_position_next 
)
inlinestatic

Compute the horizontal distance between two global positions in meters.

Parameters
global_position_nowcurrent lat [deg], lon [deg], alt AMSL [m]
global_position_nextnext lat [deg], lon [deg], alt AMSL [m]
Returns
the horizontal distance [m] between both positions

◆ vectorToGlobalPosition() [1/2]

Eigen::Vector2f px4_ros2::vectorToGlobalPosition ( const Eigen::Vector2d &  global_position_now,
const Eigen::Vector2d &  global_position_next 
)

Compute the vector to the next global position in meters.

Parameters
global_position_nowcurrent lat [deg], lon [deg]
global_position_nextnext lat [deg], lon [deg]
Returns
the vector [m^2] in local frame to the next global position (NE)

◆ vectorToGlobalPosition() [2/2]

static Eigen::Vector3f px4_ros2::vectorToGlobalPosition ( const Eigen::Vector3d &  global_position_now,
const Eigen::Vector3d &  global_position_next 
)
inlinestatic

Compute the vector to the next global position in meters.

Parameters
global_position_nowcurrent lat [deg], lon [deg], alt AMSL [m]
global_position_nextnext lat [deg], lon [deg], alt AMSL [m]
Returns
the vector [m^3] in local frame to the next global position (NED)