Added lane offset parameter

Docs/python_api.md

@@ -2381,9 +2381,14 @@ Forces a vehicle to change either to the lane on its left or right, if existing,
         - `direction` (_bool_) - Destination lane. __True__ is the one on the right and __False__ is the left one.  
 - <a name="carla.TrafficManager.global_percentage_speed_difference"></a>**<font color="#7fb800">global_percentage_speed_difference</font>**(<font color="#00a6ed">**self**</font>, <font color="#00a6ed">**percentage**</font>)  
 Sets the difference the vehicle's intended speed and its current speed limit. Speed limits can be exceeded by setting the `perc` to a negative value.
+Default is 0. Numbers high enough to cause the vehicle to drive through other lanes might break the controller.  
+    - **Parameters:**
+        - `offset` (_float_) - Percentage difference between intended speed and the current limit.  
+- <a name="carla.TrafficManager.global_lane_offset"></a>**<font color="#7fb800">global_lane_offset</font>**(<font color="#00a6ed">**self**</font>, <font color="#00a6ed">**offset**</font>)  
+Sets a global lane offset displacement from the center line. Positive values imply a right offset while negative ones mean a left one.
 Default is 30. Exceeding a speed limit can be done using negative percentages.  
     - **Parameters:**
-        - `percentage` (_float_) - Percentage difference between intended speed and the current limit.  
+        - `offset` (_float_) - Lane offset displacement from the center line.
 - <a name="carla.TrafficManager.ignore_lights_percentage"></a>**<font color="#7fb800">ignore_lights_percentage</font>**(<font color="#00a6ed">**self**</font>, <font color="#00a6ed">**actor**</font>, <font color="#00a6ed">**perc**</font>)  
 During the traffic light stage, which runs every frame, this method sets the percent chance that traffic lights will be ignored for a vehicle.  
     - **Parameters:**
