etsi_its_messages v3.3.0
 
Loading...
Searching...
No Matches
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 AccelerationMagnitudeValue>
void setAccelerationMagnitudeValue (AccelerationMagnitudeValue &accel_mag_value, const double value)
 Set the Acceleration Magnitude Value object.
 
template<typename AccelerationConfidence>
void setAccelerationMagnitudeConfidence (AccelerationConfidence &accel_mag_confidence, const double value)
 Set the AccelerationMagnitude Confidence object.
 
template<typename AccelerationMagnitude>
void setAccelerationMagnitude (AccelerationMagnitude &accel_mag, const double value, const double confidence=std::numeric_limits< double >::infinity())
 Set the AccelerationMagnitude 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 YawRate, typename YawRateValue = decltype(YawRate::yaw_rate_value), typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
void setYawRateCDD (YawRate &yaw_rate, const double value, double confidence=std::numeric_limits< double >::infinity())
 Set the Yaw Rate 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_matrixThe 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_headingThe 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 410 of file cdd_setters_common.h.

410 {
411
412 if(std::abs(covariance_matrix[1] - covariance_matrix[2]) > 1e-6) {
413 throw std::invalid_argument("Covariance matrix is not symmetric");
414 }
415 double trace = covariance_matrix[0] + covariance_matrix[3];
416 double determinant = covariance_matrix[0] * covariance_matrix[3] - covariance_matrix[1] * covariance_matrix[1];
417 if (determinant <= 0 || covariance_matrix[0] <= 0) {
418 // https://sites.math.northwestern.edu/~clark/285/2006-07/handouts/pos-def.pdf:
419 // Therefore, a necessary and sufficient condition for the quadratic form of a symmetric 2 × 2 matrix
420 // to be positive definite is for det(A) > 0 and a > 0
421 throw std::invalid_argument("Covariance matrix is not positive definite");
422 }
423 double eigenvalue1 = trace / 2 + std::sqrt(trace * trace / 4 - determinant);
424 double eigenvalue2 = trace / 2 - std::sqrt(trace * trace / 4 - determinant);
425 double semi_major_axis = std::sqrt(eigenvalue1) * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR;
426 double semi_minor_axis = std::sqrt(eigenvalue2) * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR;
427 // object_heading - orientation of the ellipse, as WGS84 has positive angles to the right
428 double orientation = object_heading - 0.5 * std::atan2(2 * covariance_matrix[1], covariance_matrix[0] - covariance_matrix[3]);
429 orientation = orientation * 180 / M_PI; // Convert to degrees
430 // Normalize to [0, 180)
431 // 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
432 orientation = std::fmod(orientation + 180, 180);
433 while (orientation < 0) {
434 orientation += 180;
435 }
436 while (orientation >= 180) {
437 orientation -= 180;
438 }
439 return std::make_tuple(semi_major_axis, semi_minor_axis, orientation);
440}

◆ 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_matrixThe 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_headingThe 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 451 of file cdd_setters_common.h.

451 {
452 // The resulting ellipse is the same as if the cov matrix was given in vehicle coordinates
453 // and the object heading was set to 0.0
454 return confidenceEllipseFromCovMatrix(covariance_matrix, 0.0);
455}
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.

◆ setAccelerationConfidence()

template<typename AccelerationConfidence>
void setAccelerationConfidence ( AccelerationConfidence & accel_confidence,
const double value )
inline

Set the Acceleration Confidence object.

Parameters
accel_confidenceobject to set
valuestandard deviation in m/s^2 as decimal number

Definition at line 209 of file cdd_setters_common.h.

209 {
210 auto accel_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
211 if (accel_conf < AccelerationConfidence::MIN && accel_conf > 0.0){
212 accel_conf = AccelerationConfidence::MIN;
213 } else if (accel_conf >= AccelerationConfidence::OUT_OF_RANGE || accel_conf <= 0.0) {
214 accel_conf = AccelerationConfidence::UNAVAILABLE;
215 }
216 accel_confidence.value = static_cast<decltype(accel_confidence.value)>(accel_conf);
217}

◆ setAccelerationMagnitude()

template<typename AccelerationMagnitude>
void setAccelerationMagnitude ( AccelerationMagnitude & accel_mag,
const double value,
const double confidence = std::numeric_limits<double>::infinity() )
inline

Set the AccelerationMagnitude object.

AccelerationConfidence is set to UNAVAILABLE

Parameters
accel_magobject to set
valueAccelerationMagnitudeValue in m/s^2 as decimal number
confidencestandard deviation in m/s^2 as decimal number (default: infinity, mapping to AccelerationConfidence::UNAVAILABLE)

Definition at line 196 of file cdd_setters_common.h.

