CARLA
ALSM.cpp
Go to the documentation of this file.
1 
2 #include "boost/pointer_cast.hpp"
3 
4 #include "carla/client/Actor.h"
5 #include "carla/client/Vehicle.h"
6 #include "carla/client/Walker.h"
7 
11 
13 
14 namespace carla {
15 namespace traffic_manager {
16 
18  AtomicActorSet &registered_vehicles,
19  BufferMap &buffer_map,
20  TrackTraffic &track_traffic,
21  std::vector<ActorId>& marked_for_removal,
22  const Parameters &parameters,
23  const cc::World &world,
24  const LocalMapPtr &local_map,
25  SimulationState &simulation_state,
26  LocalizationStage &localization_stage,
27  CollisionStage &collision_stage,
28  TrafficLightStage &traffic_light_stage,
29  MotionPlanStage &motion_plan_stage,
30  VehicleLightStage &vehicle_light_stage)
31  : registered_vehicles(registered_vehicles),
32  buffer_map(buffer_map),
33  track_traffic(track_traffic),
34  marked_for_removal(marked_for_removal),
35  parameters(parameters),
36  world(world),
37  local_map(local_map),
38  simulation_state(simulation_state),
39  localization_stage(localization_stage),
40  collision_stage(collision_stage),
41  traffic_light_stage(traffic_light_stage),
42  motion_plan_stage(motion_plan_stage),
43  vehicle_light_stage(vehicle_light_stage) {}
44 
45 void ALSM::Update() {
46 
47  bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
48 
49  std::set<ActorId> world_vehicle_ids;
50  std::set<ActorId> world_pedestrian_ids;
51  std::vector<ActorId> unregistered_list_to_be_deleted;
52 
54  ActorList world_actors = world.GetActors();
55 
56  // Find destroyed actors and perform clean up.
57  const ALSM::DestroyeddActors destroyed_actors = IdentifyDestroyedActors(world_actors);
58 
59  const ActorIdSet &destroyed_registered = destroyed_actors.first;
60  for (const auto &deletion_id: destroyed_registered) {
61  RemoveActor(deletion_id, true);
62  }
63 
64  const ActorIdSet &destroyed_unregistered = destroyed_actors.second;
65  for (auto deletion_id : destroyed_unregistered) {
66  RemoveActor(deletion_id, false);
67  }
68 
69  // Invalidate hero actor if it is not alive anymore.
70  if (hero_actors.size() != 0u) {
71  ActorIdSet hero_actors_to_delete;
72  for (auto &hero_actor_info: hero_actors) {
73  if (destroyed_unregistered.find(hero_actor_info.first) != destroyed_unregistered.end()) {
74  hero_actors_to_delete.insert(hero_actor_info.first);
75  }
76  if (destroyed_registered.find(hero_actor_info.first) != destroyed_registered.end()) {
77  hero_actors_to_delete.insert(hero_actor_info.first);
78  }
79  }
80 
81  for (auto &deletion_id: hero_actors_to_delete) {
82  hero_actors.erase(deletion_id);
83  }
84  }
85 
86  // Scan for new unregistered actors.
87  IdentifyNewActors(world_actors);
88 
89  // Update dynamic state and static attributes for all registered vehicles.
90  ALSM::IdleInfo max_idle_time = std::make_pair(0u, current_timestamp.elapsed_seconds);
91  UpdateRegisteredActorsData(hybrid_physics_mode, max_idle_time);
92 
93  // Destroy registered vehicle if stuck at a location for too long.
94  if (IsVehicleStuck(max_idle_time.first)
96  && hero_actors.find(max_idle_time.first) == hero_actors.end()) {
97  registered_vehicles.Destroy(max_idle_time.first);
98  RemoveActor(max_idle_time.first, true);
100  }
101 
102  // Destorying vehicles for marked for removal by stages.
103  if (parameters.GetOSMMode()) {
104  for (const ActorId& actor_id: marked_for_removal) {
105  registered_vehicles.Destroy(actor_id);
106  RemoveActor(actor_id, true);
107  }
108  marked_for_removal.clear();
109  }
110 
111  // Update dynamic state and static attributes for unregistered actors.
113 }
114 
115 void ALSM::IdentifyNewActors(const ActorList &actor_list) {
116  for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
117  ActorPtr actor = *iter;
118  ActorId actor_id = actor->GetId();
119  // Identify any new hero vehicle
120  if (actor->GetTypeId().front() == 'v') {
121  if (hero_actors.size() == 0u || hero_actors.find(actor_id) == hero_actors.end()) {
122  for (auto&& attribute: actor->GetAttributes()) {
123  if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") {
124  hero_actors.insert({actor_id, actor});
125  }
126  }
127  }
128  }
129  if (!registered_vehicles.Contains(actor_id)
130  && unregistered_actors.find(actor_id) == unregistered_actors.end()) {
131 
132  unregistered_actors.insert({actor_id, actor});
133  }
134  }
135 }
136 
138 
139  ALSM::DestroyeddActors destroyed_actors;
140  ActorIdSet &deleted_registered = destroyed_actors.first;
141  ActorIdSet &deleted_unregistered = destroyed_actors.second;
142 
143  // Building hash set of actors present in current frame.
144  ActorIdSet current_actors;
145  for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
146  current_actors.insert((*iter)->GetId());
147  }
148 
149  // Searching for destroyed registered actors.
150  std::vector<ActorId> registered_ids = registered_vehicles.GetIDList();
151  for (const ActorId &actor_id : registered_ids) {
152  if (current_actors.find(actor_id) == current_actors.end()) {
153  deleted_registered.insert(actor_id);
154  }
155  }
156 
157  // Searching for destroyed unregistered actors.
158  for (const auto &actor_info: unregistered_actors) {
159  const ActorId &actor_id = actor_info.first;
160  if (current_actors.find(actor_id) == current_actors.end()
161  || registered_vehicles.Contains(actor_id)) {
162  deleted_unregistered.insert(actor_id);
163  }
164  }
165 
166  return destroyed_actors;
167 }
168 
169 void ALSM::UpdateRegisteredActorsData(const bool hybrid_physics_mode, ALSM::IdleInfo &max_idle_time) {
170 
171  std::vector<ActorPtr> vehicle_list = registered_vehicles.GetList();
172  bool hero_actor_present = hero_actors.size() != 0u;
173  float physics_radius = parameters.GetHybridPhysicsRadius();
174  float physics_radius_square = SQUARE(physics_radius);
175  bool is_respawn_vehicles = parameters.GetRespawnDormantVehicles();
176  if (is_respawn_vehicles && !hero_actor_present) {
178  }
179  // Update first the information regarding any hero vehicle.
180  for (auto &hero_actor_info: hero_actors){
181  if (is_respawn_vehicles) {
182  track_traffic.SetHeroLocation(hero_actor_info.second->GetTransform().location);
183  }
184  UpdateData(hybrid_physics_mode, max_idle_time, hero_actor_info.second, hero_actor_present, physics_radius_square);
185  }
186  // Update information for all other registered vehicles.
187  for (const Actor &vehicle : vehicle_list) {
188  ActorId actor_id = vehicle->GetId();
189  if (hero_actors.find(actor_id) == hero_actors.end()) {
190  UpdateData(hybrid_physics_mode, max_idle_time, vehicle, hero_actor_present, physics_radius_square);
191  }
192  }
193 }
194 
195 void ALSM::UpdateData(const bool hybrid_physics_mode,
196  ALSM::IdleInfo &max_idle_time, const Actor &vehicle,
197  const bool hero_actor_present, const float physics_radius_square) {
198 
199  ActorId actor_id = vehicle->GetId();
200  cg::Transform vehicle_transform = vehicle->GetTransform();
201  cg::Location vehicle_location = vehicle_transform.location;
202  cg::Rotation vehicle_rotation = vehicle_transform.rotation;
203  cg::Vector3D vehicle_velocity = vehicle->GetVelocity();
204  bool state_entry_present = simulation_state.ContainsActor(actor_id);
205 
206  // Initializing idle times.
207  if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) {
208  idle_time.insert({actor_id, current_timestamp.elapsed_seconds});
209  }
210 
211  // Check if current actor is in range of hero actor and enable physics in hybrid mode.
212  bool in_range_of_hero_actor = false;
213  if (hero_actor_present && hybrid_physics_mode) {
214  for (auto &hero_actor_info: hero_actors) {
215  const ActorId &hero_actor_id = hero_actor_info.first;
216  if (simulation_state.ContainsActor(hero_actor_id)) {
217  const cg::Location &hero_location = simulation_state.GetLocation(hero_actor_id);
218  if (cg::Math::DistanceSquared(vehicle_location, hero_location) < physics_radius_square) {
219  in_range_of_hero_actor = true;
220  break;
221  }
222  }
223  }
224  }
225 
226  bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true;
227  if (!has_physics_enabled.count(actor_id) || has_physics_enabled[actor_id] != enable_physics) {
228  if (hero_actors.find(actor_id) == hero_actors.end()) {
229  vehicle->SetSimulatePhysics(enable_physics);
230  has_physics_enabled[actor_id] = enable_physics;
231  if (enable_physics == true && state_entry_present) {
232  vehicle->SetTargetVelocity(simulation_state.GetVelocity(actor_id));
233  }
234  }
235  }
236 
237  // If physics are disabled, calculate velocity based on change in position.
238  // Do not use 'enable_physics' as turning off the physics in this tick doesn't remove the velocity.
239  // To avoid issues with other clients teleporting the actors, use the previous outpout location.
240  if (state_entry_present && !simulation_state.IsPhysicsEnabled(actor_id)){
241  cg::Location previous_location = simulation_state.GetLocation(actor_id);
242  cg::Location previous_end_location = simulation_state.GetHybridEndLocation(actor_id);
243  cg::Vector3D displacement = (previous_end_location - previous_location);
244  vehicle_velocity = displacement * INV_HYBRID_DT;
245  }
246 
247  // Updated kinematic state object.
248  auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(vehicle);
249  KinematicState kinematic_state{vehicle_location, vehicle_rotation,
250  vehicle_velocity, vehicle_ptr->GetSpeedLimit(),
251  enable_physics, vehicle->IsDormant(), cg::Location()};
252 
253  // Updated traffic light state object.
254  TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
255 
256  // Update simulation state.
257  if (state_entry_present) {
258  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
259  simulation_state.UpdateTrafficLightState(actor_id, tl_state);
260  }
261  else {
262  cg::Vector3D dimensions = vehicle_ptr->GetBoundingBox().extent;
263  StaticAttributes attributes{ActorType::Vehicle, dimensions.x, dimensions.y, dimensions.z};
264 
265  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
266  }
267 
268  // Updating idle time when necessary.
269  UpdateIdleTime(max_idle_time, actor_id);
270 }
271 
272 
274  for (auto &actor_info: unregistered_actors) {
275 
276  const ActorId actor_id = actor_info.first;
277  const ActorPtr actor_ptr = actor_info.second;
278  const std::string type_id = actor_ptr->GetTypeId();
279 
280  const cg::Transform actor_transform = actor_ptr->GetTransform();
281  const cg::Location actor_location = actor_transform.location;
282  const cg::Rotation actor_rotation = actor_transform.rotation;
283  const cg::Vector3D actor_velocity = actor_ptr->GetVelocity();
284  const bool actor_is_dormant = actor_ptr->IsDormant();
285  KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true, actor_is_dormant, cg::Location()};
286 
287  TrafficLightState tl_state;
288  ActorType actor_type = ActorType::Any;
289  cg::Vector3D dimensions;
290  std::vector<SimpleWaypointPtr> nearest_waypoints;
291 
292  bool state_entry_not_present = !simulation_state.ContainsActor(actor_id);
293  if (type_id.front() == 'v') {
294  auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(actor_ptr);
295  kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit();
296 
297  tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
298 
299  if (state_entry_not_present) {
300  dimensions = vehicle_ptr->GetBoundingBox().extent;
301  actor_type = ActorType::Vehicle;
302  StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
303 
304  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
305  } else {
306  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
307  simulation_state.UpdateTrafficLightState(actor_id, tl_state);
308  }
309 
310  // Identify occupied waypoints.
311  cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
312  cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector();
313  std::vector<cg::Location> corners = {actor_location + cg::Location(extent.x * heading_vector),
314  actor_location,
315  actor_location + cg::Location(-extent.x * heading_vector)};
316  for (cg::Location &vertex: corners) {
317  SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(vertex);
318  nearest_waypoints.push_back(nearest_waypoint);
319  }
320  }
321  else if (type_id.front() == 'w') {
322  auto walker_ptr = boost::static_pointer_cast<cc::Walker>(actor_ptr);
323 
324  if (state_entry_not_present) {
325  dimensions = walker_ptr->GetBoundingBox().extent;
326  actor_type = ActorType::Pedestrian;
327  StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
328 
329  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
330  } else {
331  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
332  }
333 
334  // Identify occupied waypoints.
335  SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(actor_location);
336  nearest_waypoints.push_back(nearest_waypoint);
337  }
338 
339  track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints);
340  }
341 }
342 
343 void ALSM::UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id) {
344  if (idle_time.find(actor_id) != idle_time.end()) {
345  double &idle_duration = idle_time.at(actor_id);
347  idle_duration = current_timestamp.elapsed_seconds;
348  }
349 
350  // Checking maximum idle time.
351  if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) {
352  max_idle_time = std::make_pair(actor_id, idle_duration);
353  }
354  }
355 }
356 
357 bool ALSM::IsVehicleStuck(const ActorId& actor_id) {
358  if (idle_time.find(actor_id) != idle_time.end()) {
359  double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id);
360  TrafficLightState tl_state = simulation_state.GetTLS(actor_id);
361  if ((!tl_state.at_traffic_light && tl_state.tl_state != TLS::Red && delta_idle_time >= BLOCKED_TIME_THRESHOLD)
362  || (delta_idle_time >= RED_TL_BLOCKED_TIME_THRESHOLD)) {
363  return true;
364  }
365  }
366  return false;
367 }
368 
369 void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
370  if (registered_actor) {
371  registered_vehicles.Remove({actor_id});
372  buffer_map.erase(actor_id);
373  idle_time.erase(actor_id);
375  collision_stage.RemoveActor(actor_id);
377  motion_plan_stage.RemoveActor(actor_id);
379  }
380  else {
381  unregistered_actors.erase(actor_id);
382  hero_actors.erase(actor_id);
383  }
384 
385  track_traffic.DeleteActor(actor_id);
386  simulation_state.RemoveActor(actor_id);
387 }
388 
389 void ALSM::Reset() {
390  unregistered_actors.clear();
391  idle_time.clear();
392  hero_actors.clear();
395 }
396 
397 } // namespace traffic_manager
398 } // namespace carla
std::pair< ActorId, double > IdleInfo
Definition: ALSM.h:83
MotionPlanStage & motion_plan_stage
Definition: ALSM.h:62
CollisionStage & collision_stage
Definition: ALSM.h:60
SharedPtr< ActorList > GetActors() const
Return a list with all the actors currently present in the world.
Definition: World.cpp:106
void RemoveActor(const ActorId actor_id, const bool registered_actor)
Definition: ALSM.cpp:369
void UpdateTrafficLightState(ActorId actor_id, TrafficLightState state)
carla::SharedPtr< cc::Actor > ActorPtr
std::pair< ActorIdSet, ActorIdSet > DestroyeddActors
Definition: ALSM.h:78
std::vector< ActorId > & marked_for_removal
Definition: ALSM.h:54
void SetHeroLocation(const cg::Location location)
void UpdateData(const bool hybrid_physics_mode, ALSM::IdleInfo &max_idle_time, const Actor &vehicle, const bool hero_actor_present, const float physics_radius_square)
Definition: ALSM.cpp:195
float GetSpeedLimit() const
Return the speed limit currently affecting this vehicle.
Definition: Vehicle.cpp:106
bool GetOSMMode() const
Method to get Open Street Map mode.
Definition: Parameters.cpp:413
float SquaredLength() const
Definition: geom/Vector3D.h:45
TrafficLightState GetTLS(const ActorId actor_id) const
std::shared_ptr< InMemoryMap > LocalMapPtr
Definition: ALSM.h:36
This class holds the state of all the vehicles in the simlation.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
std::unordered_set< ActorId > ActorIdSet
std::vector< ActorId > GetIDList()
void RemoveActor(const ActorId actor_id) override
void RemoveActor(const ActorId actor_id)
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:70
DestroyeddActors IdentifyDestroyedActors(const ActorList &actor_list)
Definition: ALSM.cpp:137
std::unordered_map< carla::ActorId, Buffer > BufferMap
ALSM(AtomicActorSet &registered_vehicles, BufferMap &buffer_map, TrackTraffic &track_traffic, std::vector< ActorId > &marked_for_removal, const Parameters &parameters, const cc::World &world, const LocalMapPtr &local_map, SimulationState &simulation_state, LocalizationStage &localization_stage, CollisionStage &collision_stage, TrafficLightStage &traffic_light_stage, MotionPlanStage &motion_plan_stage, VehicleLightStage &vehicle_light_stage)
Definition: ALSM.cpp:17
void UpdateKinematicState(ActorId actor_id, KinematicState state)
IdleTimeMap idle_time
Definition: ALSM.h:49
VehicleLightStage & vehicle_light_stage
Definition: ALSM.h:63
cg::Location GetLocation(const ActorId actor_id) const
LocalizationStage & localization_stage
Definition: ALSM.h:59
std::unordered_map< ActorId, bool > has_physics_enabled
Definition: ALSM.h:67
void UpdateUnregisteredActorsData()
Definition: ALSM.cpp:273
bool GetRespawnDormantVehicles() const
Method to retrieve if we are automatically respawning vehicles.
Definition: Parameters.cpp:397
geom::Location Location
Definition: rpc/Location.h:14
const Parameters & parameters
Definition: ALSM.h:55
const Timestamp & GetTimestamp() const
Get timestamp of this snapshot.
Definition: WorldSnapshot.h:34
void UpdateUnregisteredGridPosition(const ActorId actor_id, const std::vector< SimpleWaypointPtr > waypoints)
carla::SharedPtr< cc::Actor > Actor
std::vector< ActorPtr > GetList()
#define SQUARE(a)
This file contains various constants used in traffic manager arranged into sensible namespaces for re...
Definition: Constants.h:13
double elapsed_last_actor_destruction
Definition: ALSM.h:65
void UpdateIdleTime(std::pair< ActorId, double > &max_idle_time, const ActorId &actor_id)
Definition: ALSM.cpp:343
bool GetHybridPhysicsMode() const
Method to retrieve hybrid physics mode.
Definition: Parameters.cpp:392
carla::ActorId ActorId
cg::Location GetHybridEndLocation(const ActorId actor_id) const
bool ContainsActor(ActorId actor_id) const
void RemoveActor(const ActorId actor_id) override
float GetHybridPhysicsRadius() const
Method to retrieve hybrid physics radius.
Definition: Parameters.cpp:231
This class has functionality for responding to traffic lights and managing entry into non-signalized ...
void RemoveActor(const ActorId actor_id) override
void DeleteActor(ActorId actor_id)
Method to delete actor data from tracking.
bool IsVehicleStuck(const ActorId &actor_id)
Definition: ALSM.cpp:357
BufferMap & buffer_map
Definition: ALSM.h:47
AtomicActorSet & registered_vehicles
Definition: ALSM.h:44
bool IsPhysicsEnabled(const ActorId actor_id) const
void IdentifyNewActors(const ActorList &actor_list)
Definition: ALSM.cpp:115
const cc::World & world
Definition: ALSM.h:56
TrafficLightStage & traffic_light_stage
Definition: ALSM.h:61
void RemoveActor(const ActorId actor_id) override
void AddActor(ActorId actor_id, KinematicState kinematic_state, StaticAttributes attributes, TrafficLightState tl_state)
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
SimulationState & simulation_state
Definition: ALSM.h:58
const LocalMapPtr & local_map
Definition: ALSM.h:57
This class has functionality to maintain a horizon of waypoints ahead of the vehicle for it to follow...
ActorMap unregistered_actors
Definition: ALSM.h:46
TrackTraffic & track_traffic
Definition: ALSM.h:52
carla::SharedPtr< cc::ActorList > ActorList
Definition: ALSM.h:33
cg::Vector3D GetVelocity(const ActorId actor_id) const
This class has functionality for turning on/off the vehicle lights according to the current vehicle s...
WorldSnapshot GetSnapshot() const
Return a snapshot of the world at this moment.
Definition: World.cpp:94
This class has functionality to detect potential collision with a nearby actor.
double elapsed_seconds
Simulated seconds elapsed since the beginning of the current episode.
Definition: Timestamp.h:33
void Remove(std::vector< ActorId > actor_id_list)
cc::Timestamp current_timestamp
Definition: ALSM.h:66
void UpdateRegisteredActorsData(const bool hybrid_physics_mode, IdleInfo &max_idle_time)
Definition: ALSM.cpp:169