32#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H
33#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H
38#include <GeographicLib/UTMUPS.hpp>
46inline uint32_t
getStationID(
const ItsPduHeader& header) {
return header.station_id.value; }
54inline double getLatitude(
const Latitude& latitude) {
return ((
double)latitude.value) * 1e-7; }
62inline double getLongitude(
const Longitude& longitude) {
return ((
double)longitude.value) * 1e-7; }
71 if (altitude.altitude_value.value == AltitudeValue::UNAVAILABLE) {
74 return ((
double)altitude.altitude_value.value) * 1e-2;
83inline double getSpeed(
const Speed& speed) {
return ((
double)speed.speed_value.value) * 1e-2; }
92 return ((
double)speed.speed_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-2;
101template <
typename AccelerationMagnitude>
103 return ((
double)acceleration_magnitude.acceleration_magnitude_value.value) * 1e-1;
112template <
typename AccelerationMagnitude>
114 return ((
double)acceleration_magnitude.acceleration_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-1;
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);
135 GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y);
136 std::string hemisphere;
142 utm_point.header.frame_id =
"utm_" + std::to_string(zone) + hemisphere;
143 }
catch (GeographicLib::GeographicErr& e) {
144 throw std::invalid_argument(e.what());
157template <
typename Heading>
158inline double getHeadingCDD(
const Heading& heading) {
return ((
double)heading.heading_value.value) * 1e-1; }
168template <
typename Heading>
169inline double getHeadingConfidenceCDD(
const Heading& heading) {
return ((
double)heading.heading_confidence.value) * 1e-1 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; }
177template <
typename YawRate>
179 return ((
double)yaw_rate.yaw_rate_value.value) * 1e-2;
188template <
typename YawRate,
typename YawRateConf
idence = decltype(YawRate::yaw_rate_conf
idence)>
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()},
202 return confidence_map.at(val) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
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;
223template <
typename PosConf
idenceEllipse>
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
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;
250 double angle_to_object = object_heading_rad - major_orientation_rad;
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;
259 return covariance_matrix;
284template <
typename PosConf
idenceEllipse>
285inline std::array<double, 4>
getPosConfidenceEllipse(
const PosConfidenceEllipse& position_confidence_ellipse,
const double object_heading){
297template <
typename PosConf
idenceEllipse>
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.