32#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H
33#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H
37#include <GeographicLib/UTMUPS.hpp>
53 TimestampIts& timestamp_its,
const uint64_t unix_nanosecs,
55 uint64_t t_its = unix_nanosecs * 1e-6 + (uint64_t)(n_leap_seconds * 1e3) - etsi_its_msgs::UNIX_SECONDS_2004 * 1e3;
57 timestamp_its.value = t_its;
66inline void setLatitude(Latitude& latitude,
const double deg) {
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;
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;
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;
110 altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
121 auto speed_val = std::round(value * 1e2);
123 speed.value =
static_cast<decltype(speed.value)
>(speed_val);
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;
139 speed_confidence.value =
static_cast<decltype(speed_confidence.value)
>(speed_conf);
151inline void setSpeed(Speed& speed,
const double value,
const double confidence = std::numeric_limits<double>::infinity()) {
162template <
typename AccelerationMagnitudeValue>
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);
175template <
typename AccelerationConf
idence>
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;
183 accel_mag_confidence.value =
static_cast<decltype(accel_mag_confidence.value)
>(accel_mag_conf);
195template<
typename AccelerationMagnitude>
197 const double confidence = std::numeric_limits<double>::infinity()) {
208template <
typename AccelerationConf
idence>
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;
216 accel_confidence.value =
static_cast<decltype(accel_confidence.value)
>(accel_conf);
232 const double altitude = AltitudeValue::UNAVAILABLE) {
235 if (altitude != AltitudeValue::UNAVAILABLE) {
238 ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE;
239 ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
257inline void setFromUTMPosition(T& reference_position,
const gm::PointStamped& utm_position,
const int zone,
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 +
"'!");
264 double latitude, longitude;
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());
281template <
typename HeadingValue>
283 int64_t deg = (int64_t)std::round(value * 1e1);
294template<
typename HeadingConf
idence>
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;
302 heading_confidence.value =
static_cast<decltype(heading_confidence.value)
>(heading_conf);
315template <
typename Heading,
typename HeadingConf
idence = decltype(Heading::heading_conf
idence)>
316void setHeadingCDD(Heading& heading,
const double value,
double confidence = std::numeric_limits<double>::infinity()) {
328template <
typename YawRate,
typename YawRateValue = decltype(YawRate::yaw_rate_value),
typename YawRateConf
idence = decltype(YawRate::yaw_rate_conf
idence)>
330 double confidence = std::numeric_limits<double>::infinity()) {
331 double yaw_rate_in_001_degrees = value * 100.0;
333 if (yaw_rate_in_001_degrees < YawRateValue::MIN) {
334 yaw_rate_in_001_degrees = YawRateValue::MIN;
335 }
else if (yaw_rate_in_001_degrees > YawRateValue::MAX - 1) {
336 yaw_rate_in_001_degrees = YawRateValue::MAX - 1;
339 yaw_rate.yaw_rate_value.value = yaw_rate_in_001_degrees;
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;
345 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
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},
357 for(
const auto& [thresh, conf_val] : confidence_map) {
358 if (yaw_rate_std <= thresh) {
359 yaw_rate.yaw_rate_confidence.value = conf_val;
374template <
typename SemiAxisLength>
375inline void setSemiAxis(SemiAxisLength& semi_axis_length,
const double length) {
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;
382 semi_axis_length.value =
static_cast<uint16_t
>(semi_axis_length_val);
393template <
typename PosConf
idenceEllipse>
395 const double semi_minor_axis,
const double orientation) {
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);
412 if(std::abs(covariance_matrix[1] - covariance_matrix[2]) > 1e-6) {
413 throw std::invalid_argument(
"Covariance matrix is not symmetric");
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) {
421 throw std::invalid_argument(
"Covariance matrix is not positive definite");
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;
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;
432 orientation = std::fmod(orientation + 180, 180);
433 while (orientation < 0) {
436 while (orientation >= 180) {
439 return std::make_tuple(semi_major_axis, semi_minor_axis, orientation);
466template <
typename PosConf
idenceEllipse>
467inline void setPosConfidenceEllipse(PosConfidenceEllipse& position_confidence_ellipse,
const std::array<double, 4>& covariance_matrix,
const double object_heading){
481template <
typename PosConf
idenceEllipse>
void setLongitude(Longitude &longitude, const double deg)
Set the Longitude object.
void setAccelerationConfidence(AccelerationConfidence &accel_confidence, const double value)
Set the Acceleration Confidence object.
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.
void setSpeedValue(SpeedValue &speed, const double value)
Set the SpeedValue object.
void setTimestampITS(TimestampIts ×tamp_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 setSpeedConfidence(SpeedConfidence &speed_confidence, const double value)
Set the Speed Confidence 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.
void setAccelerationMagnitudeValue(AccelerationMagnitudeValue &accel_mag_value, const double value)
Set the Acceleration Magnitude Value object.
void setSemiAxis(SemiAxisLength &semi_axis_length, const double length)
Set the Semi Axis length.
void setAltitude(Altitude &altitude, const double value)
Set the Altitude object.
void setYawRateCDD(YawRate &yaw_rate, const double value, double confidence=std::numeric_limits< double >::infinity())
Set the Yaw Rate object.
void setFromUTMPosition(T &reference_position, const gm::PointStamped &utm_position, const int zone, const bool northp)
Set the ReferencePosition from a given UTM-Position.
void setHeadingConfidence(HeadingConfidence &heading_confidence, const double value)
Set the Heading Confidence object.
void setSpeed(Speed &speed, const double value, const double confidence=std::numeric_limits< double >::infinity())
Set the Speed object.
void setAccelerationMagnitudeConfidence(AccelerationConfidence &accel_mag_confidence, const double value)
Set the AccelerationMagnitude Confidence object.
void setWGSPosConfidenceEllipse(PosConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix)
Set the Pos Confidence Ellipse object.
void setAccelerationMagnitude(AccelerationMagnitude &accel_mag, const double value, const double confidence=std::numeric_limits< double >::infinity())
Set the AccelerationMagnitude object.
void setAltitudeValue(AltitudeValue &altitude, const double value)
Set the AltitudeValue object.
void setHeadingCDD(Heading &heading, const double value, double confidence=std::numeric_limits< double >::infinity())
Set the Heading object.
void setHeadingValue(HeadingValue &heading, const double value)
Set the HeadingValue object.
void setLatitude(Latitude &latitude, const double deg)
Set the Latitude object.
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.
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.
Sanity-check functions etc.
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.
File containing constants that are used in the context of ETIS ITS Messages.
const std::map< uint64_t, uint16_t > LEAP_SECOND_INSERTIONS_SINCE_2004
std::map that stores all leap second insertions since 2004 with the corresponding unix-date of the in...