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 (hybrid_physics_mode && 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  }
77 
78  for (auto &deletion_id: hero_actors_to_delete) {
79  hero_actors.erase(deletion_id);
80  }
81  }
82 
83  // Scan for new unregistered actors.
84  ALSM::ActorVector new_actors = IdentifyNewActors(world_actors);
85  for (const ActorPtr &actor: new_actors) {
86  unregistered_actors.insert({actor->GetId(), actor});
87 
88  // Identify any new hero vehicle if the system is in hybrid physics mode.
89  if (hybrid_physics_mode) {
90  if (actor->GetTypeId().front() == 'v') {
91  ActorId hero_actor_id = actor->GetId();
92  for (auto&& attribute: actor->GetAttributes()) {
93  if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") {
94  hero_actors.insert({hero_actor_id, actor});
95  }
96  }
97  }
98  }
99  }
100 
101  // Update dynamic state and static attributes for all registered vehicles.
102  ALSM::IdleInfo max_idle_time = std::make_pair(0u, current_timestamp.elapsed_seconds);
103  UpdateRegisteredActorsData(hybrid_physics_mode, max_idle_time);
104 
105  // Destroy registered vehicle if stuck at a location for too long.
106  if (IsVehicleStuck(max_idle_time.first)
108  registered_vehicles.Destroy(max_idle_time.first);
109  RemoveActor(max_idle_time.first, true);
111  }
112 
113  // Destorying vehicles for marked for removal by stages.
114  if (parameters.GetOSMMode()) {
115  for (const ActorId& actor_id: marked_for_removal) {
116  registered_vehicles.Destroy(actor_id);
117  RemoveActor(actor_id, true);
118  }
119  marked_for_removal.clear();
120  }
121 
122  // Update dynamic state and static attributes for unregistered actors.
124 }
125 
126 std::vector<ActorPtr> ALSM::IdentifyNewActors(const ActorList &actor_list) {
127  std::vector<ActorPtr> new_actors;
128  for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
129  ActorPtr actor = *iter;
130  ActorId actor_id = actor->GetId();
131  if (!registered_vehicles.Contains(actor_id)
132  && unregistered_actors.find(actor_id) == unregistered_actors.end()) {
133  new_actors.push_back(actor);
134  }
135  }
136  return new_actors;
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 = hybrid_physics_mode && hero_actors.size() != 0u;
175  float physics_radius = parameters.GetHybridPhysicsRadius();
176  float physics_radius_square = SQUARE(physics_radius);
177  for (const Actor &vehicle : vehicle_list) {
178  ActorId actor_id = vehicle->GetId();
179  cg::Transform vehicle_transform = vehicle->GetTransform();
180  cg::Location vehicle_location = vehicle_transform.location;
181  cg::Rotation vehicle_rotation = vehicle_transform.rotation;
182  cg::Vector3D vehicle_velocity = vehicle->GetVelocity();
183 
184  // Initializing idle times.
185  if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) {
186  idle_time.insert({actor_id, current_timestamp.elapsed_seconds});
187  }
188 
189  // Check if current actor is in range of hero actor and enable physics in hybrid mode.
190  bool in_range_of_hero_actor = false;
191  if (hero_actor_present) {
192  for (auto &hero_actor_info: hero_actors) {
193  const ActorId &hero_actor_id = hero_actor_info.first;
194  if (simulation_state.ContainsActor(hero_actor_id)) {
195  const cg::Location &hero_location = simulation_state.GetLocation(hero_actor_id);
196  if (cg::Math::DistanceSquared(vehicle_location, hero_location) < physics_radius_square) {
197  in_range_of_hero_actor = true;
198  break;
199  }
200  }
201  }
202  }
203  bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true;
204  vehicle->SetSimulatePhysics(enable_physics);
205 
206  bool state_entry_present = simulation_state.ContainsActor(actor_id);
207  // If physics is disabled, calculate velocity based on change in position.
208  if (!enable_physics) {
209  cg::Location previous_location;
210  if (state_entry_present) {
211  previous_location = simulation_state.GetLocation(actor_id);
212  } else {
213  previous_location = vehicle_location;
214  }
215  cg::Vector3D displacement = (vehicle_location - previous_location);
216  vehicle_velocity = displacement * INV_HYBRID_DT;
217  }
218 
219  // Updated kinematic state object.
220  auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(vehicle);
221  KinematicState kinematic_state{vehicle_location, vehicle_rotation,
222  vehicle_velocity, vehicle_ptr->GetSpeedLimit(), enable_physics};
223 
224  // Updated traffic light state object.
225  TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
226 
227  // Update simulation state.
228  if (state_entry_present) {
229  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
230  simulation_state.UpdateTrafficLightState(actor_id, tl_state);
231  }
232  else {
233  cg::Vector3D dimensions = vehicle_ptr->GetBoundingBox().extent;
234  StaticAttributes attributes{ActorType::Vehicle, dimensions.x, dimensions.y, dimensions.z};
235 
236  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
237  }
238 
239  // Updating idle time when necessary.
240  UpdateIdleTime(max_idle_time, actor_id);
241  }
242 }
243 
245  for (auto &actor_info: unregistered_actors) {
246 
247  const ActorId actor_id = actor_info.first;
248  const ActorPtr actor_ptr = actor_info.second;
249  const std::string type_id = actor_ptr->GetTypeId();
250 
251  const cg::Transform actor_transform = actor_ptr->GetTransform();
252  const cg::Location actor_location = actor_transform.location;
253  const cg::Rotation actor_rotation = actor_transform.rotation;
254  const cg::Vector3D actor_velocity = actor_ptr->GetVelocity();
255  KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true};
256 
257  TrafficLightState tl_state;
258  ActorType actor_type = ActorType::Any;
259  cg::Vector3D dimensions;
260  std::vector<SimpleWaypointPtr> nearest_waypoints;
261 
262  bool state_entry_not_present = !simulation_state.ContainsActor(actor_id);
263  if (type_id.front() == 'v') {
264  auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(actor_ptr);
265  kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit();
266 
267  tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
268 
269  if (state_entry_not_present) {
270  dimensions = vehicle_ptr->GetBoundingBox().extent;
271  actor_type = ActorType::Vehicle;
272  StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
273 
274  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
275  } else {
276  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
277  simulation_state.UpdateTrafficLightState(actor_id, tl_state);
278  }
279 
280  // Identify occupied waypoints.
281  cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
282  cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector();
283  std::vector<cg::Location> corners = {actor_location + cg::Location(extent.x * heading_vector),
284  actor_location,
285  actor_location + cg::Location(-extent.x * heading_vector)};
286  for (cg::Location &vertex: corners) {
287  SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(vertex);
288  nearest_waypoints.push_back(nearest_waypoint);
289  }
290  }
291  else if (type_id.front() == 'w') {
292  auto walker_ptr = boost::static_pointer_cast<cc::Walker>(actor_ptr);
293 
294  if (state_entry_not_present) {
295  dimensions = walker_ptr->GetBoundingBox().extent;
296  actor_type = ActorType::Pedestrian;
297  StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
298 
299  simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
300  } else {
301  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
302  }
303 
304  // Identify occupied waypoints.
305  SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(actor_location);
306  nearest_waypoints.push_back(nearest_waypoint);
307  }
308 
309  track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints);
310  }
311 }
312 
313 void ALSM::UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id) {
314  if (idle_time.find(actor_id) != idle_time.end()) {
315  double &idle_duration = idle_time.at(actor_id);
317  idle_duration = current_timestamp.elapsed_seconds;
318  }
319 
320  // Checking maximum idle time.
321  if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) {
322  max_idle_time = std::make_pair(actor_id, idle_duration);
323  }
324  }
325 }
326 
327 bool ALSM::IsVehicleStuck(const ActorId& actor_id) {
328  if (idle_time.find(actor_id) != idle_time.end()) {
329  double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id);
330  TrafficLightState tl_state = simulation_state.GetTLS(actor_id);
331  if ((!tl_state.at_traffic_light && tl_state.tl_state != TLS::Red && delta_idle_time >= BLOCKED_TIME_THRESHOLD)
332  || (delta_idle_time >= RED_TL_BLOCKED_TIME_THRESHOLD)) {
333  return true;
334  }
335  }
336  return false;
337 }
338 
339 void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
340  if (registered_actor) {
341  registered_vehicles.Remove({actor_id});
342  buffer_map.erase(actor_id);
343  idle_time.erase(actor_id);
344  random_devices.erase(actor_id);
346  collision_stage.RemoveActor(actor_id);
348  motion_plan_stage.RemoveActor(actor_id);
349  }
350  else {
351  unregistered_actors.erase(actor_id);
352  hero_actors.erase(actor_id);
353  }
354 
355  track_traffic.DeleteActor(actor_id);
356  simulation_state.RemoveActor(actor_id);
357 }
358 
359 void ALSM::Reset() {
360  unregistered_actors.clear();
361  idle_time.clear();
362  hero_actors.clear();
365 }
366 
367 } // namespace traffic_manager
368 } // 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:99
void RemoveActor(const ActorId actor_id, const bool registered_actor)
Definition: ALSM.cpp:339
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
RandomGeneratorMap & random_devices
Definition: ALSM.h:66
std::vector< ActorPtr > ActorVector
Definition: ALSM.h:74
float GetSpeedLimit() const
Return the speed limit currently affecting this vehicle.
Definition: Vehicle.cpp:74
bool GetOSMMode() const
Method to get Open Street Map mode.
Definition: Parameters.cpp:276
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:99
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:139
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
void UpdateUnregisteredActorsData()
Definition: ALSM.cpp:244
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)
ActorVector IdentifyNewActors(const ActorList &actor_list)
Definition: ALSM.cpp:126
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:11
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:313
bool GetHybridPhysicsMode() const
Method to retrieve hybrid physics mode.
Definition: Parameters.cpp:271
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:142
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:327
BufferMap & buffer_map
Definition: ALSM.h:46
AtomicActorSet & registered_vehicles
Definition: ALSM.h:43
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:87
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:171