CARLA
TrafficManagerLocal.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 <algorithm>
8 
9 #include "carla/Logging.h"
10 
12 
14 
15 namespace carla {
16 namespace traffic_manager {
17 
18 using namespace constants::FrameMemory;
19 
21  std::vector<float> longitudinal_PID_parameters,
22  std::vector<float> longitudinal_highway_PID_parameters,
23  std::vector<float> lateral_PID_parameters,
24  std::vector<float> lateral_highway_PID_parameters,
25  float perc_difference_from_limit,
26  cc::detail::EpisodeProxy &episode_proxy,
27  uint16_t &RPCportTM)
28 
29  : longitudinal_PID_parameters(longitudinal_PID_parameters),
30  longitudinal_highway_PID_parameters(longitudinal_highway_PID_parameters),
31  lateral_PID_parameters(lateral_PID_parameters),
32  lateral_highway_PID_parameters(lateral_highway_PID_parameters),
33 
34  episode_proxy(episode_proxy),
35  world(cc::World(episode_proxy)),
36 
37  localization_stage(LocalizationStage(vehicle_id_list,
38  buffer_map,
39  simulation_state,
40  track_traffic,
41  local_map,
42  parameters,
43  marked_for_removal,
44  localization_frame,
45  random_devices)),
46 
47  collision_stage(CollisionStage(vehicle_id_list,
48  simulation_state,
49  buffer_map,
50  track_traffic,
51  parameters,
52  collision_frame,
53  random_devices)),
54 
55  traffic_light_stage(TrafficLightStage(vehicle_id_list,
56  simulation_state,
57  buffer_map,
58  parameters,
59  world,
60  tl_frame,
61  random_devices)),
62 
63  motion_plan_stage(MotionPlanStage(vehicle_id_list,
64  simulation_state,
65  parameters,
66  buffer_map,
67  track_traffic,
68  longitudinal_PID_parameters,
69  longitudinal_highway_PID_parameters,
70  lateral_PID_parameters,
71  lateral_highway_PID_parameters,
72  localization_frame,
73  collision_frame,
74  tl_frame,
75  world,
76  control_frame,
77  random_devices,
78  local_map)),
79 
80  alsm(ALSM(registered_vehicles,
81  buffer_map,
82  track_traffic,
83  marked_for_removal,
84  parameters,
85  world,
86  local_map,
87  simulation_state,
88  localization_stage,
89  collision_stage,
90  traffic_light_stage,
91  motion_plan_stage,
92  random_devices)),
93 
94  server(TrafficManagerServer(RPCportTM, static_cast<carla::traffic_manager::TrafficManagerBase *>(this))) {
95 
96  parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit);
97 
99 
100  SetupLocalMap();
101 
102  Start();
103 }
104 
106  episode_proxy.Lock()->DestroyTrafficManager(server.port());
107  Release();
108 }
109 
111  const carla::SharedPtr<const cc::Map> world_map = world.GetMap();
112  local_map = std::make_shared<InMemoryMap>(world_map);
113 
114  auto files = episode_proxy.Lock()->GetRequiredFiles("TM");
115  if (!files.empty()) {
116  auto content = episode_proxy.Lock()->GetCacheFile(files[0], true);
117  if (content.size() != 0) {
118  local_map->Load(content);
119  } else {
120  log_warning("No InMemoryMap cache found. Setting up local map. This may take a while...");
121  local_map->SetUp();
122  }
123  } else {
124  log_warning("No InMemoryMap cache found. Setting up local map. This may take a while...");
125  local_map->SetUp();
126  }
127 }
128 
130  run_traffic_manger.store(true);
131  worker_thread = std::make_unique<std::thread>(&TrafficManagerLocal::Run, this);
132 }
133 
135 
137  collision_frame.reserve(INITIAL_SIZE);
138  tl_frame.reserve(INITIAL_SIZE);
139  control_frame.reserve(INITIAL_SIZE);
141 
142  size_t last_frame = 0;
143  while (run_traffic_manger.load()) {
144 
145  bool synchronous_mode = parameters.GetSynchronousMode();
146  bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
147  parameters.SetMaxBoundaries(20.0f, episode_proxy.Lock()->GetEpisodeSettings().actor_active_distance);
148 
149  // Wait for external trigger to initiate cycle in synchronous mode.
150  if (synchronous_mode) {
151  std::unique_lock<std::mutex> lock(step_execution_mutex);
152  step_begin_trigger.wait(lock, [this]() {return step_begin.load() || !run_traffic_manger.load();});
153  step_begin.store(false);
154  }
155 
156  // Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode.
157  if (!synchronous_mode && hybrid_physics_mode) {
158  TimePoint current_instance = chr::system_clock::now();
159  chr::duration<float> elapsed_time = current_instance - previous_update_instance;
160  chr::duration<float> time_to_wait = chr::duration<float>(HYBRID_MODE_DT) - elapsed_time;
161  if (time_to_wait > chr::duration<float>(0.0f)) {
162  std::this_thread::sleep_for(time_to_wait);
163  }
164  previous_update_instance = current_instance;
165  }
166 
167  // Stop TM from processing the same frame more than once
168  if (!synchronous_mode) {
170  if (timestamp.frame == last_frame) {
171  continue;
172  }
173  last_frame = timestamp.frame;
174  }
175 
176  std::unique_lock<std::mutex> registration_lock(registration_mutex);
177  // Updating simulation state, actor life cycle and performing necessary cleanup.
178  alsm.Update();
179 
180 
181  // Re-allocating inter-stage communication frames based on changed number of registered vehicles.
182  int current_registered_vehicles_state = registered_vehicles.GetState();
183  unsigned long number_of_vehicles = vehicle_id_list.size();
184  if (registered_vehicles_state != current_registered_vehicles_state || number_of_vehicles != registered_vehicles.Size()) {
185 
187 
188  std::sort(vehicle_id_list.begin(), vehicle_id_list.end());
189 
190  number_of_vehicles = vehicle_id_list.size();
191 
192  // Reserve more space if needed.
193  uint64_t growth_factor = static_cast<uint64_t>(static_cast<float>(number_of_vehicles) * INV_GROWTH_STEP_SIZE);
194  uint64_t new_frame_capacity = INITIAL_SIZE + GROWTH_STEP_SIZE * growth_factor;
195  if (new_frame_capacity > current_reserved_capacity) {
196  localization_frame.reserve(new_frame_capacity);
197  collision_frame.reserve(new_frame_capacity);
198  tl_frame.reserve(new_frame_capacity);
199  control_frame.reserve(new_frame_capacity);
200  }
201 
203  }
204 
205  // Reset frames for current cycle.
206  localization_frame.clear();
207  localization_frame.resize(number_of_vehicles);
208  collision_frame.clear();
209  collision_frame.resize(number_of_vehicles);
210  tl_frame.clear();
211  tl_frame.resize(number_of_vehicles);
212  control_frame.clear();
213  control_frame.resize(number_of_vehicles);
214 
215  // Run core operation stages.
216 
217  for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
218  localization_stage.Update(index);
219  }
220  for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
221  collision_stage.Update(index);
222  }
224  for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
226  motion_plan_stage.Update(index);
227  }
228 
229  registration_lock.unlock();
230 
231  // Sending the current cycle's batch command to the simulator.
232  if (synchronous_mode) {
233  episode_proxy.Lock()->ApplyBatchSync(control_frame, false);
234  step_end.store(true);
235  step_end_trigger.notify_one();
236  } else {
237  if (control_frame.size() > 0){
238  episode_proxy.Lock()->ApplyBatchSync(control_frame, false);
239  }
240  }
241  }
242 }
243 
246  step_begin.store(true);
247  step_begin_trigger.notify_one();
248 
249  std::unique_lock<std::mutex> lock(step_execution_mutex);
250  step_end_trigger.wait(lock, [this]() { return step_end.load(); });
251  step_end.store(false);
252  }
253  return true;
254 }
255 
257 
258  run_traffic_manger.store(false);
260  step_begin_trigger.notify_one();
261  }
262 
263  if (worker_thread) {
264  if (worker_thread->joinable()) {
265  worker_thread->join();
266  }
267  worker_thread.release();
268  }
269 
270  vehicle_id_list.clear();
274  previous_update_instance = chr::system_clock::now();
276  random_devices.clear();
277 
283 
284  buffer_map.clear();
285  localization_frame.clear();
286  collision_frame.clear();
287  tl_frame.clear();
288  control_frame.clear();
289 
290  run_traffic_manger.store(true);
291  step_begin.store(false);
292  step_end.store(false);
293 }
294 
296 
297  Stop();
298 
299  local_map.reset();
300 }
301 
303 
304  Release();
305  episode_proxy = episode_proxy.Lock()->GetCurrentEpisode();
307  SetupLocalMap();
308  Start();
309 }
310 
311 void TrafficManagerLocal::RegisterVehicles(const std::vector<ActorPtr> &vehicle_list) {
312  std::lock_guard<std::mutex> registration_lock(registration_mutex);
313  registered_vehicles.Insert(vehicle_list);
314  for (const ActorPtr &vehicle: vehicle_list) {
315  if (!is_custom_seed) {
316  seed = vehicle->GetId() + seed;
317  } else {
318  seed = 1 + seed;
319  }
320  random_devices.insert({vehicle->GetId(), RandomGenerator(seed)});
321  }
322 }
323 
324 void TrafficManagerLocal::UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
325  std::lock_guard<std::mutex> registration_lock(registration_mutex);
326  std::vector<ActorId> actor_id_list;
327  for (auto &actor : actor_list) {
328  alsm.RemoveActor(actor->GetId(), true);
329  }
330 }
331 
332 void TrafficManagerLocal::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
333  parameters.SetPercentageSpeedDifference(actor, percentage);
334 }
335 
338 }
339 
340 void TrafficManagerLocal::SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) {
341  parameters.SetCollisionDetection(reference_actor, other_actor, detect_collision);
342 }
343 
344 void TrafficManagerLocal::SetForceLaneChange(const ActorPtr &actor, const bool direction) {
345  parameters.SetForceLaneChange(actor, direction);
346 }
347 
348 void TrafficManagerLocal::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
349  parameters.SetAutoLaneChange(actor, enable);
350 }
351 
352 void TrafficManagerLocal::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
353  parameters.SetDistanceToLeadingVehicle(actor, distance);
354 }
355 
358 }
359 
360 void TrafficManagerLocal::SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc) {
362 }
363 
364 void TrafficManagerLocal::SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc) {
366 }
367 
368 void TrafficManagerLocal::SetPercentageRunningLight(const ActorPtr &actor, const float perc) {
370 }
371 
372 void TrafficManagerLocal::SetPercentageRunningSign(const ActorPtr &actor, const float perc) {
374 }
375 
376 void TrafficManagerLocal::SetKeepRightPercentage(const ActorPtr &actor, const float percentage) {
377  parameters.SetKeepRightPercentage(actor, percentage);
378 }
379 
380 void TrafficManagerLocal::SetHybridPhysicsMode(const bool mode_switch) {
381  parameters.SetHybridPhysicsMode(mode_switch);
382 }
383 
386 }
387 
388 void TrafficManagerLocal::SetOSMMode(const bool mode_switch) {
389  parameters.SetOSMMode(mode_switch);
390 }
391 
394 }
395 
396 void TrafficManagerLocal::SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound) {
397  parameters.SetBoundariesRespawnDormantVehicles(lower_bound, upper_bound);
398 }
399 
400 void TrafficManagerLocal::SetMaxBoundaries(const float lower, const float upper) {
401  parameters.SetMaxBoundaries(lower, upper);
402 }
403 
405  for (auto &elem : tl_to_freeze) {
406  if (!elem->IsFrozen() || elem->GetState() != TLS::Red) {
407  return false;
408  }
409  }
410  return true;
411 }
412 
414  const bool previous_mode = parameters.GetSynchronousMode();
416  if (previous_mode && !mode) {
417  step_begin.store(true);
418  step_begin_trigger.notify_one();
419  }
420 }
421 
424 }
425 
427  return episode_proxy;
428 }
429 
432 }
433 
434 void TrafficManagerLocal::SetRandomDeviceSeed(const uint64_t _seed) {
435  seed = _seed;
436  is_custom_seed = true;
438 }
439 
440 } // namespace traffic_manager
441 } // namespace carla
CollisionFrame collision_frame
Array to hold output data of collision avoidance.
void SetHybridPhysicsRadius(const float radius)
Method to set hybrid physics radius.
TrafficManagerServer server
Traffic manager server instance.
void SetPercentageRunningLight(const ActorPtr &actor, const float perc)
Method to specify the % chance of running any traffic light.
void SetForceLaneChange(const ActorPtr &actor, const bool direction)
Method to force lane change on a vehicle.
void Update(const unsigned long index)
std::atomic< bool > step_begin
Flags to signal step begin and end.
void SetGlobalDistanceToLeadingVehicle(const float dist)
Method to set the distance to leading vehicle for all registered vehicles.
Definition: Parameters.cpp:114
void RemoveActor(const ActorId actor_id, const bool registered_actor)
Definition: ALSM.cpp:371
bool CheckAllFrozen(TLGroup tl_to_freeze)
Method to check if all traffic lights are frozen in a group.
void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance)
Method to specify how much distance a vehicle should maintain to the leading vehicle.
Definition: Parameters.cpp:99
carla::SharedPtr< cc::Actor > ActorPtr
uint64_t current_reserved_capacity
Variable to keep track of currently reserved array space for frames.
void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc)
Method to specify the % chance of ignoring collisions with any vehicle.
void Release()
To release the traffic manager.
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound)
Method to set boundaries for respawning vehicles.
void SetPercentageRunningSign(const ActorPtr &actor, const float perc)
Method to specify the % chance of running any traffic sign.
chr::time_point< chr::system_clock, chr::nanoseconds > TimePoint
void SetAutoLaneChange(const ActorPtr &actor, const bool enable)
Enable/disable automatic lane change on a vehicle.
Definition: Parameters.cpp:93
std::condition_variable step_begin_trigger
Condition variables for progressing synchronous execution.
void SetMaxBoundaries(const float lower, const float upper)
Method to set limits for boundaries when respawning vehicles.
void SetOSMMode(const bool mode_switch)
Method to set Open Street Map mode.
void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage)
Method to set a vehicle&#39;s % decrease in velocity with respect to the speed limit. ...
boost::shared_ptr< T > SharedPtr
Use this SharedPtr (boost::shared_ptr) to keep compatibility with boost::python, but it would be nice...
Definition: Memory.h:20
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
std::size_t frame
Number of frames elapsed since the simulator was launched.
Definition: Timestamp.h:30
SharedPtr< Map > GetMap() const
Return the map that describes this world.
Definition: World.cpp:24
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage)
Method to set probabilistic preference to keep on the right lane.
Definition: Parameters.cpp:87
void SetForceLaneChange(const ActorPtr &actor, const bool direction)
Method to force lane change on a vehicle.
Definition: Parameters.cpp:80
std::vector< ActorId > GetIDList()
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage)
Method to set probabilistic preference to keep on the right lane.
void SetMaxBoundaries(const float lower, const float upper)
Method to set limits for boundaries when respawning vehicles.
Definition: Parameters.cpp:33
void Stop()
To stop the TrafficManager.
void SetGlobalDistanceToLeadingVehicle(const float distance)
Method to specify how much distance a vehicle should maintain to the Global leading vehicle...
ALSM: Agent Lifecycle and State Managerment This class has functionality to update the local cache of...
Definition: ALSM.h:40
void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage)
Set a vehicle&#39;s % decrease in velocity with respect to the speed limit.
Definition: Parameters.cpp:43
void SetAutoLaneChange(const ActorPtr &actor, const bool enable)
Enable/disable automatic lane change on a vehicle.
void Update(const unsigned long index) override
Parameters parameters
Parameterization object.
std::vector< carla::SharedPtr< carla::client::TrafficLight > > TLGroup
void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc)
Method to specify the % chance of ignoring collisions with any walker.
void SetHybridPhysicsMode(const bool mode_switch)
Method to set hybrid physics mode.
const Timestamp & GetTimestamp() const
Get timestamp of this snapshot.
Definition: WorldSnapshot.h:34
void SetRandomDeviceSeed(const uint64_t _seed)
Method to set randomization seed.
void SetSynchronousModeTimeOutInMiliSecond(double time)
Method to set Tick timeout for synchronous execution.
void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision)
Method to set collision detection rules between vehicles.
Definition: Parameters.cpp:54
carla::client::detail::EpisodeProxy episode_proxy
Carla&#39;s client connection object.
std::mutex step_execution_mutex
Mutex for progressing synchronous execution.
SimulationState simulation_state
Type containing the current state of all actors involved in the simulation.
void Insert(std::vector< ActorPtr > actor_list)
bool GetHybridPhysicsMode() const
Method to retrieve hybrid physics mode.
Definition: Parameters.cpp:287
void Update(const unsigned long index) override
carla::client::detail::EpisodeProxy & GetEpisodeProxy()
Get CARLA episode information.
void SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance)
Method to specify how much distance a vehicle should maintain to the leading vehicle.
std::vector< ActorId > GetRegisteredVehiclesIDs()
Get list of all registered vehicles.
cc::World world
Carla client and object.
void UnregisterVehicles(const std::vector< ActorPtr > &actor_list)
This method unregisters a vehicle from traffic manager.
AtomicActorSet registered_vehicles
Set of all actors registered with traffic manager.
std::atomic< bool > run_traffic_manger
Switch to turn on / turn off traffic manager.
void ResetAllTrafficLights()
Definition: World.cpp:210
The function of this class is to integrate all the various stages of the traffic manager appropriatel...
void SetRespawnDormantVehicles(const bool mode_switch)
Method to set automatic respawn of dormant vehicles.
This class has functionality for responding to traffic lights and managing entry into non-signalized ...
void Run()
Initiates thread to run the TrafficManager sequentially.
std::mutex registration_mutex
Mutex to prevent vehicle registration during frame array re-allocation.
bool GetSynchronousMode() const
Method to get synchronous mode.
Definition: Parameters.cpp:163
void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc)
Method to set % to ignore any vehicle.
Definition: Parameters.cpp:133
void SetupLocalMap()
Method to setup InMemoryMap.
static void log_warning(Args &&... args)
Definition: Logging.h:96
TrafficManagerLocal(std::vector< float > longitudinal_PID_parameters, std::vector< float > longitudinal_highway_PID_parameters, std::vector< float > lateral_PID_parameters, std::vector< float > lateral_highway_PID_parameters, float perc_decrease_from_limit, cc::detail::EpisodeProxy &episode_proxy, uint16_t &RPCportTM)
Private constructor for singleton lifecycle management.
std::vector< ActorId > vehicle_id_list
List of vehicles registered with the traffic manager in current update cycle.
TimePoint previous_update_instance
Time instance used to calculate dt in asynchronous mode.
void SetGlobalPercentageSpeedDifference(float const percentage)
Set a global % decrease in velocity with respect to the speed limit.
Definition: Parameters.cpp:49
void SetSynchronousMode(bool mode)
Method to switch traffic manager into synchronous execution.
ControlFrame control_frame
Array to hold output data of motion planning.
void Update(const unsigned long index) override
RandomGeneratorMap random_devices
Structure holding random devices per vehicle.
void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision)
Method to set collision detection rules between vehicles.
void Start()
To start the TrafficManager.
BufferMap buffer_map
Structures to hold waypoint buffers for all vehicles.
void Reset()
To reset the traffic manager.
TrackTraffic track_traffic
Object for tracking paths of the traffic vehicles.
TLFrame tl_frame
Array to hold output data of traffic light response.
void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc)
Method to set % to ignore any vehicle.
Definition: Parameters.cpp:140
void RegisterVehicles(const std::vector< ActorPtr > &actor_list)
This method registers a vehicle with the traffic manager.
LocalizationFrame localization_frame
Array to hold output data of localization stage.
LocalMapPtr local_map
Pointer to local map cache.
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound)
Method to set boundaries for respawning vehicles.
Definition: Parameters.cpp:38
SharedPtrType Lock() const
Same as TryLock but never return nullptr.
LocalizationStage localization_stage
Various stages representing core operations of traffic manager.
void SetRespawnDormantVehicles(const bool mode_switch)
Method to set if we are automatically respawning vehicles.
Definition: Parameters.cpp:28
EpisodeProxyImpl< EpisodeProxyPointerType::Strong > EpisodeProxy
Definition: EpisodeProxy.h:69
int registered_vehicles_state
State counter to track changes in registered actors.
void SetSynchronousMode(const bool mode_switch=true)
Method to set synchronous mode.
Definition: Parameters.cpp:106
void SetHybridPhysicsRadius(const float radius)
Method to set hybrid physics radius.
Definition: Parameters.cpp:147
void SetPercentageRunningLight(const ActorPtr &actor, const float perc)
Method to set % to run any traffic light.
Definition: Parameters.cpp:119
This class has functionality to maintain a horizon of waypoints ahead of the vehicle for it to follow...
void SetPercentageRunningSign(const ActorPtr &actor, const float perc)
Method to set % to run any traffic sign.
Definition: Parameters.cpp:126
std::unique_ptr< std::thread > worker_thread
Single worker thread for sequential execution of sub-components.
WorldSnapshot GetSnapshot() const
Return a snapshot of the world at this moment.
Definition: World.cpp:94
void SetSynchronousModeTimeOutInMiliSecond(const double time)
Set Synchronous mode time out.
Definition: Parameters.cpp:110
void sort(I begin, I end, const Pred &pred)
Definition: pugixml.cpp:7444
bool SynchronousTick()
Method to provide synchronous tick.
This class has functionality to detect potential collision with a nearby actor.
void SetHybridPhysicsMode(const bool mode_switch)
Method to set hybrid physics mode.
Definition: Parameters.cpp:23
void SetGlobalPercentageSpeedDifference(float const percentage)
Methos to set a global % decrease in velocity with respect to the speed limit.
void SetOSMMode(const bool mode_switch)
Method to set Open Street Map mode.
Definition: Parameters.cpp:152