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 
17 
18 #define DEBUG_PRINT_TM 0
19 #define IP_DATA_BUFFER_SIZE 80
20 
21 namespace carla {
22 namespace traffic_manager {
23 
24 std::map<uint16_t, TrafficManagerBase*> TrafficManager::_tm_map;
25 std::mutex TrafficManager::_mutex;
26 
29  uint16_t port)
30  : _port(port) {
31 
32  if(!GetTM(_port)){
33  // Check if a TM server already exists and connect to it
34  if(!CreateTrafficManagerClient(episode_proxy, port)) {
35  // As TM server not running, create one
36  CreateTrafficManagerServer(episode_proxy, port);
37  }
38  }
39 }
40 
42  std::lock_guard<std::mutex> lock(_mutex);
43  for(auto& tm : _tm_map) {
44  tm.second->Release();
45  TrafficManagerBase *base_ptr = tm.second;
46  delete base_ptr;
47  }
48  _tm_map.clear();
49 }
50 
52  std::lock_guard<std::mutex> lock(_mutex);
53  for(auto& tm : _tm_map) {
54  tm.second->Reset();
55  }
56 }
57 
59  std::lock_guard<std::mutex> lock(_mutex);
60  for(auto& tm : _tm_map) {
61  tm.second->SynchronousTick();
62  }
63 }
64 
67  uint16_t port) {
68 
69  // Get local IP details.
70  auto GetLocalIP = [=](const uint16_t sport)-> std::pair<std::string, uint16_t> {
71  std::pair<std::string, uint16_t> localIP;
72  int sock = socket(AF_INET, SOCK_DGRAM, 0);
73  if(sock == SOCK_INVALID_INDEX) {
74  #if DEBUG_PRINT_TM
75  std::cout << "Error number 1: " << errno << std::endl;
76  std::cout << "Error message: " << strerror(errno) << std::endl;
77  #endif
78  } else {
79  int err;
80  sockaddr_in loopback;
81  std::memset(&loopback, 0, sizeof(loopback));
82  loopback.sin_family = AF_INET;
83  loopback.sin_addr.s_addr = INADDR_LOOPBACK;
84  loopback.sin_port = htons(9);
85  err = connect(sock, reinterpret_cast<sockaddr*>(&loopback), sizeof(loopback));
86  if(err == SOCK_INVALID_INDEX) {
87  #if DEBUG_PRINT_TM
88  std::cout << "Error number 2: " << errno << std::endl;
89  std::cout << "Error message: " << strerror(errno) << std::endl;
90  #endif
91  } else {
92  socklen_t addrlen = sizeof(loopback);
93  err = getsockname(sock, reinterpret_cast<struct sockaddr*> (&loopback), &addrlen);
94  if(err == SOCK_INVALID_INDEX) {
95  #if DEBUG_PRINT_TM
96  std::cout << "Error number 3: " << errno << std::endl;
97  std::cout << "Error message: " << strerror(errno) << std::endl;
98  #endif
99  } else {
100  char buffer[IP_DATA_BUFFER_SIZE];
101  const char* p = inet_ntop(AF_INET, &loopback.sin_addr, buffer, IP_DATA_BUFFER_SIZE);
102  if(p != NULL) {
103  localIP = std::pair<std::string, uint16_t>(std::string(buffer), sport);
104  } else {
105  #if DEBUG_PRINT_TM
106  std::cout << "Error number 4: " << errno << std::endl;
107  std::cout << "Error message: " << strerror(errno) << std::endl;
108  #endif
109  }
110  }
111  }
112  #ifdef _WIN32
113  closesocket(sock);
114  #else
115  close(sock);
116  #endif
117  }
118  return localIP;
119  };
120 
121  /// Define local constants
122  const std::vector<float> longitudinal_param = {2.0f, 0.01f, 0.4f};
123  const std::vector<float> longitudinal_highway_param = {4.0f, 0.02f, 0.2f};
124  const std::vector<float> lateral_param = {9.0f, 0.02f, 1.0f};
125  const std::vector<float> lateral_highway_param = {7.0f, 0.02f, 1.0f};
126  const float perc_difference_from_limit = 30.0f;
127 
128  std::pair<std::string, uint16_t> serverTM;
129 
130  /// Create local instance of TM
132  longitudinal_param,
133  longitudinal_highway_param,
134  lateral_param,
135  lateral_highway_param,
136  perc_difference_from_limit,
137  episode_proxy,
138  port);
139 
140  /// Get TM server info (Local IP & PORT)
141  serverTM = GetLocalIP(port);
142 
143  /// Set this client as the TM to server
144  episode_proxy.Lock()->AddTrafficManagerRunning(serverTM);
145 
146  #if DEBUG_PRINT_TM
147  /// Print status
148  std::cout << "NEW@: Registered TM at "
149  << serverTM.first << ":"
150  << serverTM.second << " ..... SUCCESS."
151  << std::endl;
152  #endif
153 
154  /// Set the pointer of the instance
155  _tm_map.insert(std::make_pair(port, tm_ptr));
156 
157 }
158 
161  uint16_t port) {
162 
163  bool result = false;
164 
165  if(episode_proxy.Lock()->IsTrafficManagerRunning(port)) {
166 
167  /// Get TM server info (Remote IP & PORT)
168  std::pair<std::string, uint16_t> serverTM =
169  episode_proxy.Lock()->GetTrafficManagerRunning(port);
170 
171  /// Set remote TM server IP and port
172  TrafficManagerRemote* tm_ptr = new(std::nothrow)
173  TrafficManagerRemote(serverTM, episode_proxy);
174 
175  /// Try to connect to remote TM server
176  try {
177 
178  /// Check memory allocated or not
179  if(tm_ptr != nullptr) {
180 
181  #if DEBUG_PRINT_TM
182  // Test print
183  std::cout << "OLD@: Registered TM at "
184  << serverTM.first << ":"
185  << serverTM.second << " ..... TRY "
186  << std::endl;
187  #endif
188  /// Try to reset all traffic lights
189  tm_ptr->HealthCheckRemoteTM();
190 
191  /// Set the pointer of the instance
192  _tm_map.insert(std::make_pair(port, tm_ptr));
193 
194  result = true;
195  }
196  }
197 
198  /// If Connection error occurred
199  catch (...) {
200 
201  /// Clear previously allocated memory
202  delete tm_ptr;
203 
204  #if DEBUG_PRINT_TM
205  /// Test print
206  std::cout << "OLD@: Registered TM at "
207  << serverTM.first << ":"
208  << serverTM.second << " ..... FAILED "
209  << std::endl;
210  #endif
211  }
212 
213  }
214 
215  return result;
216 }
217 
218 } // namespace traffic_manager
219 } // namespace carla
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:99
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...
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)