cdd_setters_common.h File Reference

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

#include <etsi_its_msgs_utils/impl/checks.h>
#include <etsi_its_msgs_utils/impl/constants.h>
#include <GeographicLib/UTMUPS.hpp>
#include <array>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <type_traits>

Go to the source code of this file.

Functions
void	setTimestampITS (TimestampIts &timestamp_its, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin() ->second)
	Set the TimestampITS object.
void	setLatitude (Latitude &latitude, const double deg)
	Set the Latitude object.
void	setLongitude (Longitude &longitude, const double deg)
	Set the Longitude object.
void	setAltitudeValue (AltitudeValue &altitude, const double value)
	Set the AltitudeValue object.
void	setAltitude (Altitude &altitude, const double value)
	Set the Altitude object.
void	setSpeedValue (SpeedValue &speed, const double value)
	Set the SpeedValue object.
void	setSpeedConfidence (SpeedConfidence &speed_confidence, const double value)
	Set the Speed Confidence object.
void	setSpeed (Speed &speed, const double value, const double confidence=std::numeric_limits< double >::infinity())
	Set the Speed object.
template<typename AccelerationConfidence>
void	setAccelerationConfidence (AccelerationConfidence &accel_confidence, const double value)
	Set the Acceleration Confidence object.
template<typename T>
void	setReferencePosition (T &ref_position, const double latitude, const double longitude, const double altitude=AltitudeValue::UNAVAILABLE)
	Sets the reference position in the given ReferencePostion object.
template<typename T>
void	setFromUTMPosition (T &reference_position, const gm::PointStamped &utm_position, const int zone, const bool northp)
	Set the ReferencePosition from a given UTM-Position.
template<typename HeadingValue>
void	setHeadingValue (HeadingValue &heading, const double value)
	Set the HeadingValue object.
template<typename HeadingConfidence>
void	setHeadingConfidence (HeadingConfidence &heading_confidence, const double value)
	Set the Heading Confidence object.
template<typename Heading, typename HeadingConfidence = decltype(Heading::heading_confidence)>
void	setHeadingCDD (Heading &heading, const double value, double confidence=std::numeric_limits< double >::infinity())
	Set the Heading object.
template<typename SemiAxisLength>
void	setSemiAxis (SemiAxisLength &semi_axis_length, const double length)
	Set the Semi Axis length.
template<typename PosConfidenceEllipse>
void	setPosConfidenceEllipse (PosConfidenceEllipse &position_confidence_ellipse, const double semi_major_axis, const double semi_minor_axis, const double orientation)
	Set the Pos Confidence Ellipse object.
std::tuple< double, double, double >	confidenceEllipseFromCovMatrix (const std::array< double, 4 > &covariance_matrix, const double object_heading)
	Gets the values needed to set a confidence ellipse from a covariance matrix.
std::tuple< double, double, double >	confidenceEllipseFromWGSCovMatrix (const std::array< double, 4 > &covariance_matrix)
	Gets the values needed to set a confidence ellipse from a covariance matrix.
template<typename PosConfidenceEllipse>
void	setPosConfidenceEllipse (PosConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix, const double object_heading)
	Set the Pos Confidence Ellipse object.
template<typename PosConfidenceEllipse>
void	setWGSPosConfidenceEllipse (PosConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix)
	Set the Pos Confidence Ellipse object.

Detailed Description

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

Definition in file cdd_setters_common.h.

Function Documentation

confidenceEllipseFromCovMatrix()

std::tuple< double, double, double > confidenceEllipseFromCovMatrix ( const std::array< double, 4 > & covariance_matrix, const double object_heading )

inline

Gets the values needed to set a confidence ellipse from a covariance matrix.

Parameters

covariance_matrix	The four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the object (x = longitudinal, y = lateral)
object_heading	The heading of the object in rad, with respect to WGS84

Returns

std::tuple<double, double, double> semi_major_axis [m], semi_minor_axis [m], orientation [deg], with respect to WGS84