197 {
198 setAccelerationMagnitudeConfidence(accel_mag.acceleration_confidence, confidence);
199 setAccelerationMagnitudeValue(accel_mag.acceleration_magnitude_value, value);
200}
void setAccelerationMagnitudeValue(AccelerationMagnitudeValue &accel_mag_value, const double value)
Set the Acceleration Magnitude Value object.
void setAccelerationMagnitudeConfidence(AccelerationConfidence &accel_mag_confidence, const double value)
Set the AccelerationMagnitude Confidence object.

◆ setAccelerationMagnitudeConfidence()

template<typename AccelerationConfidence>
void setAccelerationMagnitudeConfidence ( AccelerationConfidence & accel_mag_confidence,
const double value )
inline

Set the AccelerationMagnitude Confidence object.

Parameters
accel_mag_confidenceobject to set
valuestandard deviation in m/s^2 as decimal number

Definition at line 176 of file cdd_setters_common.h.

176 {
177 auto accel_mag_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
178 if (accel_mag_conf < AccelerationConfidence::MIN && accel_mag_conf > 0.0){
179 accel_mag_conf = AccelerationConfidence::MIN;
180 } else if (accel_mag_conf >= AccelerationConfidence::OUT_OF_RANGE || accel_mag_conf <= 0.0) {
181 accel_mag_conf = AccelerationConfidence::UNAVAILABLE;
182 }
183 accel_mag_confidence.value = static_cast<decltype(accel_mag_confidence.value)>(accel_mag_conf);
184}

◆ setAccelerationMagnitudeValue()

template<typename AccelerationMagnitudeValue>
void setAccelerationMagnitudeValue ( AccelerationMagnitudeValue & accel_mag_value,
const double value )
inline

Set the Acceleration Magnitude Value object.

Parameters
accel_mag_valueobject to set
valueAccelerationMagnitudeValue in m/s^2 as decimal number

Definition at line 163 of file cdd_setters_common.h.

163 {
164 auto accel_mag = std::round(value * 1e1);
165 throwIfOutOfRange(accel_mag, AccelerationMagnitudeValue::MIN, AccelerationMagnitudeValue::MAX, "AccelerationMagnitudeValue");
166 accel_mag_value.value = static_cast<decltype(accel_mag_value.value)>(accel_mag);
167}
void throwIfOutOfRange(const T1 &val, const T2 &min, const T2 &max, const std::string val_desc)
Throws an exception if a given value is out of a defined range.
Definition checks.h:46

◆ setAltitude()

void setAltitude ( Altitude & altitude,
const double value )
inline

Set the Altitude object.

AltitudeConfidence is set to UNAVAILABLE

Parameters
altitudeobject to set
valueAltitude value (above the reference ellipsoid surface) in meter as decimal number

Definition at line 109 of file cdd_setters_common.h.

109 {
110 altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
111 setAltitudeValue(altitude.altitude_value, value);
112}
void setAltitudeValue(AltitudeValue &altitude, const double value)
Set the AltitudeValue object.

◆ setAltitudeValue()

void setAltitudeValue ( AltitudeValue & altitude,
const double value )
inline

Set the AltitudeValue object.

Parameters
altitudeobject to set
valueAltitudeValue value (above the reference ellipsoid surface) in meter as decimal number

Definition at line 90 of file cdd_setters_common.h.

90 {
91 int64_t alt_in_cm = (int64_t)std::round(value * 1e2);
92 if (alt_in_cm >= AltitudeValue::MIN && alt_in_cm <= AltitudeValue::MAX) {
93 altitude.value = alt_in_cm;
94 } else if (alt_in_cm < AltitudeValue::MIN) {
95 altitude.value = AltitudeValue::MIN;
96 } else if (alt_in_cm > AltitudeValue::MAX) {
97 altitude.value = AltitudeValue::MAX;
98 }
99}

◆ 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_positionReferencePostion or ReferencePositionWithConfidence to set
[in]utm_positiongeometry_msgs::PointStamped describing the given utm position
[in]zonethe UTM zone (zero means UPS) of the given position
[in]northphemisphere (true means north, false means south)

Definition at line 257 of file cdd_setters_common.h.

258 {
259 std::string required_frame_prefix = "utm_";
260 if (utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) {
261 throw std::invalid_argument("Frame-ID of UTM Position '" + utm_position.header.frame_id +
262 "' does not start with required prefix '" + required_frame_prefix + "'!");
263 }
264 double latitude, longitude;
265 try {
266 GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude);
267 } catch (GeographicLib::GeographicErr& e) {
268 throw std::invalid_argument(e.what());
269 }
270 setReferencePosition(reference_position, latitude, longitude, utm_position.point.z);
271}
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.

