etsi_its_messages v3.3.0
 
Loading...
Searching...
No Matches
cdd_getters_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_GETTERS_COMMON_H
33#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H
34
35#include <array>
36#include <cmath>
37#include <cstdint>
38#include <GeographicLib/UTMUPS.hpp>
39
46inline uint32_t getStationID(const ItsPduHeader& header) { return header.station_id.value; }
47
54inline double getLatitude(const Latitude& latitude) { return ((double)latitude.value) * 1e-7; }
55
62inline double getLongitude(const Longitude& longitude) { return ((double)longitude.value) * 1e-7; }
63
70inline double getAltitude(const Altitude& altitude) {
71 if (altitude.altitude_value.value == AltitudeValue::UNAVAILABLE) {
72 return 0.0;
73 }
74 return ((double)altitude.altitude_value.value) * 1e-2;
75}
76
83inline double getSpeed(const Speed& speed) { return ((double)speed.speed_value.value) * 1e-2; }
84
91inline double getSpeedConfidence(const Speed& speed) {
92 return ((double)speed.speed_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-2;
93}
94
101template <typename AccelerationMagnitude>
102inline double getAccelerationMagnitude(const AccelerationMagnitude& acceleration_magnitude) {
103 return ((double)acceleration_magnitude.acceleration_magnitude_value.value) * 1e-1;
104}
105
112template <typename AccelerationMagnitude>
113inline double getAccelerationMagnitudeConfidence(const AccelerationMagnitude& acceleration_magnitude) {
114 return ((double)acceleration_magnitude.acceleration_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-1;
115}
116
128template <typename T>
129inline gm::PointStamped getUTMPosition(const T& reference_position, int& zone, bool& northp) {
130 gm::PointStamped utm_point;
131 double latitude = getLatitude(reference_position.latitude);
132 double longitude = getLongitude(reference_position.longitude);
133 utm_point.point.z = getAltitude(reference_position.altitude);
134 try {
135 GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y);
136 std::string hemisphere;
137 if (northp) {
138 hemisphere = "N";
139 } else {
140 hemisphere = "S";
141 }
142 utm_point.header.frame_id = "utm_" + std::to_string(zone) + hemisphere;
143 } catch (GeographicLib::GeographicErr& e) {
144 throw std::invalid_argument(e.what());
145 }
146 return utm_point;
147}
148
157template <typename Heading>
158inline double getHeadingCDD(const Heading& heading) { return ((double)heading.heading_value.value) * 1e-1; }
159
168template <typename Heading>
169inline double getHeadingConfidenceCDD(const Heading& heading) { return ((double)heading.heading_confidence.value) * 1e-1 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; }
170
177template <typename YawRate>
178inline double getYawRateCDD(const YawRate& yaw_rate) {
179 return ((double)yaw_rate.yaw_rate_value.value) * 1e-2; // Yaw rate in deg/s
180}
181
188template <typename YawRate, typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
189inline double getYawRateConfidenceCDD(const YawRate& yaw_rate) {
190 auto val = yaw_rate.yaw_rate_confidence.value;
191 static const std::map<uint8_t, double> confidence_map = {
192 {YawRateConfidence::UNAVAILABLE, 0.0},
193 {YawRateConfidence::DEG_SEC_000_01, 0.01},
194 {YawRateConfidence::DEG_SEC_000_05, 0.05},
195 {YawRateConfidence::DEG_SEC_000_10, 0.1},
196 {YawRateConfidence::DEG_SEC_001_00, 1.0},
197 {YawRateConfidence::DEG_SEC_005_00, 5.0},
198 {YawRateConfidence::DEG_SEC_010_00, 10.0},
199 {YawRateConfidence::DEG_SEC_100_00, 100.0},
200 {YawRateConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
201 };
202 return confidence_map.at(val) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
203}
204
205
212template <typename SemiAxisLength>
213inline double getSemiAxis(const SemiAxisLength& semi_axis_length) {
214 return ((double)semi_axis_length.value) * 1e-2 / etsi_its_msgs::OneCentimeterHelper<SemiAxisLength>::value;
215}
216
223template <typename PosConfidenceEllipse>
224inline std::tuple<double, double, double> getPosConfidenceEllipse(const PosConfidenceEllipse& position_confidence_ellipse) {
225 return {
226 getSemiAxis(position_confidence_ellipse.semi_major_confidence),
227 getSemiAxis(position_confidence_ellipse.semi_minor_confidence),
228 position_confidence_ellipse.semi_major_orientation.value * 1e-1
229 };
230}
231
243inline std::array<double, 4> CovMatrixFromConfidenceEllipse(double semi_major, double semi_minor, double major_orientation, const double object_heading) {
244 std::array<double, 4> covariance_matrix;
245 double semi_major_squared = semi_major * semi_major / (etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR);
246 double semi_minor_squared = semi_minor * semi_minor / (etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR);
247 double major_orientation_rad = major_orientation * M_PI / 180;
248 double object_heading_rad = object_heading;
249
250 double angle_to_object = object_heading_rad - major_orientation_rad;
251
252 double cos_angle = std::cos(angle_to_object);
253 double sin_angle = std::sin(angle_to_object);
254 covariance_matrix[0] = semi_major_squared * cos_angle * cos_angle + semi_minor_squared * sin_angle * sin_angle;
255 covariance_matrix[1] = (semi_major_squared - semi_minor_squared) * cos_angle * sin_angle;
256 covariance_matrix[2] = covariance_matrix[1];
257 covariance_matrix[3] = semi_major_squared * sin_angle * sin_angle + semi_minor_squared * cos_angle * cos_angle;
258
259 return covariance_matrix;
260}
261
272inline std::array<double, 4> WGSCovMatrixFromConfidenceEllipse(double semi_major, double semi_minor, double major_orientation) {
273 // The WGS covariance matrix is the same as in vehicle coordinates, if it would have a heading of 0.0
274 return CovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation, 0.0);
275}
276
284template <typename PosConfidenceEllipse>
285inline std::array<double, 4> getPosConfidenceEllipse(const PosConfidenceEllipse& position_confidence_ellipse, const double object_heading){
286 auto [semi_major, semi_minor, major_orientation] = getPosConfidenceEllipse(position_confidence_ellipse);
287 return CovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation, object_heading);
288}
289
297template <typename PosConfidenceEllipse>
298inline std::array<double, 4> getWGSPosConfidenceEllipse(const PosConfidenceEllipse& position_confidence_ellipse){
299 auto [semi_major, semi_minor, major_orientation] = getPosConfidenceEllipse(position_confidence_ellipse);
300 return WGSCovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation);
301}
302
303#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H
double getHeadingCDD(const Heading &heading)
Get the Heading value.
double getSpeedConfidence(const Speed &speed)
Get the Speed Confidence.
gm::PointStamped getUTMPosition(const T &reference_position, int &zone, bool &northp)
Get the UTM Position defined by the given ReferencePosition.
double getAccelerationMagnitude(const AccelerationMagnitude &acceleration_magnitude)
Get the AccelerationMagnitude value.
double getSemiAxis(const SemiAxisLength &semi_axis_length)
Get the Semi Axis object.
std::array< double, 4 > getWGSPosConfidenceEllipse(const PosConfidenceEllipse &position_confidence_ellipse)
Get the covariance matrix of the position confidence ellipse.
double getLatitude(const Latitude &latitude)
Get the Latitude value.
double getAccelerationMagnitudeConfidence(const AccelerationMagnitude &acceleration_magnitude)
Get the AccelerationMagnitude Confidence.
double getHeadingConfidenceCDD(const Heading &heading)
Get the Heading value.
double getYawRateConfidenceCDD(const YawRate &yaw_rate)
Get the Yaw Rate Confidence.
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 ellip...
double getLongitude(const Longitude &longitude)
Get the Longitude value.
double getYawRateCDD(const YawRate &yaw_rate)
Get the Yaw Rate value.
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.
uint32_t getStationID(const ItsPduHeader &header)
Get the StationID of ItsPduHeader.
std::array< double, 4 > WGSCovMatrixFromConfidenceEllipse(double semi_major, double semi_minor, double major_orientation)
Convert the confidence ellipse to a covariance matrix.
double getAltitude(const Altitude &altitude)
Get the Altitude value.
double getSpeed(const Speed &speed)
Get the vehicle speed.