Definition at line 319 of file cdd_setters_common.h.

                                                                                                                                                  {
  
  if(std::abs(covariance_matrix[1] - covariance_matrix[2]) > 1e-6) {
    throw std::invalid_argument("Covariance matrix is not symmetric");
  }
  double trace = covariance_matrix[0] + covariance_matrix[3];
  double determinant = covariance_matrix[0] * covariance_matrix[3] - covariance_matrix[1] * covariance_matrix[1];
  if (determinant <= 0 || covariance_matrix[0] <= 0) {
    // https://sites.math.northwestern.edu/~clark/285/2006-07/handouts/pos-def.pdf:
    // Therefore, a necessary and sufficient condition for the quadratic form of a symmetric 2 × 2 matrix
    // to be positive definite is for det(A) > 0 and a > 0
    throw std::invalid_argument("Covariance matrix is not positive definite");
  }
  double eigenvalue1 = trace / 2 + std::sqrt(trace * trace / 4 - determinant);
  double eigenvalue2 = trace / 2 - std::sqrt(trace * trace / 4 - determinant);
  double semi_major_axis = std::sqrt(eigenvalue1) * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR;
  double semi_minor_axis = std::sqrt(eigenvalue2) * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR;
  // object_heading - orientation of the ellipse, as WGS84 has positive angles to the right
  double orientation = object_heading - 0.5 * std::atan2(2 * covariance_matrix[1], covariance_matrix[0] - covariance_matrix[3]);
  orientation = orientation * 180 / M_PI; // Convert to degrees
  // Normalize to [0, 180)
  // Not to 0, 360, as the ellipse is symmetric and the orientation is defined as the angle between the semi-major axis and the x-axis
  orientation = std::fmod(orientation + 180, 180);
  while (orientation < 0) {
    orientation += 180;
  }
  while (orientation >= 180) {
    orientation -= 180;
  }
  return std::make_tuple(semi_major_axis, semi_minor_axis, orientation);
}

confidenceEllipseFromWGSCovMatrix()

std::tuple< double, double, double > confidenceEllipseFromWGSCovMatrix ( const std::array< double, 4 > & covariance_matrix )

inline

Gets the values needed to set a confidence ellipse from a covariance matrix.

Parameters

covariance_matrix	The four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the WGS axes (x = North, y = East)
object_heading	The heading of the object in rad, with respect to WGS84

Returns

std::tuple<double, double, double> semi_major_axis [m], semi_minor_axis [m], orientation [deg], with respect to WGS84

Definition at line 360 of file cdd_setters_common.h.

                                                                                                                        {
  // The resulting ellipse is the same as if the cov matrix was given in vehicle coordinates
  // and the object heading was set to 0.0
  return confidenceEllipseFromCovMatrix(covariance_matrix, 0.0);
}

setAccelerationConfidence()

template<typename AccelerationConfidence>

void setAccelerationConfidence ( AccelerationConfidence & accel_confidence, const double value )

inline

Set the Acceleration Confidence object.

Parameters

accel_confidence	object to set
value	standard deviation in m/s^2 as decimal number

Definition at line 163 of file cdd_setters_common.h.

                                                                                                    {
  auto accel_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (accel_conf < AccelerationConfidence::MIN && accel_conf > 0.0){
    accel_conf = AccelerationConfidence::MIN;
  } else if (accel_conf >= AccelerationConfidence::OUT_OF_RANGE || accel_conf <= 0.0) {
    accel_conf = AccelerationConfidence::UNAVAILABLE;
  }
  accel_confidence.value = static_cast<decltype(accel_confidence.value)>(accel_conf);
}

setAltitude()

void setAltitude ( Altitude & altitude, const double value )

inline

Set the Altitude object.

AltitudeConfidence is set to UNAVAILABLE

Parameters

altitude	object to set
value	Altitude value (above the reference ellipsoid surface) in meter as decimal number

Definition at line 109 of file cdd_setters_common.h.

                                                                {
  altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
  setAltitudeValue(altitude.altitude_value, value);
}

setAltitudeValue()

