CARLA
TrafficManager.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 //#include "carla/client/Client.h"
8 
9 
10 #include "carla/Sockets.h"
12 
18 
19 #define DEBUG_PRINT_TM 0
20 #define IP_DATA_BUFFER_SIZE 80
21 
22 namespace carla {
23 namespace traffic_manager {
24 
25 using namespace constants::SpeedThreshold;
26 using namespace constants::PID;
27 
28 std::map<uint16_t, TrafficManagerBase*> TrafficManager::_tm_map;
29 std::mutex TrafficManager::_mutex;
30 
33  uint16_t port)
34  : _port(port) {
35 
36  if(!GetTM(_port)){
37  // Check if a TM server already exists and connect to it
38  if(!CreateTrafficManagerClient(episode_proxy, port)) {
39  // As TM server not running, create one
40  CreateTrafficManagerServer(episode_proxy, port);
41  }
42  }
43 }
44 
46  std::lock_guard<std::mutex> lock(_mutex);
47  for(auto& tm : _tm_map) {
48  tm.second->Release();
49  TrafficManagerBase *base_ptr = tm.second;
50  delete base_ptr;
51  }
52  _tm_map.clear();
53 }
54 
56  std::lock_guard<std::mutex> lock(_mutex);
57  for(auto& tm : _tm_map) {
58  tm.second->Reset();
59  }
60 }
61 
63  std::lock_guard<std::mutex> lock(_mutex);
64  for(auto& tm : _tm_map) {
65  tm.second->SynchronousTick();
66  }
67 }
68 
70  TrafficManagerBase* tm_ptr = GetTM(_port);
71  std::lock_guard<std::mutex> lock(_mutex);
72  auto it = _tm_map.find(_port);
73  if (it != _tm_map.end()) {
74  _tm_map.erase(it);
75  }
76  if(tm_ptr != nullptr) {
77  tm_ptr->ShutDown();
78  delete tm_ptr;
79  }
80 }
81 
84  uint16_t port) {
85 
86  // Get local IP details.
87  auto GetLocalIP = [=](const uint16_t sport)-> std::pair<std::string, uint16_t> {
88  std::pair<std::string, uint16_t> localIP;
89  int sock = socket(AF_INET, SOCK_DGRAM, 0);
90  if(sock == SOCK_INVALID_INDEX) {
91  #if DEBUG_PRINT_TM
92  std::cout << "Error number 1: " << errno << std::endl;
93  std::cout << "Error message: " << strerror(errno) << std::endl;
94  #endif
95  } else {
96  int err;
97  sockaddr_in loopback;
98  std::memset(&loopback, 0, sizeof(loopback));
99  loopback.sin_family = AF_INET;
100  loopback.sin_addr.s_addr = INADDR_LOOPBACK;
101  loopback.sin_port = htons(9);
102  err = connect(sock, reinterpret_cast<sockaddr*>(&loopback), sizeof(loopback));
103  if(err == SOCK_INVALID_INDEX) {
104  #if DEBUG_PRINT_TM
105  std::cout << "Error number 2: " << errno << std::endl;
106  std::cout << "Error message: " << strerror(errno) << std::endl;
107  #endif
108  } else {
109  socklen_t addrlen = sizeof(loopback);
110  err = getsockname(sock, reinterpret_cast<struct sockaddr*> (&loopback), &addrlen);
111  if(err == SOCK_INVALID_INDEX) {
112  #if DEBUG_PRINT_TM
113  std::cout << "Error number 3: " << errno << std::endl;
114  std::cout << "Error message: " << strerror(errno) << std::endl;
115  #endif
116  } else {
117  char buffer[IP_DATA_BUFFER_SIZE];
118  const char* p = inet_ntop(AF_INET, &loopback.sin_addr, buffer, IP_DATA_BUFFER_SIZE);
119  if(p != NULL) {
120  localIP = std::pair<std::string, uint16_t>(std::string(buffer), sport);
121  } else {
122  #if DEBUG_PRINT_TM
123  std::cout << "Error number 4: " << errno << std::endl;
124  std::cout << "Error message: " << strerror(errno) << std::endl;
125  #endif
126  }
127  }
128  }
129  #ifdef _WIN32
130  closesocket(sock);
131  #else
132  close(sock);
133  #endif
134  }
135  return localIP;
136  };
137 
138  /// Define local constants
139  const std::vector<float> longitudinal_param = LONGITUDIAL_PARAM;
140  const std::vector<float> longitudinal_highway_param = LONGITUDIAL_HIGHWAY_PARAM;
141  const std::vector<float> lateral_param = LATERAL_PARAM;
142  const std::vector<float> lateral_highway_param = LATERAL_HIGHWAY_PARAM;
143  const float perc_difference_from_limit = INITIAL_PERCENTAGE_SPEED_DIFFERENCE;
144 
145  std::pair<std::string, uint16_t> serverTM;
146 
147  /// Create local instance of TM
149  longitudinal_param,
150  longitudinal_highway_param,
151  lateral_param,
152  lateral_highway_param,
153  perc_difference_from_limit,
154  episode_proxy,
155  port);
156 
157  /// Get TM server info (Local IP & PORT)
158  serverTM = GetLocalIP(port);
159 
160  /// Set this client as the TM to server
161  episode_proxy.Lock()->AddTrafficManagerRunning(serverTM);
162 
163  #if DEBUG_PRINT_TM
164  /// Print status
165  std::cout << "NEW@: Registered TM at "
166  << serverTM.first << ":"
167  << serverTM.second << " ..... SUCCESS."
168  << std::endl;
169  #endif
170 
171  /// Set the pointer of the instance
172  _tm_map.insert(std::make_pair(port, tm_ptr));
173 
174 }
175 
178  uint16_t port) {
179 
180  bool result = false;
181 
182  if(episode_proxy.Lock()->IsTrafficManagerRunning(port)) {
183 
184  /// Get TM server info (Remote IP & PORT)
185  std::pair<std::string, uint16_t> serverTM =
186  episode_proxy.Lock()->GetTrafficManagerRunning(port);
187 
188  /// Set remote TM server IP and port
189  TrafficManagerRemote* tm_ptr = new(std::nothrow)
190  TrafficManagerRemote(serverTM, episode_proxy);
191 
192  /// Try to connect to remote TM server
193  try {
194 
195  /// Check memory allocated or not
196  if(tm_ptr != nullptr) {
197 
198  #if DEBUG_PRINT_TM
199  // Test print
200  std::cout << "OLD@: Registered TM at "
201  << serverTM.first << ":"
202  << serverTM.second << " ..... TRY "
203  << std::endl;
204  #endif
205  /// Try to reset all traffic lights
206  tm_ptr->HealthCheckRemoteTM();
207 
208  /// Set the pointer of the instance
209  _tm_map.insert(std::make_pair(port, tm_ptr));
210 
211  result = true;
212  }
213  }
214 
215  /// If Connection error occurred
216  catch (...) {
217 
218  /// Clear previously allocated memory
219  delete tm_ptr;
220 
221  #if DEBUG_PRINT_TM
222  /// Test print
223  std::cout << "OLD@: Registered TM at "
224  << serverTM.first << ":"
225  << serverTM.second << " ..... FAILED "
226  << std::endl;
227  #endif
228  }
229 
230  }
231 
232  return result;
233 }
234 
235 } // namespace traffic_manager
236 } // namespace carla
static const std::vector< float > LATERAL_HIGHWAY_PARAM
Definition: Constants.h:155
static const std::vector< float > LATERAL_PARAM
Definition: Constants.h:154
bool CreateTrafficManagerClient(carla::client::detail::EpisodeProxy episode_proxy, uint16_t port)
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
static std::map< uint16_t, TrafficManagerBase * > _tm_map
#define SOCK_INVALID_INDEX
< socket sockaddr_in
Definition: Sockets.h:19
#define IP_DATA_BUFFER_SIZE
The function of this class is to integrate all the various stages of the traffic manager appropriatel...
static const std::vector< float > LONGITUDIAL_HIGHWAY_PARAM
Definition: Constants.h:153
static const std::vector< float > LONGITUDIAL_PARAM
Definition: Constants.h:152
TrafficManagerBase * GetTM(uint16_t port) const
The function of this class is to integrate all the various stages of the traffic manager appropriatel...
SharedPtrType Lock() const
Same as TryLock but never return nullptr.
The function of this class is to integrate all the various stages of the traffic manager appropriatel...
void HealthCheckRemoteTM()
Method to check server is alive or not.
void CreateTrafficManagerServer(carla::client::detail::EpisodeProxy episode_proxy, uint16_t port)