Fixed bug at TM when teleporting vehicles with no physics

CHANGELOG.md

@@ -1,5 +1,6 @@
 ## Latest
 
+  * Fixed bug causing the TM to block the simulation when another client teleported a vehicle with no physics.
   * Added check to avoid adding procedural trigger boxes inside intersections.
   * Python agents now accept a carla.Map and GlobalRoutePlanner instances as inputs, avoiding the need to recompute them.
   * Fix a bug at `Map.get_topology()`, causing lanes with no successors to not be part of it.

LibCarla/source/carla/trafficmanager/ALSM.cpp

@@ -201,6 +201,7 @@ void ALSM::UpdateData(const bool hybrid_physics_mode,
   cg::Location vehicle_location = vehicle_transform.location;
   cg::Rotation vehicle_rotation = vehicle_transform.rotation;
   cg::Vector3D vehicle_velocity = vehicle->GetVelocity();
+  bool state_entry_present = simulation_state.ContainsActor(actor_id);
 
   // Initializing idle times.
   if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) {
@@ -227,22 +228,19 @@ void ALSM::UpdateData(const bool hybrid_physics_mode,
     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)) {
+      if (enable_physics == true && state_entry_present) {
         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);
+  // If physics are disabled, calculate velocity based on change in position.
+  // Do not use 'enable_physics' as turning off the physics in this tick doesn't remove the velocity.
+  // To avoid issues with other clients teleporting the actors, use the previous outpout location.
+  if (state_entry_present && !simulation_state.IsPhysicsEnabled(actor_id)){
+    cg::Location previous_location = simulation_state.GetLocation(actor_id);
+    cg::Location previous_end_location = simulation_state.GetHybridEndLocation(actor_id);
+    cg::Vector3D displacement = (previous_end_location - previous_location);
     vehicle_velocity = displacement * INV_HYBRID_DT;
   }
 
@@ -250,7 +248,7 @@ void ALSM::UpdateData(const bool hybrid_physics_mode,
   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()};
+                                  enable_physics, vehicle->IsDormant(), cg::Location()};
 
   // Updated traffic light state object.
   TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()};
@@ -284,7 +282,7 @@ void ALSM::UpdateUnregisteredActorsData() {
     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};
+    KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true, actor_is_dormant, cg::Location()};
 
     TrafficLightState tl_state;
     ActorType actor_type = ActorType::Any;

LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp

@@ -28,7 +28,7 @@ using constants::Collision::EPSILON;
 
 MotionPlanStage::MotionPlanStage(
   const std::vector<ActorId> &vehicle_id_list,
-  const SimulationState &simulation_state,
+  SimulationState &simulation_state,
   const Parameters &parameters,
   const BufferMap &buffer_map,
   TrackTraffic &track_traffic,
@@ -257,6 +257,7 @@ void MotionPlanStage::Update(const unsigned long index) {
       }
       // Constructing the actuation signal.
       output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
+      simulation_state.UpdateKinematicHybridEndLocation(actor_id, teleportation_transform.location);
     }
   }
 }

LibCarla/source/carla/trafficmanager/MotionPlanStage.h

@@ -22,7 +22,7 @@ using TLMap = std::unordered_map<std::string, SharedPtr<client::Actor>>;
 class MotionPlanStage: Stage {
 private:
   const std::vector<ActorId> &vehicle_id_list;
-  const SimulationState &simulation_state;
+  SimulationState &simulation_state;
   const Parameters &parameters;
   const BufferMap &buffer_map;
   TrackTraffic &track_traffic;
@@ -70,7 +70,7 @@ private:
 
 public:
   MotionPlanStage(const std::vector<ActorId> &vehicle_id_list,
-                  const SimulationState &simulation_state,
+                  SimulationState &simulation_state,
                   const Parameters &parameters,
                   const BufferMap &buffer_map,
                   TrackTraffic &track_traffic,

LibCarla/source/carla/trafficmanager/SimulationState.cpp

@@ -38,6 +38,10 @@ void SimulationState::UpdateKinematicState(ActorId actor_id, KinematicState stat
   kinematic_state_map.at(actor_id) = state;
 }
 
+void SimulationState::UpdateKinematicHybridEndLocation(ActorId actor_id, cg::Location location) {
+  kinematic_state_map.at(actor_id).hybrid_end_location = location;
+}
+
 void SimulationState::UpdateTrafficLightState(ActorId actor_id, TrafficLightState state) {
   tl_state_map.at(actor_id) = state;
 }
@@ -46,6 +50,10 @@ cg::Location SimulationState::GetLocation(ActorId actor_id) const {
   return kinematic_state_map.at(actor_id).location;
 }
 
+cg::Location SimulationState::GetHybridEndLocation(ActorId actor_id) const {
+  return kinematic_state_map.at(actor_id).hybrid_end_location;
+}
+
 cg::Rotation SimulationState::GetRotation(ActorId actor_id) const {
   return kinematic_state_map.at(actor_id).rotation;
 }

LibCarla/source/carla/trafficmanager/SimulationState.h

@@ -21,6 +21,7 @@ struct KinematicState {
   float speed_limit;
   bool physics_enabled;
   bool is_dormant;
+  cg::Location hybrid_end_location;
 };
 using KinematicStateMap = std::unordered_map<ActorId, KinematicState>;
 
@@ -71,10 +72,14 @@ public :
 
   void UpdateKinematicState(ActorId actor_id, KinematicState state);
 
+  void UpdateKinematicHybridEndLocation(ActorId actor_id, cg::Location location);
+
   void UpdateTrafficLightState(ActorId actor_id, TrafficLightState state);
 
   cg::Location GetLocation(const ActorId actor_id) const;
 
+  cg::Location GetHybridEndLocation(const ActorId actor_id) const;
+
   cg::Rotation GetRotation(const ActorId actor_id) const;
 
   cg::Vector3D GetHeading(const ActorId actor_id) const;