Repository: HARPLab/DReyeVR (language: cpp)

Issue:
Autopilot vehicles crashing into ego vehicle
We're having issues with autopilot vehicles colliding with the ego vehicle, and we're unsure of the cause.

https://github.com/HARPLab/DReyeVR/blob/51ea84a65df001801f582051dc3899d996d3f7b9/DReyeVR/EgoVehicle.cpp#L622-L643 
This was mentioned in issue #53, but we couldn't locate the equivalent in the version we're using, DREYEVR 0.1.2, ScenarioRunner 0.9.13.

We've been testing `--sync`, but this drastically speeds up the game, the ego vehicle drives incredibly fast with sync enabled. 

We did attempt to paste the contents from the above link into EgoVehicle.cpp, but with sync enabled all traffic stood still and without sync it had no effect.


Discussion / hints:
When you are not running with `--sync` there is an asynchronous timestep communication between the PythonAPI (client) and UE4 (server) which causes weird behavior like what you're seeing; unless your client and server were running at perfectly the exact same framerate (highly unlikely). 

If you are running with `--sync` then the frametimes are forced to be synchronized between the PythonAPI and UE4 server. If this speeds up your game that means your UE4 server is ticking at a much faster rate than the PythonAPI, which needs a while to "catch up". So if you force synchronization then you enable the server to skip some frame updates (giving a sense of "speeding up" in order to match the framerate of the PythonAPI).

