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