Hybrid mode for Traffic Manager (#2674)

* Improved braking, collision negotiation.

* Improved braking algorithm for smoother approach
to lead vehicle.

* Implemented smoother path boundary modification
to aid smoother braking.

* Re-worked collision negotiation algorithm.

* Improved collision candidate filtering.

* Added safe-guard in case of vehicle removal
in collision stage.

* Used local variable for heavily referenced object
in localization stage.

* Implemented vector relative velocities
for motion planner's collision consideration.

* Moved collision candidate sorting logic
from collision stage to localization stage.

* Sorting collision candidates using their ids
instead of shared pointers to avoid memory corruption.

* Improved conditions for collision consideration
for greater efficiency.

* Removed fps limit in async mode.

* Hybrid physics mode

* Introduced hybrid physics mode parameter
* Implemented physics independent velocity computation
* Modified localization stage to be physics agnostic

* Fixing velocity compute interval in sync and async mode.
Made motion planner stage work with internally computed velocities.

* Made collision stage agnostic to actor physics

* Sampling waypoint buffer for teleportation window

* WIP: Teleportation changes

* WIP2: Teleportation changes

* Fixes waypoint window and vehicle spawning

* hotfix to performance benchmark

* comment out debugs

* changelog

* fixes collision bug

* fixes package error and out_of_range bug

* changes after review

* Left & Right Transit of a lane: Waypoint mapping

Co-authored-by: Praveen Kumar <pravinblaze@hotmail.com>
Co-authored-by: Soumyadeep <soumyadeep.dhar@kpit.com>

CHANGELOG.md

@@ -1,5 +1,6 @@
 ## Latest
 
+  * Introduced hybrid mode for Traffic Manager
   * Upgraded to Unreal Engine 4.24
   * Fixed autonomous agents' incorrect detection of red traffic lights affecting them
   * Added walkable pedestrian crosswalks in OpenDRIVE standalone mode

LibCarla/source/carla/trafficmanager/BatchControlStage.cpp

@@ -38,11 +38,18 @@ void BatchControlStage::Action() {
     }
     const carla::ActorId actor_id = element.actor->GetId();
 
-    vehicle_control.throttle = element.throttle;
+    // Apply actuation from controller if physics enabled.
-    vehicle_control.brake = element.brake;
+    if (element.physics_enabled) {
-    vehicle_control.steer = element.steer;
+      vehicle_control.throttle = element.throttle;
+      vehicle_control.brake = element.brake;
+      vehicle_control.steer = element.steer;
 
-    commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
+      commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
+    }
+    // Apply target transform for physics-less vehicles.
+    else {
+      commands->at(i) = carla::rpc::Command::ApplyTransform(actor_id, element.transform);
+    }
   }
 }
 

LibCarla/source/carla/trafficmanager/CollisionStage.cpp

@@ -72,14 +72,16 @@ namespace CollisionStageConstants {
       const Actor ego_actor = data.actor;
       const ActorId ego_actor_id = ego_actor->GetId();
       const cg::Location ego_location = ego_actor->GetLocation();
+      const cg::Vector3D ego_velocity = data.velocity;
 
       const SimpleWaypointPtr& closest_point = data.closest_waypoint;
       const SimpleWaypointPtr& junction_look_ahead = data.junction_look_ahead_waypoint;
-      const std::vector<std::pair<ActorId, Actor>>& collision_candidates = data.overlapping_actors;
+      using OverlappingActorInfo = std::vector<std::tuple<ActorId, Actor, cg::Vector3D>>;
+      const OverlappingActorInfo &collision_candidates = data.overlapping_actors;
 
       bool collision_hazard = false;
       float available_distance_margin = std::numeric_limits<float>::infinity();
-      cg::Vector3D other_vehicle_velocity;
+      cg::Vector3D obstacle_velocity;
       const SimpleWaypointPtr safe_point_junction = data.safe_point_after_junction;
 
       try {
@@ -88,14 +90,17 @@ namespace CollisionStageConstants {
             actor_info != collision_candidates.end() && !collision_hazard;
             ++actor_info) {
 
-          const ActorId other_actor_id = actor_info->first;
+          ActorId other_actor_id;
-          const Actor other_actor = actor_info->second;
+          Actor other_actor;
+          cg::Vector3D other_velocity;
+          std::tie(other_actor_id, other_actor, other_velocity) = *actor_info;
+
           if (!other_actor->IsAlive()) continue;
           const auto other_actor_type = other_actor->GetTypeId();
           const cg::Location other_location = other_actor->GetLocation();
 
           // Collision checks increase with speed
-          float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f);
+          float collision_distance = std::pow(floor(ego_velocity.Length()*3.6f/10.0f),2.0f);
           collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS);
 
           // Temporary fix to (0,0,0) bug
@@ -109,19 +114,20 @@ namespace CollisionStageConstants {
               if (parameters.GetCollisionDetection(ego_actor, other_actor)) {
 
                 std::pair<bool, float> negotiation_result = NegotiateCollision(ego_actor, other_actor, ego_location,
-                                                                              other_location, closest_point, junction_look_ahead);
+                                                                              other_location, closest_point, junction_look_ahead,
+                                                                              ego_velocity, other_velocity);
                 if ((safe_point_junction != nullptr
-                    && !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction, other_location))
+                    && !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction,
+                                                    other_location, other_velocity))
                     || negotiation_result.first)
                 {
 
                   if ((other_actor_type[0] == 'v' && parameters.GetPercentageIgnoreVehicles(ego_actor) <= (rand() % 101)) ||
                       (other_actor_type[0] == 'w' && parameters.GetPercentageIgnoreWalkers(ego_actor) <= (rand() % 101)))
                   {
-
                     collision_hazard = true;
                     available_distance_margin = negotiation_result.second;
-                    other_vehicle_velocity = other_actor->GetVelocity();
+                    obstacle_velocity = other_velocity;
                   }
                 }
               }
@@ -136,7 +142,7 @@ namespace CollisionStageConstants {
       CollisionToPlannerData &message = current_planner_frame->at(i);
       message.hazard = collision_hazard;
       message.distance_to_other_vehicle = available_distance_margin;
-      message.other_vehicle_velocity = other_vehicle_velocity;
+      message.other_vehicle_velocity = obstacle_velocity;
     }
   }
 
@@ -185,12 +191,19 @@ namespace CollisionStageConstants {
                                                             const cg::Location &reference_location,
                                                             const cg::Location &other_location,
                                                             const SimpleWaypointPtr &closest_point,
-                                                            const SimpleWaypointPtr &junction_look_ahead) {
+                                                            const SimpleWaypointPtr &junction_look_ahead,
+                                                            const cg::Vector3D reference_velocity,
+                                                            const cg::Vector3D other_velocity) {
+
+    // Vehicle IDs
+    const ActorId reference_vehicle_id = reference_vehicle->GetId();
+    const ActorId other_vehicle_id = other_vehicle->GetId();
+
     // Output variables for the method.
     bool hazard = false;
     float available_distance_margin = std::numeric_limits<float>::infinity();
 
-    // Ego vehicle heading.
+    // Ego and other vehicle heading.
     const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector();
     // Vector from ego position to position of the other vehicle.
     cg::Vector3D reference_to_other = other_location - reference_location;
@@ -209,8 +222,9 @@ namespace CollisionStageConstants {
     float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * SQUARE_ROOT_OF_TWO;
 
     float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location);
-    float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle);
+    float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id,
-    float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle);
+                                                               reference_velocity, reference_heading);
+    float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle_id, other_velocity, other_heading);
     // Calculate minimum distance between vehicle to consider collision negotiation.
     float inter_vehicle_length = reference_vehicle_length + other_vehicle_length;
     float ego_detection_range = std::pow(ego_bounding_box_extension + inter_vehicle_length, 2.0f);