◆ 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
headingobject to set
valueHeading value in degree as decimal number
confidencestandard deviation of heading in degree as decimal number (default: infinity, mapping to HeadingConfidence::UNAVAILABLE)

Definition at line 316 of file cdd_setters_common.h.

316 {
317 setHeadingConfidence(heading.heading_confidence, confidence);
318 setHeadingValue(heading.heading_value, value);
319}
void setHeadingConfidence(HeadingConfidence &heading_confidence, const double value)
Set the Heading Confidence object.
void setHeadingValue(HeadingValue &heading, const double value)
Set the HeadingValue object.

◆ setHeadingConfidence()

template<typename HeadingConfidence>
void setHeadingConfidence ( HeadingConfidence & heading_confidence,
const double value )
inline

Set the Heading Confidence object.

Parameters
heading_confidenceobject to set
valuestandard deviation of heading in degree as decimal number

Definition at line 295 of file cdd_setters_common.h.

295 {
296 auto heading_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
297 if (heading_conf < HeadingConfidence::MIN && heading_conf > 0.0){
298 heading_conf = HeadingConfidence::MIN;
299 } else if (heading_conf >= HeadingConfidence::OUT_OF_RANGE || heading_conf <= 0.0) {
300 heading_conf = HeadingConfidence::UNAVAILABLE;
301 }
302 heading_confidence.value = static_cast<decltype(heading_confidence.value)>(heading_conf);
303}

◆ 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
headingobject to set
valueHeading value in degree as decimal number

Definition at line 282 of file cdd_setters_common.h.

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

◆ setLatitude()

void setLatitude ( Latitude & latitude,
const double deg )
inline

Set the Latitude object.

Parameters
latitudeobject to set
degLatitude value in degree as decimal number

Definition at line 66 of file cdd_setters_common.h.

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

◆ setLongitude()

void setLongitude ( Longitude & longitude,
const double deg )
inline

Set the Longitude object.

Parameters
longitudeobject to set
degLongitude value in degree as decimal number

Definition at line 78 of file cdd_setters_common.h.

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

◆ 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_ellipseThe PosConfidenceEllipse to set
[in]semi_major_axislength of the semi-major axis in meters
[in]semi_minor_axislength of the semi-minor axis in meters
[in]orientationof the semi-major axis in degrees, with respect to WGS84

Definition at line 394 of file cdd_setters_common.h.

395 {
396 setSemiAxis(position_confidence_ellipse.semi_major_confidence, semi_major_axis);
397 setSemiAxis(position_confidence_ellipse.semi_minor_confidence, semi_minor_axis);
398 setHeadingValue(position_confidence_ellipse.semi_major_orientation, orientation);
399}
void setSemiAxis(SemiAxisLength &semi_axis_length, const double length)
Set the Semi Axis length.

◆ 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_matrixThe 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_headingThe heading of the object in rad, with respect to WGS84

Definition at line 467 of file cdd_setters_common.h.

467 {
468 auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromCovMatrix(covariance_matrix, object_heading);
469 setPosConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
470}
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.

◆ 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_positionReferencePostion or ReferencePositionWithConfidence object to set the reference position in.
latitudeThe latitude value position in degree as decimal number.
longitudeThe longitude value in degree as decimal number.
altitudeThe altitude value (above the reference ellipsoid surface) in meter as decimal number (optional).

Definition at line 231 of file cdd_setters_common.h.

232 {
233 setLatitude(ref_position.latitude, latitude);
234 setLongitude(ref_position.longitude, longitude);
235 if (altitude != AltitudeValue::UNAVAILABLE) {
236 setAltitude(ref_position.altitude, altitude);
237 } else {
238 ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE;
239 ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
240 }
241 // TODO: set confidence values
242}
void setLongitude(Longitude &longitude, const double deg)
Set the Longitude object.
void setAltitude(Altitude &altitude, const double value)
Set the Altitude object.
void setLatitude(Latitude &latitude, const double deg)
Set the Latitude object.

◆ 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_lengthThe SemiAxisLength to set
lengththe desired length in meters

Definition at line 375 of file cdd_setters_common.h.

375 {
376 double semi_axis_length_val = std::round(length * etsi_its_msgs::OneCentimeterHelper<SemiAxisLength>::value * 1e2);
377 if(semi_axis_length_val < SemiAxisLength::MIN) {
378 semi_axis_length_val = SemiAxisLength::MIN;
379 } else if(semi_axis_length_val >= SemiAxisLength::OUT_OF_RANGE) {
380 semi_axis_length_val = SemiAxisLength::OUT_OF_RANGE;
381 }
382 semi_axis_length.value = static_cast<uint16_t>(semi_axis_length_val);
383}

