mapem_ts_utils.h

Go to the documentation of this file.

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

 
#include <ctime>
#include <GeographicLib/UTMUPS.hpp>
 
#pragma once
 
namespace etsi_its_mapem_ts_msgs {
 
namespace access {

  inline uint64_t getUnixSecondsOfYear(const uint64_t unixSecond) {
 
    // Get current time as a time_point
    time_t ts = static_cast<time_t>(unixSecond); // Convert uint64_t to time_t
 
    struct tm* timeinfo;
    timeinfo = gmtime(&ts);
 
    // Set the timeinfo to the beginning of the year
    timeinfo->tm_sec = 0;
    timeinfo->tm_min = 0;
    timeinfo->tm_hour = 0;
    timeinfo->tm_mday = 1;
    timeinfo->tm_mon = 0;
 
    return timegm(timeinfo); // Convert struct tm back to Unix timestamp
  }

  inline uint64_t getUnixNanosecondsFromMinuteOfTheYear(const MinuteOfTheYear& moy, const uint64_t unix_nanoseconds_estimate) {
    return ((uint64_t)(moy.value*60) + getUnixSecondsOfYear(unix_nanoseconds_estimate*1e-9))*1e9;
  }

  inline uint64_t getUnixNanosecondsFromMapData(const MapData& map, const uint64_t unix_nanoseconds_estimate) {
    return getUnixNanosecondsFromMinuteOfTheYear(getMinuteOfTheYear(map), unix_nanoseconds_estimate);
  }

  inline uint64_t getUnixNanoseconds(const MAPEM& mapem, const uint64_t unix_timestamp_estimate) {
    return getUnixNanosecondsFromMapData(mapem.map, unix_timestamp_estimate);
  }

  inline gm::PointStamped getUTMPosition(const Position3D& reference_position, int& zone, bool& northp) {
    gm::PointStamped utm_point;
    double latitude = getLatitude(reference_position.lat);
    double longitude = getLongitude(reference_position.lon);
    if(reference_position.elevation_is_present) utm_point.point.z = getElevation(reference_position.elevation);
    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;
  }

  inline gm::PointStamped getUTMPositionWithConvergenceAngle(const Position3D& reference_position, int& zone, bool& northp, double& conv_angle) {
    gm::PointStamped utm_point;
    double latitude = getLatitude(reference_position.lat);
    double longitude = getLongitude(reference_position.lon);
    if(reference_position.elevation_is_present) utm_point.point.z = getElevation(reference_position.elevation);
    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 getRefPointUTMPosition(const IntersectionGeometry& intsctn, int& zone, bool& northp) {
    return getUTMPosition(intsctn.ref_point, zone, northp);
  }

  inline gm::PointStamped getRefPointUTMPositionWithConvergenceAngle(const IntersectionGeometry& intsctn, int& zone, bool& northp, double& conv_angle) {
    return getUTMPositionWithConvergenceAngle(intsctn.ref_point, zone, northp, conv_angle);
  }
 
} // namespace etsi_its_mapem_ts_msgs
} // namespace access