Common getter functions for the ETSI ITS Common Data Dictionary (CDD) v1.3.1 and v2.1.1. More...

#include <array>
#include <cmath>
#include <cstdint>
#include <GeographicLib/UTMUPS.hpp>

Functions
uint32_t	getStationID (const ItsPduHeader &header)
	Get the StationID of ItsPduHeader.

double	getLatitude (const Latitude &latitude)
	Get the Latitude value.

double	getLongitude (const Longitude &longitude)
	Get the Longitude value.

double	getAltitude (const Altitude &altitude)
	Get the Altitude value.

double	getSpeed (const Speed &speed)
	Get the vehicle speed.

double	getSpeedConfidence (const Speed &speed)
	Get the Speed Confidence.

template<typename T>
gm::PointStamped	getUTMPosition (const T &reference_position, int &zone, bool &northp)
	Get the UTM Position defined by the given ReferencePosition.

template<typename Heading>
double	getHeadingCDD (const Heading &heading)
	Get the Heading value.

template<typename Heading>
double	getHeadingConfidenceCDD (const Heading &heading)
	Get the Heading value.

template<typename SemiAxisLength>
double	getSemiAxis (const SemiAxisLength &semi_axis_length)
	Get the Semi Axis object.

template<typename PosConfidenceEllipse>
std::tuple< double, double, double >	getPosConfidenceEllipse (const PosConfidenceEllipse &position_confidence_ellipse)
	Extract major axis length, minor axis length and orientation from the given position confidence ellipse.

std::array< double, 4 >	CovMatrixFromConfidenceEllipse (double semi_major, double semi_minor, double major_orientation, const double object_heading)
	Convert the confidence ellipse to a covariance matrix.

std::array< double, 4 >	WGSCovMatrixFromConfidenceEllipse (double semi_major, double semi_minor, double major_orientation)
	Convert the confidence ellipse to a covariance matrix.

template<typename PosConfidenceEllipse>
std::array< double, 4 >	getPosConfidenceEllipse (const PosConfidenceEllipse &position_confidence_ellipse, const double object_heading)
	Get the covariance matrix of the position confidence ellipse.

template<typename PosConfidenceEllipse>
std::array< double, 4 >	getWGSPosConfidenceEllipse (const PosConfidenceEllipse &position_confidence_ellipse)
	Get the covariance matrix of the position confidence ellipse.

Detailed Description

Common getter functions for the ETSI ITS Common Data Dictionary (CDD) v1.3.1 and v2.1.1.

Definition in file cdd_getters_common.h.

Function Documentation

◆ CovMatrixFromConfidenceEllipse()

std::array< double, 4 > CovMatrixFromConfidenceEllipse	(	double	semi_major,
		double	semi_minor,
		double	major_orientation,
		const double	object_heading )

inline

Convert the confidence ellipse to a covariance matrix.

Note that the major_orientation is given in degrees, while the object_heading is given in radians!

Parameters

semi_major	Semi major axis length in meters
semi_minor	Semi minor axis length in meters
major_orientation	Orientation of the major axis in degrees, relative to WGS84
object_heading	object heading in radians, relative to WGS84

Returns: std::array<double, 4> The covariance matrix in vehicle coordinates (x = longitudinal, y = lateral)

Definition at line 181 of file cdd_getters_common.h.

                                                                                                                                                       {
  std::array<double, 4> covariance_matrix;
  double semi_major_squared = semi_major * semi_major / (etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR);
  double semi_minor_squared = semi_minor * semi_minor / (etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR);
  double major_orientation_rad = major_orientation * M_PI / 180;
  double object_heading_rad = object_heading;
  
  double angle_to_object = object_heading_rad - major_orientation_rad;
 
  double cos_angle = std::cos(angle_to_object);
  double sin_angle = std::sin(angle_to_object);
  covariance_matrix[0] = semi_major_squared * cos_angle * cos_angle + semi_minor_squared * sin_angle * sin_angle;
  covariance_matrix[1] = (semi_major_squared - semi_minor_squared) * cos_angle * sin_angle;
  covariance_matrix[2] = covariance_matrix[1];
  covariance_matrix[3] = semi_major_squared * sin_angle * sin_angle + semi_minor_squared * cos_angle * cos_angle;
 
  return covariance_matrix;
}

