cdd_setters_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_SETTERS_COMMON_H
#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H
 
#include <etsi_its_msgs_utils/impl/checks.h>
#include <etsi_its_msgs_utils/impl/constants.h>
#include <GeographicLib/UTMUPS.hpp>
#include <array>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <type_traits>

template <typename StationId>
inline void setStationId(StationId& station_id, const uint32_t id_value) {
  throwIfOutOfRange(id_value, StationId::MIN, StationId::MAX, "StationId");
  station_id.value = id_value;
}

template <typename StationType>
inline void setStationType(StationType& station_type, const uint8_t value) {
  throwIfOutOfRange(value, StationType::MIN, StationType::MAX, "StationType");
  station_type.value = value;
}

inline 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) {
  uint64_t t_its = unix_nanosecs * 1e-6 + (uint64_t)(n_leap_seconds * 1e3) - etsi_its_msgs::UNIX_SECONDS_2004 * 1e3;
  throwIfOutOfRange(t_its, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
  timestamp_its.value = t_its;
}

inline void setLatitude(Latitude& latitude, const double deg) {
  int64_t angle_in_10_micro_degree = (int64_t)std::round(deg * 1e7);
  throwIfOutOfRange(angle_in_10_micro_degree, Latitude::MIN, Latitude::MAX, "Latitude");
  latitude.value = angle_in_10_micro_degree;
}

inline void setLongitude(Longitude& longitude, const double deg) {
  int64_t angle_in_10_micro_degree = (int64_t)std::round(deg * 1e7);
  throwIfOutOfRange(angle_in_10_micro_degree, Longitude::MIN, Longitude::MAX, "Longitude");
  longitude.value = angle_in_10_micro_degree;
}

inline void setAltitudeValue(AltitudeValue& altitude, const double value) {
  int64_t alt_in_cm = (int64_t)std::round(value * 1e2);
  if (alt_in_cm >= AltitudeValue::MIN && alt_in_cm <= AltitudeValue::MAX) {
    altitude.value = alt_in_cm;
  } else if (alt_in_cm < AltitudeValue::MIN) {
    altitude.value = AltitudeValue::MIN;
  } else if (alt_in_cm > AltitudeValue::MAX) {
    altitude.value = AltitudeValue::MAX;
  }
}

inline void setAltitude(Altitude& altitude, const double value) {
  altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
  setAltitudeValue(altitude.altitude_value, value);
}

inline void setSpeedValue(SpeedValue& speed, const double value) {
  auto speed_val = std::round(value * 1e2);
  throwIfOutOfRange(speed_val, SpeedValue::MIN, SpeedValue::MAX, "SpeedValue");
  speed.value = static_cast<decltype(speed.value)>(speed_val);
}

inline void setSpeedConfidence(SpeedConfidence& speed_confidence, const double value) {
  auto speed_conf = std::round(value * 1e2 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (speed_conf < SpeedConfidence::MIN && speed_conf > 0.0){
    speed_conf = SpeedConfidence::MIN;
  } else if (speed_conf >= SpeedConfidence::OUT_OF_RANGE || speed_conf <= 0.0) {
    speed_conf = SpeedConfidence::UNAVAILABLE;
  }
  speed_confidence.value = static_cast<decltype(speed_confidence.value)>(speed_conf);
}

inline void setSpeed(Speed& speed, const double value, const double confidence = std::numeric_limits<double>::infinity()) {
  setSpeedConfidence(speed.speed_confidence, confidence);
  setSpeedValue(speed.speed_value, value);
}

template <typename AccelerationMagnitudeValue>
inline void setAccelerationMagnitudeValue(AccelerationMagnitudeValue& accel_mag_value, const double value) {
  auto accel_mag = std::round(value * 1e1);
  throwIfOutOfRange(accel_mag, AccelerationMagnitudeValue::MIN, AccelerationMagnitudeValue::MAX, "AccelerationMagnitudeValue");
  accel_mag_value.value = static_cast<decltype(accel_mag_value.value)>(accel_mag);
}

template <typename AccelerationConfidence>
inline void setAccelerationMagnitudeConfidence(AccelerationConfidence& accel_mag_confidence, const double value) {
  auto accel_mag_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (accel_mag_conf < AccelerationConfidence::MIN && accel_mag_conf > 0.0){
    accel_mag_conf = AccelerationConfidence::MIN;
  } else if (accel_mag_conf >= AccelerationConfidence::OUT_OF_RANGE || accel_mag_conf <= 0.0) {
    accel_mag_conf = AccelerationConfidence::UNAVAILABLE;
  }
  accel_mag_confidence.value = static_cast<decltype(accel_mag_confidence.value)>(accel_mag_conf);
}

template<typename AccelerationMagnitude>
inline void setAccelerationMagnitude(AccelerationMagnitude& accel_mag, const double value,
                                     const double confidence = std::numeric_limits<double>::infinity()) {
  setAccelerationMagnitudeConfidence(accel_mag.acceleration_confidence, confidence);
  setAccelerationMagnitudeValue(accel_mag.acceleration_magnitude_value, value);
}

template <typename AccelerationConfidence>
inline void setAccelerationConfidence(AccelerationConfidence& accel_confidence, const double value) {
  auto accel_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (accel_conf < AccelerationConfidence::MIN && accel_conf > 0.0){
    accel_conf = AccelerationConfidence::MIN;
  } else if (accel_conf >= AccelerationConfidence::OUT_OF_RANGE || accel_conf <= 0.0) {
    accel_conf = AccelerationConfidence::UNAVAILABLE;
  }
  accel_confidence.value = static_cast<decltype(accel_confidence.value)>(accel_conf);
}

template <typename T>
inline void setReferencePosition(T& ref_position, const double latitude, const double longitude,
                                 const double altitude = AltitudeValue::UNAVAILABLE) {
  setLatitude(ref_position.latitude, latitude);
  setLongitude(ref_position.longitude, longitude);
  if (altitude != AltitudeValue::UNAVAILABLE) {
    setAltitude(ref_position.altitude, altitude);
  } else {
    ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE;
    ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE;
  }
  // TODO: set confidence values
}

template <typename T>
inline void setFromUTMPosition(T& reference_position, const gm::PointStamped& utm_position, const int zone,
                               const bool northp) {
  std::string required_frame_prefix = "utm_";
  if (utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) {
    throw std::invalid_argument("Frame-ID of UTM Position '" + utm_position.header.frame_id +
                                "' does not start with required prefix '" + required_frame_prefix + "'!");
  }
  double latitude, longitude;
  try {
    GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude);
  } catch (GeographicLib::GeographicErr& e) {
    throw std::invalid_argument(e.what());
  }
  setReferencePosition(reference_position, latitude, longitude, utm_position.point.z);
}

template <typename HeadingValue>
inline void setHeadingValue(HeadingValue& heading, const double value) {
  int64_t deg = (int64_t)std::round(value * 1e1);
  throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue");
  heading.value = deg;
}

template<typename HeadingConfidence>
inline void setHeadingConfidence(HeadingConfidence& heading_confidence, const double value) {
  auto heading_conf = std::round(value * 1e1 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (heading_conf < HeadingConfidence::MIN && heading_conf > 0.0){
    heading_conf = HeadingConfidence::MIN;
  } else if (heading_conf >= HeadingConfidence::OUT_OF_RANGE || heading_conf <= 0.0) {
    heading_conf = HeadingConfidence::UNAVAILABLE;
  }
  heading_confidence.value = static_cast<decltype(heading_confidence.value)>(heading_conf);
}

template <typename Heading, typename HeadingConfidence = decltype(Heading::heading_confidence)>
void setHeadingCDD(Heading& heading, const double value, double confidence = std::numeric_limits<double>::infinity()) {
  setHeadingConfidence(heading.heading_confidence, confidence);
  setHeadingValue(heading.heading_value, value);
}

template <typename YawRate, typename YawRateValue = decltype(YawRate::yaw_rate_value), typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
inline void setYawRateCDD(YawRate& yaw_rate, const double value,
                                        double confidence = std::numeric_limits<double>::infinity()) {
  double yaw_rate_in_001_degrees = value * 100.0;
  // limit value range
  if (yaw_rate_in_001_degrees < YawRateValue::MIN) {
    yaw_rate_in_001_degrees = YawRateValue::MIN; // MIN should be NEGATIVE_OUT_OF_RANGE, but CAM only has MIN
  } else if (yaw_rate_in_001_degrees > YawRateValue::MAX - 1) {
    yaw_rate_in_001_degrees = YawRateValue::MAX - 1; // MAX - 1 should be POSITIVE_OUT_OF_RANGE, but CAM only has MAX
  }
 
  yaw_rate.yaw_rate_value.value = static_cast<decltype(yaw_rate.yaw_rate_value.value)>(yaw_rate_in_001_degrees);
 
  double yaw_rate_std = confidence;
  if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
    yaw_rate.yaw_rate_confidence.value = YawRateConfidence::UNAVAILABLE;
  } else {
    yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
    // How stupid is this?!
    static const std::map<double, uint8_t> confidence_map = {
        {0.01, YawRateConfidence::DEG_SEC_000_01},
        {0.05, YawRateConfidence::DEG_SEC_000_05},
        {0.1, YawRateConfidence::DEG_SEC_000_10},
        {1.0, YawRateConfidence::DEG_SEC_001_00},
        {5.0, YawRateConfidence::DEG_SEC_005_00},
        {10.0, YawRateConfidence::DEG_SEC_010_00},
        {100.0, YawRateConfidence::DEG_SEC_100_00},
        {std::numeric_limits<double>::infinity(), YawRateConfidence::OUT_OF_RANGE},
    };
    for(const auto& [thresh, conf_val] : confidence_map) {
      if (yaw_rate_std <= thresh) {
        yaw_rate.yaw_rate_confidence.value = conf_val;
        break;
      }
    }
  }
}

template <typename SemiAxisLength>
inline void setSemiAxis(SemiAxisLength& semi_axis_length, const double length) {
  double semi_axis_length_val = std::round(length * etsi_its_msgs::OneCentimeterHelper<SemiAxisLength>::value * 1e2);
  if(semi_axis_length_val < SemiAxisLength::MIN) {
    semi_axis_length_val = SemiAxisLength::MIN;
  } else if(semi_axis_length_val >= SemiAxisLength::OUT_OF_RANGE) {
    semi_axis_length_val = SemiAxisLength::OUT_OF_RANGE;
  }
  semi_axis_length.value = static_cast<uint16_t>(semi_axis_length_val);
}

template <typename PosConfidenceEllipse>
inline void setPosConfidenceEllipse(PosConfidenceEllipse& position_confidence_ellipse, const double semi_major_axis,
  const double semi_minor_axis, const double orientation) {
  setSemiAxis(position_confidence_ellipse.semi_major_confidence, semi_major_axis);
  setSemiAxis(position_confidence_ellipse.semi_minor_confidence, semi_minor_axis);
  setHeadingValue(position_confidence_ellipse.semi_major_orientation, orientation);
}

inline std::tuple<double, double, double> confidenceEllipseFromCovMatrix(const std::array<double, 4>& covariance_matrix, const double object_heading) {
 
  if(std::abs(covariance_matrix[1] - covariance_matrix[2]) > 1e-6) {
    throw std::invalid_argument("Covariance matrix is not symmetric");
  }
  double trace = covariance_matrix[0] + covariance_matrix[3];
  double determinant = covariance_matrix[0] * covariance_matrix[3] - covariance_matrix[1] * covariance_matrix[1];
  if (determinant <= 0 || covariance_matrix[0] <= 0) {
    // https://sites.math.northwestern.edu/~clark/285/2006-07/handouts/pos-def.pdf:
    // Therefore, a necessary and sufficient condition for the quadratic form of a symmetric 2 × 2 matrix
    // to be positive definite is for det(A) > 0 and a > 0
    throw std::invalid_argument("Covariance matrix is not positive definite");
  }
  double eigenvalue1 = trace / 2 + std::sqrt(trace * trace / 4 - determinant);
  double eigenvalue2 = trace / 2 - std::sqrt(trace * trace / 4 - determinant);
  double semi_major_axis = std::sqrt(eigenvalue1) * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR;
  double semi_minor_axis = std::sqrt(eigenvalue2) * etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR;
  // object_heading - orientation of the ellipse, as WGS84 has positive angles to the right
  double orientation = object_heading - 0.5 * std::atan2(2 * covariance_matrix[1], covariance_matrix[0] - covariance_matrix[3]);
  orientation = orientation * 180 / M_PI; // Convert to degrees
  // Normalize to [0, 180)
  // 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
  orientation = std::fmod(orientation + 180, 180);
  while (orientation < 0) {
    orientation += 180;
  }
  while (orientation >= 180) {
    orientation -= 180;
  }
  return std::make_tuple(semi_major_axis, semi_minor_axis, orientation);
}

inline std::tuple<double, double, double> confidenceEllipseFromWGSCovMatrix(const std::array<double, 4>& covariance_matrix) {
  // The resulting ellipse is the same as if the cov matrix was given in vehicle coordinates
  // and the object heading was set to 0.0
  return confidenceEllipseFromCovMatrix(covariance_matrix, 0.0);
}

template <typename PosConfidenceEllipse>
inline void setPosConfidenceEllipse(PosConfidenceEllipse& position_confidence_ellipse, const std::array<double, 4>& covariance_matrix, const double object_heading){
  auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromCovMatrix(covariance_matrix, object_heading);
  setPosConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
}

template <typename PosConfidenceEllipse>
inline void setWGSPosConfidenceEllipse(PosConfidenceEllipse& position_confidence_ellipse, const std::array<double, 4>& covariance_matrix){
  auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromWGSCovMatrix(covariance_matrix);
  setPosConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
}
 
#endif  // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H