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  RandomGeneratorMap &random_devices)
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  random_devices(random_devices) {}
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 
205  // Initializing idle times.
206  if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) {
207  idle_time.insert({actor_id, current_timestamp.elapsed_seconds});
208  }
209 
210  // Check if current actor is in range of hero actor and enable physics in hybrid mode.
211  bool in_range_of_hero_actor = false;
212  if (hero_actor_present && hybrid_physics_mode) {
213  for (auto &hero_actor_info: hero_actors) {
214  const ActorId &hero_actor_id = hero_actor_info.first;
215  if (simulation_state.ContainsActor(hero_actor_id)) {
216  const cg::Location &hero_location = simulation_state.GetLocation(hero_actor_id);
217  if (cg::Math::DistanceSquared(vehicle_location, hero_location) < physics_radius_square) {
218  in_range_of_hero_actor = true;
219  break;
220  }
221  }
222  }
223  }
224 
225  bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true;
226  if (!has_physics_enabled.count(actor_id) || has_physics_enabled[actor_id] != enable_physics) {
227  if (hero_actors.find(actor_id) == hero_actors.end()) {
228  vehicle->SetSimulatePhysics(enable_physics);
229  has_physics_enabled[actor_id] = enable_physics;
230  if (enable_physics == true && simulation_state.ContainsActor(actor_id)) {
231  vehicle->SetTargetVelocity(simulation_state.GetVelocity(actor_id));
232  }
233  }
234  }
235 
236  bool state_entry_present = simulation_state.ContainsActor(actor_id);
237  // If physics is disabled, calculate velocity based on change in position.
238  if (!enable_physics) {
239  cg::Location previous_location;
240  if (state_entry_present) {
241  previous_location = simulation_state.GetLocation(actor_id);
242  } else {
243  previous_location = vehicle_location;
244  }
245  cg::Vector3D displacement = (vehicle_location - previous_location);
246  vehicle_velocity = displacement * INV_HYBRID_DT;
247  }
248 
249  // Updated kinematic state object.
250  auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(vehicle);
251  KinematicState kinematic_state{vehicle_location, vehicle_rotation,
252  vehicle_velocity, vehicle_ptr->GetSpeedLimit(),
253  enable_physics, vehicle->IsDormant()};
254 
255  // Updated traffic light state object.
256  TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
257 
258  // Update simulation state.
259  if (state_entry_present) {
260  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
261  simulation_state.UpdateTrafficLightState(actor_id, tl_state);
262  }
263  else {
264  cg::Vector3D dimensions = vehicle_ptr->GetBoundingBox().extent;
265  StaticAttributes attributes{ActorType::Vehicle, dimensions.x, dimensions.y, dimensions.z};
266 
267  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
268  }
269 
270  // Updating idle time when necessary.
271  UpdateIdleTime(max_idle_time, actor_id);
272 }
273 
274 
276  for (auto &actor_info: unregistered_actors) {
277 
278  const ActorId actor_id = actor_info.first;
279  const ActorPtr actor_ptr = actor_info.second;
280  const std::string type_id = actor_ptr->GetTypeId();
281 
282  const cg::Transform actor_transform = actor_ptr->GetTransform();
283  const cg::Location actor_location = actor_transform.location;
284  const cg::Rotation actor_rotation = actor_transform.rotation;
285  const cg::Vector3D actor_velocity = actor_ptr->GetVelocity();
286  const bool actor_is_dormant = actor_ptr->IsDormant();
287  KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true, actor_is_dormant};
288 
289  TrafficLightState tl_state;
290  ActorType actor_type = ActorType::Any;
291  cg::Vector3D dimensions;
292  std::vector<SimpleWaypointPtr> nearest_waypoints;
293 
294  bool state_entry_not_present = !simulation_state.ContainsActor(actor_id);
295  if (type_id.front() == 'v') {
296  auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(actor_ptr);
297  kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit();
298 
299  tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
300 
301  if (state_entry_not_present) {
302  dimensions = vehicle_ptr->GetBoundingBox().extent;
303  actor_type = ActorType::Vehicle;
304  StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
305 
306  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
307  } else {
308  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
309  simulation_state.UpdateTrafficLightState(actor_id, tl_state);
310  }
311 
312  // Identify occupied waypoints.
313  cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
314  cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector();
315  std::vector<cg::Location> corners = {actor_location + cg::Location(extent.x * heading_vector),
316  actor_location,
317  actor_location + cg::Location(-extent.x * heading_vector)};
318  for (cg::Location &vertex: corners) {
319  SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(vertex);
320  nearest_waypoints.push_back(nearest_waypoint);
321  }
322  }
323  else if (type_id.front() == 'w') {
324  auto walker_ptr = boost::static_pointer_cast<cc::Walker>(actor_ptr);
325 
326  if (state_entry_not_present) {
327  dimensions = walker_ptr->GetBoundingBox().extent;
328  actor_type = ActorType::Pedestrian;
329  StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
330 
331  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
332  } else {
333  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
334  }
335 
336  // Identify occupied waypoints.
337  SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(actor_location);
338  nearest_waypoints.push_back(nearest_waypoint);
339  }
340 
341  track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints);
342  }
343 }
344 
345 void ALSM::UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id) {
346  if (idle_time.find(actor_id) != idle_time.end()) {
347  double &idle_duration = idle_time.at(actor_id);
349  idle_duration = current_timestamp.elapsed_seconds;
350  }
351 
352  // Checking maximum idle time.
353  if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) {
354  max_idle_time = std::make_pair(actor_id, idle_duration);
355  }
356  }
357 }
358 
359 bool ALSM::IsVehicleStuck(const ActorId& actor_id) {
360  if (idle_time.find(actor_id) != idle_time.end()) {
361  double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id);
362  TrafficLightState tl_state = simulation_state.GetTLS(actor_id);
363  if ((!tl_state.at_traffic_light && tl_state.tl_state != TLS::Red && delta_idle_time >= BLOCKED_TIME_THRESHOLD)
364  || (delta_idle_time >= RED_TL_BLOCKED_TIME_THRESHOLD)) {
365  return true;
366  }
367  }
368  return false;
369 }
370 
371 void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
372  if (registered_actor) {
373  registered_vehicles.Remove({actor_id});
374  buffer_map.erase(actor_id);
375  idle_time.erase(actor_id);
376  random_devices.erase(actor_id);
378  collision_stage.RemoveActor(actor_id);
380  motion_plan_stage.RemoveActor(actor_id);
381  }
382  else {
383  unregistered_actors.erase(actor_id);
384  hero_actors.erase(actor_id);
385  }
386 
387  track_traffic.DeleteActor(actor_id);
388  simulation_state.RemoveActor(actor_id);
389 }
390 
391 void ALSM::Reset() {
392  unregistered_actors.clear();
393  idle_time.clear();
394  hero_actors.clear();
397 }
398 
399 } // namespace traffic_manager
400 } // namespace carla
std::pair< ActorId, double > IdleInfo
Definition: ALSM.h:83
MotionPlanStage & motion_plan_stage
Definition: ALSM.h:61
CollisionStage & collision_stage
Definition: ALSM.h:59
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:371
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:53
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
RandomGeneratorMap & random_devices
Definition: ALSM.h:66
float GetSpeedLimit() const
Return the speed limit currently affecting this vehicle.
Definition: Vehicle.cpp:86
bool GetOSMMode() const
Method to get Open Street Map mode.
Definition: Parameters.cpp:308
float SquaredLength() const
Definition: geom/Vector3D.h:45
TrafficLightState GetTLS(const ActorId actor_id) const
std::shared_ptr< InMemoryMap > LocalMapPtr
Definition: ALSM.h:35
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:66
DestroyeddActors IdentifyDestroyedActors(const ActorList &actor_list)
Definition: ALSM.cpp:137
std::unordered_map< carla::ActorId, Buffer > BufferMap
void UpdateKinematicState(ActorId actor_id, KinematicState state)
IdleTimeMap idle_time
Definition: ALSM.h:48
cg::Location GetLocation(const ActorId actor_id) const
LocalizationStage & localization_stage
Definition: ALSM.h:58
std::unordered_map< ActorId, bool > has_physics_enabled
Definition: ALSM.h:67
void UpdateUnregisteredActorsData()
Definition: ALSM.cpp:275
bool GetRespawnDormantVehicles() const
Method to retrieve if we are automatically respawning vehicles.
Definition: Parameters.cpp:292
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, RandomGeneratorMap &random_devices)
Definition: ALSM.cpp:17
geom::Location Location
Definition: rpc/Location.h:14
const Parameters & parameters
Definition: ALSM.h:54
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:63
void UpdateIdleTime(std::pair< ActorId, double > &max_idle_time, const ActorId &actor_id)
Definition: ALSM.cpp:345
bool GetHybridPhysicsMode() const
Method to retrieve hybrid physics mode.
Definition: Parameters.cpp:287
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:158
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:359
BufferMap & buffer_map
Definition: ALSM.h:46
AtomicActorSet & registered_vehicles
Definition: ALSM.h:43
void IdentifyNewActors(const ActorList &actor_list)
Definition: ALSM.cpp:115
const cc::World & world
Definition: ALSM.h:55
TrafficLightStage & traffic_light_stage
Definition: ALSM.h:60
void AddActor(ActorId actor_id, KinematicState kinematic_state, StaticAttributes attributes, TrafficLightState tl_state)
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
SimulationState & simulation_state
Definition: ALSM.h:57
const LocalMapPtr & local_map
Definition: ALSM.h:56
This class has functionality to maintain a horizon of waypoints ahead of the vehicle for it to follow...
ActorMap unregistered_actors
Definition: ALSM.h:45
TrackTraffic & track_traffic
Definition: ALSM.h:51
carla::SharedPtr< cc::ActorList > ActorList
Definition: ALSM.h:32
cg::Vector3D GetVelocity(const ActorId actor_id) const
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:64
void UpdateRegisteredActorsData(const bool hybrid_physics_mode, IdleInfo &max_idle_time)
Definition: ALSM.cpp:169