@@ -2410,6 +2415,12 @@ Default is 30. Exceeding a speed limit can be done using negative percentages.
     - **Parameters:**
         - `actor` (_[carla.Actor](#carla.Actor)_) - Vehicle whose speed behaviour is being changed.  
         - `percentage` (_float_) - Percentage difference between intended speed and the current limit.  
+- <a name="carla.TrafficManager.vehicle_lane_offset"></a>**<font color="#7fb800">vehicle_lane_offset</font>**(<font color="#00a6ed">**self**</font>, <font color="#00a6ed">**actor**</font>, <font color="#00a6ed">**offset**</font>)  
+Sets a lane offset displacement from the center line. Positive values imply a right offset while negative ones mean a left one.
+Default is 30. Exceeding a speed limit can be done using negative percentages.
+    - **Parameters:**
+        - `actor` (_[carla.Actor](#carla.Actor)_) - Vehicle whose speed behaviour is being changed.  
+        - `offset` (_float_) - Lane offset displacement from the center line.  
 
 ##### Getters
 - <a name="carla.TrafficManager.get_port"></a>**<font color="#7fb800">get_port</font>**(<font color="#00a6ed">**self**</font>)  

LibCarla/source/carla/trafficmanager/Constants.h

@@ -124,8 +124,8 @@ static const float LANDMARK_DETECTION_TIME = 2.5f;
 static const float TL_GREEN_TARGET_VELOCITY = 20.0f / 3.6f;
 static const float TL_RED_TARGET_VELOCITY = 15.0f / 3.6f;
 static const float TL_UNKNOWN_TARGET_VELOCITY = TL_RED_TARGET_VELOCITY;
-static const float STOP_TARGET_VELOCITY = 15.0f / 3.6f;
-static const float YIELD_TARGET_VELOCITY = 15.0f / 3.6f;
+static const float STOP_TARGET_VELOCITY = 10.0f / 3.6f;
+static const float YIELD_TARGET_VELOCITY = 10.0f / 3.6f;
 static const float FRICTION = 0.6f;
 static const float GRAVITY = 9.81f;
 static const float PI = 3.1415927f;

LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp

@@ -159,7 +159,13 @@ void MotionPlanStage::Update(const unsigned long index) {
       const float target_point_distance = std::max(vehicle_speed * TARGET_WAYPOINT_TIME_HORIZON,
                                                   MIN_TARGET_WAYPOINT_DISTANCE);
       const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
-      const cg::Location target_location = target_waypoint->GetLocation();
+      cg::Location target_location = target_waypoint->GetLocation();
+      
+      float offset = parameters.GetLaneOffset(actor_id);
+      auto right_vector = target_waypoint->GetTransform().GetRightVector();
+      auto offset_location = cg::Location(cg::Vector3D(offset*right_vector.x, offset*right_vector.y, 0.0f));
+      target_location = target_location + offset_location;
+      
       float dot_product = DeviationDotProduct(vehicle_location, vehicle_heading, target_location);
       float cross_product = DeviationCrossProduct(vehicle_location, vehicle_heading, target_location);
       dot_product = acos(dot_product) / PI;

LibCarla/source/carla/trafficmanager/Parameters.cpp

@@ -49,6 +49,11 @@ void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float
   }
 }
 
+void Parameters::SetLaneOffset(const ActorPtr &actor, const float offset) {
+  const auto entry = std::make_pair(actor->GetId(), offset);
+  lane_offset.AddEntry(entry);
+}
+
 void Parameters::SetDesiredSpeed(const ActorPtr &actor, const float value) {
 
   float new_value = std::max(0.0f, value);
@@ -63,6 +68,10 @@ void Parameters::SetGlobalPercentageSpeedDifference(const float percentage) {
   global_percentage_difference_from_limit = new_percentage;
 }
 
+void Parameters::SetGlobalLaneOffset(const float offset) {
+  global_lane_offset = offset;
+}
+
 void Parameters::SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) {
   const ActorId reference_id = reference_actor->GetId();
   const ActorId other_id = other_actor->GetId();
@@ -254,6 +263,16 @@ float Parameters::GetVehicleTargetVelocity(const ActorId &actor_id, const float
   return speed_limit * (1.0f - percentage_difference / 100.0f);
 }
 
+float Parameters::GetLaneOffset(const ActorId &actor_id) const {
+  float offset = global_lane_offset;
+
+  if (lane_offset.Contains(actor_id)) {
+    offset = lane_offset.GetValue(actor_id);
+  }
+
+  return offset;
+}
+
 bool Parameters::GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const {
 
   bool avoid_collision = true;

LibCarla/source/carla/trafficmanager/Parameters.h

@@ -39,10 +39,14 @@ class Parameters {
 private:
   /// Target velocity map for individual vehicles, based on a % diffrerence from speed limit.
   AtomicMap<ActorId, float> percentage_difference_from_speed_limit;
+  /// Lane offset map for individual vehicles.
+  AtomicMap<ActorId, float> lane_offset;
   /// Target velocity map for individual vehicles, based on a desired velocity.
   AtomicMap<ActorId, float> exact_desired_speed;
   /// Global target velocity limit % difference.
   float global_percentage_difference_from_limit = 0;
+  /// Global lane offset
+  float global_lane_offset = 0;
   /// Map containing a set of actors to be ignored during collision detection.
   AtomicMap<ActorId, std::shared_ptr<AtomicActorSet>> ignore_collision;
   /// Map containing distance to leading vehicle command.
@@ -106,6 +110,10 @@ public:
   /// If less than 0, it's a % increase.
   void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage);
 
+  /// Method to set a lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetLaneOffset(const ActorPtr &actor, const float offset);
+
   /// Set a vehicle's exact desired velocity.
   void SetDesiredSpeed(const ActorPtr &actor, const float value);
 
@@ -113,6 +121,10 @@ public:
   /// If less than 0, it's a % increase.
   void SetGlobalPercentageSpeedDifference(float const percentage);
 
+  /// Method to set a global lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetGlobalLaneOffset(float const offset);
+
   /// Method to set collision detection rules between vehicles.
   void SetCollisionDetection(
       const ActorPtr &reference_actor,
@@ -207,6 +219,9 @@ public:
   /// Method to query target velocity for a vehicle.
   float GetVehicleTargetVelocity(const ActorId &actor_id, const float speed_limit) const;
 
+  /// Method to query lane offset for a vehicle.
+  float GetLaneOffset(const ActorId &actor_id) const;
+
   /// Method to query collision avoidance rule between a pair of vehicles.
   bool GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const;
 

LibCarla/source/carla/trafficmanager/TrafficManager.h

@@ -177,6 +177,15 @@ public:
     }
   }
 
+  /// Method to set a lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetLaneOffset(const ActorPtr &actor, const float offset) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if(tm_ptr != nullptr){
+      tm_ptr->SetLaneOffset(actor, offset);
+    }
+  }
+
   /// Set a vehicle's exact desired velocity.
   void SetDesiredSpeed(const ActorPtr &actor, const float value) {
     TrafficManagerBase* tm_ptr = GetTM(_port);
@@ -194,6 +203,15 @@ public:
     }
   }
 
+  /// Method to set a global lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetGlobalLaneOffset(float const offset){
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if(tm_ptr != nullptr){
+      tm_ptr->SetGlobalLaneOffset(offset);
+    }
+  }
+
   /// Set the automatic management of the vehicle lights
   void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update){
     TrafficManagerBase* tm_ptr = GetTM(_port);

LibCarla/source/carla/trafficmanager/TrafficManagerBase.h

@@ -54,6 +54,10 @@ public:
   /// If less than 0, it's a % increase.
   virtual void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) = 0;
 
