ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
sumotls2ros.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2022 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Stephan Lapoehn - initial API and implementation
13  * Matthias Nichting - update to libsumo
14  ********************************************************************************/
15 
16 
17 #pragma once
18 
19 
20 #include "tcd/MAPEMIntersection.h"
21 #include "coordinate.h"
23 #include <unordered_map>
24 #include <adore_v2x_sim/SimMAPEM.h>
25 #include <adore_v2x_sim/SimSPATEM.h>
26 #include <math.h>
27 #include <libsumo/libsumo.h>
28 namespace adore
29 {
30  namespace sumo_if_ros
31  {
36  {
37  public:
38 
39  struct hash_pair {
40  template <class T1, class T2>
41  size_t operator()(const std::pair<T1, T2>& p) const
42  {
43  auto lhs = std::hash<T1>{}(p.first);
44  auto rhs = std::hash<T2>{}(p.second);
45 
46  lhs ^= rhs + 0x9e3779b9 + (lhs << 6) + (lhs >> 2);
47  return lhs;
48  }
49  };
50 
51  private:
52 
53  // maps the sumo-lane-id strings to the v2x lane ids for SPATEM generation
54  std::unordered_map<std::string, uint8_t> sumo_lane_id_mapping_;
55 
56  // maps the sumo-intersection-id strings to the v2x intersection ids for MAPEM / SPATEM generation
57  std::unordered_map<std::string, int> sumo_intersection_id_mapping_;
58 
59  // MAPEM data will only be generated once, due to its static nature
60  std::unordered_map<std::string, adore::sumo_if_ros::MAPEMIntersection> intersections_;
61 
62  // MAPEM data will only be generated once, due to its static nature
63  std::unordered_map<int, adore_v2x_sim::SimMAPEM> _MAPEM;
64 
65  // one signal group for every ingress - egress lane pair
66  std::unordered_map<std::pair<std::string,std::string>, uint8_t, SumoTLs2Ros::hash_pair> sumo_signal_group_id_mapping_;
67 
68  // generates n nodes for one sumo lane as vector of adore coordinates
69  std::vector<adore::sumo_if_ros::Coordinate> getNodesFromSUMOLane(libsumo::TraCIPositionVector & lane, int nodeCount = 5);
70 
71  // gathers information of the lane links on a intersection and generates coordinate vectors for these links
72  void getGeoreferencedLinks(std::string sumoIntersectionID, std::unordered_map<std::string, std::vector<std::string>>& linklist, std::unordered_map<std::string, std::vector<adore::sumo_if_ros::Coordinate>> & id_2_shape);
73 
74  void autofill_bit_string(std::size_t size, std::vector<uint8_t> & items);
75 
76  // entry point for MAPEM generation
77  std::unordered_map<std::string, adore::sumo_if_ros::MAPEMIntersection> generateMAPEMFromSUMO();
78 
79  // convert adore internal MAPEM representation to ROS msg
80  std::unordered_map<int, adore_v2x_sim::SimMAPEM> convertToROSMsg(std::unordered_map<std::string, adore::sumo_if_ros::MAPEMIntersection> mapem_data, double time = 0,double power = 23);
81 
82  // calculates the distance btw. 2 wgs84 points in m
83  double wgs84_distance(double lat1, double lon1, double lat2,double lon2);
84 
85  double getSystemTime();
86 
87  int32_t getMOY(double time);
88 
89  int getLeapYearSeconds(double time);
90 
91  double getSecondOfYearFromUTC(double time);
92 
93  public:
94 
95  // get MAPEM information based on sumo traffic-light
96  std::unordered_map<int, adore_v2x_sim::SimMAPEM> getMAPEMFromSUMO(double time,double power=23);
97 
98  // get SPATEM information based on sumo traffic-light
99  std::vector<adore_v2x_sim::SimSPATEM> getSPATEMFromSUMO(double time, double power=23);
100 
101  int getIntersectionIDForSUMOString(std::string sumo_intersection_id);
102 
103  std::string getSUMOStringFromIntersectionID(int intersection_id);
104 
106 
108 
110 
112 
114  };
115  }
116 }
Definition: sumotls2ros.h:36
void autofill_bit_string(std::size_t size, std::vector< uint8_t > &items)
Definition: sumotls2ros.cpp:760
bool _generate_spat_timing
Definition: sumotls2ros.h:113
std::unordered_map< std::string, adore::sumo_if_ros::MAPEMIntersection > generateMAPEMFromSUMO()
Definition: sumotls2ros.cpp:355
std::vector< adore_v2x_sim::SimSPATEM > getSPATEMFromSUMO(double time, double power=23)
Definition: sumotls2ros.cpp:546
int getIntersectionIDForSUMOString(std::string sumo_intersection_id)
Definition: sumotls2ros.cpp:733
double getSystemTime()
Definition: sumotls2ros.cpp:87
int getLeapYearSeconds(double time)
Definition: sumotls2ros.cpp:526
bool is_south_hemi_
Definition: sumotls2ros.h:109
std::unordered_map< std::pair< std::string, std::string >, uint8_t, SumoTLs2Ros::hash_pair > sumo_signal_group_id_mapping_
Definition: sumotls2ros.h:66
std::unordered_map< int, adore_v2x_sim::SimMAPEM > _MAPEM
Definition: sumotls2ros.h:63
void getGeoreferencedLinks(std::string sumoIntersectionID, std::unordered_map< std::string, std::vector< std::string >> &linklist, std::unordered_map< std::string, std::vector< adore::sumo_if_ros::Coordinate >> &id_2_shape)
Definition: sumotls2ros.cpp:22
std::unordered_map< std::string, uint8_t > sumo_lane_id_mapping_
Definition: sumotls2ros.h:54
SumoTLs2Ros()
Definition: sumotls2ros.h:105
double wgs84_distance(double lat1, double lon1, double lat2, double lon2)
Definition: sumotls2ros.cpp:768
std::unordered_map< int, adore_v2x_sim::SimMAPEM > convertToROSMsg(std::unordered_map< std::string, adore::sumo_if_ros::MAPEMIntersection > mapem_data, double time=0, double power=23)
Definition: sumotls2ros.cpp:122
std::unordered_map< std::string, int > sumo_intersection_id_mapping_
Definition: sumotls2ros.h:57
double getSecondOfYearFromUTC(double time)
Definition: sumotls2ros.cpp:536
std::unordered_map< int, adore_v2x_sim::SimMAPEM > getMAPEMFromSUMO(double time, double power=23)
Definition: sumotls2ros.cpp:95
int utm_zone_
Definition: sumotls2ros.h:105
std::string getSUMOStringFromIntersectionID(int intersection_id)
Definition: sumotls2ros.cpp:749
bool _use_system_time
Definition: sumotls2ros.h:111
int32_t getMOY(double time)
Definition: sumotls2ros.cpp:541
std::unordered_map< std::string, adore::sumo_if_ros::MAPEMIntersection > intersections_
Definition: sumotls2ros.h:60
std::vector< adore::sumo_if_ros::Coordinate > getNodesFromSUMOLane(libsumo::TraCIPositionVector &lane, int nodeCount=5)
Definition: sumotls2ros.cpp:511
Definition: areaofeffectconverter.h:20
Definition: sumotls2ros.h:39
size_t operator()(const std::pair< T1, T2 > &p) const
Definition: sumotls2ros.h:41