@@ -226,14 +240,14 @@ namespace CollisionStageConstants {
     bool ego_stopped_by_light = reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green;
     bool ego_at_junction_entrance = !closest_point->CheckJunction() && junction_look_ahead->CheckJunction();
 
-    const ActorId reference_vehicle_id = reference_vehicle->GetId();
     // Conditions to consider collision negotiation.
     if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light)
         && ((ego_inside_junction && other_vehicles_in_cross_detection_range)
             || (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) {
 
       GeometryComparisonCache cache = GetGeometryBetweenActors(reference_vehicle, other_vehicle,
-                                                               reference_location, other_location);
+                                                               reference_location, other_location,
+                                                               reference_velocity, other_velocity);
 
       // Conditions for collision negotiation.
       bool geodesic_path_bbox_touching = cache.inter_geodesic_distance < 0.1;
@@ -256,7 +270,6 @@ namespace CollisionStageConstants {
         available_distance_margin = static_cast<float>(MAX(cache.reference_vehicle_to_other_geodesic
                                                                  - specific_distance_margin, 0.0));
 
-        ActorId other_vehicle_id = other_vehicle->GetId();
         ///////////////////////////////////// Collision locking mechanism /////////////////////////////////
         // The idea is, when encountering a possible collision,
         // we should ensure that the bounding box extension doesn't decrease too fast and loose collision tracking.
@@ -306,23 +319,27 @@ namespace CollisionStageConstants {
     return boundary_polygon;
   }
 
-  LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location) {
+  LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location,
-    if (geodesic_boundaries.find(actor->GetId()) != geodesic_boundaries.end()) {
+                                                   const cg::Vector3D velocity) {
-      return geodesic_boundaries.at(actor->GetId());
+
+    const ActorId actor_id = actor->GetId();
+
+    if (geodesic_boundaries.find(actor_id) != geodesic_boundaries.end()) {
+      return geodesic_boundaries.at(actor_id);
     }
 
-    const LocationList bbox = GetBoundary(actor, vehicle_location);
+    const LocationList bbox = GetBoundary(actor, vehicle_location, velocity);
 
-    if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) {
+    if (vehicle_id_to_index.find(actor_id) != vehicle_id_to_index.end()) {
 
-      float bbox_extension = GetBoundingBoxExtention(actor);
+      float bbox_extension = GetBoundingBoxExtention(actor_id, velocity, actor->GetTransform().GetForwardVector());
 
       const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor);
       if (specific_distance_margin > 0.0f) {
         bbox_extension = MAX(specific_distance_margin, bbox_extension);
       }
 
-      const auto &waypoint_buffer =  localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer;
+      const auto &waypoint_buffer =  localization_frame->at(vehicle_id_to_index.at(actor_id)).buffer;
 
       LocationList left_boundary;
       LocationList right_boundary;
@@ -393,17 +410,17 @@ namespace CollisionStageConstants {
 
   }
 
-  float CollisionStage::GetBoundingBoxExtention(const Actor &actor) {
+  float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id,
+                                                const cg::Vector3D velocity_vector,
+                                                const cg::Vector3D heading_vector) {
 
-    // Calculate velocity along vehicle's heading. This also avoids calculating square root.
+    const float velocity = cg::Math::Dot(velocity_vector, heading_vector);
-    const float velocity = cg::Math::Dot(actor->GetVelocity(), actor->GetTransform().GetForwardVector());
     float bbox_extension;
     // Using a linear function to calculate boundary length.
     // min speed : 0kmph -> BOUNDARY_EXTENSION_MINIMUM
     // max speed : 100kmph -> BOUNDARY_EXTENSION_MAXIMUM
     float slope = (BOUNDARY_EXTENSION_MAXIMUM - BOUNDARY_EXTENSION_MINIMUM) / ARBITRARY_MAX_SPEED;
     bbox_extension = slope * velocity + BOUNDARY_EXTENSION_MINIMUM;
-    const ActorId actor_id = actor->GetId();
     // If a valid collision lock present, change boundary length to maintain lock.
     if (collision_locks.find(actor_id) != collision_locks.end())
     {
@@ -421,7 +438,9 @@ namespace CollisionStageConstants {
     return bbox_extension;
   }
 
-  LocationList CollisionStage::GetBoundary(const Actor &actor, const cg::Location &location) {
+  LocationList CollisionStage::GetBoundary(const Actor &actor,
+                                           const cg::Location &location,
+                                           const cg::Vector3D velocity) {
 
     const auto actor_type = actor->GetTypeId();
     cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
@@ -437,7 +456,7 @@ namespace CollisionStageConstants {
       const auto walker = boost::static_pointer_cast<cc::Walker>(actor);
       bbox = walker->GetBoundingBox();
       // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
-      forward_extension = walker->GetVelocity().Length() * WALKER_TIME_EXTENSION;
+      forward_extension = velocity.Length() * WALKER_TIME_EXTENSION;
     }
 
     const cg::Vector3D extent = bbox.extent;
@@ -461,11 +480,12 @@ namespace CollisionStageConstants {
   bool CollisionStage::IsLocationAfterJunctionSafe(const Actor &ego_actor,
                                                    const Actor &other_actor,
                                                    const SimpleWaypointPtr safe_point,
-                                                   const cg::Location &other_location) {
+                                                   const cg::Location &other_location,
+                                                   const cg::Vector3D other_velocity){
 
     bool safe_junction = true;
 
-    if (other_actor->GetVelocity().Length() < EPSILON_VELOCITY){
+    if (other_velocity.Length() < EPSILON_VELOCITY){
 
       cg::Location safe_location = safe_point->GetLocation();
       cg::Vector3D heading_vector = safe_point->GetForwardVector();
@@ -490,7 +510,7 @@ namespace CollisionStageConstants {
       };
 
       const Polygon reference_polygon = GetPolygon(ego_actor_boundary);
-      const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location));
+      const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location, other_velocity));
 
       const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
       if (inter_bbox_distance < INTER_BBOX_DISTANCE_THRESHOLD){
@@ -511,7 +531,9 @@ namespace CollisionStageConstants {
   }
 
 GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle,
-                                     const cg::Location &reference_location, const cg::Location &other_location) {
+                                     const cg::Location &reference_location, const cg::Location &other_location,
+                                     const cg::Vector3D reference_velocity, const cg::Vector3D other_velocity) {
+
    std::string actor_id_key = (reference_vehicle->GetId() < other_vehicle->GetId()) ?
                               std::to_string(reference_vehicle->GetId()) + "|" + std::to_string(other_vehicle->GetId())
                               : std::to_string(other_vehicle->GetId()) +"|"+ std::to_string(reference_vehicle->GetId());
@@ -525,10 +547,10 @@ GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &r
     return mCache;
    }
 
-  const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location));
+  const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location, reference_velocity));
-  const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location));
+  const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location, other_velocity));
-  const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location));
+  const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location, reference_velocity));
-  const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location));
+  const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location, other_velocity));
 
   const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon);
   const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon);