+  /// Method to set a lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  virtual void SetLaneOffset(const ActorPtr &actor, const float offset) = 0;
+
   /// Set a vehicle's exact desired velocity.
   virtual void SetDesiredSpeed(const ActorPtr &actor, const float value) = 0;
 
@@ -61,6 +65,10 @@ public:
   /// If less than 0, it's a % increase.
   virtual void SetGlobalPercentageSpeedDifference(float const percentage) = 0;
 
+  /// Method to set a global lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  virtual void SetGlobalLaneOffset(float const offset) = 0;
+
   /// Method to set the automatic management of the vehicle lights
   virtual void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update) = 0;
 

LibCarla/source/carla/trafficmanager/TrafficManagerClient.h

@@ -81,6 +81,13 @@ public:
     _client->call("set_percentage_speed_difference", std::move(_actor), percentage);
   }
 
+  /// Method to set a lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetLaneOffset(const carla::rpc::Actor &_actor, const float offset) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("set_lane_offset", std::move(_actor), offset);
+  }
+
   /// Set a vehicle's exact desired velocity.
   void SetDesiredSpeed(const carla::rpc::Actor &_actor, const float value) {
     DEBUG_ASSERT(_client != nullptr);
@@ -94,6 +101,13 @@ public:
     _client->call("set_global_percentage_speed_difference", percentage);
   }
 
+  /// Method to set a global lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetGlobalLaneOffset(const float offset) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("set_global_lane_offset", offset);
+  }
+
   /// Method to set the automatic management of the vehicle lights
   void SetUpdateVehicleLights(const carla::rpc::Actor &_actor, const bool do_update) {
     DEBUG_ASSERT(_client != nullptr);

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp

@@ -334,6 +334,14 @@ void TrafficManagerLocal::SetGlobalPercentageSpeedDifference(const float percent
   parameters.SetGlobalPercentageSpeedDifference(percentage);
 }
 
+void TrafficManagerLocal::SetLaneOffset(const ActorPtr &actor, const float offset) {
+  parameters.SetLaneOffset(actor, offset);
+}
+
+void TrafficManagerLocal::SetGlobalLaneOffset(const float offset) {
+  parameters.SetGlobalLaneOffset(offset);
+}
+
 void TrafficManagerLocal::SetDesiredSpeed(const ActorPtr &actor, const float value) {
   parameters.SetDesiredSpeed(actor, value);
 }

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h

@@ -164,10 +164,18 @@ public:
   /// Set a vehicle's exact desired velocity.
   void SetDesiredSpeed(const ActorPtr &actor, const float value);
 
-  /// Methos to set a global % decrease in velocity with respect to the speed limit.
+  /// Method to set a global % decrease in velocity with respect to the speed limit.
   /// If less than 0, it's a % increase.
   void SetGlobalPercentageSpeedDifference(float const percentage);
 
+  /// Method to set a lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetLaneOffset(const ActorPtr &actor, const float offset);
+
+  /// Method to set a global lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetGlobalLaneOffset(float const offset);
+
   /// Method to set the automatic management of the vehicle lights
   void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update);
 

LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp

@@ -121,6 +121,16 @@ void TrafficManagerRemote::SetGlobalPercentageSpeedDifference(const float percen
   client.SetGlobalPercentageSpeedDifference(percentage);
 }
 
+void TrafficManagerRemote::SetLaneOffset(const ActorPtr &_actor, const float offset) {
+  carla::rpc::Actor actor(_actor->Serialize());
+
+  client.SetLaneOffset(actor, offset);
+}
+
+void TrafficManagerRemote::SetGlobalLaneOffset(const float offset) {
+  client.SetGlobalLaneOffset(offset);
+}
+
 void TrafficManagerRemote::SetUpdateVehicleLights(const ActorPtr &_actor, const bool do_update) {
   carla::rpc::Actor actor(_actor->Serialize());
 

LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h

@@ -57,6 +57,10 @@ public:
   /// If less than 0, it's a % increase.
   void SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage);
 
