mcm_getters.h

Go to the documentation of this file.

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

 
#pragma once
 
namespace etsi_its_mcm_uulm_msgs::access {

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;
}

template <typename T>
inline gm::PointStamped getUTMPosition(const T& reference_position, int& zone, bool& northp, double& conv_angle) {
  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 {
    double scale;
    GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y, conv_angle,
                                   scale);
    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;
}

inline gm::PointStamped getUTMPosition(const MCM& mcm, int& zone, bool& northp, double& conv_angle) {
  return getUTMPosition(mcm.mcm.mcm_parameters.basic_container.reference_position, zone, northp, conv_angle);
}

inline gm::PointStamped getUTMPosition(const MCM& mcm, int& zone, bool& northp) {
  double conv_angle;  // unused, but required by the function signature
  return getUTMPosition(mcm.mcm.mcm_parameters.basic_container.reference_position, zone, northp, conv_angle);
}

inline gm::PointStamped getUTMPosition(const MCM& mcm) {
  int zone;           // unused, but required by the function signature
  bool northp;        // unused, but required by the function signature
  double conv_angle;  // unused, but required by the function signature
  return getUTMPosition(mcm.mcm.mcm_parameters.basic_container.reference_position, zone, northp, conv_angle);
}
 
// ---------- suggested maneuver container getters ----------

inline SuggestedManeuverContainer getSuggestedManeuverContainer(const MCM& mcm) {
  if (mcm.mcm.mcm_parameters.maneuver_container.choice != ManeuverContainer::CHOICE_SUGGESTED_MANEUVER_CONTAINER) {
    throw std::invalid_argument("No suggested maneuver container present in MCM");
  }
  return mcm.mcm.mcm_parameters.maneuver_container.suggested_maneuver_container;
}

inline uint32_t getTargetStationId(const SuggestedManeuverContainer& suggested_maneuver_container) {
  return suggested_maneuver_container.target_station_id.value;
}

inline SuggestedManeuver getSuggestedManeuver(const SuggestedManeuverContainer& suggested_maneuver_container) {
  if (!suggested_maneuver_container.suggested_maneuver_is_present) {
    throw std::invalid_argument("No suggested maneuver present in SuggestedManeuverContainer");
  }
  return suggested_maneuver_container.suggested_maneuver;
}

inline SuggestedManeuver getSuggestedManeuver(const MCM& mcm) {
  return getSuggestedManeuver(getSuggestedManeuverContainer(mcm));
}

inline uint16_t getManeuverId(const SuggestedManeuver& suggested_maneuver) {
  return suggested_maneuver.maneuver_id.value;
}

inline uint16_t getAdviceUpdateId(const SuggestedManeuver& suggested_maneuver) {
  return suggested_maneuver.advice_update_id.value;
}

inline bool getConfirmationRequiredFlag(const SuggestedManeuver& suggested_maneuver) {
  return suggested_maneuver.confirmation_required_flag.value;
}

inline ManeuverConstraints getManeuverConstraints(const SuggestedManeuver& suggested_maneuver) {
  if (suggested_maneuver.maneuver_parameters.choice != ManeuverParameters::CHOICE_MANEUVER_CONSTRAINTS) {
    throw std::invalid_argument("No maneuver constraints present in SuggestedManeuver");
  }
  return suggested_maneuver.maneuver_parameters.maneuver_constraints;
}

inline ManeuverConstraints getManeuverConstraints(const MCM& mcm) {
  return getManeuverConstraints(getSuggestedManeuver(mcm));
}

inline std::vector<LongitudinalWaypoint> getLongitudinalWaypoints(const ManeuverConstraints& maneuver_constraints) {
  return maneuver_constraints.longitudinal_maneuver_waypoint_container.array;
}

inline std::vector<LongitudinalWaypoint> getLongitudinalWaypoints(const MCM& mcm) {
  return getLongitudinalWaypoints(getManeuverConstraints(mcm));
}

inline gm::Point getWaypointDelta(const LongitudinalWaypoint& longitudinal_waypoint) {
  gm::Point point;
  point.x = static_cast<double>(longitudinal_waypoint.waypoint.x_distance.value * 1e-2);  // convert cm to m
  point.y = static_cast<double>(longitudinal_waypoint.waypoint.y_distance.value * 1e-2);  // convert cm to m
  point.z = 0.0;  // z-coordinate is not used in longitudinal waypoints
  return point;
}

inline double getMinArrivalTimeDelta(const LongitudinalWaypoint& longitudinal_waypoint) {
  return static_cast<double>(longitudinal_waypoint.min_arrival_time.value * 1e-3);  // convert ms to s
}

inline double getMaxArrivalTimeDelta(const LongitudinalWaypoint& longitudinal_waypoint) {
  return static_cast<double>(longitudinal_waypoint.max_arrival_time.value * 1e-3);  // convert ms to s
}

inline double getMinVelocity(const LongitudinalWaypoint& longitudinal_waypoint) {
  if (!longitudinal_waypoint.min_velocity_is_present) {
    throw std::invalid_argument("No min velocity present in LongitudinalWaypoint");
  } else if (longitudinal_waypoint.min_velocity.value == SpeedValue::UNAVAILABLE) {
    return std::numeric_limits<double>::quiet_NaN();
  }
  return static_cast<double>(longitudinal_waypoint.min_velocity.value * 1e-2);  // convert cm/s to m/s
}

inline double getMaxVelocity(const LongitudinalWaypoint& longitudinal_waypoint) {
  if (!longitudinal_waypoint.max_velocity_is_present) {
    throw std::invalid_argument("No max velocity present in LongitudinalWaypoint");
  } else if (longitudinal_waypoint.max_velocity.value == SpeedValue::UNAVAILABLE) {
    return std::numeric_limits<double>::quiet_NaN();
  }
  return static_cast<double>(longitudinal_waypoint.max_velocity.value * 1e-2);  // convert cm/s to m/s
}
 
// ---------- road user container getters ----------

inline RoadUserContainer getRoadUserContainer(const MCM& mcm) {
  if (mcm.mcm.mcm_parameters.maneuver_container.choice != ManeuverContainer::CHOICE_ROAD_USER_CONTAINER) {
    throw std::invalid_argument("No road user container present in MCM");
  }
  return mcm.mcm.mcm_parameters.maneuver_container.road_user_container;
}
 
}  // namespace etsi_its_mcm_uulm_msgs::access