cdd_getters_common.h

Go to the documentation of this file.

// SPDX-License-Identifier: MIT
// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University

 
#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H
#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H
 
#include <array>
#include <cmath>
#include <cstdint>
#include <GeographicLib/UTMUPS.hpp>

inline uint32_t getStationID(const ItsPduHeader& header) { return header.station_id.value; }

inline double getLatitude(const Latitude& latitude) { return ((double)latitude.value) * 1e-7; }

inline double getLongitude(const Longitude& longitude) { return ((double)longitude.value) * 1e-7; }

inline double getAltitude(const Altitude& altitude) {
  if (altitude.altitude_value.value == AltitudeValue::UNAVAILABLE) {
    return 0.0;
  }
  return ((double)altitude.altitude_value.value) * 1e-2;
}

inline double getSpeed(const Speed& speed) { return ((double)speed.speed_value.value) * 1e-2; }

inline double getSpeedConfidence(const Speed& speed) {
  return ((double)speed.speed_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-2;
}

template <typename AccelerationMagnitude>
inline double getAccelerationMagnitude(const AccelerationMagnitude& acceleration_magnitude) {
  return ((double)acceleration_magnitude.acceleration_magnitude_value.value) * 1e-1;
}

template <typename AccelerationMagnitude>
inline double getAccelerationMagnitudeConfidence(const AccelerationMagnitude& acceleration_magnitude) {
  return ((double)acceleration_magnitude.acceleration_confidence.value) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR * 1e-1;
}

template <typename T>
inline gm::PointStamped getUTMPosition(const T& reference_position, int& zone, bool& northp) {
  gm::PointStamped utm_point;
  double latitude = getLatitude(reference_position.latitude);
  double longitude = getLongitude(reference_position.longitude);
  utm_point.point.z = getAltitude(reference_position.altitude);
  try {
    GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y);
    std::string hemisphere;
    if (northp) {
      hemisphere = "N";
    } else {
      hemisphere = "S";
    }
    utm_point.header.frame_id = "utm_" + std::to_string(zone) + hemisphere;
  } catch (GeographicLib::GeographicErr& e) {
    throw std::invalid_argument(e.what());
  }
  return utm_point;
}

template <typename Heading>
inline double getHeadingCDD(const Heading& heading) { return ((double)heading.heading_value.value) * 1e-1; }

template <typename Heading>
inline double getHeadingConfidenceCDD(const Heading& heading) { return ((double)heading.heading_confidence.value) * 1e-1 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; }

template <typename YawRate>
inline double getYawRateCDD(const YawRate& yaw_rate) {
  return ((double)yaw_rate.yaw_rate_value.value) * 1e-2; // Yaw rate in deg/s
}

template <typename YawRate, typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
inline double getYawRateConfidenceCDD(const YawRate& yaw_rate) {
  auto val = yaw_rate.yaw_rate_confidence.value;
  static const std::map<uint8_t, double> confidence_map = {
      {YawRateConfidence::UNAVAILABLE, std::numeric_limits<double>::infinity()},
      {YawRateConfidence::DEG_SEC_000_01, 0.01},
      {YawRateConfidence::DEG_SEC_000_05, 0.05},
      {YawRateConfidence::DEG_SEC_000_10, 0.1},
      {YawRateConfidence::DEG_SEC_001_00, 1.0},
      {YawRateConfidence::DEG_SEC_005_00, 5.0},
      {YawRateConfidence::DEG_SEC_010_00, 10.0},
      {YawRateConfidence::DEG_SEC_100_00, 100.0},
      {YawRateConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
  };
  return confidence_map.at(val) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
}
 

template <typename SemiAxisLength>
inline double getSemiAxis(const SemiAxisLength& semi_axis_length) {
  return ((double)semi_axis_length.value) * 1e-2 / etsi_its_msgs::OneCentimeterHelper<SemiAxisLength>::value;
}

template <typename PosConfidenceEllipse>
inline std::tuple<double, double, double> getPosConfidenceEllipse(const PosConfidenceEllipse& position_confidence_ellipse) {
  return {
    getSemiAxis(position_confidence_ellipse.semi_major_confidence),
    getSemiAxis(position_confidence_ellipse.semi_minor_confidence),
    position_confidence_ellipse.semi_major_orientation.value * 1e-1
  };
}

inline std::array<double, 4> CovMatrixFromConfidenceEllipse(double semi_major, double semi_minor, double major_orientation, const double object_heading) {
  std::array<double, 4> covariance_matrix;
  double semi_major_squared = semi_major * semi_major / (etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR);
  double semi_minor_squared = semi_minor * semi_minor / (etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR);
  double major_orientation_rad = major_orientation * M_PI / 180;
  double object_heading_rad = object_heading;
 
  double angle_to_object = object_heading_rad - major_orientation_rad;
 
  double cos_angle = std::cos(angle_to_object);
  double sin_angle = std::sin(angle_to_object);
  covariance_matrix[0] = semi_major_squared * cos_angle * cos_angle + semi_minor_squared * sin_angle * sin_angle;
  covariance_matrix[1] = (semi_major_squared - semi_minor_squared) * cos_angle * sin_angle;
  covariance_matrix[2] = covariance_matrix[1];
  covariance_matrix[3] = semi_major_squared * sin_angle * sin_angle + semi_minor_squared * cos_angle * cos_angle;
 
  return covariance_matrix;
}

inline std::array<double, 4> WGSCovMatrixFromConfidenceEllipse(double semi_major, double semi_minor, double major_orientation) {
  // The WGS covariance matrix is the same as in vehicle coordinates, if it would have a heading of 0.0
  return CovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation, 0.0);
}

template <typename PosConfidenceEllipse>
inline std::array<double, 4> getPosConfidenceEllipse(const PosConfidenceEllipse& position_confidence_ellipse, const double object_heading){
  auto [semi_major, semi_minor, major_orientation] = getPosConfidenceEllipse(position_confidence_ellipse);
  return CovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation, object_heading);
}

template <typename PosConfidenceEllipse>
inline std::array<double, 4> getWGSPosConfidenceEllipse(const PosConfidenceEllipse& position_confidence_ellipse){
  auto [semi_major, semi_minor, major_orientation] = getPosConfidenceEllipse(position_confidence_ellipse);
  return WGSCovMatrixFromConfidenceEllipse(semi_major, semi_minor, major_orientation);
}
 
#endif  // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H