mapem_ts_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 <GeographicLib/UTMUPS.hpp>
 
namespace etsi_its_mapem_ts_msgs {
 
namespace access {
 
#include <etsi_its_msgs_utils/impl/checks.h>

  inline void setMinuteOfTheYear(MinuteOfTheYear& moy, const uint32_t moy_value) {
    throwIfOutOfRange(moy_value, MinuteOfTheYear::MIN, MinuteOfTheYear::MAX, "MinuteOfTheYear");
    moy.value = moy_value;
  }

  inline void setMinuteOfTheYear(MapData& map, const uint32_t moy_value) {
    setMinuteOfTheYear(map.time_stamp, moy_value);
  }

  inline void setMinuteOfTheYear(MAPEM& mapem, const uint32_t moy_value) {
    setMinuteOfTheYear(mapem.map, moy_value);
    mapem.map.time_stamp_is_present = true;
  }

  inline void setIntersectionID(IntersectionID& intsct_id, const uint16_t id_value) {
    throwIfOutOfRange(id_value, IntersectionID::MIN, IntersectionID::MAX, "IntersectionID");
    intsct_id.value = id_value;
  }

  inline void setIntersectionID(IntersectionGeometry& intsct, const uint16_t id_value) {
    setIntersectionID(intsct.id.id, id_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 setElevation(Elevation& elevation, const double value) {
    int64_t alt_in_dm = (int64_t)std::round(value*1e1);
    if(alt_in_dm>=Elevation::MIN && alt_in_dm<=Elevation::MAX) elevation.value = alt_in_dm;
    else if(alt_in_dm<Elevation::MIN) elevation.value = Elevation::MIN;
    else if(alt_in_dm>Elevation::MAX) elevation.value = Elevation::MAX;
  }

  inline void setPosition3D(Position3D& pos, const double latitude, const double longitude) {
    setLatitude(pos.lat, latitude);
    setLongitude(pos.lon, longitude);
    pos.elevation_is_present = false;
  }

  inline void setPosition3D(Position3D& pos, const double latitude, const double longitude, const double altitude) {
    setPosition3D(pos, latitude, longitude);
    setElevation(pos.elevation, altitude);
    pos.elevation_is_present = true;
  }

  inline void setPosition3D(IntersectionGeometry& intsct, double latitude, double longitude, double altitude) {
    setPosition3D(intsct.ref_point, latitude, longitude, altitude);
  }

  inline void setPosition3DFromUTMPosition(Position3D& 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());
    }
    setPosition3D(reference_position, latitude, longitude, utm_position.point.z);
  }
 
} // namespace access
 
} // namespace etsi_its_mapem_ts_msgs