LibCarla/source/carla/trafficmanager/CollisionStage.h

@@ -11,6 +11,7 @@
 #include <deque>
 #include <stdlib.h>
 #include <string>
+#include <tuple>
 #include <unordered_map>
 #include <vector>
 
@@ -105,37 +106,43 @@ private:
   /// Snippet profiler for measuring execution time.
   SnippetProfiler snippet_profiler;
 
-  /// Returns the bounding box corners of the vehicle passed to the method.
+    /// Returns the bounding box corners of the vehicle passed to the method.
-  LocationList GetBoundary(const Actor &actor, const cg::Location &location);
+    LocationList GetBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity);
 
-  /// Returns the extrapolated bounding box of the vehicle along its
+    /// Returns the extrapolated bounding box of the vehicle along its
-  /// trajectory.
+    /// trajectory.
-  LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location);
+    LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity);
 
-  /// Method to construct a boost polygon object.
+    /// Method to construct a boost polygon object.
-  Polygon GetPolygon(const LocationList &boundary);
+    Polygon GetPolygon(const LocationList &boundary);
 
-  /// The method returns true if ego_vehicle should stop and wait for
+    /// The method returns true if ego_vehicle should stop and wait for
-  /// other_vehicle to pass.
+    /// other_vehicle to pass.
     std::pair<bool, float> NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle,
                                               const cg::Location &reference_location,
                                               const cg::Location &other_location,
                                               const SimpleWaypointPtr& closest_point,
-                                              const SimpleWaypointPtr& junction_look_ahead);
+                                              const SimpleWaypointPtr& junction_look_ahead,
+                                              const cg::Vector3D reference_velocity,
+                                              const cg::Vector3D other_velocity);
 
-  /// Method to calculate the speed dependent bounding box extention for a vehicle.
+    /// Method to calculate the speed dependent bounding box extention for a vehicle.
-  float GetBoundingBoxExtention(const Actor &ego_vehicle);
+    float GetBoundingBoxExtention(const ActorId actor_id,
+                                  const cg::Vector3D velocity_vector,
+                                  const cg::Vector3D heading_vector);
 
-  /// At intersections, used to see if there is space after the junction
+    /// At intersections, used to see if there is space after the junction
-  bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor,
+    bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor,
-                                   const SimpleWaypointPtr safe_point, const cg::Location &other_location);
+                                     const SimpleWaypointPtr safe_point, const cg::Location &other_location,
+                                     const cg::Vector3D other_velocity);
 
   /// A simple method used to draw bounding boxes around vehicles
   void DrawBoundary(const LocationList &boundary);
 
   /// Method to compute Geometry result between two vehicles
   GeometryComparisonCache GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle,
-                                                   const cg::Location &reference_location, const cg::Location &other_location);
+                                                   const cg::Location &reference_location, const cg::Location &other_location,
+                                                   const cg::Vector3D reference_velocity,const cg::Vector3D other_velocity);
 
 public:
 

LibCarla/source/carla/trafficmanager/InMemoryMap.cpp