◆ getAltitude()

double getAltitude ( const Altitude & altitude )

inline

Get the Altitude value.

Parameters

altitude to get the Altitude value from

Returns: Altitude value (above the reference ellipsoid surface) in meter as decimal number

Definition at line 70 of file cdd_getters_common.h.

70{ return ((double)altitude.altitude_value.value) * 1e-2; }

◆ getHeadingCDD()

template<typename Heading>

double getHeadingCDD ( const Heading & heading )

inline

Get the Heading value.

0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West

Parameters

heading to get the Heading value from

Returns: Heading value in degree as decimal number

Definition at line 131 of file cdd_getters_common.h.

131{ return ((double)heading.heading_value.value) * 1e-1; }

◆ getHeadingConfidenceCDD()

template<typename Heading>

double getHeadingConfidenceCDD ( const Heading & heading )

inline

Get the Heading value.

0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West

Parameters

heading to get the Heading standard deviation from

Returns: Heading standard deviation in degree as decimal number

Definition at line 142 of file cdd_getters_common.h.

142{ return ((double)heading.heading_confidence.value) * 1e-1 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; }

◆ getLatitude()

double getLatitude ( const Latitude & latitude )

inline

Get the Latitude value.

Parameters

latitude to get the Latitude value from

Returns: Latitude value in degree as decimal number

Definition at line 54 of file cdd_getters_common.h.

54{ return ((double)latitude.value) * 1e-7; }

◆ getLongitude()

double getLongitude ( const Longitude & longitude )

inline

Get the Longitude value.

Parameters

longitude to get the Longitude value from

Returns: Longitude value in degree as decimal number

Definition at line 62 of file cdd_getters_common.h.

62{ return ((double)longitude.value) * 1e-7; }

◆ getPosConfidenceEllipse() [1/2]

template<typename PosConfidenceEllipse>

std::tuple< double, double, double > getPosConfidenceEllipse ( const PosConfidenceEllipse & position_confidence_ellipse )

inline

Extract major axis length, minor axis length and orientation from the given position confidence ellipse.

Parameters

position_confidence_ellipse The position confidence ellipse to extract the values from

Returns: std::tuple<double, double, double> major axis length in meters, minor axis length in meters, and orientation in degrees

Definition at line 162 of file cdd_getters_common.h.

                                                                                                                         {
  return {
    getSemiAxis(position_confidence_ellipse.semi_major_confidence),
    getSemiAxis(position_confidence_ellipse.semi_minor_confidence),
    position_confidence_ellipse.semi_major_orientation.value * 1e-1
  };
}

◆ getPosConfidenceEllipse() [2/2]

template<typename PosConfidenceEllipse>

std::array< double, 4 > getPosConfidenceEllipse	(	const PosConfidenceEllipse &	position_confidence_ellipse,
		const double	object_heading )

inline

Get the covariance matrix of the position confidence ellipse.

Parameters

position_confidence_ellipse	The position confidence ellipse to get the covariance matrix from
object_heading	The object heading in radians

Returns: std::array<double, 4> The covariance matrix of the position confidence ellipse in vehicle coordinates (x = longitudinal, y = lateral)

Definition at line 223 of file cdd_getters_common.h.

                                                                                                                                        {
  auto [semi_major, semi_minor, major_orientation] = getPosConfidenceEllipse(position_confidence_ellipse);
  return CovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation, object_heading);
}

◆ getSemiAxis()

template<typename SemiAxisLength>

double getSemiAxis ( const SemiAxisLength & semi_axis_length )

inline

Get the Semi Axis object.

Parameters

semi_axis_length The SemiAxisLength object to get the semi axis from

Returns: double the semi axis length in meters

Definition at line 151 of file cdd_getters_common.h.

                                                                  {
  return ((double)semi_axis_length.value) * 1e-2 / etsi_its_msgs::OneCentimeterHelper<SemiAxisLength>::value;
}

◆ getSpeed()

double getSpeed ( const Speed & speed )

inline

Get the vehicle speed.

Parameters

speed to get the speed value from

Returns: speed value in m/s as decimal number

Definition at line 78 of file cdd_getters_common.h.