Have you tried using `--sync` alongside fixing the server framerate using a fixed time-step such as [this](https://github.com/carla-simulator/carla/blob/master/Docs/adv_synchrony_timestep.md#fixed-time-step). For real-ish-time performance we like >30FPS so typically would fix the `fixed_delta_seconds` to `0.025` so each frame takes 1/40th of a second. This works well as long as your simulator server (UE4) can keep up with a consistent 40FPS, otherwise weird stuff will happen like what you're seeing. 

In summary:
```python
settings = world.get_settings()
settings.fixed_delta_seconds = 1.0 / desired_fps # limits server framerate
settings.synchronous_mode = True # Enables synchronous mode
world.apply_settings(settings)
```

See more about how Carla's timestep logic works [here](https://github.com/carla-simulator/carla/blob/master/Docs/adv_synchrony_timestep.md)

Hope this helps!

Also, if you are using DReyeVR 0.1.2 (the latest as of writing) you should no longer need that `AEgoVehicle::Register` snippet since this has been redone as [part of the spawning mechanism](https://github.com/HARPLab/DReyeVR/blob/70a15b5b05b135682cc2daf8ddc4f469e9f9f05c/DReyeVR/DReyeVRFactory.cpp#L29-L38). 
Thank you for the reply!

We're experimenting with framerate and `--sync` and the simulation is noticeably better. Framerate is currently set to 35, though there are still occassional speed-ups that happen when we make a right/left turn, drive through a tunnel or just simply when looking down and not seeing the surroundings. Could this have something to do with the server not being able to keep up?

Collisions with autopilot vehicles still happens in sync mode even after we have corrected the framerate, as if they do not react at all to our existence. Could that be caused by the PythonAPI and UE4 server still not being in sync?
Pardon the closing and opening, miss click 
I'll look into the frame rate issue and post when I find something.

When you say collisions with other vehicles do you mean they collide with the DReyeVR Ego vehicle or with each other and the surrounding environment?
Sorry, I should have elaborated more. I was referring to other vehicles colliding with the DreyeVR Ego vehicle, other than that their behavior is normal.
Ah I see, okay then this is not the issue I thought it was. I'll look into it today and try to reproduce & address the issue.
hey @RStorm98 after some debugging today I found a workaround that is still hacky so I don't want to push it to `main` yet. 

This seems to be happening because the other vehicles can't "see" the DReyeVR EgoVehicle, although it is being registered correctly, the other vehicles only see other actors currently with the `"vehicle.*"` tag, which is not the case for the DReyeVR EgoVehicle as of v0.1.2 (since we use `"harplab.dreyevr_vehicle.*"` to avoid users accidentally spawning additional `"vehicle.dreyevr_vehicle.*"` ego-vehicles if they use a vanilla Carla script that calls `world.get_actors().filter("vehicle.*")` as many of them do. 

However, if we rename ourselves to be under `"vehicle.*"` such as `"vehicle.dreyevr_vehicle.*"` then the other vehicles no longer ignore the EgoVehicle (as it should be). 

So, long story short. You'll need to do 2 (but really 3) things:
1. Go to [`DReyeVRFactory.cpp`](https://github.com/HARPLab/DReyeVR/blob/70a15b5b05b135682cc2daf8ddc4f469e9f9f05c/DReyeVR/DReyeVRFactory.cpp#L16) and change the CATEGORY to just `vehicle`: as follows `#define CATEGORY TEXT("vehicle")`
2. Go to [`DReyeVR_utils.py`](https://github.com/HARPLab/DReyeVR/blob/70a15b5b05b135682cc2daf8ddc4f469e9f9f05c/PythonAPI/examples/DReyeVR_utils.py#L25-L48) and change the filter commands from `"harplab.dreyevr_..."` to `"vehicle.dreyevr_vehicle.*"` and `"vehicle.dreyevr_sensor.*"` for the `find_ego_vehicle` and `find_ego_sensor` functions respectively. 
3. (technically optional, but now you need to be extra careful) In order to no longer spawn additional EgoVehicles in the world (since this is undefined behavior for us) you'll need to verify that all the blueprints you use do NOT contain the DReyeVR blueprints. See the bottom of [this issue](https://github.com/HARPLab/DReyeVR/issues/42#issuecomment-1183976066) for how that was resolved previously. You'll basically want to guarantee this by adding `blueprints = [bp for bp in blueprints if "dreyevr" not in bp.id]` every time you have a blueprints list that you sample from. 

While this isn't a great fix, I wanted to let you know the workaround ASAP so you could continue with your work. We'll work on fixing the issue properly soon. 
Thank you for the fast replies and the workaround, we'll test it out soon and report back. Much appreciated.

Fix patch:
diff --git a/CHANGELOG.md b/CHANGELOG.md
index c7e9d15..737d612 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,10 +1,14 @@
-## DReyeVR 0.1.2 (for Carla 0.9.13)
+## DReyeVR 0.1.3
+- Fix bug where other non-ego Autopilto vehicles ignored the EgoVehicle and would routinely crash into it
+- Fix bug where some EgoVehicle attributes were missing, notably `"number_of_wheels"`
+
+## DReyeVR 0.1.2
 - Update documentation to refer to CarlaUnreal UE4 fork rather than HarpLab fork
 - Apply patches for installation of zlib (broken link) and xerces (broken link & `-Dtranscoder=windows` flag) and PythonAPI
 - Fix crash on BeginPlay when EgoVehicle blueprint is already present in map (before world start)
 - Prefer building PythonAPI with `python` rather than Windows built-in `py -3` (good if using conda) but fallback if `python` is not available 
 
-## DReyeVR 0.1.1 (for Carla 0.9.13)
+## DReyeVR 0.1.1
 - Update documentation, add developer-centric in-depth documentation
 - Adding missing includes for TrafficSign RoadInfoSignal in SignComponent
 - Replacing `DReyeVRData.inl` with `DReyeVRData.cpp` and corresponding virtual classes
@@ -15,7 +19,7 @@
 - Fix bug with spectator being reenacted by Carla, preferring to use our own ASpectator
 - Automatically possess spectator pawn when EgoVehicle is destroyed. Can re-spawn EgoVehicle by pressing 1. 
 
-## DReyeVR 0.1.0 (for Carla 0.9.13)
+## DReyeVR 0.1.0
 - Replace existing `LevelScript` (`ADReyeVRLevel`) with `GameMode` (`ADReyeVRGameMode`). This allows us to not need to carry the (large) map blueprint files (ue4 binary) and we can use the vanilla Carla maps without modification. By default we spawn the EgoVehicle in the first of the recommended Carla locations, but this default behavior can be changed in the PythonAPI. For instance, you can delay spawning the EgoVehicle until via PythonAPI where you can specify the spawn transform. Existing functionality is preserved using `find_ego_vehicle` and `find_ego_sensor` which spawn the DReyeVR EgoVehicle if it does not exist in the world. 
 - Added `ADReyeVRFactory` as the Carla-esque spawning and registry functionality so the `EgoVehicle` and `EgoSensor` are spawned with the same "Factory" mechanisms as existing Carla vehicles/sensors/props/etc.
 - Renamed DReyeVR-specific actors to be addressible in PythonAPI as `"harplab.dreyevr_$X.$id"` such as `"harplab.dreyevr_vehicle.model3"` and `"harplab.dreyevr_sensor.ego_sensor"`. Avoids conflicts with existing Carla PythonAPI scripts that may filter on `"vehicle.*"`.
diff --git a/DReyeVR/DReyeVRFactory.cpp b/DReyeVR/DReyeVRFactory.cpp
index 740d296..ca60c8e 100644
--- a/DReyeVR/DReyeVRFactory.cpp
+++ b/DReyeVR/DReyeVRFactory.cpp
@@ -63,46 +63,28 @@ void ADReyeVRFactory::MakeVehicleDefinition(const FVehicleParameters &Parameters
     Definition = MakeGenericDefinition(CATEGORY, TEXT("DReyeVR_Vehicle"), Parameters.Model);
     Definition.Class = Parameters.Class;
 
-    FActorVariation ActorRole;
-    {
-        ActorRole.Id = TEXT("role_name");
-        ActorRole.Type = EActorAttributeType::String;
-        ActorRole.RecommendedValues = {TEXT("ego_vehicle")}; // assume this is the CARLA "hero"
-        ActorRole.bRestrictToRecommended = false;
-    }
-    Definition.Variations.Emplace(ActorRole);
-
-    FActorVariation StickyControl;
-    {
-        StickyControl.Id = TEXT("sticky_control");
-        StickyControl.Type = EActorAttributeType::Bool;
-        StickyControl.bRestrictToRecommended = false;
-        StickyControl.RecommendedValues.Emplace(TEXT("false"));
-    }
-    Definition.Variations.Emplace(StickyControl);
+    FActorAttribute ActorRole;
+    ActorRole.Id = "role_name";
+    ActorRole.Type = EActorAttributeType::String;
+    ActorRole.Value = "hero";
+    Definition.Attributes.Emplace(ActorRole);
 
     FActorAttribute ObjectType;
-    {
-        ObjectType.Id = TEXT("object_type");
-        ObjectType.Type = EActorAttributeType::String;
-        ObjectType.Value = Parameters.ObjectType;
-    }
+    ObjectType.Id = "object_type";
+    ObjectType.Type = EActorAttributeType::String;
+    ObjectType.Value = Parameters.ObjectType;
     Definition.Attributes.Emplace(ObjectType);
 
     FActorAttribute NumberOfWheels;
-    {
-        NumberOfWheels.Id = TEXT("number_of_wheels");
-        NumberOfWheels.Type = EActorAttributeType::Int;
-        NumberOfWheels.Value = FString::FromInt(Parameters.NumberOfWheels);
-    }
+    NumberOfWheels.Id = "number_of_wheels";
+    NumberOfWheels.Type = EActorAttributeType::Int;
+    NumberOfWheels.Value = FString::FromInt(Parameters.NumberOfWheels);
     Definition.Attributes.Emplace(NumberOfWheels);
 
     FActorAttribute Generation;
-    {
-        Generation.Id = TEXT("generation");
-        Generation.Type = EActorAttributeType::Int;
-        Generation.Value = FString::FromInt(Parameters.Generation);
-    }
+    Generation.Id = "generation";
+    Generation.Type = EActorAttributeType::Int;
+    Generation.Value = FString::FromInt(Parameters.Generation);
     Definition.Attributes.Emplace(Generation);
 }
 
diff --git a/DReyeVR/DReyeVRGameMode.cpp b/DReyeVR/DReyeVRGameMode.cpp
index 24af619..1da988c 100644
--- a/DReyeVR/DReyeVRGameMode.cpp
+++ b/DReyeVR/DReyeVRGameMode.cpp
@@ -563,12 +563,11 @@ void ADReyeVRGameMode::SpawnEgoVehicle(const FTransform &SpawnPt)
         DReyeVRDescr.UId = EgoVehicleDefn.UId;
         DReyeVRDescr.Id = EgoVehicleDefn.Id;
         DReyeVRDescr.Class = EgoVehicleDefn.Class;
-        // ensure this vehicle is denoted by the 'hero' attribute
-        FActorAttribute HeroRole;
-        HeroRole.Id = "role_name";
-        HeroRole.Type = EActorAttributeType::String;
-        HeroRole.Value = "hero";
-        DReyeVRDescr.Variations.Add(HeroRole.Id, std::move(HeroRole));
+        // add all the attributes from the definition to the description
+        for (FActorAttribute A : EgoVehicleDefn.Attributes)
+        {
+            DReyeVRDescr.Variations.Add(A.Id, std::move(A));
+        }
     }
     // calls Episode::SpawnActor => SpawnActorWithInfo => ActorDispatcher->SpawnActor => SpawnFunctions[UId]
     EgoVehiclePtr = static_cast<AEgoVehicle *>(Episode->SpawnActor(SpawnPt, DReyeVRDescr));
diff --git a/LibCarla/source/carla/trafficmanager/ALSM.cpp b/LibCarla/source/carla/trafficmanager/ALSM.cpp
new file mode 100644
index 0000000..eee90c2
--- /dev/null
+++ b/LibCarla/source/carla/trafficmanager/ALSM.cpp
@@ -0,0 +1,403 @@
+
+#include "boost/pointer_cast.hpp"
+
+#include "carla/client/Actor.h"
+#include "carla/client/Vehicle.h"
+#include "carla/client/Walker.h"
+
+#include "carla/trafficmanager/Constants.h"
+#include "carla/trafficmanager/LocalizationUtils.h"
+#include "carla/trafficmanager/SimpleWaypoint.h"
+
+#include "carla/trafficmanager/ALSM.h"
+
+namespace carla {
+namespace traffic_manager {
+
+ALSM::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)
+  : registered_vehicles(registered_vehicles),
+    buffer_map(buffer_map),
+    track_traffic(track_traffic),
+    marked_for_removal(marked_for_removal),
+    parameters(parameters),
+    world(world),
+    local_map(local_map),
+    simulation_state(simulation_state),
+    localization_stage(localization_stage),
+    collision_stage(collision_stage),
+    traffic_light_stage(traffic_light_stage),
+    motion_plan_stage(motion_plan_stage),
+    vehicle_light_stage(vehicle_light_stage),
+    random_devices(random_devices) {}
+
+void ALSM::Update() {
+
+  bool hybrid_physics_mode = parameters.GetHybridPhysicsMode();
+
+  std::set<ActorId> world_vehicle_ids;
+  std::set<ActorId> world_pedestrian_ids;
+  std::vector<ActorId> unregistered_list_to_be_deleted;
+
+  current_timestamp = world.GetSnapshot().GetTimestamp();
+  ActorList world_actors = world.GetActors();
+
+  // Find destroyed actors and perform clean up.
+  const ALSM::DestroyeddActors destroyed_actors = IdentifyDestroyedActors(world_actors);
+
+  const ActorIdSet &destroyed_registered = destroyed_actors.first;
+  for (const auto &deletion_id: destroyed_registered) {
+    RemoveActor(deletion_id, true);
+  }
+
+  const ActorIdSet &destroyed_unregistered = destroyed_actors.second;
+  for (auto deletion_id : destroyed_unregistered) {
+    RemoveActor(deletion_id, false);
+  }
+
+  // Invalidate hero actor if it is not alive anymore.
+  if (hero_actors.size() != 0u) {
+    ActorIdSet hero_actors_to_delete;
+    for (auto &hero_actor_info: hero_actors) {
+      if (destroyed_unregistered.find(hero_actor_info.first) != destroyed_unregistered.end()) {
+        hero_actors_to_delete.insert(hero_actor_info.first);
+      }
+      if (destroyed_registered.find(hero_actor_info.first) != destroyed_registered.end()) {
+        hero_actors_to_delete.insert(hero_actor_info.first);
+      }
+    }
+
+    for (auto &deletion_id: hero_actors_to_delete) {
+      hero_actors.erase(deletion_id);
+    }
+  }
+
+  // Scan for new unregistered actors.
+  IdentifyNewActors(world_actors);
+
+  // Update dynamic state and static attributes for all registered vehicles.
+  ALSM::IdleInfo max_idle_time = std::make_pair(0u, current_timestamp.elapsed_seconds);
+  UpdateRegisteredActorsData(hybrid_physics_mode, max_idle_time);
+
+  // Destroy registered vehicle if stuck at a location for too long.
+  if (IsVehicleStuck(max_idle_time.first)
+      && (current_timestamp.elapsed_seconds - elapsed_last_actor_destruction) > DELTA_TIME_BETWEEN_DESTRUCTIONS
+      && hero_actors.find(max_idle_time.first) == hero_actors.end()) {
+    registered_vehicles.Destroy(max_idle_time.first);
+    RemoveActor(max_idle_time.first, true);
+    elapsed_last_actor_destruction = current_timestamp.elapsed_seconds;
+  }
+
+  // Destorying vehicles for marked for removal by stages.
+  if (parameters.GetOSMMode()) {
+    for (const ActorId& actor_id: marked_for_removal) {
+      registered_vehicles.Destroy(actor_id);
+      RemoveActor(actor_id, true);
+    }
+    marked_for_removal.clear();
+  }
+
+  // Update dynamic state and static attributes for unregistered actors.
+  UpdateUnregisteredActorsData();
+}
+
+void ALSM::IdentifyNewActors(const ActorList &actor_list) {
+  for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
+    ActorPtr actor = *iter;
+    ActorId actor_id = actor->GetId();
+    // Identify any new hero vehicle
+    if (actor->GetTypeId().front() == 'v' || actor->GetTypeId().rfind("harplab.dreyevr_vehicle.") == 0) {
+     if (hero_actors.size() == 0u || hero_actors.find(actor_id) == hero_actors.end()) {
+      for (auto&& attribute: actor->GetAttributes()) {
+        if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") {
+          hero_actors.insert({actor_id, actor});
+        }
+      }
+    }
+  }
+    if (!registered_vehicles.Contains(actor_id)
+        && unregistered_actors.find(actor_id) == unregistered_actors.end()) {
+
+      unregistered_actors.insert({actor_id, actor});
+    }
+  }
+}
+
+ALSM::DestroyeddActors ALSM::IdentifyDestroyedActors(const ActorList &actor_list) {
+
+  ALSM::DestroyeddActors destroyed_actors;
+  ActorIdSet &deleted_registered = destroyed_actors.first;
+  ActorIdSet &deleted_unregistered = destroyed_actors.second;
+
+  // Building hash set of actors present in current frame.
+  ActorIdSet current_actors;
+  for  (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) {
+    current_actors.insert((*iter)->GetId());
+  }
+
+  // Searching for destroyed registered actors.
+  std::vector<ActorId> registered_ids = registered_vehicles.GetIDList();
+  for (const ActorId &actor_id : registered_ids) {
+    if (current_actors.find(actor_id) == current_actors.end()) {
+      deleted_registered.insert(actor_id);
+    }
+  }
+
+  // Searching for destroyed unregistered actors.
+  for (const auto &actor_info: unregistered_actors) {
+    const ActorId &actor_id = actor_info.first;
+     if (current_actors.find(actor_id) == current_actors.end()
+         || registered_vehicles.Contains(actor_id)) {
+      deleted_unregistered.insert(actor_id);
+    }
+  }
+
+  return destroyed_actors;
+}
+
+void ALSM::UpdateRegisteredActorsData(const bool hybrid_physics_mode, ALSM::IdleInfo &max_idle_time) {
+
+  std::vector<ActorPtr> vehicle_list = registered_vehicles.GetList();
+  bool hero_actor_present = hero_actors.size() != 0u;
+  float physics_radius = parameters.GetHybridPhysicsRadius();
+  float physics_radius_square = SQUARE(physics_radius);
+  bool is_respawn_vehicles = parameters.GetRespawnDormantVehicles();
+  if (is_respawn_vehicles && !hero_actor_present) {
+    track_traffic.SetHeroLocation(cg::Location(0,0,0));
+  }
+  // Update first the information regarding any hero vehicle.
+  for (auto &hero_actor_info: hero_actors){
+    if (is_respawn_vehicles) {
+      track_traffic.SetHeroLocation(hero_actor_info.second->GetTransform().location);
+    }
+    UpdateData(hybrid_physics_mode, max_idle_time, hero_actor_info.second, hero_actor_present, physics_radius_square);
+  }
+  // Update information for all other registered vehicles.
+  for (const Actor &vehicle : vehicle_list) {
+    ActorId actor_id = vehicle->GetId();
+    if (hero_actors.find(actor_id) == hero_actors.end()) {
+      UpdateData(hybrid_physics_mode, max_idle_time, vehicle, hero_actor_present, physics_radius_square);
+    }
+  }
+}
+
+void ALSM::UpdateData(const bool hybrid_physics_mode,
+                      ALSM::IdleInfo &max_idle_time, const Actor &vehicle,
+                      const bool hero_actor_present, const float physics_radius_square) {
+
+  ActorId actor_id = vehicle->GetId();
+  cg::Transform vehicle_transform = vehicle->GetTransform();
+  cg::Location vehicle_location = vehicle_transform.location;
+  cg::Rotation vehicle_rotation = vehicle_transform.rotation;
+  cg::Vector3D vehicle_velocity = vehicle->GetVelocity();
+
+  // Initializing idle times.
+  if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) {
+    idle_time.insert({actor_id, current_timestamp.elapsed_seconds});
+  }
+
+  // Check if current actor is in range of hero actor and enable physics in hybrid mode.
+  bool in_range_of_hero_actor = false;
+  if (hero_actor_present && hybrid_physics_mode) {
+    for (auto &hero_actor_info: hero_actors) {
+      const ActorId &hero_actor_id =  hero_actor_info.first;
+      if (simulation_state.ContainsActor(hero_actor_id)) {
+        const cg::Location &hero_location = simulation_state.GetLocation(hero_actor_id);
+        if (cg::Math::DistanceSquared(vehicle_location, hero_location) < physics_radius_square) {
+          in_range_of_hero_actor = true;
+          break;
+        }
+      }
+    }
+  }
+
+  bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true;
+  if (!has_physics_enabled.count(actor_id) || has_physics_enabled[actor_id] != enable_physics) {
+    if (hero_actors.find(actor_id) == hero_actors.end()) {
+      vehicle->SetSimulatePhysics(enable_physics);
+      has_physics_enabled[actor_id] = enable_physics;
+      if (enable_physics == true && simulation_state.ContainsActor(actor_id)) {
+        vehicle->SetTargetVelocity(simulation_state.GetVelocity(actor_id));
+      }
+    }
+  }
+
+  bool state_entry_present = simulation_state.ContainsActor(actor_id);
+  // If physics is disabled, calculate velocity based on change in position.
+  if (!enable_physics) {
+    cg::Location previous_location;
+    if (state_entry_present) {
+      previous_location = simulation_state.GetLocation(actor_id);
+    } else {
+      previous_location = vehicle_location;
+    }
+    cg::Vector3D displacement = (vehicle_location - previous_location);
+    vehicle_velocity = displacement * INV_HYBRID_DT;
+  }
+
+  // Updated kinematic state object.
+  auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(vehicle);
+  KinematicState kinematic_state{vehicle_location, vehicle_rotation,
+                                  vehicle_velocity, vehicle_ptr->GetSpeedLimit(),
+                                  enable_physics, vehicle->IsDormant()};
+
+  // Updated traffic light state object.
+  TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
+
+  // Update simulation state.
+  if (state_entry_present) {
+    simulation_state.UpdateKinematicState(actor_id, kinematic_state);
+    simulation_state.UpdateTrafficLightState(actor_id, tl_state);
+  }
+  else {
+    cg::Vector3D dimensions = vehicle_ptr->GetBoundingBox().extent;
+    StaticAttributes attributes{ActorType::Vehicle, dimensions.x, dimensions.y, dimensions.z};
+
+    simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
+  }
+
+  // Updating idle time when necessary.
+  UpdateIdleTime(max_idle_time, actor_id);
+}
+
+
+void ALSM::UpdateUnregisteredActorsData() {
+  for (auto &actor_info: unregistered_actors) {
+
+    const ActorId actor_id = actor_info.first;
+    const ActorPtr actor_ptr = actor_info.second;
+    const std::string type_id = actor_ptr->GetTypeId();
+
+    const cg::Transform actor_transform = actor_ptr->GetTransform();
+    const cg::Location actor_location = actor_transform.location;
+    const cg::Rotation actor_rotation = actor_transform.rotation;
+    const cg::Vector3D actor_velocity = actor_ptr->GetVelocity();
+    const bool actor_is_dormant = actor_ptr->IsDormant();
+    KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true, actor_is_dormant};
+
+    TrafficLightState tl_state;
+    ActorType actor_type = ActorType::Any;
+    cg::Vector3D dimensions;
+    std::vector<SimpleWaypointPtr> nearest_waypoints;
+
+    bool state_entry_not_present = !simulation_state.ContainsActor(actor_id);
+    if (type_id.front() == 'v' || type_id.rfind("harplab.dreyevr_vehicle.") == 0) { // include DReyeVR vehicle
+      auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(actor_ptr);
+      kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit();
+
+      tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
+
+      if (state_entry_not_present) {
+        dimensions = vehicle_ptr->GetBoundingBox().extent;
+        actor_type = ActorType::Vehicle;
+        StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
+
+        simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
+      } else {
+        simulation_state.UpdateKinematicState(actor_id, kinematic_state);
+        simulation_state.UpdateTrafficLightState(actor_id, tl_state);
+      }
+
+      // Identify occupied waypoints.
+      cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
+      cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector();
+      std::vector<cg::Location> corners = {actor_location + cg::Location(extent.x * heading_vector),
+                                           actor_location,
+                                           actor_location + cg::Location(-extent.x * heading_vector)};
+      for (cg::Location &vertex: corners) {
+        SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(vertex);
+        nearest_waypoints.push_back(nearest_waypoint);
+      }
+    }
+    else if (type_id.front() == 'w') {
+      auto walker_ptr = boost::static_pointer_cast<cc::Walker>(actor_ptr);
+
+      if (state_entry_not_present) {
+        dimensions = walker_ptr->GetBoundingBox().extent;
+        actor_type = ActorType::Pedestrian;
+        StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z};
+
+        simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state);
+      } else {
+        simulation_state.UpdateKinematicState(actor_id, kinematic_state);
+      }
+
+      // Identify occupied waypoints.
+      SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(actor_location);
+      nearest_waypoints.push_back(nearest_waypoint);
+    }
+
+    track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints);
+  }
+}
+
+void ALSM::UpdateIdleTime(std::pair<ActorId, double>& max_idle_time, const ActorId& actor_id) {
+  if (idle_time.find(actor_id) != idle_time.end()) {
+    double &idle_duration = idle_time.at(actor_id);
+    if (simulation_state.GetVelocity(actor_id).SquaredLength() > SQUARE(STOPPED_VELOCITY_THRESHOLD)) {
+      idle_duration = current_timestamp.elapsed_seconds;
+    }
+
+    // Checking maximum idle time.
+    if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) {
+      max_idle_time = std::make_pair(actor_id, idle_duration);
+    }
+  }
+}
+
+bool ALSM::IsVehicleStuck(const ActorId& actor_id) {
+  if (idle_time.find(actor_id) != idle_time.end()) {
+    double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id);
+    TrafficLightState tl_state = simulation_state.GetTLS(actor_id);
+    if ((!tl_state.at_traffic_light && tl_state.tl_state != TLS::Red && delta_idle_time >= BLOCKED_TIME_THRESHOLD)
+    || (delta_idle_time >= RED_TL_BLOCKED_TIME_THRESHOLD)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
+  if (registered_actor) {
+    registered_vehicles.Remove({actor_id});
+    buffer_map.erase(actor_id);
+    idle_time.erase(actor_id);
+    random_devices.erase(actor_id);
+    localization_stage.RemoveActor(actor_id);
+    collision_stage.RemoveActor(actor_id);
+    traffic_light_stage.RemoveActor(actor_id);
+    motion_plan_stage.RemoveActor(actor_id);
+    vehicle_light_stage.RemoveActor(actor_id);
+  }
+  else {
+    unregistered_actors.erase(actor_id);
+    hero_actors.erase(actor_id);
+  }
+
+  track_traffic.DeleteActor(actor_id);
+  simulation_state.RemoveActor(actor_id);
+}
+
+void ALSM::Reset() {
+  unregistered_actors.clear();
+  idle_time.clear();
+  hero_actors.clear();
+  elapsed_last_actor_destruction = 0.0;
+  current_timestamp = world.GetSnapshot().GetTimestamp();
+}
+
+} // namespace traffic_manager
+} // namespace carla


Test patch: