diff --git a/CHANGELOG.md b/CHANGELOG.md index ecbe8c634..b846e9ac2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,7 +19,7 @@ * Fixed bug causing the scene lights to return an incorrect location at large maps. * Fixed bug causing the `world.ground_projection()` function to return an incorrect location at large maps. * Added failure state to vehicles, which can be retrieved by using `Vehicle.get_failure_state()`. Only Rollover failure state is currently supported. - * Fixed bug causing the TM to block the simulation when another client teleported a vehicle with no physics. + * Fixed bug causing the TM to block the simulation when a vehicle with no physics was teleported. * Fixed bug causing the TM to block the simulation when travelling through a short roads that looped on themselves. * Improved the TM's handling of non signalized junctions, resulting in a more fluid overall behavior. * Added check to avoid adding procedural trigger boxes inside intersections. diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp index 83aa528ba..31b694cd3 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -68,6 +68,7 @@ void MotionPlanStage::Update(const unsigned long index) { const float vehicle_speed = vehicle_velocity.Length(); const cg::Vector3D vehicle_heading = simulation_state.GetHeading(actor_id); const bool vehicle_physics_enabled = simulation_state.IsPhysicsEnabled(actor_id); + const float vehicle_speed_limit = simulation_state.GetSpeedLimit(actor_id); const Buffer &waypoint_buffer = buffer_map.at(actor_id); const LocalizationData &localization = localization_frame.at(index); const CollisionHazardData &collision_hazard = collision_frame.at(index); @@ -117,12 +118,19 @@ void MotionPlanStage::Update(const unsigned long index) { } } output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform); + + // Update the simulation state with the new transform of the vehicle after teleporting it. + KinematicState kinematic_state{teleportation_transform.location, + teleportation_transform.rotation, + vehicle_velocity, vehicle_speed_limit, + vehicle_physics_enabled, simulation_state.IsDormant(actor_id), + teleportation_transform.location}; + simulation_state.UpdateKinematicState(actor_id, kinematic_state); } else { // Target velocity for vehicle. - const float vehicle_speed_limit = simulation_state.GetSpeedLimit(actor_id); float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, vehicle_speed_limit) / 3.6f; // Algorithm to reduce speed near landmarks