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 AccelerationMagnitude>
double	getAccelerationMagnitude (const AccelerationMagnitude &acceleration_magnitude)
	Get the AccelerationMagnitude value.

template<typename AccelerationMagnitude>
double	getAccelerationMagnitudeConfidence (const AccelerationMagnitude &acceleration_magnitude)
	Get the AccelerationMagnitude 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 YawRate>
double	getYawRateCDD (const YawRate &yaw_rate)
	Get the Yaw Rate value.

template<typename YawRate, typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
double	getYawRateConfidenceCDD (const YawRate &yaw_rate)
	Get the Yaw Rate Confidence.

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 243 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;
}

◆ getAccelerationMagnitude()

template<typename AccelerationMagnitude>

double getAccelerationMagnitude ( const AccelerationMagnitude & acceleration_magnitude )

inline

Get the AccelerationMagnitude value.

Parameters

acceleration_magnitude to get the AccelerationMagnitude from

Returns: double acceleration magnitude in m/s^2 as decimal number

Definition at line 102 of file cdd_getters_common.h.

                                                                                            {
  return ((double)acceleration_magnitude.acceleration_magnitude_value.value) * 1e-1;
}

◆ getAccelerationMagnitudeConfidence()

template<typename AccelerationMagnitude>

double getAccelerationMagnitudeConfidence ( const AccelerationMagnitude & acceleration_magnitude )

inline

Get the AccelerationMagnitude Confidence.

Parameters

acceleration_magnitude to get the AccelerationMagnitudeConfidence from

Returns: double acceleration magnitude standard deviation in m/s^2 as decimal number

Definition at line 113 of file cdd_getters_common.h.

                                                                                                      {
  return ((double)acceleration_magnitude.acceleration_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-1;
}

◆ 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 (0 if unavailable)

Definition at line 70 of file cdd_getters_common.h.

                                                    {
  if (altitude.altitude_value.value == AltitudeValue::UNAVAILABLE) {
    return 0.0;
  }
  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 158 of file cdd_getters_common.h.

158{ 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 169 of file cdd_getters_common.h.

169{ 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 224 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 285 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 213 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 83 of file cdd_getters_common.h.

83{ 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 91 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 129 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 298 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);
}

◆ getYawRateCDD()

template<typename YawRate>

double getYawRateCDD ( const YawRate & yaw_rate )

inline

Get the Yaw Rate value.

Parameters

yaw_rate The YawRate object to get the yaw rate from

Returns: double The yaw rate in degrees per second

Definition at line 178 of file cdd_getters_common.h.

                                                     {
  return ((double)yaw_rate.yaw_rate_value.value) * 1e-2; // Yaw rate in deg/s
}

◆ getYawRateConfidenceCDD()

template<typename YawRate, typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>

double getYawRateConfidenceCDD ( const YawRate & yaw_rate )

inline

Get the Yaw Rate Confidence.

Parameters

yaw_rate The YawRate object to get the yaw rate confidence from

Returns: double The yaw rate standard deviation in degrees per second

Definition at line 189 of file cdd_getters_common.h.

                                                               {
  auto val = yaw_rate.yaw_rate_confidence.value;
  static const std::map<uint8_t, double> confidence_map = {
      {YawRateConfidence::UNAVAILABLE, 0.0},
      {YawRateConfidence::DEG_SEC_000_01, 0.01},
      {YawRateConfidence::DEG_SEC_000_05, 0.05},
      {YawRateConfidence::DEG_SEC_000_10, 0.1},
      {YawRateConfidence::DEG_SEC_001_00, 1.0},
      {YawRateConfidence::DEG_SEC_005_00, 5.0},
      {YawRateConfidence::DEG_SEC_010_00, 10.0},
      {YawRateConfidence::DEG_SEC_100_00, 100.0},
      {YawRateConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
  };
  return confidence_map.at(val) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; 
}

◆ 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 272 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()

◆ getAccelerationMagnitude()

◆ getAccelerationMagnitudeConfidence()

◆ getAltitude()

◆ getHeadingCDD()

◆ getHeadingConfidenceCDD()

◆ getLatitude()

◆ getLongitude()

◆ getPosConfidenceEllipse() [1/2]

◆ getPosConfidenceEllipse() [2/2]

◆ getSemiAxis()

◆ getSpeed()

◆ getSpeedConfidence()

◆ getStationID()

◆ getUTMPosition()

◆ getWGSPosConfidenceEllipse()

◆ getYawRateCDD()

◆ getYawRateConfidenceCDD()

◆ WGSCovMatrixFromConfidenceEllipse()