diff --git a/CHANGELOG.md b/CHANGELOG.md index 553bf1e5f..15f7fb589 100644 --- a/CHANGELOG.md +++ b/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. diff --git a/LibCarla/source/carla/trafficmanager/ALSM.cpp b/LibCarla/source/carla/trafficmanager/ALSM.cpp index c9066f39a..3e7c05ae2 100644 --- a/LibCarla/source/carla/trafficmanager/ALSM.cpp +++ b/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(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; diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp index e30943c56..59dc902e2 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -28,7 +28,7 @@ using constants::Collision::EPSILON; MotionPlanStage::MotionPlanStage( const std::vector &vehicle_id_list, - const SimulationState &simulation_state, + SimulationState &simulation_state, const Parameters ¶meters, 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); } } } diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.h b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h index 3309ab079..84b753f50 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h @@ -22,7 +22,7 @@ using TLMap = std::unordered_map>; class MotionPlanStage: Stage { private: const std::vector &vehicle_id_list; - const SimulationState &simulation_state; + SimulationState &simulation_state; const Parameters ¶meters; const BufferMap &buffer_map; TrackTraffic &track_traffic; @@ -70,7 +70,7 @@ private: public: MotionPlanStage(const std::vector &vehicle_id_list, - const SimulationState &simulation_state, + SimulationState &simulation_state, const Parameters ¶meters, const BufferMap &buffer_map, TrackTraffic &track_traffic, diff --git a/LibCarla/source/carla/trafficmanager/SimulationState.cpp b/LibCarla/source/carla/trafficmanager/SimulationState.cpp index eda6ef555..cec6a2e3f 100644 --- a/LibCarla/source/carla/trafficmanager/SimulationState.cpp +++ b/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; } diff --git a/LibCarla/source/carla/trafficmanager/SimulationState.h b/LibCarla/source/carla/trafficmanager/SimulationState.h index e97844648..e0b1eb82c 100644 --- a/LibCarla/source/carla/trafficmanager/SimulationState.h +++ b/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; @@ -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;