@@ -339,30 +339,14 @@ namespace MapConstants {
 
     const WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
     const crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange();
-    const auto change_right = crd::element::LaneMarking::LaneChange::Right;
-    const auto change_left = crd::element::LaneMarking::LaneChange::Left;
-    const auto change_both = crd::element::LaneMarking::LaneChange::Both;
 
-    try {
+    /// Cheack for transits
-      if (lane_change == change_right || lane_change == change_both) {
+    switch(lane_change)
-
+    {
-        const WaypointPtr right_waypoint =  raw_waypoint->GetRight();
+      /// Left transit way point present only
-        if (right_waypoint != nullptr &&
+      case crd::element::LaneMarking::LaneChange::Left:
-        right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
+      {
-        (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
+        const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
-
-          SimpleWaypointPtr closest_simple_waypoint =
-          GetWaypointInVicinity(right_waypoint->GetTransform().location);
-          if (closest_simple_waypoint == nullptr) {
-            closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location);
-          }
-          reference_waypoint->SetRightWaypoint(closest_simple_waypoint);
-        }
-      }
-
-      if (lane_change == change_left || lane_change == change_both) {
-
-        const WaypointPtr left_waypoint =  raw_waypoint->GetLeft();
         if (left_waypoint != nullptr &&
         left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
         (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
@@ -375,13 +359,61 @@ namespace MapConstants {
           reference_waypoint->SetLeftWaypoint(closest_simple_waypoint);
         }
       }
-    } catch (const std::invalid_argument &e) {
+      break;
-      cg::Location loc = reference_waypoint->GetLocation();
+
-      carla::log_info(
+      /// Right transit way point present only
-      "Unable to link lane change connection at: " +
+      case crd::element::LaneMarking::LaneChange::Right:
-      std::to_string(loc.x) + " " +
+      {
-      std::to_string(loc.y) + " " +
+	    const WaypointPtr right_waypoint = raw_waypoint->GetRight();
-      std::to_string(loc.z));
+	    if(right_waypoint != nullptr &&
+	    right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
+	    (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
+
+	      SimpleWaypointPtr closest_simple_waypoint =
+	      GetWaypointInVicinity(right_waypoint->GetTransform().location);
+	      if (closest_simple_waypoint == nullptr) {
+		    closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location);
+	      }
+	      reference_waypoint->SetRightWaypoint(closest_simple_waypoint);
+	    }
+      }
+      break;
+
+      /// Both left and right transit present
+      case crd::element::LaneMarking::LaneChange::Both:
+      {
+        /// Right transit way point
+        const WaypointPtr right_waypoint = raw_waypoint->GetRight();
+        if (right_waypoint != nullptr &&
+        right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
+        (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
+
+          SimpleWaypointPtr closest_simple_waypointR =
+          GetWaypointInVicinity(right_waypoint->GetTransform().location);
+          if (closest_simple_waypointR == nullptr) {
+            closest_simple_waypointR = GetWaypoint(right_waypoint->GetTransform().location);
+          }
+          reference_waypoint->SetRightWaypoint(closest_simple_waypointR);
+        }
+
+        /// Left transit way point
+        const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
+        if (left_waypoint != nullptr &&
+        left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
+        (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
+
+          SimpleWaypointPtr closest_simple_waypointL =
+          GetWaypointInVicinity(left_waypoint->GetTransform().location);
+          if (closest_simple_waypointL == nullptr) {
+            closest_simple_waypointL = GetWaypoint(left_waypoint->GetTransform().location);
+          }
+          reference_waypoint->SetLeftWaypoint(closest_simple_waypointL);
+        }
+      }
+      break;
+
+      /// For no transit waypoint (left or right)
+      default: break;
     }
   }
 

LibCarla/source/carla/trafficmanager/LocalizationStage.cpp

@@ -27,7 +27,9 @@ namespace LocalizationConstants {
   static const float STOPPED_VELOCITY_THRESHOLD = 0.8f;  // meters per second.
   static const float INTER_LANE_CHANGE_DISTANCE = 10.0f;
   static const float MAX_COLLISION_RADIUS = 100.0f;
-
+  static const float PHYSICS_RADIUS = 50.0f;
+  static const float POSITION_WINDOW_SIZE = 2.1f;
+  static const float HYBRID_MODE_DT = 0.05f;
 } // namespace LocalizationConstants
 
   using namespace LocalizationConstants;
@@ -66,7 +68,8 @@ namespace LocalizationConstants {
     maximum_idle_time = std::make_pair(nullptr, 0.0);
     // Initializing srand.
     srand(static_cast<unsigned>(time(NULL)));
-
+    // Initializing kinematic update clock.
+    previous_update_instance = chr::system_clock::now();
   }
 
   LocalizationStage::~LocalizationStage() {}
@@ -75,6 +78,8 @@ namespace LocalizationConstants {
 
     ScanUnregisteredVehicles();
 
+    UpdateSwarmVelocities();
+
     // Selecting output frames based on selector keys.
     const auto current_planner_frame = planner_frame_selector ? planner_frame_a : planner_frame_b;
     const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b;
@@ -90,7 +95,8 @@ namespace LocalizationConstants {
       const Actor vehicle = actor_list.at(i);
       const ActorId actor_id = vehicle->GetId();
       const cg::Location vehicle_location = vehicle->GetLocation();
-      const float vehicle_velocity = vehicle->GetVelocity().Length();
+      const cg::Vector3D vehicle_velocity_vector = GetVelocity(actor_id);
+      const float vehicle_velocity = vehicle_velocity_vector.Length();
 
       // Initializing idle times.
       if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0) {
@@ -303,18 +309,54 @@ namespace LocalizationConstants {
                 });
 
       // Constructing output vector.
-      std::vector<std::pair<ActorId, Actor>> collision_candidates;
+      std::vector<std::tuple<ActorId, Actor, cg::Vector3D>> collision_candidates;
       std::for_each(collision_candidate_ids.begin(), collision_candidate_ids.end(),
-                    [&overlapping_actor_info, &collision_candidates] (const ActorId& a_id) {
+                    [&overlapping_actor_info, &collision_candidates, this] (const ActorId& a_id) {
-                      collision_candidates.push_back({a_id, overlapping_actor_info.at(a_id).first});
+                      collision_candidates.push_back({a_id,
+                                                      overlapping_actor_info.at(a_id).first,
+                                                      this->GetVelocity(a_id)});
                     });
 
+      // Sampling waypoint window for teleportation in hybrid physics mode.
+      std::vector<SimpleWaypointPtr> position_window;
+      if (hybrid_physics_mode) {
+
+        cg::Vector3D heading = vehicle->GetTransform().GetForwardVector();
+        bool window_begin = false;
+        SimpleWaypointPtr begining_wp;
+        bool window_complete = false;
+
+        for (uint32_t j = 0u; j < waypoint_buffer.size() && !window_complete; ++j) {
+
+          SimpleWaypointPtr &swp = waypoint_buffer.at(j);
+          cg::Vector3D relative_position = swp->GetLocation() - vehicle_location;
+
+          // Sample waypoints in front of the vehicle;
+          if (!window_begin && cg::Math::Dot(relative_position, heading) > 0.0f) {
+            window_begin = true;
+            begining_wp = swp;
+          }
+
+          if (window_begin && !window_complete) {
+            if (swp->DistanceSquared(begining_wp) > std::pow(POSITION_WINDOW_SIZE, 2)) {
+              // Stop when maximum size is reached.
+              window_complete = true;
+            } else {
+              position_window.push_back(swp);
+            }
+          }
+        }
+      }
+
       // Editing output frames.
       LocalizationToPlannerData &planner_message = current_planner_frame->at(i);
       planner_message.actor = vehicle;
       planner_message.deviation = dot_product;
       planner_message.distance = distance;
       planner_message.approaching_true_junction = approaching_junction;
+      planner_message.velocity = vehicle_velocity_vector;
+      planner_message.position_window = std::move(position_window);
+      planner_message.physics_enabled = IsPhysicsEnabled(actor_id);
 
       LocalizationToCollisionData &collision_message = current_collision_frame->at(i);
       collision_message.actor = vehicle;
@@ -323,6 +365,7 @@ namespace LocalizationConstants {
       collision_message.closest_waypoint = updated_front_waypoint;
       collision_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index);
       collision_message.safe_point_after_junction = final_safe_points[actor_id];
+      collision_message.velocity = vehicle_velocity_vector;
 
       LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i);
       traffic_light_message.actor = vehicle;
@@ -342,6 +385,9 @@ namespace LocalizationConstants {
   }
 
   void LocalizationStage::DataReceiver() {
+
+    hybrid_physics_mode = parameters.GetHybridPhysicsMode();
+
     bool is_deleted_actors_present = false;
     std::set<uint32_t> world_actor_id;
     std::vector<ActorPtr> actor_list_to_be_deleted;
@@ -357,6 +403,7 @@ namespace LocalizationConstants {
       return filtered;
     };
 
+    // TODO: Combine this actor scanning logic with ScanUnregisteredActors()
     // Get all the actors.
     auto world_actors_list = episode_proxy_ls.Lock()->GetAllTheActorsInTheEpisode();
 
@@ -366,14 +413,34 @@ namespace LocalizationConstants {
     // Building a set of vehicle ids in the world.
     for (const auto &actor : vehicles) {
       world_actor_id.insert(actor.GetId());
+
+      // Identify hero vehicle if currently not present
+      // and system is in hybrid physics mode.
+      if (hybrid_physics_mode && hero_actor == nullptr) {
+        Actor actor_ptr = actor.Get(episode_proxy_ls);
+        for (auto&& attribute: actor_ptr->GetAttributes()) {
+          if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") {
+            hero_actor = actor_ptr;
+            break;
+          }
+        }
+      }
+    }
+
+    // Invalidate hero actor pointer if it is not alive anymore.
+    if (hybrid_physics_mode && hero_actor != nullptr
+        && world_actor_id.find(hero_actor->GetId()) == world_actor_id.end()) {
+      hero_actor = nullptr;
     }
 
     // Search for invalid/destroyed vehicles.
     for (auto &actor : actor_list) {
-      if (world_actor_id.find(actor->GetId()) == world_actor_id.end()) {
+      ActorId deletion_id = actor->GetId();
+      if (world_actor_id.find(deletion_id) == world_actor_id.end()) {
         actor_list_to_be_deleted.emplace_back(actor);
-        track_traffic.DeleteActor(actor->GetId());
+        track_traffic.DeleteActor(deletion_id);
-        last_lane_change_location.erase(actor->GetId());
+        last_lane_change_location.erase(deletion_id);
+        kinematic_state_map.erase(deletion_id);
       }
     }
 
@@ -531,7 +598,7 @@ namespace LocalizationConstants {
   {
 
     const ActorId actor_id = vehicle->GetId();
-    const float vehicle_velocity = vehicle->GetVelocity().Length();
+    const float vehicle_velocity = GetVelocity(actor_id).Length();
 
     // Waypoint representing the new starting point for the waypoint buffer
     // due to lane change. Remains nullptr if lane change not viable.
@@ -760,7 +827,8 @@ namespace LocalizationConstants {
     }
 
     const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
-    if (actor->GetVelocity().Length() > STOPPED_VELOCITY_THRESHOLD || (vehicle->IsAtTrafficLight() && vehicle->GetTrafficLightState() != TLS::Green)) {
+    if (GetVelocity(actor->GetId()).Length() > STOPPED_VELOCITY_THRESHOLD
+        || (vehicle->IsAtTrafficLight() && vehicle->GetTrafficLightState() != TLS::Green)) {
       idle_time[actor->GetId()] = current_timestamp.elapsed_seconds;
     }
 
@@ -814,5 +882,87 @@ namespace LocalizationConstants {
     }
     return false;
   }
+
+  void LocalizationStage::UpdateSwarmVelocities() {
+
+    // Location of hero vehicle if present.
+    cg::Location hero_location;
+    if (hybrid_physics_mode && hero_actor != nullptr) {
+      hero_location = hero_actor->GetLocation();
+    }
+
+    // Using (1/20)s time delta for computing velocity.
+    float dt = HYBRID_MODE_DT;
+    // Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode.
+    if (!parameters.GetSynchronousMode()) {
+      TimePoint current_instance = chr::system_clock::now();
+      chr::duration<float> elapsed_time = current_instance - previous_update_instance;
+      if (elapsed_time.count() > dt) {
+        previous_update_instance = current_instance;
+      } else if (hybrid_physics_mode) {
+        return;
+      }
+    }
+
+    // Update velocity for all registered vehicles.
+    for (const Actor &actor: actor_list) {
+
+      ActorId actor_id = actor->GetId();
+      cg::Location vehicle_location = actor->GetLocation();
+
+      // Adding entry if not present.
+      if (kinematic_state_map.find(actor_id) == kinematic_state_map.end()) {
+        kinematic_state_map.insert({actor_id, KinematicState{true, vehicle_location, cg::Vector3D()}});
+      }
+
+      // Check if current actor is in range of hero actor and enable physics in hybrid mode.
+      bool in_range_of_hero_actor = false;
+      if (hybrid_physics_mode
+          && hero_actor != nullptr
+          && (cg::Math::DistanceSquared(vehicle_location, hero_location) < std::pow(PHYSICS_RADIUS, 2))) {
+        in_range_of_hero_actor = true;
+      }
+      bool enable_physics = hybrid_physics_mode? in_range_of_hero_actor: true;
+      kinematic_state_map.at(actor_id).physics_enabled = enable_physics;
+      actor->SetSimulatePhysics(enable_physics);
+
+      // When we say velocity, we usually mean velocity for a vehicle along it's heading.
+      // Velocity component due to rotation can be removed by taking dot product with heading vector.
+      cg::Vector3D heading = actor->GetTransform().GetForwardVector();
+      if (enable_physics) {
+
+        kinematic_state_map.at(actor_id).velocity = cg::Math::Dot(actor->GetVelocity(), heading) * heading;
+
+      } else {
+
+        cg::Vector3D displacement = (vehicle_location - kinematic_state_map.at(actor_id).location);
+        cg::Vector3D displacement_along_heading = cg::Math::Dot(displacement, heading) * heading;
+        cg::Vector3D velocity = displacement_along_heading/dt;
+        kinematic_state_map.at(actor_id).velocity = velocity;
+      }
+
+      // Updating location after velocity is computed.
+      kinematic_state_map.at(actor_id).location = vehicle_location;
+    }
+  }
+
+  cg::Vector3D LocalizationStage::GetVelocity(ActorId actor_id) {
+    cg::Vector3D vel;
+    if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) {
+      vel = kinematic_state_map.at(actor_id).velocity;
+    } else if (unregistered_actors.find(actor_id) != unregistered_actors.end()) {
+      vel = unregistered_actors.at(actor_id)->GetVelocity();
+    }
+    return vel;
+  }
+
+  bool LocalizationStage::IsPhysicsEnabled(ActorId actor_id) {
+    bool enabled = true;
+    if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) {
+      enabled = kinematic_state_map.at(actor_id).physics_enabled;
+    }
+    return enabled;
+  }
+
 } // namespace traffic_manager
 } // namespace carla

LibCarla/source/carla/trafficmanager/LocalizationStage.h

@@ -13,6 +13,7 @@
 #include <deque>
 #include <memory>
 #include <mutex>
+#include <tuple>
 #include <unordered_map>
 
 #include "carla/StringUtil.h"
@@ -42,14 +43,23 @@
 namespace carla {
 namespace traffic_manager {
 
-  using namespace std::chrono;
   namespace cc = carla::client;
+  namespace cg = carla::geom;
+  namespace chr = std::chrono;
+  using namespace chr;
   using Actor = carla::SharedPtr<cc::Actor>;
   using Vehicle = carla::SharedPtr<cc::Vehicle>;
   using ActorId = carla::ActorId;
   using ActorIdSet = std::unordered_set<ActorId>;
   using TLS = carla::rpc::TrafficLightState;
 
+  /// Structure to hold kinematic state of actors.
+  struct KinematicState {
+    bool physics_enabled;
+    cg::Location location;
+    cg::Vector3D velocity;
+  };
+
   /// This class is responsible for maintaining a horizon of waypoints ahead
   /// of the vehicle for it to follow.
   /// The class is also responsible for managing lane change decisions and
@@ -122,6 +132,14 @@ namespace traffic_manager {
     SnippetProfiler snippet_profiler;
     /// Map to keep track of last lane change location.
     std::unordered_map<ActorId, cg::Location> last_lane_change_location;
+    /// Reference of hero vehicle.
+    Actor hero_actor {nullptr};
+    /// Switch indicating hybrid physics mode.
+    bool hybrid_physics_mode {false};
+    /// Structure to hold previous state of physics-less vehicle.
+    std::unordered_map<ActorId, KinematicState> kinematic_state_map;
+    /// Time instance used to calculate dt in asynchronous mode.
+    TimePoint previous_update_instance;
 
     /// A simple method used to draw waypoint buffer ahead of a vehicle.
     void DrawBuffer(Buffer &buffer);
@@ -146,6 +164,11 @@ namespace traffic_manager {
     void CleanActor(const ActorId actor_id);
     bool TryDestroyVehicle(const Actor& actor);
 
+    /// Methods for actor state management.
+    void UpdateSwarmVelocities();
+    cg::Vector3D GetVelocity(ActorId actor_id);
+    bool IsPhysicsEnabled(ActorId actor_id);
+
   public:
 
     LocalizationStage(

LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp

@@ -234,7 +234,7 @@ namespace LocalizationConstants {
 
   }
 
-  std::pair<SimpleWaypointPtr,uint64_t> TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) {
+std::pair<SimpleWaypointPtr,uint64_t> TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) {
 
     SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
     const SimpleWaypointPtr& buffer_front = waypoint_buffer.front();
@@ -250,27 +250,28 @@ namespace LocalizationConstants {
       }
 
       if(mScanForward) {
-        for (index = startPosn ;
+        for (uint64_t i = startPosn;
-            (index < waypoint_buffer.size()) &&
+             (i < waypoint_buffer.size())
-            (buffer_front->DistanceSquared(target_waypoint)
+             && (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power);
-            < target_point_dist_power);
+             ++i) {
-            ++index) {
+          target_waypoint = waypoint_buffer.at(i);
-            target_waypoint = waypoint_buffer.at(index);
+          index = i;
-            }
+        }
       }
       else {
-        for (index = startPosn ;
+        for (uint64_t i = startPosn;
-          (buffer_front->DistanceSquared(target_waypoint)
+             (buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power);
-          > target_point_dist_power);
+             --i) {
-          --index) {
+          target_waypoint = waypoint_buffer.at(i);
-          target_waypoint = waypoint_buffer.at(index);
+          index = i;
-          }
+        }
       }
     }
     else {
       target_waypoint = waypoint_buffer.back();
+      index = waypoint_buffer.size() - 1;
     }
-  return std::make_pair(target_waypoint,index);
+  return std::make_pair(target_waypoint, index);
   }
 
 } // namespace traffic_manager

LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h

@@ -7,11 +7,13 @@
 #pragma once
 
 #include <memory>
+#include <tuple>
 #include <unordered_map>
 #include <vector>
 
 #include "carla/client/Actor.h"
 #include "carla/geom/Vector3D.h"
+#include "carla/geom/Transform.h"
 #include "carla/Memory.h"
 #include "carla/rpc/ActorId.h"
 
@@ -22,6 +24,7 @@ namespace carla {
 namespace traffic_manager {
 
   namespace cc = carla::client;
+  namespace cg = carla::geom;
 
   /// Convenience typing.
 
@@ -41,6 +44,9 @@ namespace traffic_manager {
     float deviation;
     float distance;
     bool approaching_true_junction;
+    cg::Vector3D velocity;
+    bool physics_enabled;
+    std::vector<std::shared_ptr<SimpleWaypoint>> position_window;
   };
 
   /// Type of data sent by the motion planner stage to the batch control stage.
@@ -49,16 +55,19 @@ namespace traffic_manager {
     float throttle;
     float brake;
     float steer;
+    bool physics_enabled;
+    cg::Transform transform;
   };
 
   /// Type of data sent by the localization stage to the collision stage.
   struct LocalizationToCollisionData {
     Actor actor;
     Buffer buffer;
-    std::vector<std::pair<ActorId, Actor>> overlapping_actors;
+    std::vector<std::tuple<ActorId, Actor, cg::Vector3D>> overlapping_actors;
     std::shared_ptr<SimpleWaypoint> safe_point_after_junction;
     std::shared_ptr<SimpleWaypoint> closest_waypoint;
     std::shared_ptr<SimpleWaypoint> junction_look_ahead_waypoint;
+    cg::Vector3D velocity;
   };
 
   /// Type of data sent by the collision stage to the motion planner stage.

LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp

@@ -18,6 +18,8 @@ namespace PlannerConstants {
   static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f;
   static const float FOLLOW_DISTANCE_RATE = (MAX_FOLLOW_LEAD_DISTANCE-MIN_FOLLOW_LEAD_DISTANCE)/ARBITRARY_MAX_SPEED;
   static const float CRITICAL_BRAKING_MARGIN = 0.25f;
+  static const float HYBRID_MODE_DT = 0.05f;
+  static const float EPSILON_VELOCITY = 0.001f;
 } // namespace PlannerConstants
 
   using namespace PlannerConstants;
@@ -82,7 +84,7 @@ namespace PlannerConstants {
       const ActorId actor_id = actor->GetId();
 
       const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
-      const cg::Vector3D current_velocity_vector = vehicle->GetVelocity();
+      const cg::Vector3D current_velocity_vector = localization_data.velocity;
       const float current_velocity = current_velocity_vector.Length();
 
       const auto current_time = chr::system_clock::now();
@@ -120,7 +122,7 @@ namespace PlannerConstants {
 
         // Consider collision avoidance decisions only if there is positive relative velocity
         // of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle).
-        if (ego_relative_velocity > 0.0f) {
+        if (ego_relative_velocity > EPSILON_VELOCITY) {
           // If other vehicle is approaching lead vehicle and lead vehicle is further
           // than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
           float follow_lead_distance = ego_relative_velocity * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE;
@@ -139,29 +141,120 @@ namespace PlannerConstants {
             collision_emergency_stop = true;
           }
         }
+        if (collision_data.distance_to_other_vehicle < CRITICAL_BRAKING_MARGIN) {
+          collision_emergency_stop = true;
+        }
       }
       ///////////////////////////////////////////////////////////////////////////////////
 
       // Clip dynamic target velocity to maximum allowed speed for the vehicle.
       dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity);
 
-      // State update for vehicle.
+      bool emergency_stop = false;
-      StateEntry current_state = controller.StateUpdate(previous_state, current_velocity,
+      // In case of collision or traffic light hazard.
-                                                        dynamic_target_velocity, current_deviation,
-                                                        current_distance, current_time);
-
-      // Controller actuation.
-      ActuationSignal actuation_signal = controller.RunStep(current_state, previous_state,
-                                                            longitudinal_parameters, lateral_parameters);
-
-      // In case of traffic light hazard.
       if (traffic_light_frame->at(i).traffic_light_hazard
           || collision_emergency_stop) {
+      emergency_stop = true;
+      }
+
+      // Message items to be sent to batch control stage.
+      ActuationSignal actuation_signal {0.0f, 0.0f, 0.0f};
+      bool physics_enabled = true;
+      cg::Transform teleportation_transform;
+
+      // If physics is enabled for the vehicle, use PID controller.
+      StateEntry current_state;
+      if (localization_data.physics_enabled) {
+
+        // State update for vehicle.
+        current_state = controller.StateUpdate(previous_state, current_velocity,
+                                               dynamic_target_velocity, current_deviation,
+                                               current_distance, current_time);
+
+        // Controller actuation.
+        actuation_signal = controller.RunStep(current_state, previous_state,
+                                              longitudinal_parameters, lateral_parameters);
+
+        if (emergency_stop) {
+
+          current_state.deviation_integral = 0.0f;
+          current_state.velocity_integral = 0.0f;
+          actuation_signal.throttle = 0.0f;
+          actuation_signal.brake = 1.0f;
+        }
+
+      }
+      // For physics-less vehicles, determine position and orientation for teleportation.
+      else if (hybrid_physics_mode) {
+
+        physics_enabled = false;
+
+        // Flushing controller state for vehicle.
+        current_state = {0.0f, 0.0f, 0.0f,
+                         chr::system_clock::now(),
+                         0.0f, 0.0f, 0.0f};
+
+        // Add entry to teleportation duration clock table if not present.
+        if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
+          teleportation_instance.insert({actor_id, chr::system_clock::now()});
+        }
+
+        // Measuring time elapsed since last teleportation for the vehicle.
+        chr::duration<float> elapsed_time = current_time - teleportation_instance.at(actor_id);
+
+        // Find a location ahead of the vehicle for teleportation to achieve intended velocity.
+        if (!emergency_stop && !(!parameters.GetSynchronousMode() && elapsed_time.count() < HYBRID_MODE_DT)) {
+
+          // Target displacement magnitude to achieve target velocity.
+          float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT;
+
+          SimpleWaypointPtr target_interval_begin = nullptr;
+          SimpleWaypointPtr target_interval_end = nullptr;
+          bool teleportation_interval_found = false;
+          cg::Location vehicle_location = actor->GetLocation();
+
+          // Find the interval containing position to achieve target displacement.
+          for (uint32_t j = 0u;
+                j+1 < localization_data.position_window.size() && !teleportation_interval_found;
+                ++j) {
+
+            target_interval_begin = localization_data.position_window.at(j);
+            target_interval_end = localization_data.position_window.at(j+1);
+
+            if (target_interval_begin->DistanceSquared(vehicle_location) > std::pow(target_displacement, 2)
+                || (target_interval_begin->DistanceSquared(vehicle_location) < std::pow(target_displacement, 2)
+                    && target_interval_end->DistanceSquared(vehicle_location) > std::pow(target_displacement, 2))
+            ) {
+              teleportation_interval_found = true;
+            }
+          }
+
+          if (target_interval_begin != nullptr && target_interval_end != nullptr) {
+
+            // Construct target transform to accurately achieve desired velocity.
+            float missing_displacement = target_displacement - (target_interval_begin->Distance(vehicle_location));
+            cg::Transform target_base_transform = target_interval_begin->GetTransform();
+            cg::Location target_base_location = target_base_transform.location;
+            cg::Vector3D target_heading = target_base_transform.GetForwardVector();
+            cg::Location teleportation_location = target_base_location
+                                                  + cg::Location(target_heading * missing_displacement);
+            teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
+
+          } else {
+
+            teleportation_transform = actor->GetTransform();
+
+          }
+
+        }
+        // In case of an emergency stop, stay in the same location.
+        // Also, teleport only once every dt in asynchronous mode.
+        else {
+
+          teleportation_transform = actor->GetTransform();
+
+        }
 
-        current_state.deviation_integral = 0.0f;
-        current_state.velocity_integral = 0.0f;
-        actuation_signal.throttle = 0.0f;
-        actuation_signal.brake = 1.0f;
       }
 
       // Updating PID state.
@@ -174,6 +267,8 @@ namespace PlannerConstants {
       message.throttle = actuation_signal.throttle;
       message.brake = actuation_signal.brake;
       message.steer = actuation_signal.steer;
+      message.physics_enabled = physics_enabled;
+      message.transform = teleportation_transform;
     }
   }
 
@@ -192,6 +287,8 @@ namespace PlannerConstants {
       control_frame_a = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
       control_frame_b = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
     }
+
+    hybrid_physics_mode = parameters.GetHybridPhysicsMode();
   }
 
   void MotionPlannerStage::DataSender() {

LibCarla/source/carla/trafficmanager/MotionPlannerStage.h

@@ -7,6 +7,7 @@
 #pragma once
 
 #include <chrono>
+#include <memory>
 #include <unordered_map>
 #include <vector>
 
@@ -18,6 +19,7 @@
 #include "carla/trafficmanager/Parameters.h"
 #include "carla/trafficmanager/PIDController.h"
 #include "carla/trafficmanager/PipelineStage.h"
+#include "carla/trafficmanager/SimpleWaypoint.h"
 
 namespace carla {
 namespace traffic_manager {
@@ -27,6 +29,7 @@ namespace traffic_manager {
 
   using Actor = carla::SharedPtr<cc::Actor>;
   using ActorId = carla::rpc::ActorId;
+  using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
 
   /// The class is responsible for aggregating information from various stages
   /// like the localization stage, traffic light stage, collision detection
@@ -68,7 +71,10 @@ namespace traffic_manager {
     uint64_t number_of_vehicles;
     /// Reference to Carla's debug helper object.
     cc::DebugHelper &debug_helper;
-
+    /// Switch indicating hybrid physics mode.
+    bool hybrid_physics_mode {false};
+    /// Teleportation duration clock;
+    std::unordered_map<ActorId, TimePoint> teleportation_instance;
 
   public:
 

LibCarla/source/carla/trafficmanager/Parameters.cpp

@@ -19,6 +19,11 @@ Parameters::Parameters() {
 
 Parameters::~Parameters() {}
 
+void Parameters::SetHybridPhysicsMode(const bool mode_switch) {
+
+  hybrid_physics_mode.store(mode_switch);
+}
+
 void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
 
   float new_percentage = std::min(100.0f, percentage);
@@ -260,5 +265,10 @@ float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) {
   return percentage;
 }
 
+bool Parameters::GetHybridPhysicsMode() {
+
+  return hybrid_physics_mode.load();
+}
+
 } // namespace traffic_manager
 } // namespace carla

LibCarla/source/carla/trafficmanager/Parameters.h

@@ -60,6 +60,8 @@ namespace traffic_manager {
     std::atomic<bool> synchronous_mode {false};
     /// Distance Margin
     std::atomic<float> distance_margin {2.0};
+    /// Hybrid physics mode switch.
+    std::atomic<bool> hybrid_physics_mode {false};
 
   public:
     Parameters();
@@ -150,6 +152,12 @@ namespace traffic_manager {
     /// Set Synchronous mode time out.
     void SetSynchronousModeTimeOutInMiliSecond(const double time);
 
+    /// Method to set hybrid physics mode.
+    void SetHybridPhysicsMode(const bool mode_switch);
+
+    /// Method to retrieve hybrid physics mode.
+    bool GetHybridPhysicsMode();
+
     /// Synchronous mode time out variable.
     std::chrono::duration<double, std::milli> synchronous_time_out;
 

LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp

@@ -64,25 +64,21 @@ namespace traffic_manager {
     return static_cast<uint64_t>(waypoints.size());
   }
 
-  void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr _waypoint) {
+  void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr &_waypoint) {
 
     const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
     const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
     if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f) {
       next_left_waypoint = _waypoint;
-    } else {
-      throw std::invalid_argument("Argument not on the left side!");
     }
   }
 
-  void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr _waypoint) {
+  void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr &_waypoint) {
 
     const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
     const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
     if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0.0f) {
       next_right_waypoint = _waypoint;
-    } else {
-      throw std::invalid_argument("Argument not on the right side!");
     }
   }
 
@@ -132,5 +128,9 @@ namespace traffic_manager {
     return waypoint->GetJunctionId();
   }
 
+  cg::Transform SimpleWaypoint::GetTransform() const {
+    return waypoint->GetTransform();
+  }
+
 } // namespace traffic_manager
 } // namespace carla

LibCarla/source/carla/trafficmanager/SimpleWaypoint.h

@@ -12,6 +12,7 @@
 #include "carla/client/Waypoint.h"
 #include "carla/geom/Location.h"
 #include "carla/geom/Math.h"
+#include "carla/geom/Transform.h"
 #include "carla/geom/Vector3D.h"
 #include "carla/Memory.h"
 #include "carla/road/RoadTypes.h"
@@ -77,10 +78,10 @@ namespace traffic_manager {
     uint64_t SetPreviousWaypoint(const std::vector<SimpleWaypointPtr> &next_waypoints);
 
     /// This method is used to set the closest left waypoint for a lane change.
-    void SetLeftWaypoint(SimpleWaypointPtr waypoint);
+    void SetLeftWaypoint(SimpleWaypointPtr &waypoint);
 
     /// This method is used to set the closest right waypoint for a lane change.
-    void SetRightWaypoint(SimpleWaypointPtr waypoint);
+    void SetRightWaypoint(SimpleWaypointPtr &waypoint);
 
     /// This method is used to get the closest left waypoint for a lane change.
     SimpleWaypointPtr GetLeftWaypoint();
@@ -117,6 +118,8 @@ namespace traffic_manager {
     /// Returns true if the object's waypoint belongs to an intersection (Doesn't use OpenDrive).
     bool CheckIntersection() const;
 
+    /// Return transform object for the current waypoint.
+    cg::Transform GetTransform() const;
   };
 
 } // namespace traffic_manager

LibCarla/source/carla/trafficmanager/TrafficManager.h

@@ -71,6 +71,14 @@ public:
 
   static void Tick();
 
+  /// This method sets the hybrid physics mode.
+  void SetHybridPhysicsMode(const bool mode_switch) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if(tm_ptr != nullptr){
+      tm_ptr->SetHybridPhysicsMode(mode_switch);
+    }
+  }
+
   /// This method registers a vehicle with the traffic manager.
   void RegisterVehicles(const std::vector<ActorPtr> &actor_list) {
     TrafficManagerBase* tm_ptr = GetTM(_port);

LibCarla/source/carla/trafficmanager/TrafficManagerBase.h

@@ -103,6 +103,9 @@ public:
   /// Method to set probabilistic preference to keep on the right lane.
   virtual void SetKeepRightPercentage(const ActorPtr &actor,const float percentage) = 0;
 
+  /// Method to set hybrid physics mode.
+  virtual void SetHybridPhysicsMode(const bool mode_switch) = 0;
+
 protected:
 
 };

LibCarla/source/carla/trafficmanager/TrafficManagerClient.h

@@ -179,6 +179,12 @@ public:
     _client->call("set_percentage_keep_right_rule", actor, percentage);
   }
 
+  /// Method to set hybrid physics mode.
+  void SetHybridPhysicsMode(const bool mode_switch) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("set_hybrid_physics_mode", mode_switch);
+  }
+
 private:
 
   /// RPC client.

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp

@@ -201,6 +201,10 @@ void TrafficManagerLocal::SetKeepRightPercentage(const ActorPtr &actor, const fl
   parameters.SetKeepRightPercentage(actor, percentage);
 }
 
+void TrafficManagerLocal::SetHybridPhysicsMode(const bool mode_switch) {
+  parameters.SetHybridPhysicsMode(mode_switch);
+}
+
 bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) {
   for (auto& elem : tl_to_freeze) {
     if (!elem->IsFrozen() || elem->GetState() != TLS::Red) {

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h

@@ -184,6 +184,9 @@ namespace traffic_manager {
 
     /// Method to set probabilistic preference to keep on the right lane.
     void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
+
+    /// Method to set hybrid physics mode.
+    void SetHybridPhysicsMode(const bool mode_switch);
   };
 
 } // namespace traffic_manager

LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp

@@ -176,6 +176,10 @@ void TrafficManagerRemote::SetKeepRightPercentage(const ActorPtr &_actor, const
   client.SetKeepRightPercentage(actor, percentage);
 }
 
+void TrafficManagerRemote::SetHybridPhysicsMode(const bool mode_switch) {
+  client.SetHybridPhysicsMode(mode_switch);
+}
+
 void TrafficManagerRemote::ResetAllTrafficLights() {
   client.ResetAllTrafficLights();
 }

LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h

@@ -100,6 +100,9 @@ public:
   /// Method to set probabilistic preference to keep on the right lane.
   void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
 
+  /// Method to set hybrid physics mode.
+  void SetHybridPhysicsMode(const bool mode_switch);
+
   /// Method to provide synchronous tick
   bool SynchronousTick();
 

LibCarla/source/carla/trafficmanager/TrafficManagerServer.h

@@ -150,6 +150,11 @@ public:
         tm->SetKeepRightPercentage(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
       });
 
+      /// Method to set hybrid physics mode.
+      server->bind("set_hybrid_physics_mode", [=](const bool mode_switch) {
+        tm->SetHybridPhysicsMode(mode_switch);
+      });
+
       /// Method to set synchronous mode.
       server->bind("set_synchronous_mode", [=](const bool mode) {
         tm->SetSynchronousMode(mode);

PythonAPI/carla/source/libcarla/TrafficManager.cpp

@@ -30,5 +30,6 @@ void export_trafficmanager() {
       .def("ignore_signs_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningSign)
       .def("set_global_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetGlobalDistanceToLeadingVehicle)
       .def("set_percentage_keep_right_rule", &carla::traffic_manager::TrafficManager::SetKeepRightPercentage)
-      .def("set_synchronous_mode", &carla::traffic_manager::TrafficManager::SetSynchronousMode);
+      .def("set_synchronous_mode", &carla::traffic_manager::TrafficManager::SetSynchronousMode)
+      .def("set_hybrid_physics_mode", &carla::traffic_manager::TrafficManager::SetHybridPhysicsMode);
 }

PythonAPI/examples/spawn_npc.py

@@ -78,6 +78,10 @@ def main():
         '--sync',
         action='store_true',
         help='Synchronous mode execution')
+    argparser.add_argument(
+        '--hybrid',
+        action='store_true',
+        help='Enanble')
     args = argparser.parse_args()
 
     logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
@@ -90,9 +94,13 @@ def main():
 
     try:
 
+        world = client.get_world()
+
         traffic_manager = client.get_trafficmanager(args.tm_port)
         traffic_manager.set_global_distance_to_leading_vehicle(2.0)
-        world = client.get_world()
+        if args.hybrid:
+            traffic_manager.set_hybrid_physics_mode(True)
+
 
         synchronous_master = False
 

PythonAPI/util/performance_benchmark.py

@@ -325,7 +325,7 @@ def run_benchmark(world, sensors, n_vehicles, n_walkers, client, debug=False):
     sensor_list = sensors_ret
 
   ticks = 0
-  while ticks < args.ticks:
+  while ticks < int(args.ticks):
     _ = world.wait_for_tick()
     if debug:
       print("== Samples {} / {}".format(ticks + 1, args.ticks))