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 
10 
12 
13 namespace carla {
14 namespace traffic_manager {
15 
16 using namespace constants::FrameMemory;
17 
19  std::vector<float> longitudinal_PID_parameters,
20  std::vector<float> longitudinal_highway_PID_parameters,
21  std::vector<float> lateral_PID_parameters,
22  std::vector<float> lateral_highway_PID_parameters,
23  float perc_difference_from_limit,
24  cc::detail::EpisodeProxy &episode_proxy,
25  uint16_t &RPCportTM)
26 
27  : longitudinal_PID_parameters(longitudinal_PID_parameters),
28  longitudinal_highway_PID_parameters(longitudinal_highway_PID_parameters),
29  lateral_PID_parameters(lateral_PID_parameters),
30  lateral_highway_PID_parameters(lateral_highway_PID_parameters),
31 
32  episode_proxy(episode_proxy),
33  world(cc::World(episode_proxy)),
34  debug_helper(world.MakeDebugHelper()),
35 
36  localization_stage(LocalizationStage(vehicle_id_list,
37  buffer_map,
38  simulation_state,
39  track_traffic,
40  local_map,
41  parameters,
42  marked_for_removal,
43  localization_frame,
44  debug_helper,
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  debug_helper,
54  random_devices)),
55 
56  traffic_light_stage(TrafficLightStage(vehicle_id_list,
57  simulation_state,
58  buffer_map,
59  parameters,
60  world,
61  tl_frame,
62  random_devices)),
63 
64  motion_plan_stage(MotionPlanStage(vehicle_id_list,
65  simulation_state,
66  parameters,
67  buffer_map,
68  track_traffic,
69  longitudinal_PID_parameters,
70  longitudinal_highway_PID_parameters,
71  lateral_PID_parameters,
72  lateral_highway_PID_parameters,
73  localization_frame,
74  collision_frame,
75  tl_frame,
76  world,
77  control_frame)),
78 
79  alsm(ALSM(registered_vehicles,
80  buffer_map,
81  track_traffic,
82  marked_for_removal,
83  parameters,
84  world,
85  local_map,
86  simulation_state,
87  localization_stage,
88  collision_stage,
89  traffic_light_stage,
90  motion_plan_stage,
91  random_devices)),
92 
93  server(TrafficManagerServer(RPCportTM, static_cast<carla::traffic_manager::TrafficManagerBase *>(this))) {
94 
95  parameters.SetGlobalPercentageSpeedDifference(perc_difference_from_limit);
96 
98 
99  SetupLocalMap();
100 
101  Start();
102 }
103 
105  episode_proxy.Lock()->DestroyTrafficManager(server.port());
106  Release();
107 }
108 
110  const carla::SharedPtr<cc::Map> world_map = world.GetMap();
111  local_map = std::make_shared<InMemoryMap>(world_map);
112  local_map->SetUp();
113 }
114 
116  run_traffic_manger.store(true);
117  worker_thread = std::make_unique<std::thread>(&TrafficManagerLocal::Run, this);
118 }
119 
121 
123  collision_frame.reserve(INITIAL_SIZE);
124  tl_frame.reserve(INITIAL_SIZE);
125  control_frame.reserve(INITIAL_SIZE);
127 
128  while (run_traffic_manger.load()) {
129 
130  bool synchronous_mode = parameters.GetSynchronousMode();
131  bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
132 
133  // Wait for external trigger to initiate cycle in synchronous mode.
134  if (synchronous_mode) {
135  std::unique_lock<std::mutex> lock(step_execution_mutex);
136  step_begin_trigger.wait(lock, [this]() {return step_begin.load() || !run_traffic_manger.load();});
137  step_begin.store(false);
138  }
139 
140  // Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode.
141  if (!synchronous_mode && hybrid_physics_mode) {
142  TimePoint current_instance = chr::system_clock::now();
143  chr::duration<float> elapsed_time = current_instance - previous_update_instance;
144  chr::duration<float> time_to_wait = chr::duration<float>(HYBRID_MODE_DT) - elapsed_time;
145  if (time_to_wait > chr::duration<float>(0.0f)) {
146  std::this_thread::sleep_for(time_to_wait);
147  }
148  previous_update_instance = current_instance;
149  }
150 
151  std::unique_lock<std::mutex> registration_lock(registration_mutex);
152  // Updating simulation state, actor life cycle and performing necessary cleanup.
153  alsm.Update();
154 
155  // Re-allocating inter-stage communication frames based on changed number of registered vehicles.
156  int current_registered_vehicles_state = registered_vehicles.GetState();
157  unsigned long number_of_vehicles = vehicle_id_list.size();
158  if (registered_vehicles_state != current_registered_vehicles_state || number_of_vehicles != registered_vehicles.Size()) {
159 
161 
162  std::sort(vehicle_id_list.begin(), vehicle_id_list.end());
163 
164  number_of_vehicles = vehicle_id_list.size();
165 
166  // Reserve more space if needed.
167  uint64_t growth_factor = static_cast<uint64_t>(static_cast<float>(number_of_vehicles) * INV_GROWTH_STEP_SIZE);
168  uint64_t new_frame_capacity = INITIAL_SIZE + GROWTH_STEP_SIZE * growth_factor;
169  if (new_frame_capacity > current_reserved_capacity) {
170  localization_frame.reserve(new_frame_capacity);
171  collision_frame.reserve(new_frame_capacity);
172  tl_frame.reserve(new_frame_capacity);
173  control_frame.reserve(new_frame_capacity);
174  }
175 
177  }
178 
179  // Reset frames for current cycle.
180  localization_frame.clear();
181  localization_frame.resize(number_of_vehicles);
182  collision_frame.clear();
183  collision_frame.resize(number_of_vehicles);
184  tl_frame.clear();
185  tl_frame.resize(number_of_vehicles);
186  control_frame.clear();
187  control_frame.resize(number_of_vehicles);
188 
189  // Run core operation stages.
190  for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
191  localization_stage.Update(index);
192  }
193  for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
194  collision_stage.Update(index);
195  }
197 
198  for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
200  motion_plan_stage.Update(index);
201  }
202 
203  registration_lock.unlock();
204 
205  // Sending the current cycle's batch command to the simulator.
206  if (synchronous_mode) {
207  episode_proxy.Lock()->ApplyBatchSync(control_frame, false);
208  step_end.store(true);
209  step_end_trigger.notify_one();
210  } else {
211  episode_proxy.Lock()->ApplyBatch(control_frame, false);
212  }
213  }
214 }
215 
218  step_begin.store(true);
219  step_begin_trigger.notify_one();
220 
221  std::unique_lock<std::mutex> lock(step_execution_mutex);
222  step_end_trigger.wait(lock, [this]() { return step_end.load(); });
223  step_end.store(false);
224  }
225  return true;
226 }
227 
229 
230  run_traffic_manger.store(false);
232  step_begin_trigger.notify_one();
233  }
234 
235  if (worker_thread) {
236  if (worker_thread->joinable()) {
237  worker_thread->join();
238  }
239  worker_thread.release();
240  }
241 
242  vehicle_id_list.clear();
246  previous_update_instance = chr::system_clock::now();
248  random_devices.clear();
249 
255 
256  buffer_map.clear();
257  localization_frame.clear();
258  collision_frame.clear();
259  tl_frame.clear();
260  control_frame.clear();
261 
262  run_traffic_manger.store(true);
263  step_begin.store(false);
264  step_end.store(false);
265 }
266 
268 
269  Stop();
270 
271  local_map.reset();
272 }
273 
275 
276  Release();
277  episode_proxy = episode_proxy.Lock()->GetCurrentEpisode();
279  SetupLocalMap();
280  Start();
281 }
282 
283 void TrafficManagerLocal::RegisterVehicles(const std::vector<ActorPtr> &vehicle_list) {
284  std::lock_guard<std::mutex> registration_lock(registration_mutex);
285  registered_vehicles.Insert(vehicle_list);
286  for (const ActorPtr &vehicle: vehicle_list) {
287  random_devices.insert({vehicle->GetId(), RandomGenerator(seed)});
288  }
289 }
290 
291 void TrafficManagerLocal::UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
292  std::lock_guard<std::mutex> registration_lock(registration_mutex);
293  std::vector<ActorId> actor_id_list;
294  for (auto &actor : actor_list) {
295  alsm.RemoveActor(actor->GetId(), true);
296  }
297 }
298 
299 void TrafficManagerLocal::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
300  parameters.SetPercentageSpeedDifference(actor, percentage);
301 }
302 
305 }
306 
307 void TrafficManagerLocal::SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) {
308  parameters.SetCollisionDetection(reference_actor, other_actor, detect_collision);
309 }
310 
311 void TrafficManagerLocal::SetForceLaneChange(const ActorPtr &actor, const bool direction) {
312  parameters.SetForceLaneChange(actor, direction);
313 }
314 
315 void TrafficManagerLocal::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
316  parameters.SetAutoLaneChange(actor, enable);
317 }
318 
319 void TrafficManagerLocal::SetDistanceToLeadingVehicle(const ActorPtr &actor, const float distance) {
320  parameters.SetDistanceToLeadingVehicle(actor, distance);
321 }
322 
325 }
326 
327 void TrafficManagerLocal::SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc) {
329 }
330 
331 void TrafficManagerLocal::SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc) {
333 }
334 
335 void TrafficManagerLocal::SetPercentageRunningLight(const ActorPtr &actor, const float perc) {
337 }
338 
339 void TrafficManagerLocal::SetPercentageRunningSign(const ActorPtr &actor, const float perc) {
341 }
342 
343 void TrafficManagerLocal::SetKeepRightPercentage(const ActorPtr &actor, const float percentage) {
344  parameters.SetKeepRightPercentage(actor, percentage);
345 }
346 
347 void TrafficManagerLocal::SetHybridPhysicsMode(const bool mode_switch) {
348  parameters.SetHybridPhysicsMode(mode_switch);
349 }
350 
353 }
354 
355 void TrafficManagerLocal::SetOSMMode(const bool mode_switch) {
356  parameters.SetOSMMode(mode_switch);
357 }
358 
360  for (auto &elem : tl_to_freeze) {
361  if (!elem->IsFrozen() || elem->GetState() != TLS::Red) {
362  return false;
363  }
364  }
365  return true;
366 }
367 
369  const bool previous_mode = parameters.GetSynchronousMode();
371  if (previous_mode && !mode) {
372  step_begin.store(true);
373  step_begin_trigger.notify_one();
374  }
375 }
376 
379 }
380 
382  return episode_proxy;
383 }
384 
387 }
388 
389 void TrafficManagerLocal::SetRandomDeviceSeed(const uint64_t _seed) {
390  seed = _seed;
392 }
393 
394 } // namespace traffic_manager
395 } // 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:98
void RemoveActor(const ActorId actor_id, const bool registered_actor)
Definition: ALSM.cpp:339
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:83
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 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:77
std::condition_variable step_begin_trigger
Condition variables for progressing synchronous execution.
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:99
SharedPtr< Map > GetMap() const
Return the map that describes this world.
Definition: World.cpp:21
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage)
Method to set probabilistic preference to keep on the right lane.
Definition: Parameters.cpp:71
void SetForceLaneChange(const ActorPtr &actor, const bool direction)
Method to force lane change on a vehicle.
Definition: Parameters.cpp:64
std::vector< ActorId > GetIDList()
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage)
Method to set probabilistic preference to keep on the right lane.
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:27
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.
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:38
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:271
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:183
The function of this class is to integrate all the various stages of the traffic manager appropriatel...
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:147
void SetPercentageIgnoreVehicles(const ActorPtr &actor, const float perc)
Method to set % to ignore any vehicle.
Definition: Parameters.cpp:117
void SetupLocalMap()
Method to setup InMemoryMap.
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:33
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:124
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.
SharedPtrType Lock() const
Same as TryLock but never return nullptr.
LocalizationStage localization_stage
Various stages representing core operations of traffic manager.
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:90
void SetHybridPhysicsRadius(const float radius)
Method to set hybrid physics radius.
Definition: Parameters.cpp:131
void SetPercentageRunningLight(const ActorPtr &actor, const float perc)
Method to set % to run any traffic light.
Definition: Parameters.cpp:103
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:110
std::unique_ptr< std::thread > worker_thread
Single worker thread for sequential execution of sub-components.
void SetSynchronousModeTimeOutInMiliSecond(const double time)
Set Synchronous mode time out.
Definition: Parameters.cpp:94
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:22
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:136