void setAltitudeValue ( AltitudeValue & altitude, const double value )

inline

Set the AltitudeValue object.

Parameters

altitude	object to set
value	AltitudeValue value (above the reference ellipsoid surface) in meter as decimal number

Definition at line 90 of file cdd_setters_common.h.

                                                                          {
  int64_t alt_in_cm = (int64_t)std::round(value * 1e2);
  if (alt_in_cm >= AltitudeValue::MIN && alt_in_cm <= AltitudeValue::MAX) {
    altitude.value = alt_in_cm;
  } else if (alt_in_cm < AltitudeValue::MIN) {
    altitude.value = AltitudeValue::MIN;
  } else if (alt_in_cm > AltitudeValue::MAX) {
    altitude.value = AltitudeValue::MAX;
  }
}

setFromUTMPosition()

template<typename T>

void setFromUTMPosition ( T & reference_position, const gm::PointStamped & utm_position, const int zone, const bool northp )

inline

Set the ReferencePosition from a given UTM-Position.

The position is transformed to latitude and longitude by using GeographicLib::UTMUPS The z-Coordinate is directly used as altitude value The frame_id of the given utm_position must be set to 'utm_<zone><N/S>'

Parameters

[out]	reference_position	ReferencePostion or ReferencePositionWithConfidence to set
[in]	utm_position	geometry_msgs::PointStamped describing the given utm position
[in]	zone	the UTM zone (zero means UPS) of the given position
[in]	northp	hemisphere (true means north, false means south)

Definition at line 211 of file cdd_setters_common.h.

                                                  {
  std::string required_frame_prefix = "utm_";
  if (utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) {
    throw std::invalid_argument("Frame-ID of UTM Position '" + utm_position.header.frame_id +
                                "' does not start with required prefix '" + required_frame_prefix + "'!");
  }
  double latitude, longitude;
  try {
    GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude);
  } catch (GeographicLib::GeographicErr& e) {
    throw std::invalid_argument(e.what());
  }
  setReferencePosition(reference_position, latitude, longitude, utm_position.point.z);
}

setHeadingCDD()

template<typename Heading, typename HeadingConfidence = decltype(Heading::heading_confidence)>

void setHeadingCDD	(	Heading &	heading,
		const double	value,
		double	confidence = std::numeric_limits<double>::infinity() )

Set the Heading object.

0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West HeadingConfidence is set to UNAVAILABLE

Parameters

heading	object to set
value	Heading value in degree as decimal number
confidence	standard deviation of heading in degree as decimal number (default: infinity, mapping to HeadingConfidence::UNAVAILABLE)

Definition at line 270 of file cdd_setters_common.h.

                                                                                                                  {
  setHeadingConfidence(heading.heading_confidence, confidence);
  setHeadingValue(heading.heading_value, value);
}

setHeadingConfidence()

template<typename HeadingConfidence>

void setHeadingConfidence ( HeadingConfidence & heading_confidence, const double value )

inline

Set the Heading Confidence object.

Parameters

heading_confidence	object to set
value	standard deviation of heading in degree as decimal number

Definition at line 249 of file cdd_setters_common.h.

                                                                                            {
  auto heading_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (heading_conf < HeadingConfidence::MIN && heading_conf > 0.0){
    heading_conf = HeadingConfidence::MIN;
  } else if (heading_conf >= HeadingConfidence::OUT_OF_RANGE || heading_conf <= 0.0) {
    heading_conf = HeadingConfidence::UNAVAILABLE;
  }
  heading_confidence.value = static_cast<decltype(heading_confidence.value)>(heading_conf);
}

setHeadingValue()

template<typename HeadingValue>

void setHeadingValue ( HeadingValue & heading, const double value )

inline

Set the HeadingValue object.

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

Parameters

heading	object to set
value	Heading value in degree as decimal number

Definition at line 236 of file cdd_setters_common.h.

                                                                       {
  int64_t deg = (int64_t)std::round(value * 1e1);
  throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue");
  heading.value = deg;
}

setLatitude()