◆ 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
speedobject to set
valueSpeed in in m/s as decimal number
confidencestandard 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.

151 {
152 setSpeedConfidence(speed.speed_confidence, confidence);
153 setSpeedValue(speed.speed_value, value);
154}
void setSpeedValue(SpeedValue &speed, const double value)
Set the SpeedValue object.
void setSpeedConfidence(SpeedConfidence &speed_confidence, const double value)
Set the Speed Confidence object.

◆ setSpeedConfidence()

void setSpeedConfidence ( SpeedConfidence & speed_confidence,
const double value )
inline

Set the Speed Confidence object.

Parameters
speed_confidenceobject to set
valuestandard deviation in m/s as decimal number

Definition at line 132 of file cdd_setters_common.h.

132 {
133 auto speed_conf = std::round(value * 1e2 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
134 if (speed_conf < SpeedConfidence::MIN && speed_conf > 0.0){
135 speed_conf = SpeedConfidence::MIN;
136 } else if (speed_conf >= SpeedConfidence::OUT_OF_RANGE || speed_conf <= 0.0) {
137 speed_conf = SpeedConfidence::UNAVAILABLE;
138 }
139 speed_confidence.value = static_cast<decltype(speed_confidence.value)>(speed_conf);
140}

◆ setSpeedValue()

void setSpeedValue ( SpeedValue & speed,
const double value )
inline

Set the SpeedValue object.

Parameters
speedobject to set
valueSpeedValue in m/s as decimal number

Definition at line 120 of file cdd_setters_common.h.

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

◆ 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_itsTimestampITS object to set the timestamp
[in]unix_nanosecsUnix-Nanoseconds to set the timestamp for
[in]n_leap_secondsNumber of leap-seconds since 2004. (Defaults to the todays number of leap seconds since 2004.)
[in]epoch_offsetUnix-Timestamp in seconds for the 01.01.2004 at 00:00:00

Definition at line 52 of file cdd_setters_common.h.

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

◆ 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_matrixThe 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_headingThe heading of the object in rad, with respect to WGS84

Definition at line 482 of file cdd_setters_common.h.

482 {
483 auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromWGSCovMatrix(covariance_matrix);
484 setPosConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
485}
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.

◆ setYawRateCDD()

template<typename YawRate, typename YawRateValue = decltype(YawRate::yaw_rate_value), typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
void setYawRateCDD ( YawRate & yaw_rate,
const double value,
double confidence = std::numeric_limits<double>::infinity() )
inline

Set the Yaw Rate object.

Parameters
yaw_rateobject to set
valueYaw rate in degrees per second as decimal number
confidencestandard deviation of yaw rate in degrees per second as decimal number (default: infinity, mapping to YawRateConfidence::UNAVAILABLE)

Definition at line 329 of file cdd_setters_common.h.

330 {
331 double yaw_rate_in_001_degrees = value * 100.0;
332 // limit value range
333 if (yaw_rate_in_001_degrees < YawRateValue::MIN) {
334 yaw_rate_in_001_degrees = YawRateValue::MIN; // MIN should be NEGATIVE_OUT_OF_RANGE, but CAM only has MIN
335 } else if (yaw_rate_in_001_degrees > YawRateValue::MAX - 1) {
336 yaw_rate_in_001_degrees = YawRateValue::MAX - 1; // MAX - 1 should be POSITIVE_OUT_OF_RANGE, but CAM only has MAX
337 }
338
339 yaw_rate.yaw_rate_value.value = yaw_rate_in_001_degrees;
340
341 double yaw_rate_std = confidence;
342 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
343 yaw_rate.yaw_rate_confidence.value = YawRateConfidence::UNAVAILABLE;
344 } else {
345 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
346 // How stupid is this?!
347 static const std::map<double, uint8_t> confidence_map = {
348 {0.01, YawRateConfidence::DEG_SEC_000_01},
349 {0.05, YawRateConfidence::DEG_SEC_000_05},
350 {0.1, YawRateConfidence::DEG_SEC_000_10},
351 {1.0, YawRateConfidence::DEG_SEC_001_00},
352 {5.0, YawRateConfidence::DEG_SEC_005_00},
353 {10.0, YawRateConfidence::DEG_SEC_010_00},
354 {100.0, YawRateConfidence::DEG_SEC_100_00},
355 {std::numeric_limits<double>::infinity(), YawRateConfidence::OUT_OF_RANGE},
356 };
357 for(const auto& [thresh, conf_val] : confidence_map) {
358 if (yaw_rate_std <= thresh) {
359 yaw_rate.yaw_rate_confidence.value = conf_val;
360 break;
361 }
362 }
363 }
364}