etsi_its_messages v3.3.0
 
Loading...
Searching...
No Matches
cdd_setters_common.h
Go to the documentation of this file.
1/*
2=============================================================================
3MIT License
4
5Copyright (c) 2023-2025 Institute for Automotive Engineering (ika), RWTH Aachen University
6
7Permission is hereby granted, free of charge, to any person obtaining a copy
8of this software and associated documentation files (the "Software"), to deal
9in the Software without restriction, including without limitation the rights
10to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11copies of the Software, and to permit persons to whom the Software is
12furnished to do so, subject to the following conditions:
13
14The above copyright notice and this permission notice shall be included in all
15copies or substantial portions of the Software.
16
17THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
23SOFTWARE.
24=============================================================================
25*/
26
31
32#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H
33#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H
34
37#include <GeographicLib/UTMUPS.hpp>
38#include <array>
39#include <cmath>
40#include <cstdint>
41#include <cstring>
42#include <type_traits>
43
52inline void setTimestampITS(
53 TimestampIts& timestamp_its, const uint64_t unix_nanosecs,
54 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
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}
59
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;
70}
71
78inline void setLongitude(Longitude& longitude, const double deg) {
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}
83
90inline void setAltitudeValue(AltitudeValue& altitude, const double value) {
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}
100
109inline void setAltitude(Altitude& altitude, const double value) {
110 altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
111 setAltitudeValue(altitude.altitude_value, value);
112}
113
120inline void setSpeedValue(SpeedValue& speed, const double value) {
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}
125
132inline void setSpeedConfidence(SpeedConfidence& speed_confidence, const double value) {
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}
141
151inline void setSpeed(Speed& speed, const double value, const double confidence = std::numeric_limits<double>::infinity()) {
152 setSpeedConfidence(speed.speed_confidence, confidence);
153 setSpeedValue(speed.speed_value, value);
154}
155
162template <typename AccelerationMagnitudeValue>
163inline void setAccelerationMagnitudeValue(AccelerationMagnitudeValue& accel_mag_value, const double value) {
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}
168
175template <typename AccelerationConfidence>
176inline void setAccelerationMagnitudeConfidence(AccelerationConfidence& accel_mag_confidence, const double value) {
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}
185
195template<typename AccelerationMagnitude>
196inline void setAccelerationMagnitude(AccelerationMagnitude& accel_mag, const double value,
197 const double confidence = std::numeric_limits<double>::infinity()) {
198 setAccelerationMagnitudeConfidence(accel_mag.acceleration_confidence, confidence);
199 setAccelerationMagnitudeValue(accel_mag.acceleration_magnitude_value, value);
200}
201
208template <typename AccelerationConfidence>
209inline void setAccelerationConfidence(AccelerationConfidence& accel_confidence, const double value) {
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}
218
230template <typename T>
231inline void setReferencePosition(T& ref_position, const double latitude, const double longitude,
232 const double altitude = AltitudeValue::UNAVAILABLE) {
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}
243
256template <typename T>
257inline void setFromUTMPosition(T& reference_position, const gm::PointStamped& utm_position, const int zone,
258 const bool northp) {
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}
272
281template <typename HeadingValue>
282inline void setHeadingValue(HeadingValue& heading, const double value) {
283 int64_t deg = (int64_t)std::round(value * 1e1);
284 throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue");
285 heading.value = deg;
286}
287
294template<typename HeadingConfidence>
295inline void setHeadingConfidence(HeadingConfidence& heading_confidence, const double value) {
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}
304
315template <typename Heading, typename HeadingConfidence = decltype(Heading::heading_confidence)>
316void setHeadingCDD(Heading& heading, const double value, double confidence = std::numeric_limits<double>::infinity()) {
317 setHeadingConfidence(heading.heading_confidence, confidence);
318 setHeadingValue(heading.heading_value, value);
319}
320
328template <typename YawRate, typename YawRateValue = decltype(YawRate::yaw_rate_value), typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
329inline void setYawRateCDD(YawRate& yaw_rate, const double value,
330 double confidence = std::numeric_limits<double>::infinity()) {
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}
365
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;
381 }
382 semi_axis_length.value = static_cast<uint16_t>(semi_axis_length_val);
383}
384
393template <typename PosConfidenceEllipse>
394inline void setPosConfidenceEllipse(PosConfidenceEllipse& position_confidence_ellipse, const double semi_major_axis,
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);
399}
400
410inline std::tuple<double, double, double> confidenceEllipseFromCovMatrix(const std::array<double, 4>& covariance_matrix, const double object_heading) {
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}
441
451inline std::tuple<double, double, double> confidenceEllipseFromWGSCovMatrix(const std::array<double, 4>& covariance_matrix) {
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}
456
466template <typename PosConfidenceEllipse>
467inline void setPosConfidenceEllipse(PosConfidenceEllipse& position_confidence_ellipse, const std::array<double, 4>& covariance_matrix, const double object_heading){
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}
471
481template <typename PosConfidenceEllipse>
482inline void setWGSPosConfidenceEllipse(PosConfidenceEllipse& position_confidence_ellipse, const std::array<double, 4>& covariance_matrix){
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}
486
487
488
489#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H
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 &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 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.
Definition checks.h:46
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...
Definition constants.h:45