void setLatitude ( Latitude & latitude, const double deg )

inline

Set the Latitude object.

Parameters

latitude	object to set
deg	Latitude value in degree as decimal number

Definition at line 66 of file cdd_setters_common.h.

                                                              {
  int64_t angle_in_10_micro_degree = (int64_t)std::round(deg * 1e7);
  throwIfOutOfRange(angle_in_10_micro_degree, Latitude::MIN, Latitude::MAX, "Latitude");
  latitude.value = angle_in_10_micro_degree;
}

setLongitude()

void setLongitude ( Longitude & longitude, const double deg )

inline

Set the Longitude object.

Parameters

longitude	object to set
deg	Longitude value in degree as decimal number

Definition at line 78 of file cdd_setters_common.h.

                                                                 {
  int64_t angle_in_10_micro_degree = (int64_t)std::round(deg * 1e7);
  throwIfOutOfRange(angle_in_10_micro_degree, Longitude::MIN, Longitude::MAX, "Longitude");
  longitude.value = angle_in_10_micro_degree;
}

setPosConfidenceEllipse() [1/2]

template<typename PosConfidenceEllipse>

void setPosConfidenceEllipse ( PosConfidenceEllipse & position_confidence_ellipse, const double semi_major_axis, const double semi_minor_axis, const double orientation )

inline

Set the Pos Confidence Ellipse object.

Parameters

[out]	position_confidence_ellipse	The PosConfidenceEllipse to set
[in]	semi_major_axis	length of the semi-major axis in meters
[in]	semi_minor_axis	length of the semi-minor axis in meters
[in]	orientation	of the semi-major axis in degrees, with respect to WGS84

Definition at line 303 of file cdd_setters_common.h.

                                                          {
  setSemiAxis(position_confidence_ellipse.semi_major_confidence, semi_major_axis);
  setSemiAxis(position_confidence_ellipse.semi_minor_confidence, semi_minor_axis);
  setHeadingValue(position_confidence_ellipse.semi_major_orientation, orientation);
}

setPosConfidenceEllipse() [2/2]

template<typename PosConfidenceEllipse>

void setPosConfidenceEllipse ( PosConfidenceEllipse & position_confidence_ellipse, const std::array< double, 4 > & covariance_matrix, const double object_heading )

inline

Set the Pos Confidence Ellipse object.

Parameters

position_confidence_ellipse
covariance_matrix	The four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the object (x = longitudinal, y = lateral)
object_heading	The heading of the object in rad, with respect to WGS84

Definition at line 376 of file cdd_setters_common.h.

                                                                                                                                                                 {
  auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromCovMatrix(covariance_matrix, object_heading);
  setPosConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
}

setReferencePosition()

template<typename T>

void setReferencePosition ( T & ref_position, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE )

inline

Sets the reference position in the given ReferencePostion object.

This function sets the latitude, longitude, and altitude of the reference position. If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE.

Parameters

ref_position	ReferencePostion or ReferencePositionWithConfidence object to set the reference position in.
latitude	The latitude value position in degree as decimal number.
longitude	The longitude value in degree as decimal number.
altitude	The altitude value (above the reference ellipsoid surface) in meter as decimal number (optional).

Definition at line 185 of file cdd_setters_common.h.

                                                                                   {
  setLatitude(ref_position.latitude, latitude);
  setLongitude(ref_position.longitude, longitude);
  if (altitude != AltitudeValue::UNAVAILABLE) {
    setAltitude(ref_position.altitude, altitude);
  } else {
    ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE;
    ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
  }
  // TODO: set confidence values
}

setSemiAxis()

template<typename SemiAxisLength>

void setSemiAxis ( SemiAxisLength & semi_axis_length, const double length )

inline

Set the Semi Axis length.

// See https://godbolt.org/z/Eceavfo99 on how the OneCentimeterHelper works with this template

Parameters

semi_axis_length	The SemiAxisLength to set
length	the desired length in meters

