mcm_setters.h

Go to the documentation of this file.

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

 
#pragma once
 
#include <etsi_its_msgs_utils/impl/checks.h>
 
namespace etsi_its_mcm_uulm_msgs::access {
 
// ---------- header ----------

inline void setItsPduHeader(MCM& mcm, const uint32_t station_id, const uint8_t protocol_version = 2) {
  mcm.header.message_id.value = MessageId::MCM;
  mcm.header.station_id.value = station_id;
  throwIfOutOfRange(protocol_version, OrdinalNumber1B::MIN, OrdinalNumber1B::MAX, "ProtocolVersion");
  mcm.header.protocol_version.value = protocol_version;
}

inline void setGenerationDeltaTime(
    MCM& mcm, 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;
  uint16_t gdt_value = t_its % 65536;
  throwIfOutOfRange(gdt_value, GenerationDeltaTime::MIN, GenerationDeltaTime::MAX, "GenerationDeltaTime");
  mcm.mcm.generation_delta_time.value = gdt_value;
}
 
// ---------- basic_container ----------

inline void setStationType(MCM& mcm, const int value) {
  throwIfOutOfRange(value, TrafficParticipantType::MIN, TrafficParticipantType::MAX, "StationType");
  mcm.mcm.mcm_parameters.basic_container.station_type.value = value;
}

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 setReferencePosition(MCM& mcm, const double latitude, const double longitude,
                                 const double altitude = AltitudeValue::UNAVAILABLE) {
  setLatitude(mcm.mcm.mcm_parameters.basic_container.reference_position.latitude, latitude);
  setLongitude(mcm.mcm.mcm_parameters.basic_container.reference_position.longitude, longitude);
  if (altitude != AltitudeValue::UNAVAILABLE) {
    setAltitude(mcm.mcm.mcm_parameters.basic_container.reference_position.altitude, altitude);
  } else {
    mcm.mcm.mcm_parameters.basic_container.reference_position.altitude.altitude_value.value =
        AltitudeValue::UNAVAILABLE;
    mcm.mcm.mcm_parameters.basic_container.reference_position.altitude.altitude_confidence.value =
        AltitudeConfidence::UNAVAILABLE;
  }
 
  // reset PositionConfidenceEllipse to unavailable
  mcm.mcm.mcm_parameters.basic_container.reference_position.position_confidence_ellipse.semi_major_axis_length.value =
      SemiAxisLength::UNAVAILABLE;
  mcm.mcm.mcm_parameters.basic_container.reference_position.position_confidence_ellipse.semi_minor_axis_length.value =
      SemiAxisLength::UNAVAILABLE;
  mcm.mcm.mcm_parameters.basic_container.reference_position.position_confidence_ellipse.semi_major_axis_orientation
      .value = Wgs84AngleValue::UNAVAILABLE;
}

inline void setFromUTMPosition(MCM& mcm, 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(mcm, latitude, longitude, utm_position.point.z);
}
 
// ---------- road_user_container ----------

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 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 T>
inline void setRoadUserDimension(T& dim, const double value) {
  int64_t dim_value = (int64_t)std::round(value * 1e1);
  throwIfOutOfRange(dim_value, T::MIN, T::MAX, "RoadUserDimension (Width/Length)");
  dim.value = dim_value;
}

inline void setRoadUserState(RoadUserContainer& road_user_container, const uint8_t type, const double speed,
                             const double heading, const double length, const double width) {
  road_user_container.road_user_state.road_user_type.value = type;
  setSpeedValue(road_user_container.road_user_state.speed, speed);
  setHeadingValue(road_user_container.road_user_state.heading, heading);
  setRoadUserDimension(road_user_container.road_user_state.length, length);
  setRoadUserDimension(road_user_container.road_user_state.width, width);
}

inline void setCartesianCoordinateLarge(CartesianCoordinateLarge& coordinate, const double value) {
  auto coord_value = std::round(value * 1e2);  // convert m to cm
  if (coord_value < CartesianCoordinateLarge::MIN) {
    coord_value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
  } else if (coord_value > CartesianCoordinateLarge::MAX) {
    coord_value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
  }
  coordinate.value = static_cast<decltype(coordinate.value)>(coord_value);
}

inline void setWaypoint(Waypoint& waypoint, const double x, const double y) {
  setCartesianCoordinateLarge(waypoint.x_distance, x);
  setCartesianCoordinateLarge(waypoint.y_distance, y);
}
 
}  // namespace etsi_its_mcm_uulm_msgs::access