78{ return ((double)speed.speed_value.value) * 1e-2; }

◆ getSpeedConfidence()

double getSpeedConfidence ( const Speed & speed )

inline

Get the Speed Confidence.

Parameters

speed to get the SpeedConfidence from

Returns: double speed standard deviation in m/s as decimal number

Definition at line 86 of file cdd_getters_common.h.

                                                     {
  return ((double)speed.speed_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-2;
}

◆ getStationID()

uint32_t getStationID ( const ItsPduHeader & header )

inline

Get the StationID of ItsPduHeader.

Parameters

header ItsPduHeader to get the StationID value from

Returns: stationID value

Definition at line 46 of file cdd_getters_common.h.

46{ return header.station_id.value; }

◆ getUTMPosition()

template<typename T>

gm::PointStamped getUTMPosition	(	const T &	reference_position,
		int &	zone,
		bool &	northp )

inline

Get the UTM Position defined by the given ReferencePosition.

The position is transformed into UTM by using GeographicLib::UTMUPS The altitude value is directly used as z-Coordinate

Parameters

[in]	reference_position	ReferencePosition or ReferencePositionWithConfidence to get the UTM Position from
[out]	zone	the UTM zone (zero means UPS)
[out]	northp	hemisphere (true means north, false means south)

Returns: gm::PointStamped geometry_msgs::PointStamped of the given position

Definition at line 102 of file cdd_getters_common.h.

                                                                                           {
  gm::PointStamped utm_point;
  double latitude = getLatitude(reference_position.latitude);
  double longitude = getLongitude(reference_position.longitude);
  utm_point.point.z = getAltitude(reference_position.altitude);
  try {
    GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y);
    std::string hemisphere;
    if (northp) {
      hemisphere = "N";
    } else {
      hemisphere = "S";
    }
    utm_point.header.frame_id = "utm_" + std::to_string(zone) + hemisphere;
  } catch (GeographicLib::GeographicErr& e) {
    throw std::invalid_argument(e.what());
  }
  return utm_point;
}

◆ getWGSPosConfidenceEllipse()

template<typename PosConfidenceEllipse>

std::array< double, 4 > getWGSPosConfidenceEllipse ( const PosConfidenceEllipse & position_confidence_ellipse )

inline

Get the covariance matrix of the position confidence ellipse.

Parameters

position_confidence_ellipse	The position confidence ellipse to get the covariance matrix from
object_heading	The object heading in radians

Returns: std::array<double, 4> The covariance matrix of the position confidence ellipse in WGS coordinates (x = North, y = East)

Definition at line 236 of file cdd_getters_common.h.

                                                                                                              {
  auto [semi_major, semi_minor, major_orientation] = getPosConfidenceEllipse(position_confidence_ellipse);
  return WGSCovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation);
}

◆ WGSCovMatrixFromConfidenceEllipse()

std::array< double, 4 > WGSCovMatrixFromConfidenceEllipse	(	double	semi_major,
		double	semi_minor,
		double	major_orientation )

inline

Convert the confidence ellipse to a covariance matrix.

Note that the major_orientation is given in degrees, while the object_heading is given in radians!

Parameters

semi_major	Semi major axis length in meters
semi_minor	Semi minor axis length in meters
major_orientation	Orientation of the major axis in degrees, relative to WGS84

Returns: std::array<double, 4> The covariance matrix in WGS coordinates (x = North, y = East)

Definition at line 210 of file cdd_getters_common.h.

                                                                                                                             {
  // The WGS covariance matrix is the same as in vehicle coordinates, if it would have a heading of 0.0
  return CovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation, 0.0);
}

Functions

Detailed Description

Function Documentation

◆ CovMatrixFromConfidenceEllipse()

◆ getAltitude()

◆ getHeadingCDD()

◆ getHeadingConfidenceCDD()

◆ getLatitude()

◆ getLongitude()

◆ getPosConfidenceEllipse() [1/2]

◆ getPosConfidenceEllipse() [2/2]

◆ getSemiAxis()

◆ getSpeed()

◆ getSpeedConfidence()

◆ getStationID()

◆ getUTMPosition()

◆ getWGSPosConfidenceEllipse()

◆ WGSCovMatrixFromConfidenceEllipse()