Definition at line 284 of file cdd_setters_common.h.

                                                                               {
  double semi_axis_length_val = std::round(length * etsi_its_msgs::OneCentimeterHelper<SemiAxisLength>::value * 1e2);
  if(semi_axis_length_val < SemiAxisLength::MIN) {
    semi_axis_length_val = SemiAxisLength::MIN;
  } else if(semi_axis_length_val >= SemiAxisLength::OUT_OF_RANGE) {
    semi_axis_length_val = SemiAxisLength::OUT_OF_RANGE;
  }  
  semi_axis_length.value = static_cast<uint16_t>(semi_axis_length_val);
}

setSpeed()

void setSpeed ( Speed & speed, const double value, const double confidence = std::numeric_limits<double>::infinity() )

inline

Set the Speed object.

SpeedConfidence is set to UNAVAILABLE

Parameters

speed	object to set
value	Speed in in m/s as decimal number
confidence	standard deviation in m/s as decimal number (Optional. Default is std::numeric_limits<double>::infinity(), mapping to SpeedConfidence::UNAVAILABLE)

Definition at line 151 of file cdd_setters_common.h.

                                                                                                                      {
  setSpeedConfidence(speed.speed_confidence, confidence);
  setSpeedValue(speed.speed_value, value);
}

setSpeedConfidence()

void setSpeedConfidence ( SpeedConfidence & speed_confidence, const double value )

inline

Set the Speed Confidence object.

Parameters

speed_confidence	object to set
value	standard deviation in m/s as decimal number

Definition at line 132 of file cdd_setters_common.h.

                                                                                      {
  auto speed_conf = std::round(value * 1e2 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (speed_conf < SpeedConfidence::MIN && speed_conf > 0.0){
    speed_conf = SpeedConfidence::MIN;
  } else if (speed_conf >= SpeedConfidence::OUT_OF_RANGE || speed_conf <= 0.0) {
    speed_conf = SpeedConfidence::UNAVAILABLE;
  }
  speed_confidence.value = static_cast<decltype(speed_confidence.value)>(speed_conf);
}

setSpeedValue()

void setSpeedValue ( SpeedValue & speed, const double value )

inline

Set the SpeedValue object.

Parameters

speed	object to set
value	SpeedValue in m/s as decimal number

Definition at line 120 of file cdd_setters_common.h.

                                                                 {
  auto speed_val = std::round(value * 1e2);
  throwIfOutOfRange(speed_val, SpeedValue::MIN, SpeedValue::MAX, "SpeedValue");
  speed.value = static_cast<decltype(speed.value)>(speed_val);
}

setTimestampITS()

void setTimestampITS ( TimestampIts & timestamp_its, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second )

inline

Set the TimestampITS object.

Parameters

[in]	timestamp_its	TimestampITS object to set the timestamp
[in]	unix_nanosecs	Unix-Nanoseconds to set the timestamp for
[in]	n_leap_seconds	Number of leap-seconds since 2004. (Defaults to the todays number of leap seconds since 2004.)
[in]	epoch_offset	Unix-Timestamp in seconds for the 01.01.2004 at 00:00:00

Definition at line 52 of file cdd_setters_common.h.

                                                                                                     {
  uint64_t t_its = unix_nanosecs * 1e-6 + (uint64_t)(n_leap_seconds * 1e3) - etsi_its_msgs::UNIX_SECONDS_2004 * 1e3;
  throwIfOutOfRange(t_its, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
  timestamp_its.value = t_its;
}

setWGSPosConfidenceEllipse()

template<typename PosConfidenceEllipse>

void setWGSPosConfidenceEllipse ( PosConfidenceEllipse & position_confidence_ellipse, const std::array< double, 4 > & covariance_matrix )

inline

Set the Pos Confidence Ellipse object.

Parameters

position_confidence_ellipse
covariance_matrix	The four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the WGS axes (x = North, y = East)
object_heading	The heading of the object in rad, with respect to WGS84

Definition at line 391 of file cdd_setters_common.h.

                                                                                                                                       {
  auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromWGSCovMatrix(covariance_matrix);
  setPosConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
}