+  /// Method to set a lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetLaneOffset(const ActorPtr &actor, const float offset);
+
   /// Set a vehicle's exact desired velocity.
   void SetDesiredSpeed(const ActorPtr &actor, const float value);
 
@@ -64,6 +68,10 @@ public:
   /// If less than 0, it's a % increase.
   void SetGlobalPercentageSpeedDifference(float const percentage);
 
+  /// Method to set a global lane offset displacement from the center line.
+  /// Positive values imply a right offset while negative ones mean a left one.
+  void SetGlobalLaneOffset(float const offset);
+
   /// Method to set the automatic management of the vehicle lights
   void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update);
 

LibCarla/source/carla/trafficmanager/TrafficManagerServer.h

@@ -94,6 +94,12 @@ public:
         tm->SetPercentageSpeedDifference(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
       });
 
+      /// Method to set a lane offset displacement from the center line.
+      /// Positive values imply a right offset while negative ones mean a left one.
+      server->bind("set_lane_offset", [=](carla::rpc::Actor actor, const float offset) {
+        tm->SetLaneOffset(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), offset);
+      });
+
       /// Set a vehicle's exact desired velocity.
       server->bind("set_desired_speed", [=](carla::rpc::Actor actor, const float value) {
         tm->SetDesiredSpeed(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), value);
@@ -110,6 +116,13 @@ public:
         tm->SetGlobalPercentageSpeedDifference(percentage);
       });
 
+      /// Method to set a global lane offset displacement from the center line.
+      /// Positive values imply a right offset while negative ones mean a left one.
+      server->bind("set_global_lane_offset", [=](const float offset) {
+        tm->SetGlobalLaneOffset(offset);
+      });
+
+
       /// Method to set collision detection rules between vehicles.
       server->bind("set_collision_detection", [=](const carla::rpc::Actor &reference_actor, const carla::rpc::Actor &other_actor, const bool detect_collision) {
         const auto reference = carla::client::detail::ActorVariant(reference_actor).Get(tm->GetEpisodeProxy());

PythonAPI/carla/source/libcarla/TrafficManager.cpp

@@ -87,8 +87,10 @@ void export_trafficmanager() {
   class_<ctm::TrafficManager>("TrafficManager", no_init)
     .def("get_port", &ctm::TrafficManager::Port)
     .def("vehicle_percentage_speed_difference", &ctm::TrafficManager::SetPercentageSpeedDifference)
+    .def("vehicle_lane_offset", &ctm::TrafficManager::SetLaneOffset)
     .def("set_desired_speed", &ctm::TrafficManager::SetDesiredSpeed)
     .def("global_percentage_speed_difference", &ctm::TrafficManager::SetGlobalPercentageSpeedDifference)
+    .def("global_lane_offset", &ctm::TrafficManager::SetGlobalLaneOffset)
     .def("update_vehicle_lights", &ctm::TrafficManager::SetUpdateVehicleLights)
     .def("collision_detection", &ctm::TrafficManager::SetCollisionDetection)
     .def("force_lane_change", &ctm::TrafficManager::SetForceLaneChange)

PythonAPI/docs/client.yml

@@ -414,6 +414,17 @@
 
         Default is 30. Exceeding a speed limit can be done using negative percentages.
     # --------------------------------------
+    - def_name: global_lane_offset
+      params:
+      - param_name: offset
+        type: float
+        doc: >
+          Lane offset displacement from the center line.
+      doc: >
+        Sets a global lane offset displacement from the center line. Positive values imply a right offset while negative ones mean a left one.
+
+        Default is 0. Numbers high enough to cause the vehicle to drive through other lanes might break the controller.
+    # --------------------------------------
     - def_name: ignore_lights_percentage
       params:
       - param_name: actor
@@ -481,6 +492,21 @@
 
         Default is 30. Exceeding a speed limit can be done using negative percentages.
     # --------------------------------------
+    - def_name: vehicle_lane_offset
+      params:
+      - param_name: actor
+        type: carla.Actor
+        doc: >
+          Vehicle whose lane offset behaviour is being changed.
+      - param_name: offset
+        type: float
+        doc: >
+          Lane offset displacement from the center line.
+      doc: >
+        Sets a lane offset displacement from the center line. Positive values imply a right offset while negative ones mean a left one.
+
+        Default is 0. Numbers high enough to cause the vehicle to drive through other lanes might break the controller.
+    # --------------------------------------
     - def_name: update_vehicle_lights
       params:
       - param_name: actor