Merge branch 'dev' into julee/fix_enum_to_string

.github/FUNDING.yml

@@ -0,0 +1,3 @@
+# These are supported funding model platforms
+
+github: carla-simulator

CHANGELOG.md

@@ -1,12 +1,47 @@
 ## Latest
-  * Improved handling of collisions in Traffic Manager when driving at very high speeds.
-  * Added open/close doors feature for vehicles.
-  * Added API functions to 3D vectors: `squared_length`, `length`, `make_unit_vector`, `dot`, `dot_2d`, `distance`, `distance_2d`, `distance_squared`, `distance_squared_2d`, `get_vector_angle`
-  * Added API functions to 2D vectors: `squared_length`, `length`, `make_unit_vector`
-  * Added missing dependency `libomp5` to Release.Dockerfile
-  * Added physical simulation to vehicle doors, capable of opening and closing
-  * Fixed global route planner crash when being used at maps without lane markings
-  * Added two new examples to PythonAPI/util: Conversion of OpenStreetMaps to OpenDRIVE maps `osm_to_xodr.py` and Extraction of map spawn points `extract_spawn_points.py`
+
+  * Added native ackermann controller:
+    - `apply_ackermann_control`:  to apply an ackermann control command to a vehicle
+    - `get_ackermann_controller_settings`: to get the last ackermann controller settings applied
+    - `apply_ackermann_controller_settings`: to apply new ackermann controller settings
+  * Fixed bug causing the Traffic Manager to not be deterministic when using hybrid mode
+  * Added `NormalsSensor`, a new sensor with normals information
+  * Added support for N wheeled vehicles
+  * Added support for new batch commands ConsoleCommand, ApplyLocation (to actor), SetTrafficLightState
+  * Switch to boost::variant2 for rpc::Command as that allows more than 20 RPC commands
+  * Added post process effects for rainy and dusty weathers.
+
+## CARLA 0.9.13
+
+  * Added new **instance aware semantic segmentation** sensor `sensor.camera.instance_segmentation`
+  * Added new API classes: `MaterialParameter`, `TextureColor` and  `TextureFloatColor` to encode texture data and field (normal map, diffuse, etc)
+  * Added new API functions: `apply_color_texture_to_object`, `apply_float_color_texture_to_object` and `apply_textures_to_object` to paint objects in **runtime**
+  * Added the option for users to set a **route** using RoadOption elements to a vehicle controlled by the Traffic Manager.
+  * **Cache** now has an extra folder with current version of CARLA (so different cache per version)
+  * Added **set_percentage_random_left_lanechange** and **set_percentage_random_right_lanechange**.
+  * Improved handling of **collisions** in Traffic Manager when driving at **very high speeds**.
+  * Added physical simulation to **vehicle doors**, capable of opening and closing
+  * Added **open/close doors** feature for vehicles.
+  * Added API functions to **3D vectors**: `squared_length`, `length`, `make_unit_vector`, `dot`, `dot_2d`, `distance`, `distance_2d`, `distance_squared`, `distance_squared_2d`, `get_vector_angle`
+  * Added API functions to **2D vectors**: `squared_length`, `length`, `make_unit_vector`
+  * Added a **seed** for better reproducibility of pedestrians
+    - New API function `set_pedestrians_seed`
+    - New parameter **--seedw** in generate_traffic.py script
+  * Added missing dependency `libomp5` to **Release.Dockerfile**
+  * Added API functions to interact with **pedestrian bones**:
+    - `get_bones / set_bones`:  to get/set the bones of a pedestrian
+    - `blend_pose`:  to blend a custom pose with current animation
+    - `show_pose / hide_pose`:  to show or hide the custom pose
+    - `get_pose_from_animation`:  to set the custom pose with the animation current frame
+  * Added a new script in **PythonAPI/examples/draw_skeleton.py** to draw the bones of a pedestrian from client side
+  * Improved **collision** detection of the Python agents
+  * Added the new **VehicleLightStage** to the Traffic Manager to dynamically update the vehicle lights.
+  * Added two new examples to **PythonAPI/util**: Conversion of OpenStreetMaps to OpenDRIVE maps `osm_to_xodr.py` and Extraction of map spawn points `extract_spawn_points.py`
+  * Fixed the **import of props** without any map
+  * Fixed **global route planner** crash when being used at maps without lane markings
+  * Fixed bug causing the server to **sigsegv** when a vehicle collides an environment object in recording mode.
+  * Fixed **RSSSensor**: made client side calculations threaded
+  * Fixed **keep_right_rule** parameter.
 
 ## CARLA 0.9.12
 

Docs/adv_traffic_manager.md

@@ -76,6 +76,11 @@ The TM generates viable commands for all vehicles in the [vehicle registry](#veh
 
 >Vehicle movement is computed based on the defined path. A [PID controller](#pid-controller) determines how to reach the target waypoints. This is then translated into a CARLA command for application in the next step.
 
+>__2.5 - [Vehicle Lights Stage](#stage-5-vehicle-lights-stage).__
+
+> The vehicle lights switch on/off dynamically based on environmental factors (e.g. sunlight and the presence of fog or rain) and vehicle behavior (e.g. turning on direction indicators if the vehicle will turn left/right at the next junction, or turn on the stop lights if braking).
+
+
 __3. Apply the commands in the simulation.__
 
 Commands generated in the previous step are collected into the [command array](#command-array) and sent to the CARLA server in a batch to be applied in the same frame.
@@ -119,7 +124,7 @@ __Related .cpp files:__ `SimulationState.cpp`, `SimulationState.h`.
 
 ### Control loop
 
-The control loop manages the calculations of the next command for all autopilot vehicles so they are performed synchronously. The control loop consists of four different [stages](#stages-of-the-control-loop); localization, collision, traffic light, and motion planner.
+The control loop manages the calculations of the next command for all autopilot vehicles so they are performed synchronously. The control loop consists of five different [stages](#stages-of-the-control-loop); localization, collision, traffic light, motion planner and vehicle lights.
 
 The control loop:
 
@@ -128,7 +133,7 @@ The control loop:
 - Divides calculations into a series of [stages](#stages-of-the-control-loop).
 - Creates synchronization barriers between stages to guarantee consistency. Calculations for all vehicles are finished before any of them move to the next stage, ensuring all vehicles are updated in the same frame.
 - Coordinates the transition between [stages](#stages-of-the-control-loop) so all calculations are done in sync.
-- Sends the [command array](#command-array) to the server when the last stage ([Motion Planner Stage](#stage-4-motion-planner-stage)) finishes so there are no frame delays between the command calculations and the command application.
+- Sends the [command array](#command-array) to the server when the last stages ([Motion Planner Stage](#stage-4-motion-planner-stage) and [Vehicle Lights Stage](#stage-5-vehicle-lights-stage)) finishes so there are no frame delays between the command calculations and the command application.
 
 __Related .cpp files:__ `TrafficManagerLocal.cpp`.
 
@@ -228,6 +233,25 @@ The Motion Planner Stage:
 
 __Related .cpp files:__ `MotionPlannerStage.cpp`.
 
+
+##### Stage 5- Vehicle Lights Stage
+
+The Vehicle Lights Stage activates the lights based on the condition of the vehicle and the surrounding environment.
+	
+The Vehicle Lights Stage:
+
+- Retrieves the planned waypoints for the vehicle, information about vehicle lights (eg. light state and the planned command to be applied) and the weather conditions.
+
+- Determines the new state of the vehicle lights:
+	- Turns on the blinkers if the vehicle is planning to turn left/right at the next junction.
+	- Turns on the stop lights if the applied command is asking the vehicle to brake.
+	- Turns on the low beams and the position lights from sunset to dawn, or under heavy rain.
+	- Turns on the fog lights under heavy fog conditions.
+
+- Update the vehicle lights state if it has changed.
+
+__Related .cpp files:__ `VehicleLightStage.cpp`.
+
 ---
 ## Using the Traffic Manager
 
@@ -315,6 +339,20 @@ for v in my_vehicles:
   tm.auto_lane_change(v,False)
 ``` 
 
+#### Delegating the Traffic Manager to automatically update vehicle lights
+
+By default, vehicle lights (brake, turn indicators, etc...) of the vehicles managed by the TM are never updated. It is possible to delegate the TM to update the vehicle lights of a given vehicle actor:
+
+```python
+tm = client.get_trafficmanager(port)
+for actor in my_vehicles:
+  tm.update_vehicle_lights(actor, True)
+```
+
+Vehicle lights management has to be specified on a per-vehicle basis, and there could be at any given time both vehicles with and without the automatic light management.
+
+
+
 ### Stopping a Traffic Manager
 
 The TM is not an actor that needs to be destroyed; it will stop when the client that created it stops. This is automatically managed by the API, the user does not have to do anything. However, when shutting down a TM, the user must destroy the vehicles controlled by it, otherwise they will remain immobile on the map. The script `generate_traffic.py` does this automatically:

Docs/build_linux.md

@@ -28,6 +28,11 @@ If you come across errors or difficulties then have a look at the **[F.A.Q.](bui
 * __Two TCP ports and good internet connection.__ 2000 and 2001 by default. Make sure that these ports are not blocked by firewalls or any other applications. 
 
 
+!!! Warning
+    __If you are upgrading from CARLA 0.9.12 to 0.9.13__: you must first upgrade the CARLA fork of the UE4 engine to the latest version. See the [__Unreal Engine__](#unreal-engine) section for details on upgrading UE4
+
+
+
 ### Software requirements
 
 CARLA requires many different kinds of software to run. Some are built during the CARLA build process itself, such as *Boost.Python*. Others are binaries that should be installed before starting the build (*cmake*, *clang*, different versions of *Python*, etc.). To install these requirements, run the following commands:

Docs/build_windows.md

@@ -33,6 +33,10 @@ In this section you will find details of system requirements, minor and major so
 * __An adequate GPU.__ CARLA aims for realistic simulations, so the server needs at least a 6 GB GPU although 8 GB is recommended. A dedicated GPU is highly recommended for machine learning. 
 * __Two TCP ports and good internet connection.__ 2000 and 2001 by default. Make sure that these ports are not blocked by firewalls or any other applications. 
 
+
+!!! Warning
+    __If you are upgrading from CARLA 0.9.12 to 0.9.13__: you must first upgrade the CARLA fork of the UE4 engine to the latest version. See the [__Unreal Engine__](#unreal-engine) section for details on upgrading UE4
+
 ### Software requirements
 
 #### Minor installations

Docs/download.md

@@ -2,7 +2,7 @@
 
 ### Latest Release
 
-- [CARLA 0.9.12](https://github.com/carla-simulator/carla/releases/tag/0.9.12/) - [Documentation](https://carla.readthedocs.io/en/0.9.12/)
+- [CARLA 0.9.13](https://github.com/carla-simulator/carla/releases/tag/0.9.13/) - [Documentation](https://carla.readthedocs.io/en/0.9.13/)
 
 ### Nightly build
 
@@ -17,6 +17,7 @@
 
 > Here are the previous versions of CARLA with links to the specific documentation for each version:
 
+- [CARLA 0.9.12](https://github.com/carla-simulator/carla/releases/tag/0.9.12/) - [Documentation](https://carla.readthedocs.io/en/0.9.12/)
 - [CARLA 0.9.11](https://github.com/carla-simulator/carla/releases/tag/0.9.11/) - [Documentation](https://carla.readthedocs.io/en/0.9.11/)
 - [CARLA 0.9.10](https://github.com/carla-simulator/carla/releases/tag/0.9.10/) - [Documentation](https://carla.readthedocs.io/en/0.9.10/)
 - [CARLA 0.9.9](https://github.com/carla-simulator/carla/releases/tag/0.9.9/) - [Documentation](https://carla.readthedocs.io/en/0.9.9/)

Docs/tuto_D_create_semantic_tags.md

@@ -61,7 +61,7 @@ The new semantic tag is ready to be used. Only the meshes stored inside the UE f
 
 This step is not directly related with semantic segmentation. However, these tags can be used to filter the bounding box query in [carla.World](python_api.md#carla.World). In order to do this, the tag must be added to the [carla.CityObjectLabel](python_api.md#carla.CityObjectLabel) enum in the PythonAPI.  
 
-__Open `World.cpp`__ in `carla/PythonAPI/carla/sourc/libcarla` and add the new tag by the end of the enum.  
+__Open `World.cpp`__ in `carla/PythonAPI/carla/source/libcarla` and add the new tag by the end of the enum.  
 
 ![city_object_label](img/tuto_D_create_semantic_tags/06_city_object_label.jpg)
 
@@ -84,4 +84,4 @@ Sensors reference</a>
 Add new props</a>
 </p>
 
-</div>
+</div>

LibCarla/source/carla/MsgPackAdaptors.h

@@ -10,7 +10,16 @@
 #include "carla/MsgPack.h"
 
 #include <boost/optional.hpp>
-#include <boost/variant.hpp>
+
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
 
 #include <tuple>
 
@@ -83,15 +92,15 @@ namespace adaptor {
   };
 
   // ===========================================================================
-  // -- Adaptors for boost::variant --------------------------------------------
+  // -- Adaptors for boost::variant2::variant ----------------------------------
   // ===========================================================================
 
   template<typename... Ts>
-  struct convert<boost::variant<Ts...>> {
+  struct convert<boost::variant2::variant<Ts...>> {
 
     const clmdep_msgpack::object &operator()(
         const clmdep_msgpack::object &o,
-        boost::variant<Ts...> &v) const {
+        boost::variant2::variant<Ts...> &v) const {
       if (o.type != clmdep_msgpack::type::ARRAY) {
         ::carla::throw_exception(clmdep_msgpack::type_error());
       }
@@ -108,7 +117,7 @@ namespace adaptor {
     template <uint64_t I>
     static void copy_to_variant_impl(
         const clmdep_msgpack::object &o,
-        boost::variant<Ts...> &v) {
+        boost::variant2::variant<Ts...> &v) {
       /// @todo Workaround for finding the type.
       auto dummy = std::get<I>(std::tuple<Ts...>{});
       using T = decltype(dummy);
@@ -119,7 +128,7 @@ namespace adaptor {
     static void copy_to_variant(
         const uint64_t index,
         const clmdep_msgpack::object &o,
-        boost::variant<Ts...> &v,
+        boost::variant2::variant<Ts...> &v,
         std::index_sequence<Is...>) {
       std::initializer_list<int> ({
         (index == Is ? copy_to_variant_impl<Is>(o, v), 0 : 0)...
@@ -128,30 +137,30 @@ namespace adaptor {
   };
 
   template<typename... Ts>
-  struct pack<boost::variant<Ts...>> {
+  struct pack<boost::variant2::variant<Ts...>> {
     template <typename Stream>
     packer<Stream> &operator()(
         clmdep_msgpack::packer<Stream> &o,
-        const boost::variant<Ts...> &v) const {
+        const boost::variant2::variant<Ts...> &v) const {
       o.pack_array(2);
-      o.pack(static_cast<uint64_t>(v.which()));
-      boost::apply_visitor([&](const auto &value) { o.pack(value); }, v);
+      o.pack(static_cast<uint64_t>(v.index()));
+      boost::variant2::visit([&](const auto &value) { o.pack(value); }, v);
       return o;
     }
   };
 
   template<typename... Ts>
-  struct object_with_zone<boost::variant<Ts...>> {
+  struct object_with_zone<boost::variant2::variant<Ts...>> {
     void operator()(
         clmdep_msgpack::object::with_zone &o,
-        const boost::variant<Ts...> &v) const {
+        const boost::variant2::variant<Ts...> &v) const {
       o.type = type::ARRAY;
       o.via.array.size = 2;
       o.via.array.ptr = static_cast<clmdep_msgpack::object*>(o.zone.allocate_align(
           sizeof(clmdep_msgpack::object) * o.via.array.size,
           MSGPACK_ZONE_ALIGNOF(clmdep_msgpack::object)));
-      o.via.array.ptr[0] = clmdep_msgpack::object(static_cast<uint64_t>(v.which()), o.zone);
-      boost::apply_visitor([&](const auto &value) {
+      o.via.array.ptr[0] = clmdep_msgpack::object(static_cast<uint64_t>(v.index()), o.zone);
+      boost::variant2::visit([&](const auto &value) {
         o.via.array.ptr[1] = clmdep_msgpack::object(value, o.zone);
       }, v);
     }

LibCarla/source/carla/RecurrentSharedFuture.h

@@ -10,7 +10,15 @@
 #include "carla/Time.h"
 
 #include <boost/optional.hpp>
-#include <boost/variant.hpp>
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
 
 #include <condition_variable>
 #include <exception>
@@ -63,7 +71,7 @@ namespace detail {
 
     struct mapped_type {
       bool should_wait;
-      boost::variant<SharedException, T> value;
+      boost::variant2::variant<SharedException, T> value;
     };
 
     std::map<const char *, mapped_type> _map;
@@ -109,10 +117,10 @@ namespace detail {
     if (!_cv.wait_for(lock, timeout.to_chrono(), [&]() { return !r.should_wait; })) {
       return {};
     }
-    if (r.value.which() == 0) {
-      throw_exception(boost::get<SharedException>(r.value));
+    if (r.value.index() == 0) {
+      throw_exception(boost::variant2::get<SharedException>(r.value));
     }
-    return boost::get<T>(std::move(r.value));
+    return boost::variant2::get<T>(std::move(r.value));
   }
 
   template <typename T>

LibCarla/source/carla/client/FIleTransfer.cpp

@@ -5,6 +5,7 @@
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
 #include "FileTransfer.h"
+#include "carla/Version.h"
 
 namespace carla {
 namespace client {
@@ -33,11 +34,21 @@ namespace client {
   bool FileTransfer::FileExists(std::string file) {
     // Check if the file exists or not
     struct stat buffer;
-    return (stat((_filesBaseFolder + file).c_str(), &buffer) == 0);
+    std::string fullpath = _filesBaseFolder;
+    fullpath += "/";
+    fullpath += ::carla::version();
+    fullpath += "/";
+    fullpath += file;
+
+    return (stat(fullpath.c_str(), &buffer) == 0);
   }
 
   bool FileTransfer::WriteFile(std::string path, std::vector<uint8_t> content) {
-    std::string writePath = _filesBaseFolder + path;
+    std::string writePath = _filesBaseFolder;
+    writePath += "/";
+    writePath += ::carla::version();
+    writePath += "/";
+    writePath += path;
 
     // Validate and create the file path
     carla::FileSystem::ValidateFilePath(writePath);
@@ -56,8 +67,13 @@ namespace client {
   }
 
   std::vector<uint8_t> FileTransfer::ReadFile(std::string path) {
+    std::string fullpath = _filesBaseFolder;
+    fullpath += "/";
+    fullpath += ::carla::version();
+    fullpath += "/";
+    fullpath += path;
     // Read the binary file from the base folder
-    std::ifstream file(_filesBaseFolder + path, std::ios::binary);
+    std::ifstream file(fullpath, std::ios::binary);
     std::vector<uint8_t> content(std::istreambuf_iterator<char>(file), {});
     return content;
   }

LibCarla/source/carla/client/Vehicle.cpp

@@ -55,6 +55,18 @@ namespace client {
     }
   }
 
+  void Vehicle::ApplyAckermannControl(const AckermannControl &control) {
+    GetEpisode().Lock()->ApplyAckermannControlToVehicle(*this, control);
+  }
+
+  rpc::AckermannControllerSettings Vehicle::GetAckermannControllerSettings() const {
+    return GetEpisode().Lock()->GetAckermannControllerSettings(*this);
+  }
+
+  void Vehicle::ApplyAckermannControllerSettings(const rpc::AckermannControllerSettings &settings) {
+    GetEpisode().Lock()->ApplyAckermannControllerSettings(*this, settings);
+  }
+
   void Vehicle::ApplyPhysicsControl(const PhysicsControl &physics_control) {
     GetEpisode().Lock()->ApplyPhysicsControlToVehicle(*this, physics_control);
   }

LibCarla/source/carla/client/Vehicle.h

@@ -7,7 +7,9 @@
 #pragma once
 
 #include "carla/client/Actor.h"
+#include "carla/rpc/AckermannControllerSettings.h"
 #include "carla/rpc/TrafficLightState.h"
+#include "carla/rpc/VehicleAckermannControl.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/VehicleDoor.h"
 #include "carla/rpc/VehicleLightState.h"
@@ -31,6 +33,7 @@ namespace client {
   public:
 
     using Control = rpc::VehicleControl;
+    using AckermannControl = rpc::VehicleAckermannControl;
     using PhysicsControl = rpc::VehiclePhysicsControl;
     using LightState = rpc::VehicleLightState::LightState;
     using TM = traffic_manager::TrafficManager;
@@ -49,6 +52,17 @@ namespace client {
     /// Apply @a control to this vehicle.
     void ApplyControl(const Control &control);
 
+    /// Apply @a control to this vehicle.
+    void ApplyAckermannControl(const AckermannControl &control);
+
+    /// Return the last Ackermann controller settings applied to this vehicle.
+    ///
+    /// @warning This function does call the simulator.
+    rpc::AckermannControllerSettings GetAckermannControllerSettings() const;
+
+    /// Apply Ackermann control settings to this vehicle
+    void ApplyAckermannControllerSettings(const rpc::AckermannControllerSettings &settings);
+
     /// Apply physics control to this vehicle.
     void ApplyPhysicsControl(const PhysicsControl &physics_control);
 

LibCarla/source/carla/client/Walker.cpp

@@ -18,12 +18,25 @@ namespace client {
     }
   }
 
-  void Walker::ApplyControl(const BoneControl &bone_control) {
-    GetEpisode().Lock()->ApplyBoneControlToWalker(*this, bone_control);
-  }
-
   Walker::Control Walker::GetWalkerControl() const {
     return GetEpisode().Lock()->GetActorSnapshot(*this).state.walker_control;
   }
+
+  Walker::BoneControlOut Walker::GetBonesTransform() {
+    return GetEpisode().Lock()->GetBonesTransform(*this);
+  }
+
+  void Walker::SetBonesTransform(const Walker::BoneControlIn &bones) {
+    return GetEpisode().Lock()->SetBonesTransform(*this, bones);
+  }
+
+  void Walker::BlendPose(float blend) {
+    return GetEpisode().Lock()->BlendPose(*this, blend);
+  }
+
+  void Walker::GetPoseFromAnimation() {
+    return GetEpisode().Lock()->GetPoseFromAnimation(*this);
+  }
+
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/Walker.h

@@ -8,7 +8,8 @@
 
 #include "carla/client/Actor.h"
 #include "carla/rpc/WalkerControl.h"
-#include "carla/rpc/WalkerBoneControl.h"
+#include "carla/rpc/WalkerBoneControlIn.h"
+#include "carla/rpc/WalkerBoneControlOut.h"
 
 namespace carla {
 namespace client {
@@ -17,21 +18,27 @@ namespace client {
   public:
 
     using Control = rpc::WalkerControl;
-    using BoneControl = rpc::WalkerBoneControl;
+    using BoneControlIn = rpc::WalkerBoneControlIn;
+    using BoneControlOut = rpc::WalkerBoneControlOut;
 
     explicit Walker(ActorInitializer init) : Actor(std::move(init)) {}
 
     /// Apply @a control to this Walker.
     void ApplyControl(const Control &control);
 
-    void ApplyControl(const BoneControl &bone_control);
-
     /// Return the control last applied to this Walker.
     ///
     /// @note This function does not call the simulator, it returns the Control
     /// received in the last tick.
     Control GetWalkerControl() const;
 
+    BoneControlOut GetBonesTransform();
+    void SetBonesTransform(const BoneControlIn &bones);
+    void BlendPose(float blend);
+    void ShowPose() { BlendPose(1.0f); };
+    void HidePose() { BlendPose(0.0f); };
+    void GetPoseFromAnimation();
+
   private:
 
     Control _control;

LibCarla/source/carla/client/World.cpp

@@ -160,6 +160,10 @@ namespace client {
     _episode.Lock()->SetPedestriansCrossFactor(percentage);
   }
 
+  void World::SetPedestriansSeed(unsigned int seed) {
+    _episode.Lock()->SetPedestriansSeed(seed);
+  }
+
   SharedPtr<Actor> World::GetTrafficSign(const Landmark& landmark) const {
     SharedPtr<ActorList> actors = GetActors();
     SharedPtr<TrafficSign> result;
@@ -291,5 +295,93 @@ namespace client {
     return Result;
   }
 
+  void World::ApplyColorTextureToObject(
+      const std::string &object_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureColor& Texture) {
+    _episode.Lock()->ApplyColorTextureToObjects({object_name}, parameter, Texture);
+  }
+
+  void World::ApplyColorTextureToObjects(
+      const std::vector<std::string> &objects_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureColor& Texture) {
+    _episode.Lock()->ApplyColorTextureToObjects(objects_name, parameter, Texture);
+  }
+
+  void World::ApplyFloatColorTextureToObject(
+      const std::string &object_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureFloatColor& Texture) {
+    _episode.Lock()->ApplyColorTextureToObjects({object_name}, parameter, Texture);
+  }
+
+  void World::ApplyFloatColorTextureToObjects(
+      const std::vector<std::string> &objects_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureFloatColor& Texture) {
+    _episode.Lock()->ApplyColorTextureToObjects(objects_name, parameter, Texture);
+  }
+
+  std::vector<std::string> World::GetNamesOfAllObjects() const {
+    return _episode.Lock()->GetNamesOfAllObjects();
+  }
+
+  void World::ApplyTexturesToObject(
+      const std::string &object_name,
+      const rpc::TextureColor& diffuse_texture,
+      const rpc::TextureFloatColor& emissive_texture,
+      const rpc::TextureFloatColor& normal_texture,
+      const rpc::TextureFloatColor& ao_roughness_metallic_emissive_texture)
+  {
+    if (diffuse_texture.GetWidth() && diffuse_texture.GetHeight()) {
+      ApplyColorTextureToObject(
+          object_name, rpc::MaterialParameter::Tex_Diffuse, diffuse_texture);
+    }
+    if (normal_texture.GetWidth() && normal_texture.GetHeight()) {
+      ApplyFloatColorTextureToObject(
+          object_name, rpc::MaterialParameter::Tex_Normal, normal_texture);
+    }
+    if (ao_roughness_metallic_emissive_texture.GetWidth() &&
+        ao_roughness_metallic_emissive_texture.GetHeight()) {
+      ApplyFloatColorTextureToObject(
+          object_name,
+          rpc::MaterialParameter::Tex_Ao_Roughness_Metallic_Emissive,
+          ao_roughness_metallic_emissive_texture);
+    }
+    if (emissive_texture.GetWidth() && emissive_texture.GetHeight()) {
+      ApplyFloatColorTextureToObject(
+          object_name, rpc::MaterialParameter::Tex_Emissive, emissive_texture);
+    }
+  }
+
+  void World::ApplyTexturesToObjects(
+      const std::vector<std::string> &objects_names,
+      const rpc::TextureColor& diffuse_texture,
+      const rpc::TextureFloatColor& emissive_texture,
+      const rpc::TextureFloatColor& normal_texture,
+      const rpc::TextureFloatColor& ao_roughness_metallic_emissive_texture)
+  {
+    if (diffuse_texture.GetWidth() && diffuse_texture.GetHeight()) {
+      ApplyColorTextureToObjects(
+          objects_names, rpc::MaterialParameter::Tex_Diffuse, diffuse_texture);
+    }
+    if (normal_texture.GetWidth() && normal_texture.GetHeight()) {
+      ApplyFloatColorTextureToObjects(
+          objects_names, rpc::MaterialParameter::Tex_Normal, normal_texture);
+    }
+    if (ao_roughness_metallic_emissive_texture.GetWidth() &&
+        ao_roughness_metallic_emissive_texture.GetHeight()) {
+      ApplyFloatColorTextureToObjects(
+          objects_names,
+          rpc::MaterialParameter::Tex_Ao_Roughness_Metallic_Emissive,
+          ao_roughness_metallic_emissive_texture);
+    }
+    if (emissive_texture.GetWidth() && emissive_texture.GetHeight()) {
+      ApplyFloatColorTextureToObjects(
+          objects_names, rpc::MaterialParameter::Tex_Emissive, emissive_texture);
+    }
+  }
+
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/World.h

@@ -26,6 +26,8 @@
 #include "carla/rpc/VehiclePhysicsControl.h"
 #include "carla/rpc/WeatherParameters.h"
 #include "carla/rpc/VehicleLightStateList.h"
+#include "carla/rpc/Texture.h"
+#include "carla/rpc/MaterialParameter.h"
 
 #include <boost/optional.hpp>
 
@@ -143,6 +145,9 @@ namespace client {
     /// percentage of 1.0f means all pedestrians can cross roads if needed
     void SetPedestriansCrossFactor(float percentage);
 
+    /// set the seed to use with random numbers in the pedestrians module
+    void SetPedestriansSeed(unsigned int seed);
+
     SharedPtr<Actor> GetTrafficSign(const Landmark& landmark) const;
 
     SharedPtr<Actor> GetTrafficLight(const Landmark& landmark) const;
@@ -187,6 +192,44 @@ namespace client {
     std::vector<SharedPtr<Actor>> GetTrafficLightsInJunction(
         const road::JuncId junc_id) const;
 
+    // std::vector<std::string> GetObjectNameList();
+
+    void ApplyColorTextureToObject(
+        const std::string &actor_name,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureColor& Texture);
+
+    void ApplyColorTextureToObjects(
+        const std::vector<std::string> &objects_names,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureColor& Texture);
+
+    void ApplyFloatColorTextureToObject(
+        const std::string &actor_name,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureFloatColor& Texture);
+
+    void ApplyFloatColorTextureToObjects(
+        const std::vector<std::string> &objects_names,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureFloatColor& Texture);
+
+    void ApplyTexturesToObject(
+        const std::string &actor_name,
+        const rpc::TextureColor& diffuse_texture,
+        const rpc::TextureFloatColor& emissive_texture,
+        const rpc::TextureFloatColor& normal_texture,
+        const rpc::TextureFloatColor& ao_roughness_metallic_emissive_texture);
+
+    void ApplyTexturesToObjects(
+        const std::vector<std::string> &objects_names,
+        const rpc::TextureColor& diffuse_texture,
+        const rpc::TextureFloatColor& emissive_texture,
+        const rpc::TextureFloatColor& normal_texture,
+        const rpc::TextureFloatColor& ao_roughness_metallic_emissive_texture);
+
+    std::vector<std::string> GetNamesOfAllObjects() const;
+
   private:
 
     detail::EpisodeProxy _episode;

LibCarla/source/carla/client/detail/ActorVariant.cpp

@@ -16,7 +16,7 @@ namespace detail {
   void ActorVariant::MakeActor(EpisodeProxy episode) const {
     _value = detail::ActorFactory::MakeActor(
         episode,
-        boost::get<rpc::Actor>(std::move(_value)),
+        boost::variant2::get<rpc::Actor>(std::move(_value)),
         GarbageCollectionPolicy::Disabled);
   }
 

LibCarla/source/carla/client/detail/ActorVariant.h

@@ -12,7 +12,15 @@
 #include "carla/client/detail/EpisodeProxy.h"
 #include "carla/rpc/Actor.h"
 
-#include <boost/variant.hpp>
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
 
 namespace carla {
 namespace client {
@@ -39,15 +47,15 @@ namespace detail {
     }
 
     SharedPtr<client::Actor> Get(EpisodeProxy episode) const {
-      if (_value.which() == 0u) {
+      if (_value.index() == 0u) {
         MakeActor(episode);
       }
-      DEBUG_ASSERT(_value.which() == 1u);
-      return boost::get<SharedPtr<client::Actor>>(_value);
+      DEBUG_ASSERT(_value.index() == 1u);
+      return boost::variant2::get<SharedPtr<client::Actor>>(_value);
     }
 
     const rpc::Actor &Serialize() const {
-      return boost::apply_visitor(Visitor(), _value);
+      return boost::variant2::visit(Visitor(), _value);
     }
 
     ActorId GetId() const {
@@ -83,7 +91,7 @@ namespace detail {
 
     void MakeActor(EpisodeProxy episode) const;
 
-    mutable boost::variant<rpc::Actor, SharedPtr<client::Actor>> _value;
+    mutable boost::variant2::variant<rpc::Actor, SharedPtr<client::Actor>> _value;
   };
 
 } // namespace detail

LibCarla/source/carla/client/detail/Client.cpp

@@ -10,14 +10,17 @@
 #include "carla/Version.h"
 #include "carla/client/FileTransfer.h"
 #include "carla/client/TimeoutException.h"
+#include "carla/rpc/AckermannControllerSettings.h"
 #include "carla/rpc/ActorDescription.h"
-#include "carla/rpc/BoneTransformData.h"
+#include "carla/rpc/BoneTransformDataIn.h"
 #include "carla/rpc/Client.h"
 #include "carla/rpc/DebugShape.h"
 #include "carla/rpc/Response.h"
+#include "carla/rpc/VehicleAckermannControl.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/VehicleLightState.h"
-#include "carla/rpc/WalkerBoneControl.h"
+#include "carla/rpc/WalkerBoneControlIn.h"
+#include "carla/rpc/WalkerBoneControlOut.h"
 #include "carla/rpc/WalkerControl.h"
 #include "carla/streaming/Client.h"
 
@@ -161,6 +164,24 @@ namespace detail {
     _pimpl->CallAndWait<void>("copy_opendrive_to_file", std::move(opendrive), params);
   }
 
+  void Client::ApplyColorTextureToObjects(
+      const std::vector<std::string> &objects_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureColor& Texture) {
+    _pimpl->CallAndWait<void>("apply_color_texture_to_objects", objects_name, parameter, Texture);
+  }
+
+  void Client::ApplyColorTextureToObjects(
+      const std::vector<std::string> &objects_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureFloatColor& Texture) {
+    _pimpl->CallAndWait<void>("apply_float_color_texture_to_objects", objects_name, parameter, Texture);
+  }
+
+  std::vector<std::string> Client::GetNamesOfAllObjects() const {
+    return _pimpl->CallAndWait<std::vector<std::string>>("get_names_of_all_objects");
+  }
+
   rpc::EpisodeInfo Client::GetEpisodeInfo() {
     return _pimpl->CallAndWait<rpc::EpisodeInfo>("get_episode_info");
   }
@@ -386,7 +407,7 @@ namespace detail {
   }
 
   void Client::SetActorSimulatePhysics(rpc::ActorId actor, const bool enabled) {
-    _pimpl->AsyncCall("set_actor_simulate_physics", actor, enabled);
+    _pimpl->CallAndWait<void>("set_actor_simulate_physics", actor, enabled);
   }
 
   void Client::SetActorEnableGravity(rpc::ActorId actor, const bool enabled) {
@@ -405,6 +426,19 @@ namespace detail {
     _pimpl->AsyncCall("apply_control_to_vehicle", vehicle, control);
   }
 
+  void Client::ApplyAckermannControlToVehicle(rpc::ActorId vehicle, const rpc::VehicleAckermannControl &control) {
+    _pimpl->AsyncCall("apply_ackermann_control_to_vehicle", vehicle, control);
+  }
+
+  rpc::AckermannControllerSettings Client::GetAckermannControllerSettings(
+      rpc::ActorId vehicle) const {
+    return _pimpl->CallAndWait<carla::rpc::AckermannControllerSettings>("get_ackermann_controller_settings", vehicle);
+  }
+
+  void Client::ApplyAckermannControllerSettings(rpc::ActorId vehicle, const rpc::AckermannControllerSettings &settings) {
+    _pimpl->AsyncCall("apply_ackermann_controller_settings", vehicle, settings);
+  }
+
   void Client::EnableCarSim(rpc::ActorId vehicle, std::string simfile_path) {
     _pimpl->AsyncCall("enable_carsim", vehicle, simfile_path);
   }
@@ -435,8 +469,21 @@ namespace detail {
     _pimpl->AsyncCall("apply_control_to_walker", walker, control);
   }
 
-  void Client::ApplyBoneControlToWalker(rpc::ActorId walker, const rpc::WalkerBoneControl &control) {
-    _pimpl->AsyncCall("apply_bone_control_to_walker", walker, control);
+  rpc::WalkerBoneControlOut Client::GetBonesTransform(rpc::ActorId walker) {
+    auto res = _pimpl->CallAndWait<rpc::WalkerBoneControlOut>("get_bones_transform", walker);
+    return res;
+  }
+
+  void Client::SetBonesTransform(rpc::ActorId walker, const rpc::WalkerBoneControlIn &bones) {
+    _pimpl->AsyncCall("set_bones_transform", walker, bones);
+  }
+
+  void Client::BlendPose(rpc::ActorId walker, float blend) {
+    _pimpl->AsyncCall("blend_pose", walker, blend);
+  }
+
+  void Client::GetPoseFromAnimation(rpc::ActorId walker) {
+    _pimpl->AsyncCall("get_pose_from_animation", walker);
   }
 
   void Client::SetTrafficLightState(

LibCarla/source/carla/client/detail/Client.h

@@ -31,6 +31,8 @@
 #include "carla/rpc/VehiclePhysicsControl.h"
 #include "carla/rpc/VehicleWheels.h"
 #include "carla/rpc/WeatherParameters.h"
+#include "carla/rpc/Texture.h"
+#include "carla/rpc/MaterialParameter.h"
 
 #include <functional>
 #include <memory>
@@ -41,11 +43,14 @@
 namespace carla {
   class Buffer;
 namespace rpc {
+  class AckermannControllerSettings;
   class ActorDescription;
   class DebugShape;
+  class VehicleAckermannControl;
   class VehicleControl;
   class WalkerControl;
-  class WalkerBoneControl;
+  class WalkerBoneControlIn;
+  class WalkerBoneControlOut;
 }
 namespace sensor {
   class SensorData;
@@ -102,6 +107,18 @@ namespace detail {
     void CopyOpenDriveToServer(
         std::string opendrive, const rpc::OpendriveGenerationParameters & params);
 
+    void ApplyColorTextureToObjects(
+        const std::vector<std::string> &objects_name,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureColor& Texture);
+
+    void ApplyColorTextureToObjects(
+        const std::vector<std::string> &objects_name,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureFloatColor& Texture);
+
+    std::vector<std::string> GetNamesOfAllObjects() const;
+
     rpc::EpisodeInfo GetEpisodeInfo();
 
     rpc::MapInfo GetMapInfo();
@@ -235,6 +252,16 @@ namespace detail {
         rpc::ActorId vehicle,
         const rpc::VehicleControl &control);
 
+    void ApplyAckermannControlToVehicle(
+        rpc::ActorId vehicle,
+        const rpc::VehicleAckermannControl &control);
+
+    rpc::AckermannControllerSettings GetAckermannControllerSettings(rpc::ActorId vehicle) const;
+
+    void ApplyAckermannControllerSettings(
+        rpc::ActorId vehicle,
+        const rpc::AckermannControllerSettings &settings);
+
     void EnableCarSim(
         rpc::ActorId vehicle,
         std::string simfile_path);
@@ -267,9 +294,19 @@ namespace detail {
         rpc::ActorId walker,
         const rpc::WalkerControl &control);
 
-    void ApplyBoneControlToWalker(
+    rpc::WalkerBoneControlOut GetBonesTransform(
+        rpc::ActorId walker);
+
+    void SetBonesTransform(
         rpc::ActorId walker,
-        const rpc::WalkerBoneControl &control);
+        const rpc::WalkerBoneControlIn &bones);
+
+    void BlendPose(
+        rpc::ActorId walker, 
+        float blend);
+
+    void GetPoseFromAnimation(
+        rpc::ActorId walker);
 
     void SetTrafficLightState(
         rpc::ActorId traffic_light,

LibCarla/source/carla/client/detail/Episode.h

@@ -101,6 +101,12 @@ namespace detail {
       nav->SetPedestriansCrossFactor(percentage);
     }
 
+    void SetPedestriansSeed(unsigned int seed) {
+      auto nav = _navigation.load();
+      DEBUG_ASSERT(nav != nullptr);
+      nav->SetPedestriansSeed(seed);
+    }
+
     void AddPendingException(std::string e) {
       _pending_exceptions = true;
       _pending_exceptions_msg = e;

LibCarla/source/carla/client/detail/Simulator.cpp

@@ -308,6 +308,13 @@ namespace detail {
     navigation->SetPedestriansCrossFactor(percentage);
   }
 
+  void Simulator::SetPedestriansSeed(unsigned int seed) {
+    DEBUG_ASSERT(_episode != nullptr);
+    auto navigation = _episode->CreateNavigationIfMissing();
+    DEBUG_ASSERT(navigation != nullptr);
+    navigation->SetPedestriansSeed(seed);
+  }
+
   // ===========================================================================
   // -- General operations with actors -----------------------------------------
   // ===========================================================================
@@ -380,6 +387,28 @@ namespace detail {
     _client.FreezeAllTrafficLights(frozen);
   }
 
+  // =========================================================================
+  /// -- Texture updating operations
+  // =========================================================================
+
+  void Simulator::ApplyColorTextureToObjects(
+      const std::vector<std::string> &objects_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureColor& Texture) {
+    _client.ApplyColorTextureToObjects(objects_name, parameter, Texture);
+  }
+
+  void Simulator::ApplyColorTextureToObjects(
+      const std::vector<std::string> &objects_name,
+      const rpc::MaterialParameter& parameter,
+      const rpc::TextureFloatColor& Texture) {
+    _client.ApplyColorTextureToObjects(objects_name, parameter, Texture);
+  }
+
+  std::vector<std::string> Simulator::GetNamesOfAllObjects() const {
+    return _client.GetNamesOfAllObjects();
+  }
+
 } // namespace detail
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/detail/Simulator.h

@@ -25,6 +25,8 @@
 #include "carla/rpc/VehicleLightStateList.h"
 #include "carla/rpc/LabelledPoint.h"
 #include "carla/rpc/VehicleWheels.h"
+#include "carla/rpc/Texture.h"
+#include "carla/rpc/MaterialParameter.h"
 
 #include <boost/optional.hpp>
 
@@ -294,6 +296,8 @@ namespace detail {
 
     void SetPedestriansCrossFactor(float percentage);
 
+    void SetPedestriansSeed(unsigned int seed);
+
     /// @}
     // =========================================================================
     /// @name General operations with actors
@@ -453,12 +457,36 @@ namespace detail {
       _client.ApplyControlToVehicle(vehicle.GetId(), control);
     }
 
+    void ApplyAckermannControlToVehicle(Vehicle &vehicle, const rpc::VehicleAckermannControl &control) {
+      _client.ApplyAckermannControlToVehicle(vehicle.GetId(), control);
+    }
+
+    rpc::AckermannControllerSettings GetAckermannControllerSettings(const Vehicle &vehicle) const {
+      return _client.GetAckermannControllerSettings(vehicle.GetId());
+    }
+
+    void ApplyAckermannControllerSettings(Vehicle &vehicle, const rpc::AckermannControllerSettings &settings) {
+      _client.ApplyAckermannControllerSettings(vehicle.GetId(), settings);
+    }
+
     void ApplyControlToWalker(Walker &walker, const rpc::WalkerControl &control) {
       _client.ApplyControlToWalker(walker.GetId(), control);
     }
 
-    void ApplyBoneControlToWalker(Walker &walker, const rpc::WalkerBoneControl &control) {
-      _client.ApplyBoneControlToWalker(walker.GetId(), control);
+    rpc::WalkerBoneControlOut GetBonesTransform(Walker &walker) {
+      return _client.GetBonesTransform(walker.GetId());
+    }
+
+    void SetBonesTransform(Walker &walker, const rpc::WalkerBoneControlIn &bones) {
+      return _client.SetBonesTransform(walker.GetId(), bones);
+    }
+
+    void BlendPose(Walker &walker, float blend) {
+      return _client.BlendPose(walker.GetId(), blend);
+    }
+
+    void GetPoseFromAnimation(Walker &walker) {
+      return _client.GetPoseFromAnimation(walker.GetId());
     }
 
     void ApplyPhysicsControlToVehicle(Vehicle &vehicle, const rpc::VehiclePhysicsControl &physicsControl) {
@@ -663,6 +691,24 @@ namespace detail {
     void FreezeAllTrafficLights(bool frozen);
 
     /// @}
+    // =========================================================================
+    /// @name Texture updating operations
+    // =========================================================================
+    /// @{
+
+    void ApplyColorTextureToObjects(
+        const std::vector<std::string> &objects_name,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureColor& Texture);
+
+    void ApplyColorTextureToObjects(
+        const std::vector<std::string> &objects_name,
+        const rpc::MaterialParameter& parameter,
+        const rpc::TextureFloatColor& Texture);
+
+    std::vector<std::string> GetNamesOfAllObjects() const;
+
+    /// @}
 
   private:
 

LibCarla/source/carla/client/detail/WalkerNavigation.h

@@ -85,6 +85,10 @@ namespace detail {
       _nav.SetPedestriansCrossFactor(percentage);
     }
 
+    void SetPedestriansSeed(unsigned int seed) {
+      _nav.SetSeed(seed);
+    }
+
   private:
 
     Client &_client;

LibCarla/source/carla/nav/Navigation.cpp

@@ -67,6 +67,11 @@ namespace nav {
     dtFreeNavMesh(_nav_mesh);
   }
 
+  // set the seed to use with random numbers
+  void Navigation::SetSeed(unsigned int seed) {
+    srand(seed);
+  }
+
   // load navigation data
   bool Navigation::Load(const std::string &filename) {
     std::ifstream f;

LibCarla/source/carla/nav/Navigation.h

@@ -71,6 +71,8 @@ namespace nav {
     bool GetAgentRoute(ActorId id, carla::geom::Location from, carla::geom::Location to,
     std::vector<carla::geom::Location> &path, std::vector<unsigned char> &area);
 
+    /// set the seed to use with random numbers
+    void SetSeed(unsigned int seed);
     /// create the crowd object
     void CreateCrowd(void);
     /// create a new walker

LibCarla/source/carla/nav/WalkerEvent.h

@@ -6,7 +6,15 @@
 
 #pragma once
 
-#include <boost/variant.hpp>
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
 
 #include "carla/rpc/ActorId.h"
 
@@ -40,7 +48,7 @@ namespace nav {
     };
 
     /// walker event variant
-    using WalkerEvent = boost::variant<WalkerEventIgnore, WalkerEventWait, WalkerEventStopAndCheck>;
+    using WalkerEvent = boost::variant2::variant<WalkerEventIgnore, WalkerEventWait, WalkerEventStopAndCheck>;
 
     /// visitor class
     class WalkerEventVisitor {

LibCarla/source/carla/nav/WalkerManager.cpp

@@ -254,7 +254,7 @@ namespace nav {
         // build the visitor structure
         WalkerEventVisitor visitor(this, id, delta);
         // run the event
-        return boost::apply_visitor(visitor, rp.event);
+        return boost::variant2::visit(visitor, rp.event);
     }
 
 

LibCarla/source/carla/rpc/AckermannControllerSettings.h

@@ -0,0 +1,92 @@
+// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#  include "Carla/Vehicle/AckermannControllerSettings.h"
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace rpc {
+
+  class AckermannControllerSettings {
+  public:
+
+    AckermannControllerSettings() = default;
+
+    AckermannControllerSettings(
+        float speed_kp,
+        float speed_ki,
+        float speed_kd,
+        float accel_kp,
+        float accel_ki,
+        float accel_kd)
+      : speed_kp(speed_kp),
+        speed_ki(speed_ki),
+        speed_kd(speed_kd),
+        accel_kp(accel_kp),
+        accel_ki(accel_ki),
+        accel_kd(accel_kd) {}
+
+    float speed_kp = 0.0f;
+    float speed_ki = 0.0f;
+    float speed_kd = 0.0f;
+    float accel_kp = 0.0f;
+    float accel_ki = 0.0f;
+    float accel_kd = 0.0f;
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    AckermannControllerSettings(const FAckermannControllerSettings &Settings)
+      : speed_kp(Settings.SpeedKp),
+        speed_ki(Settings.SpeedKi),
+        speed_kd(Settings.SpeedKd),
+        accel_kp(Settings.AccelKp),
+        accel_ki(Settings.AccelKi),
+        accel_kd(Settings.AccelKd) {}
+
+    operator FAckermannControllerSettings() const {
+      FAckermannControllerSettings Settings;
+      Settings.SpeedKp = speed_kp;
+      Settings.SpeedKi = speed_ki;
+      Settings.SpeedKd = speed_kd;
+      Settings.AccelKp = accel_kp;
+      Settings.AccelKi = accel_ki;
+      Settings.AccelKd = accel_kd;
+      return Settings;
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+    bool operator!=(const AckermannControllerSettings &rhs) const {
+      return
+          speed_kp != rhs.speed_kp ||
+          speed_ki != rhs.speed_ki ||
+          speed_kd != rhs.speed_kd ||
+          accel_kp != rhs.accel_kp ||
+          accel_ki != rhs.accel_ki ||
+          accel_kd != rhs.accel_kd;
+    }
+
+    bool operator==(const AckermannControllerSettings &rhs) const {
+      return !(*this != rhs);
+    }
+
+    MSGPACK_DEFINE_ARRAY(
+        speed_kp,
+        speed_ki,
+        speed_kd,
+        accel_kp,
+        accel_ki,
+        accel_kd
+    );
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/BoneTransformDataIn.h

@@ -14,7 +14,7 @@
 namespace carla {
 namespace rpc {
 
-  using BoneTransformData = std::pair<std::string, geom::Transform>;
+  using BoneTransformDataIn = std::pair<std::string, geom::Transform>;
 
 } // namespace rpc
 } // namespace carla

LibCarla/source/carla/rpc/BoneTransformDataOut.h

@@ -0,0 +1,43 @@
+// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/rpc/Transform.h"
+
+#include <string>
+#include <utility>
+
+namespace carla {
+namespace rpc {
+
+  // using BoneTransformDataOut = std::pair<std::string, geom::Transform>;
+  class BoneTransformDataOut
+  {
+    public:
+    std::string bone_name;
+    Transform world;
+    Transform component;
+    Transform relative;
+  
+    bool operator!=(const BoneTransformDataOut &rhs) const {
+      return
+      bone_name != rhs.bone_name ||
+      world != rhs.world ||
+      component != rhs.component ||
+      relative != rhs.relative;
+    }
+
+    bool operator==(const BoneTransformDataOut &rhs) const {
+      return !(*this != rhs);
+    }
+
+
+    MSGPACK_DEFINE_ARRAY(bone_name, world, component, relative);
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/Command.h

@@ -11,12 +11,22 @@
 #include "carla/geom/Transform.h"
 #include "carla/rpc/ActorDescription.h"
 #include "carla/rpc/ActorId.h"
+#include "carla/rpc/TrafficLightState.h"
+#include "carla/rpc/VehicleAckermannControl.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/VehiclePhysicsControl.h"
 #include "carla/rpc/VehicleLightState.h"
 #include "carla/rpc/WalkerControl.h"
 
-#include <boost/variant.hpp>
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
 
 namespace carla {
 
@@ -74,6 +84,16 @@ namespace rpc {
       MSGPACK_DEFINE_ARRAY(actor, control);
     };
 
+    struct ApplyVehicleAckermannControl : CommandBase<ApplyVehicleAckermannControl> {
+      ApplyVehicleAckermannControl() = default;
+      ApplyVehicleAckermannControl(ActorId id, const VehicleAckermannControl &value)
+        : actor(id),
+          control(value) {}
+      ActorId actor;
+      VehicleAckermannControl control;
+      MSGPACK_DEFINE_ARRAY(actor, control);
+    };
+
     struct ApplyWalkerControl : CommandBase<ApplyWalkerControl> {
       ApplyWalkerControl() = default;
       ApplyWalkerControl(ActorId id, const WalkerControl &value)
@@ -104,6 +124,16 @@ namespace rpc {
       MSGPACK_DEFINE_ARRAY(actor, transform);
     };
 
+    struct ApplyLocation : CommandBase<ApplyLocation> {
+      ApplyLocation() = default;
+      ApplyLocation(ActorId id, const geom::Location &value)
+        : actor(id),
+          location(value) {}
+      ActorId actor;
+      geom::Location location;
+      MSGPACK_DEFINE_ARRAY(actor, location);
+    };
+
     struct ApplyWalkerState : CommandBase<ApplyWalkerState> {
       ApplyWalkerState() = default;
       ApplyWalkerState(ActorId id, const geom::Transform &value, const float speed) : actor(id), transform(value), speed(speed) {}
@@ -232,10 +262,30 @@ namespace rpc {
       MSGPACK_DEFINE_ARRAY(actor, light_state);
     };
 
-    using CommandType = boost::variant<
+    struct ConsoleCommand : CommandBase<ConsoleCommand> {
+      ConsoleCommand() = default;
+      ConsoleCommand(std::string cmd) : cmd(cmd) {}
+      std::string cmd;
+      MSGPACK_DEFINE_ARRAY(cmd);
+    };
+
+    struct SetTrafficLightState : CommandBase<SetTrafficLightState> {
+      SetTrafficLightState() = default;
+      SetTrafficLightState(
+          ActorId id,
+          rpc::TrafficLightState state)
+        : actor(id),
+          traffic_light_state(state) {}
+      ActorId actor;
+      rpc::TrafficLightState traffic_light_state;
+      MSGPACK_DEFINE_ARRAY(actor, traffic_light_state);
+    };
+
+    using CommandType = boost::variant2::variant<
         SpawnActor,
         DestroyActor,
         ApplyVehicleControl,
+        ApplyVehicleAckermannControl,
         ApplyWalkerControl,
         ApplyVehiclePhysicsControl,
         ApplyTransform,
@@ -250,7 +300,10 @@ namespace rpc {
         SetEnableGravity,
         SetAutopilot,
         ShowDebugTelemetry,
-        SetVehicleLightState>;
+        SetVehicleLightState,
+        ApplyLocation,
+        ConsoleCommand,
+        SetTrafficLightState>;
 
     CommandType command;
 

LibCarla/source/carla/rpc/DebugShape.h

@@ -13,7 +13,15 @@
 #include "carla/geom/Rotation.h"
 #include "carla/rpc/Color.h"
 
-#include <boost/variant.hpp>
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
 
 namespace carla {
 namespace rpc {
@@ -54,7 +62,7 @@ namespace rpc {
       MSGPACK_DEFINE_ARRAY(location, text, draw_shadow);
     };
 
-    boost::variant<Point, Line, Arrow, Box, String> primitive;
+    boost::variant2::variant<Point, Line, Arrow, Box, String> primitive;
 
     Color color = {255u, 0u, 0u};
 

LibCarla/source/carla/rpc/FloatColor.h

@@ -0,0 +1,72 @@
+// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+
+#include <cstdint>
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#  include "Math/Color.h"
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace rpc {
+
+#pragma pack(push, 1)
+  struct FloatColor {
+  public:
+
+    float r = 0.f;
+    float g = 0.f;
+    float b = 0.f;
+    float a = 1.f;
+
+    FloatColor() = default;
+    FloatColor(const FloatColor &) = default;
+
+    FloatColor(float r, float g, float b, float a = 1.f)
+      : r(r), g(g), b(b), a(a) {}
+
+    FloatColor &operator=(const FloatColor &) = default;
+
+    bool operator==(const FloatColor &rhs) const  {
+      return (r == rhs.r) && (g == rhs.g) && (b == rhs.b) && (a == rhs.a);
+    }
+
+    bool operator!=(const FloatColor &rhs) const  {
+      return !(*this == rhs);
+    }
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    FloatColor(const FColor &color)
+      : FloatColor(color.R / 255.f, color.G / 255.f, color.B / 255.f, color.A / 255.f) {}
+
+    FloatColor(const FLinearColor &color)
+      : FloatColor(color.R, color.G, color.B, color.A) {}
+
+    FColor ToFColor() const {
+      return FColor{
+        static_cast<uint8>(r * 255),
+        static_cast<uint8>(g * 255),
+        static_cast<uint8>(b * 255),
+        static_cast<uint8>(a * 255)};
+    }
+
+    operator FLinearColor() const {
+      return FLinearColor{ r, g, b, a };
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+    MSGPACK_DEFINE_ARRAY(r, g, b, a);
+  };
+#pragma pack(pop)
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/MaterialParameter.cpp

@@ -0,0 +1,25 @@
+// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "MaterialParameter.h"
+
+namespace carla {
+namespace rpc {
+
+std::string MaterialParameterToString(MaterialParameter material_parameter)
+{
+  switch(material_parameter)
+  {
+    case MaterialParameter::Tex_Normal:                         return "Normal";
+    case MaterialParameter::Tex_Ao_Roughness_Metallic_Emissive: return "AO / Roughness / Metallic / Emissive";
+    case MaterialParameter::Tex_Diffuse:                      return "Diffuse";
+    case MaterialParameter::Tex_Emissive:                       return "Emissive";
+    default:                                                    return "Invalid";
+  }
+}
+
+}
+}

LibCarla/source/carla/rpc/MaterialParameter.h

@@ -0,0 +1,29 @@
+// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+
+#include <cstdint>
+
+namespace carla {
+namespace rpc {
+
+enum class MaterialParameter
+{
+  Tex_Normal,
+  Tex_Ao_Roughness_Metallic_Emissive,
+  Tex_Diffuse,
+  Tex_Emissive
+};
+
+std::string MaterialParameterToString(MaterialParameter material_parameter);
+
+} // namespace rpc
+} // namespace carla
+
+MSGPACK_ADD_ENUM(carla::rpc::MaterialParameter);

LibCarla/source/carla/rpc/Response.h

@@ -10,7 +10,16 @@
 #include "carla/MsgPackAdaptors.h"
 
 #include <boost/optional.hpp>
-#include <boost/variant.hpp>
+
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
 
 #include <string>
 
@@ -58,7 +67,7 @@ namespace rpc {
     }
 
     bool HasError() const {
-      return _data.which() == 0;
+      return _data.index() == 0;
     }
 
     template <typename... Ts>
@@ -68,17 +77,17 @@ namespace rpc {
 
     const error_type &GetError() const {
       DEBUG_ASSERT(HasError());
-      return boost::get<error_type>(_data);
+      return boost::variant2::get<error_type>(_data);
     }
 
     value_type &Get() {
       DEBUG_ASSERT(!HasError());
-      return boost::get<value_type>(_data);
+      return boost::variant2::get<value_type>(_data);
     }
 
     const value_type &Get() const {
       DEBUG_ASSERT(!HasError());
-      return boost::get<value_type>(_data);
+      return boost::variant2::get<value_type>(_data);
     }
 
     operator bool() const {
@@ -89,7 +98,7 @@ namespace rpc {
 
   private:
 
-    boost::variant<error_type, value_type> _data;
+    boost::variant2::variant<error_type, value_type> _data;
   };
 
   template <>

LibCarla/source/carla/rpc/Texture.h

@@ -0,0 +1,70 @@
+// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+#include "carla/rpc/FloatColor.h"
+#include "carla/sensor/data/Color.h"
+
+#include <vector>
+
+namespace carla {
+namespace rpc {
+
+  template<typename T>
+  class Texture {
+  public:
+
+    Texture() = default;
+
+    Texture(uint32_t width, uint32_t height)
+      : _width(width), _height(height) {
+      _texture_data.resize(_width*_height);
+    }
+
+    uint32_t GetWidth() const {
+      return _width;
+    }
+
+    uint32_t GetHeight() const {
+      return _height;
+    }
+
+    void SetDimensions(uint32_t width, uint32_t height) {
+      _width = width;
+      _height = height;
+      _texture_data.resize(_width*_height);
+    }
+
+    T& At (uint32_t x, uint32_t y) {
+      return _texture_data[y*_width + x];
+    }
+
+    const T& At (uint32_t x, uint32_t y) const {
+      return _texture_data[y*_width + x];
+    }
+
+    const T* GetDataPtr() const {
+      return _texture_data.data();
+    }
+
+  private:
+
+    uint32_t _width = 0;
+    uint32_t _height = 0;
+    std::vector<T> _texture_data;
+
+  public:
+
+    MSGPACK_DEFINE_ARRAY(_width, _height, _texture_data);
+  };
+
+  using TextureColor = Texture<sensor::data::Color>;
+  using TextureFloatColor = Texture<FloatColor>;
+
+}
+}

LibCarla/source/carla/rpc/VehicleAckermannControl.h

@@ -0,0 +1,84 @@
+// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#  include "Carla/Vehicle/VehicleAckermannControl.h"
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace rpc {
+
+  class VehicleAckermannControl {
+  public:
+
+    VehicleAckermannControl() = default;
+
+    VehicleAckermannControl(
+        float in_steer,
+        float in_steer_speed,
+        float in_speed,
+        float in_acceleration,
+        float in_jerk)
+      : steer(in_steer),
+        steer_speed(in_steer_speed),
+        speed(in_speed),
+        acceleration(in_acceleration),
+        jerk(in_jerk) {}
+
+    float steer = 0.0f;
+    float steer_speed = 0.0f;
+    float speed = 0.0f;
+    float acceleration = 0.0f;
+    float jerk = 0.0f;
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    VehicleAckermannControl(const FVehicleAckermannControl &Control)
+      : steer(Control.Steer),
+        steer_speed(Control.SteerSpeed),
+        speed(Control.Speed),
+        acceleration(Control.Acceleration),
+        jerk(Control.Jerk) {}
+
+    operator FVehicleAckermannControl() const {
+      FVehicleAckermannControl Control;
+      Control.Steer = steer;
+      Control.SteerSpeed = steer_speed;
+      Control.Speed = speed;
+      Control.Acceleration = acceleration;
+      Control.Jerk = jerk;
+      return Control;
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+    bool operator!=(const VehicleAckermannControl &rhs) const {
+      return
+          steer != rhs.steer ||
+          steer_speed != rhs.steer_speed ||
+          speed != rhs.speed ||
+          acceleration != rhs.acceleration ||
+          jerk != rhs.jerk;
+    }
+
+    bool operator==(const VehicleAckermannControl &rhs) const {
+      return !(*this != rhs);
+    }
+
+    MSGPACK_DEFINE_ARRAY(
+        steer,
+        steer_speed,
+        speed,
+        acceleration,
+        jerk);
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/WalkerBoneControlIn.h

@@ -7,12 +7,12 @@
 #pragma once
 
 #include "carla/MsgPack.h"
-#include "carla/rpc/BoneTransformData.h"
+#include "carla/rpc/BoneTransformDataIn.h"
 #include "carla/rpc/String.h"
 #include "carla/rpc/Transform.h"
 
 #ifdef LIBCARLA_INCLUDED_FROM_UE4
-#  include "Carla/Walker/WalkerBoneControl.h"
+#  include "Carla/Walker/WalkerBoneControlIn.h"
 #endif // LIBCARLA_INCLUDED_FROM_UE4
 
 #include <vector>
@@ -20,19 +20,19 @@
 namespace carla {
 namespace rpc {
 
-  class WalkerBoneControl {
+  class WalkerBoneControlIn {
   public:
 
-    WalkerBoneControl() = default;
+    WalkerBoneControlIn() = default;
 
-    explicit WalkerBoneControl(
-        std::vector<rpc::BoneTransformData> bone_transforms)
+    explicit WalkerBoneControlIn(
+        std::vector<rpc::BoneTransformDataIn> bone_transforms)
       : bone_transforms(bone_transforms) {}
 
 #ifdef LIBCARLA_INCLUDED_FROM_UE4
 
-    operator FWalkerBoneControl() const {
-      FWalkerBoneControl Control;
+    operator FWalkerBoneControlIn() const {
+      FWalkerBoneControlIn Control;
       for (auto &bone_transform : bone_transforms) {
         Control.BoneTransforms.Add(ToFString(bone_transform.first), bone_transform.second);
       }
@@ -41,7 +41,7 @@ namespace rpc {
 
 #endif // LIBCARLA_INCLUDED_FROM_UE4
 
-    std::vector<rpc::BoneTransformData> bone_transforms;
+    std::vector<rpc::BoneTransformDataIn> bone_transforms;
 
     MSGPACK_DEFINE_ARRAY(bone_transforms);
   };

LibCarla/source/carla/rpc/WalkerBoneControlOut.h

@@ -0,0 +1,38 @@
+// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+#include "carla/rpc/BoneTransformDataOut.h"
+#include "carla/rpc/String.h"
+#include "carla/rpc/Transform.h"
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#  include "Carla/Walker/WalkerBoneControlOut.h"
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+#include <vector>
+
+namespace carla {
+namespace rpc {
+
+  class WalkerBoneControlOut {
+  public:
+
+    WalkerBoneControlOut() = default;
+
+    explicit WalkerBoneControlOut(
+        std::vector<rpc::BoneTransformDataOut> bone_transforms)
+      : bone_transforms(bone_transforms) {}
+
+    std::vector<rpc::BoneTransformDataOut> bone_transforms;
+
+    MSGPACK_DEFINE_ARRAY(bone_transforms);
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/WeatherParameters.cpp

@@ -11,29 +11,30 @@ namespace rpc {
 
   using WP = WeatherParameters;
 
-//                        cloudiness   precip.  prec.dep.     wind   azimuth   altitude  fog dens  fog dist  fog fall  wetness  scat.i  mie.scat.s rayleigh.scat.scale
-  WP WP::Default         = {    -1.0f,   -1.0f,    -1.0f,   -1.00f,    -1.0f,     -1.0f,    -1.0f,     -1.0f,    -1.0f,   -1.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::ClearNoon       = {     5.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     45.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::CloudyNoon      = {    60.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::WetNoon         = {     5.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::WetCloudyNoon   = {    60.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::MidRainyNoon    = {    60.0f,   60.0f,    60.0f,    60.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::HardRainNoon    = {   100.0f,  100.0f,    90.0f,    100.0f,   -1.0f,     45.0f,     7.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::SoftRainNoon    = {    20.0f,   30.0f,    50.0f,    30.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::ClearSunset     = {     5.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::CloudySunset    = {    60.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     15.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::WetSunset       = {     5.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::WetCloudySunset = {    60.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::MidRainSunset   = {    60.0f,   60.0f,    60.0f,    60.0f,    -1.0f,     15.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::HardRainSunset  = {   100.0f,  100.0f,    90.0f,    100.0f,   -1.0f,     15.0f,     7.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::SoftRainSunset  = {    20.0f,   30.0f,    50.0f,    30.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::ClearNight      = {     5.0f,    0.0f,     0.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     75.0f,     1.0f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::CloudyNight     = {    60.0f,    0.0f,     0.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::WetNight        = {     5.0f,    0.0f,    50.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     75.0f,     1.0f,   60.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::WetCloudyNight  = {    60.0f,    0.0f,    50.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,   60.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::SoftRainNight   = {    60.0f,   30.0f,    50.0f,    30.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,   60.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::MidRainyNight   = {    80.0f,   60.0f,    60.0f,    60.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,   80.0f,   1.0f,   0.03f,   0.0331f};
-  WP WP::HardRainNight   = {   100.0f,  100.0f,    90.0f,    100.0f,   -1.0f,    -90.0f,   100.0f,     0.75f,     0.1f,  100.0f,   1.0f,   0.03f,   0.0331f};
+//                        cloudiness   precip.  prec.dep.     wind   azimuth   altitude  fog dens  fog dist  fog fall  wetness  scat.i  mie.scat.s rayleigh.scat.scale  dust storm
+  WP WP::Default         = {    -1.0f,   -1.0f,    -1.0f,   -1.00f,    -1.0f,     -1.0f,    -1.0f,     -1.0f,    -1.0f,   -1.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::ClearNoon       = {     5.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     45.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::CloudyNoon      = {    60.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::WetNoon         = {     5.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::WetCloudyNoon   = {    60.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::MidRainyNoon    = {    60.0f,   60.0f,    60.0f,    60.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::HardRainNoon    = {   100.0f,  100.0f,    90.0f,    100.0f,   -1.0f,     45.0f,     7.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::SoftRainNoon    = {    20.0f,   30.0f,    50.0f,    30.0f,    -1.0f,     45.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::ClearSunset     = {     5.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::CloudySunset    = {    60.0f,    0.0f,     0.0f,    10.0f,    -1.0f,     15.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::WetSunset       = {     5.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::WetCloudySunset = {    60.0f,    0.0f,    50.0f,    10.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::MidRainSunset   = {    60.0f,   60.0f,    60.0f,    60.0f,    -1.0f,     15.0f,     3.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::HardRainSunset  = {   100.0f,  100.0f,    90.0f,    100.0f,   -1.0f,     15.0f,     7.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::SoftRainSunset  = {    20.0f,   30.0f,    50.0f,    30.0f,    -1.0f,     15.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::ClearNight      = {     5.0f,    0.0f,     0.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     75.0f,     1.0f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::CloudyNight     = {    60.0f,    0.0f,     0.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::WetNight        = {     5.0f,    0.0f,    50.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     75.0f,     1.0f,   60.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::WetCloudyNight  = {    60.0f,    0.0f,    50.0f,    10.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,   60.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::SoftRainNight   = {    60.0f,   30.0f,    50.0f,    30.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,   60.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::MidRainyNight   = {    80.0f,   60.0f,    60.0f,    60.0f,    -1.0f,    -90.0f,    60.0f,     0.75f,     0.1f,   80.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::HardRainNight   = {   100.0f,  100.0f,    90.0f,    100.0f,   -1.0f,    -90.0f,   100.0f,     0.75f,     0.1f,  100.0f,   1.0f,   0.03f,   0.0331f,            false };
+  WP WP::DustStorm       = {   100.0f,     0.0f,    0.0f,    100.0f,   -1.0f,     45.0f,     2.0f,     0.75f,     0.1f,    0.0f,   1.0f,   0.03f,   0.0331f,            true  };
 
 } // namespace rpc
 } // namespace carla

LibCarla/source/carla/rpc/WeatherParameters.h

@@ -43,6 +43,7 @@ namespace rpc {
     static WeatherParameters SoftRainNight;
     static WeatherParameters MidRainyNight;
     static WeatherParameters HardRainNight;
+    static WeatherParameters DustStorm;
 
     /// @}
 
@@ -61,7 +62,8 @@ namespace rpc {
         float in_wetness,
         float in_scattering_intensity,
         float in_mie_scattering_scale,
-        float in_rayleigh_scattering_scale)
+        float in_rayleigh_scattering_scale,
+        bool in_dust_storm)
       : cloudiness(in_cloudiness),
         precipitation(in_precipitation),
         precipitation_deposits(in_precipitation_deposits),
@@ -74,7 +76,8 @@ namespace rpc {
         wetness(in_wetness),
         scattering_intensity(in_scattering_intensity),
         mie_scattering_scale(in_mie_scattering_scale),
-        rayleigh_scattering_scale(in_rayleigh_scattering_scale) {}
+        rayleigh_scattering_scale(in_rayleigh_scattering_scale),
+        dust_storm(in_dust_storm) {}
 
     float cloudiness = 0.0f;
     float precipitation = 0.0f;
@@ -89,6 +92,7 @@ namespace rpc {
     float scattering_intensity = 0.0f;
     float mie_scattering_scale = 0.0f;
     float rayleigh_scattering_scale = 0.0331f;
+    bool dust_storm = false;
 
 #ifdef LIBCARLA_INCLUDED_FROM_UE4
 
@@ -105,7 +109,8 @@ namespace rpc {
         wetness(Weather.Wetness),
         scattering_intensity(Weather.ScatteringIntensity),
         mie_scattering_scale(Weather.MieScatteringScale),
-        rayleigh_scattering_scale(Weather.RayleighScatteringScale) {}
+        rayleigh_scattering_scale(Weather.RayleighScatteringScale),
+        dust_storm(Weather.DustStorm) {}
 
     operator FWeatherParameters() const {
       FWeatherParameters Weather;
@@ -122,7 +127,7 @@ namespace rpc {
       Weather.ScatteringIntensity = scattering_intensity;
       Weather.MieScatteringScale = mie_scattering_scale;
       Weather.RayleighScatteringScale = rayleigh_scattering_scale;
-
+      Weather.DustStorm = dust_storm;
       return Weather;
     }
 
@@ -142,7 +147,8 @@ namespace rpc {
           wetness != rhs.wetness ||
           scattering_intensity != rhs.scattering_intensity ||
           mie_scattering_scale != rhs.mie_scattering_scale ||
-          rayleigh_scattering_scale != rhs.rayleigh_scattering_scale;
+          rayleigh_scattering_scale != rhs.rayleigh_scattering_scale ||
+          dust_storm != rhs.dust_storm;
     }
 
     bool operator==(const WeatherParameters &rhs) const {
@@ -162,7 +168,8 @@ namespace rpc {
         wetness,
         scattering_intensity,
         mie_scattering_scale,
-        rayleigh_scattering_scale);
+        rayleigh_scattering_scale,
+        dust_storm);
   };
 
 } // namespace rpc

LibCarla/source/carla/rss/RssCheck.h

@@ -144,6 +144,11 @@ public:
   /// @brief destructor
   ~RssCheck();
 
+  /// @brief get the logger of this
+  std::shared_ptr<spdlog::logger> GetLogger() {
+    return _logger;
+  }
+
   /// @brief main function to trigger the RSS check at a certain point in time
   ///
   /// This function has to be called cyclic with increasing timestamps to ensure

LibCarla/source/carla/rss/RssRestrictor.cpp

@@ -22,11 +22,19 @@ RssRestrictor::RssRestrictor() {
   if (!_logger) {
     _logger = spdlog::create<spdlog::sinks::stdout_color_sink_st>(logger_name);
   }
-  //_logger->set_level(spdlog::level::debug);
+
+  SetLogLevel(spdlog::level::warn);
 }
 
 RssRestrictor::~RssRestrictor() = default;
 
+void RssRestrictor::SetLogLevel(const uint8_t log_level) {
+  if (log_level < spdlog::level::n_levels) {
+    const auto log_level_value = static_cast<spdlog::level::level_enum>(log_level);
+    _logger->set_level(log_level_value);
+  }
+}
+
 carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl(
     const carla::rpc::VehicleControl &vehicle_control, const ::ad::rss::state::ProperResponse &proper_response,
     const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route,

LibCarla/source/carla/rss/RssRestrictor.h

@@ -50,6 +50,8 @@ public:
                                                     const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route,
                                                     const carla::rpc::VehiclePhysicsControl &vehicle_physics);
 
+  void SetLogLevel(const uint8_t log_level);
+
 private:
   /// @brief the logger instance
   std::shared_ptr<spdlog::logger> _logger;

LibCarla/source/carla/rss/RssSensor.cpp

@@ -22,12 +22,15 @@
 namespace carla {
 namespace client {
 
+std::atomic_uint RssSensor::_global_map_initialization_counter_{0u};
+
 RssSensor::RssSensor(ActorInitializer init) : Sensor(std::move(init)), _on_tick_register_id(0u), _drop_route(false) {}
 
 RssSensor::~RssSensor() {
-  // ensure there is no processing anymore when deleting rss_check object
-  const std::lock_guard<std::mutex> lock(_processing_lock);
-  _rss_check.reset();
+  // ensure there is no processing anymore
+  if (IsListening()) {
+    Stop();
+  }
 }
 
 void RssSensor::RegisterActorConstellationCallback(ActorConstellationCallbackFunctionType callback) {
@@ -68,12 +71,16 @@ void RssSensor::Listen(CallbackFunctionType callback) {
   DEBUG_ASSERT(map != nullptr);
   std::string const open_drive_content = map->GetOpenDrive();
 
-  _rss_check = nullptr;
-  ::ad::map::access::cleanup();
+  auto mapInitializationResult = ::ad::map::access::initFromOpenDriveContent(
+      open_drive_content, 0.2, ::ad::map::intersection::IntersectionType::TrafficLight,
+      ::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN);
 
-  ::ad::map::access::initFromOpenDriveContent(open_drive_content, 0.2,
-                                              ::ad::map::intersection::IntersectionType::TrafficLight,
-                                              ::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN);
+  if (!mapInitializationResult) {
+    log_error(GetDisplayId(), ": Initialization of map failed");
+    return;
+  }
+
+  _global_map_initialization_counter_++;
 
   if (_rss_actor_constellation_callback == nullptr) {
     _rss_check = std::make_shared<::carla::rss::RssCheck>(max_steering_angle);
@@ -84,15 +91,13 @@ void RssSensor::Listen(CallbackFunctionType callback) {
 
   auto self = boost::static_pointer_cast<RssSensor>(shared_from_this());
 
+  _last_processed_frame=0u;
   log_debug(GetDisplayId(), ": subscribing to tick event");
   _on_tick_register_id = GetEpisode().Lock()->RegisterOnTickEvent(
       [ cb = std::move(callback), weak_self = WeakPtr<RssSensor>(self) ](const auto &snapshot) {
         auto self = weak_self.lock();
         if (self != nullptr) {
-          auto data = self->TickRssSensor(snapshot.GetTimestamp());
-          if (data != nullptr) {
-            cb(std::move(data));
-          }
+          self->TickRssSensor(snapshot.GetTimestamp(), cb);
         }
       });
 }
@@ -106,6 +111,25 @@ void RssSensor::Stop() {
   log_debug(GetDisplayId(), ": unsubscribing from tick event");
   GetEpisode().Lock()->RemoveOnTickEvent(_on_tick_register_id);
   _on_tick_register_id = 0u;
+
+  if ( bool(_rss_check) ) {
+    _rss_check->GetLogger()->info("RssSensor stopping");
+  }
+  // don't remove the braces since they protect the lock_guard
+  {
+     // ensure there is no processing anymore when deleting rss_check object
+    const std::lock_guard<std::mutex> lock(_processing_lock);
+
+    if ( bool(_rss_check) ) {
+      _rss_check->GetLogger()->info("RssSensor delete checker");
+    }
+    _rss_check.reset();
+    auto const map_initialization_counter_value = _global_map_initialization_counter_--;
+    if (map_initialization_counter_value == 0u) {
+      // last one stop listening is cleaning up the map
+      ::ad::map::access::cleanup();
+    }
+  }
 }
 
 void RssSensor::SetLogLevel(const uint8_t &log_level) {
@@ -269,59 +293,84 @@ void RssSensor::DropRoute() {
   _drop_route = true;
 }
 
-SharedPtr<sensor::SensorData> RssSensor::TickRssSensor(const Timestamp &timestamp) {
+void RssSensor::TickRssSensor(const client::Timestamp &timestamp, CallbackFunctionType callback) {
+  if (_processing_lock.try_lock()) {
+    if (!bool(_rss_check)){
+      _processing_lock.unlock();
+      return;
+    }
+    if ((timestamp.frame < _last_processed_frame) && ((_last_processed_frame - timestamp.frame) < 0xffffffffu)) {
+      _processing_lock.unlock();
+      _rss_check->GetLogger()->warn("RssSensor[{}] outdated tick dropped, LastProcessed={}", timestamp.frame, _last_processed_frame);
+      return;
+    }
+    _last_processed_frame = timestamp.frame;
+    SharedPtr<carla::client::ActorList> actors = GetWorld().GetActors();
+
+    auto const settings = GetWorld().GetSettings();
+    if ( settings.synchronous_mode ) {
+      _rss_check->GetLogger()->trace("RssSensor[{}] sync-tick", timestamp.frame);
+      TickRssSensorThreadLocked(timestamp, actors, callback);
+    }
+    else {
+      // store the future to prevent the destructor of the future from blocked waiting
+      _rss_check->GetLogger()->trace("RssSensor[{}] async-tick", timestamp.frame);
+      _tick_future = std::async(&RssSensor::TickRssSensorThreadLocked, this, timestamp, actors, callback);
+    }
+  } else {
+    if (bool(_rss_check)){
+      _rss_check->GetLogger()->debug("RssSensor[{}] tick dropped", timestamp.frame);
+    }
+  }
+}
+
+void RssSensor::TickRssSensorThreadLocked(const client::Timestamp &timestamp,
+                                          SharedPtr<carla::client::ActorList> actors, CallbackFunctionType callback) {
   try {
-    bool result = false;
+    double const time_since_epoch_check_start_ms =
+        std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+    if (_drop_route) {
+      _drop_route = false;
+      _rss_check->DropRoute();
+    }
+
+    // check all object<->ego pairs with RSS and calculate proper response
     ::ad::rss::state::ProperResponse response;
     ::ad::rss::state::RssStateSnapshot rss_state_snapshot;
     ::ad::rss::situation::SituationSnapshot situation_snapshot;
     ::ad::rss::world::WorldModel world_model;
     carla::rss::EgoDynamicsOnRoute ego_dynamics_on_route;
-    if (_processing_lock.try_lock()) {
-      spdlog::debug("RssSensor tick: T={}", timestamp.frame);
+    auto const result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, rss_state_snapshot,
+                                                             situation_snapshot, world_model, ego_dynamics_on_route);
 
-      if ((timestamp.frame < _last_processed_frame) && ((_last_processed_frame - timestamp.frame) < 0xffffffffu))
-      {
-        _processing_lock.unlock();
-        spdlog::warn("RssSensor tick dropped: T={}", timestamp.frame);
-        return nullptr;
-      }
-      _last_processed_frame = timestamp.frame;
-
-      carla::client::World world = GetWorld();
-      SharedPtr<carla::client::ActorList> actors = world.GetActors();
-
-      if (_drop_route) {
-        _drop_route = false;
-        _rss_check->DropRoute();
-      }
-
-      // check all object<->ego pairs with RSS and calculate proper response
-      result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, rss_state_snapshot,
-                                        situation_snapshot, world_model, ego_dynamics_on_route);
-      _processing_lock.unlock();
-
-      spdlog::debug(
-          "RssSensor response: T={} S:{}->E:{} DeltaT:{}", timestamp.frame,
-          ego_dynamics_on_route.time_since_epoch_check_start_ms, ego_dynamics_on_route.time_since_epoch_check_end_ms,
-          ego_dynamics_on_route.time_since_epoch_check_end_ms - ego_dynamics_on_route.time_since_epoch_check_start_ms);
-      return MakeShared<sensor::data::RssResponse>(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), result,
-                                                   response, rss_state_snapshot, situation_snapshot, world_model,
-                                                   ego_dynamics_on_route);
-    } else {
-      spdlog::debug("RssSensor tick dropped: T={}", timestamp.frame);
-      return nullptr;
+    double const time_since_epoch_check_end_ms =
+        std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
+    auto const delta_time_ms = time_since_epoch_check_end_ms - time_since_epoch_check_start_ms;
+    _rss_check->GetLogger()->debug("RssSensor[{}] response: S:{}->E:{} DeltaT:{}", timestamp.frame,
+                                  time_since_epoch_check_start_ms, time_since_epoch_check_end_ms,
+                                  delta_time_ms);
+    _rss_check_timings.push_back(delta_time_ms);
+    while (_rss_check_timings.size() > 10u) {
+      _rss_check_timings.pop_front();
     }
+    double agv_time=0.;
+    for (auto run_time: _rss_check_timings) {
+      agv_time += run_time;
+    }
+    agv_time /= _rss_check_timings.size();
+    _rss_check->GetLogger()->info("RssSensor[{}] runtime {} avg {}", timestamp.frame, delta_time_ms, agv_time);
+    _processing_lock.unlock();
+
+    callback(MakeShared<sensor::data::RssResponse>(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), result,
+                                                 response, rss_state_snapshot, situation_snapshot, world_model,
+                                                   ego_dynamics_on_route));
   } catch (const std::exception &e) {
-    /// @todo do we need to unsubscribe the sensor here?
-    std::cout << e.what() << std::endl;
+    _rss_check->GetLogger()->error("RssSensor[{}] tick exception", timestamp.frame);
     _processing_lock.unlock();
-    spdlog::error("RssSensor tick exception");
-    return nullptr;
   } catch (...) {
+    _rss_check->GetLogger()->error("RssSensor[{}] tick exception", timestamp.frame);
     _processing_lock.unlock();
-    spdlog::error("RssSensor tick exception");
-    return nullptr;
   }
 }
 

LibCarla/source/carla/rss/RssSensor.h

@@ -5,6 +5,9 @@
 
 #pragma once
 
+#include <atomic>
+#include <future>
+#include <list>
 #include <memory>
 #include <mutex>
 #include <vector>
@@ -66,6 +69,7 @@ public:
   void Listen(CallbackFunctionType callback) override;
 
   /// Stop listening for new measurements.
+  /// Be aware:  GIL has to be unlocked to be able to wait for callbacks not active.
   void Stop() override;
 
   /// Return whether this Sensor instance is currently listening to the
@@ -123,7 +127,12 @@ public:
 
 private:
   /// the acutal sensor tick callback function
-  SharedPtr<sensor::SensorData> TickRssSensor(const Timestamp &timestamp);
+  void TickRssSensor(const client::Timestamp &timestamp, CallbackFunctionType callback);
+  void TickRssSensorThreadLocked(const client::Timestamp &timestamp, SharedPtr<carla::client::ActorList> actors,
+                                 CallbackFunctionType callback);
+
+  //// the object actually performing the RSS processing
+  std::shared_ptr<::carla::rss::RssCheck> _rss_check;
 
   /// the id got when registering for the on tick event
   std::size_t _on_tick_register_id;
@@ -131,8 +140,11 @@ private:
   /// the mutex to protect the actual RSS processing and in case it takes too long to process ever frame
   std::mutex _processing_lock;
 
-  //// the object actually performing the RSS processing
-  std::shared_ptr<::carla::rss::RssCheck> _rss_check;
+  /// the future for the async ticking thread
+  std::future<void> _tick_future;
+
+  /// some debug timings
+  std::list<double> _rss_check_timings;
 
   //// the rss actor constellation callback function
   ActorConstellationCallbackFunctionType _rss_actor_constellation_callback;
@@ -142,6 +154,8 @@ private:
 
   /// last processed frame
   std::size_t _last_processed_frame;
+
+  static std::atomic_uint _global_map_initialization_counter_;
 };
 
 }  // namespace client

LibCarla/source/carla/sensor/SensorRegistry.h

@@ -1,4 +1,4 @@
-// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
+// Copyright (c) 2022 Computer Vision Center (CVC) at the Universitat Autonoma
 // de Barcelona (UAB).
 //
 // This work is licensed under the terms of the MIT license.
@@ -30,6 +30,7 @@
 // 2. Add a forward-declaration of the sensor here.
 class ACollisionSensor;
 class ADepthCamera;
+class ANormalsCamera;
 class ADVSCamera;
 class AGnssSensor;
 class AInertialMeasurementUnit;
@@ -41,6 +42,7 @@ class ARayCastSemanticLidar;
 class ARayCastLidar;
 class ASceneCaptureCamera;
 class ASemanticSegmentationCamera;
+class AInstanceSegmentationCamera;
 class ARssSensor;
 class FWorldObserver;
 
@@ -57,6 +59,7 @@ namespace sensor {
   using SensorRegistry = CompositeSerializer<
     std::pair<ACollisionSensor *, s11n::CollisionEventSerializer>,
     std::pair<ADepthCamera *, s11n::ImageSerializer>,
+    std::pair<ANormalsCamera *, s11n::ImageSerializer>,
     std::pair<ADVSCamera *, s11n::DVSEventArraySerializer>,
     std::pair<AGnssSensor *, s11n::GnssSerializer>,
     std::pair<AInertialMeasurementUnit *, s11n::IMUSerializer>,
@@ -69,6 +72,7 @@ namespace sensor {
     std::pair<ARssSensor *, s11n::NoopSerializer>,
     std::pair<ASceneCaptureCamera *, s11n::ImageSerializer>,
     std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>,
+    std::pair<AInstanceSegmentationCamera *, s11n::ImageSerializer>,
     std::pair<FWorldObserver *, s11n::EpisodeStateSerializer>
   >;
 
@@ -82,6 +86,7 @@ namespace sensor {
 // 4. Include the sensor here.
 #include "Carla/Sensor/CollisionSensor.h"
 #include "Carla/Sensor/DepthCamera.h"
+#include "Carla/Sensor/NormalsCamera.h"
 #include "Carla/Sensor/DVSCamera.h"
 #include "Carla/Sensor/GnssSensor.h"
 #include "Carla/Sensor/InertialMeasurementUnit.h"
@@ -94,6 +99,7 @@ namespace sensor {
 #include "Carla/Sensor/RssSensor.h"
 #include "Carla/Sensor/SceneCaptureCamera.h"
 #include "Carla/Sensor/SemanticSegmentationCamera.h"
+#include "Carla/Sensor/InstanceSegmentationCamera.h"
 #include "Carla/Sensor/WorldObserver.h"
 
 #endif // LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES

LibCarla/source/carla/sensor/data/Color.h

@@ -7,6 +7,7 @@
 #pragma once
 
 #include "carla/rpc/Color.h"
+#include "carla/rpc/FloatColor.h"
 
 #include <cstdint>
 
@@ -36,11 +37,15 @@ namespace data {
     operator rpc::Color() const {
       return {r, g, b};
     }
+    operator rpc::FloatColor() const {
+      return {r/255.f, g/255.f, b/255.f, a/255.f};
+    }
 
     uint8_t b = 0u;
     uint8_t g = 0u;
     uint8_t r = 0u;
     uint8_t a = 0u;
+    MSGPACK_DEFINE_ARRAY(r, g, b, a);
   };
 #pragma pack(pop)
 

LibCarla/source/carla/streaming/detail/tcp/Client.cpp

@@ -127,7 +127,7 @@ namespace tcp {
                   ReadData();
                 } else {
                   // Else try again.
-                  log_info("streaming client: failed to send stream id:", ec.message());
+                  log_debug("streaming client: failed to send stream id:", ec.message());
                   Connect();
                 }
               }));
@@ -186,7 +186,7 @@ namespace tcp {
           ReadData();
         } else {
           // As usual, if anything fails start over from the very top.
-          log_info("streaming client: failed to read data:", ec.message());
+          log_debug("streaming client: failed to read data:", ec.message());
           Connect();
         }
       };
@@ -207,7 +207,7 @@ namespace tcp {
               message->buffer(),
               boost::asio::bind_executor(_strand, handle_read_data));
         } else if (!_done) {
-          log_info("streaming client: failed to read header:", ec.message());
+          log_debug("streaming client: failed to read header:", ec.message());
           DEBUG_ONLY(log_debug("size  = ", message->size()));
           DEBUG_ONLY(log_debug("bytes = ", bytes));
           Connect();

LibCarla/source/carla/trafficmanager/ALSM.cpp

@@ -27,7 +27,7 @@ ALSM::ALSM(
   CollisionStage &collision_stage,
   TrafficLightStage &traffic_light_stage,
   MotionPlanStage &motion_plan_stage,
-  RandomGeneratorMap &random_devices)
+  VehicleLightStage &vehicle_light_stage)
   : registered_vehicles(registered_vehicles),
     buffer_map(buffer_map),
     track_traffic(track_traffic),
@@ -40,7 +40,7 @@ ALSM::ALSM(
     collision_stage(collision_stage),
     traffic_light_stage(traffic_light_stage),
     motion_plan_stage(motion_plan_stage),
-    random_devices(random_devices) {}
+    vehicle_light_stage(vehicle_light_stage) {}
 
 void ALSM::Update() {
 
@@ -373,11 +373,11 @@ void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
     registered_vehicles.Remove({actor_id});
     buffer_map.erase(actor_id);
     idle_time.erase(actor_id);
-    random_devices.erase(actor_id);
     localization_stage.RemoveActor(actor_id);
     collision_stage.RemoveActor(actor_id);
     traffic_light_stage.RemoveActor(actor_id);
     motion_plan_stage.RemoveActor(actor_id);
+    vehicle_light_stage.RemoveActor(actor_id);
   }
   else {
     unregistered_actors.erase(actor_id);

LibCarla/source/carla/trafficmanager/ALSM.h

@@ -18,6 +18,7 @@
 #include "carla/trafficmanager/RandomGenerator.h"
 #include "carla/trafficmanager/SimulationState.h"
 #include "carla/trafficmanager/TrafficLightStage.h"
+#include "carla/trafficmanager/VehicleLightStage.h"
 
 namespace carla {
 namespace traffic_manager {
@@ -59,11 +60,10 @@ private:
   CollisionStage &collision_stage;
   TrafficLightStage &traffic_light_stage;
   MotionPlanStage &motion_plan_stage;
+  VehicleLightStage &vehicle_light_stage;
   // Time elapsed since last vehicle destruction due to being idle for too long.
   double elapsed_last_actor_destruction {0.0};
   cc::Timestamp current_timestamp;
-  // Random devices.
-  RandomGeneratorMap &random_devices;
   std::unordered_map<ActorId, bool> has_physics_enabled;
 
   // Updates the duration for which a registered vehicle is stuck at a location.
@@ -102,7 +102,7 @@ public:
        CollisionStage &collision_stage,
        TrafficLightStage &traffic_light_stage,
        MotionPlanStage &motion_plan_stage,
-       RandomGeneratorMap &random_devices);
+       VehicleLightStage &vehicle_light_stage);
 
   void Update();
 

LibCarla/source/carla/trafficmanager/AtomicActorSet.h

@@ -7,7 +7,7 @@
 #pragma once
 
 #include <mutex>
-#include <unordered_map>
+#include <map>
 #include <vector>
 
 #include "carla/client/Actor.h"
@@ -25,7 +25,7 @@ namespace traffic_manager {
   private:
 
     std::mutex modification_mutex;
-    std::unordered_map<ActorId, ActorPtr> actor_set;
+    std::map<ActorId, ActorPtr> actor_set;
     int state_counter;
 
   public:

LibCarla/source/carla/trafficmanager/CachedSimpleWaypoint.cpp

@@ -35,6 +35,7 @@ namespace traffic_manager {
 
     this->geodesic_grid_id = simple_waypoint->GetGeodesicGridId();
     this->is_junction = simple_waypoint->CheckJunction();
+    this->road_option = static_cast<uint8_t>(simple_waypoint->GetRoadOption());
   }
 
   void CachedSimpleWaypoint::Write(std::ofstream &out_file) {
@@ -70,6 +71,9 @@ namespace traffic_manager {
 
     // is_junction
     WriteValue<bool>(out_file, this->is_junction);
+
+    // road_option
+    WriteValue<uint8_t>(out_file, this->road_option);
   }
 
   void CachedSimpleWaypoint::Read(std::ifstream &in_file) {
@@ -109,6 +113,9 @@ namespace traffic_manager {
 
     // is_junction
     ReadValue<bool>(in_file, this->is_junction);
+
+    // road_option
+    ReadValue<uint8_t>(in_file, this->road_option);
   }
 
   void CachedSimpleWaypoint::Read(const std::vector<uint8_t>& content, unsigned long& start) {
@@ -147,6 +154,9 @@ namespace traffic_manager {
 
     // is_junction
     ReadValue<bool>(content, start, this->is_junction);
+
+    // road_option
+    ReadValue<uint8_t>(content, start, this->road_option);
   }
 
 } // namespace traffic_manager

LibCarla/source/carla/trafficmanager/CachedSimpleWaypoint.h

@@ -28,6 +28,7 @@ namespace traffic_manager {
     uint64_t next_right_waypoint = 0;
     int32_t geodesic_grid_id;
     bool is_junction;
+    uint8_t road_option;
 
     CachedSimpleWaypoint() = default;
     CachedSimpleWaypoint(const SimpleWaypointPtr& simple_waypoint);

LibCarla/source/carla/trafficmanager/CollisionStage.cpp

@@ -22,14 +22,14 @@ CollisionStage::CollisionStage(
   const TrackTraffic &track_traffic,
   const Parameters &parameters,
   CollisionFrame &output_array,
-  RandomGeneratorMap &random_devices)
+  RandomGenerator &random_device)
   : vehicle_id_list(vehicle_id_list),
     simulation_state(simulation_state),
     buffer_map(buffer_map),
     track_traffic(track_traffic),
     parameters(parameters),
     output_array(output_array),
-    random_devices(random_devices) {}
+    random_device(random_device) {}
 
 void CollisionStage::Update(const unsigned long index) {
   ActorId obstacle_id = 0u;
@@ -47,15 +47,16 @@ void CollisionStage::Update(const unsigned long index) {
     std::vector<ActorId> collision_candidate_ids;
     // Run through vehicles with overlapping paths and filter them;
     const float distance_to_leading = parameters.GetDistanceToLeadingVehicle(ego_actor_id);
-    float collision_radius_square = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN) + distance_to_leading;;
+    float collision_radius_square = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN);
     if (velocity < 2.0f) {
       const float length = simulation_state.GetDimensions(ego_actor_id).x;
       const float collision_radius_stop = COLLISION_RADIUS_STOP + length;
       collision_radius_square = SQUARE(collision_radius_stop);
-      if (distance_to_leading > collision_radius_stop) {
-        collision_radius_square = SQUARE(collision_radius_stop) + distance_to_leading;
-      }
     }
+    if (distance_to_leading > collision_radius_square) {
+        collision_radius_square = SQUARE(distance_to_leading);
+    }
+
     for (ActorId overlapping_actor_id : overlapping_actors) {
       // If actor is within maximum collision avoidance and vertical overlap range.
       const cg::Location &overlapping_actor_location = simulation_state.GetLocation(overlapping_actor_id);
@@ -90,9 +91,9 @@ void CollisionStage::Update(const unsigned long index) {
                                                                        look_ahead_index);
         if (negotiation_result.first) {
           if ((other_actor_type == ActorType::Vehicle
-               && parameters.GetPercentageIgnoreVehicles(ego_actor_id) <= random_devices.at(ego_actor_id).next())
+               && parameters.GetPercentageIgnoreVehicles(ego_actor_id) <= random_device.next())
               || (other_actor_type == ActorType::Pedestrian
-                  && parameters.GetPercentageIgnoreWalkers(ego_actor_id) <= random_devices.at(ego_actor_id).next())) {
+                  && parameters.GetPercentageIgnoreWalkers(ego_actor_id) <= random_device.next())) {
             collision_hazard = true;
             obstacle_id = other_actor_id;
             available_distance_margin = negotiation_result.second;
@@ -121,7 +122,7 @@ float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id) {
   const float velocity = cg::Math::Dot(simulation_state.GetVelocity(actor_id), simulation_state.GetHeading(actor_id));
   float bbox_extension;
   // Using a function to calculate boundary length.
-  float velocity_extension = VEL_EXT_FACTOR*velocity;
+  float velocity_extension = VEL_EXT_FACTOR * velocity;
   bbox_extension = BOUNDARY_EXTENSION_MINIMUM + velocity_extension * velocity_extension;
   // If a valid collision lock present, change boundary length to maintain lock.
   if (collision_locks.find(actor_id) != collision_locks.end()) {

LibCarla/source/carla/trafficmanager/CollisionStage.h

@@ -57,7 +57,7 @@ private:
   // to avoid repeated computation within a cycle.
   GeometryComparisonMap geometry_cache;
   GeodesicBoundaryMap geodesic_boundary_map;
-  RandomGeneratorMap &random_devices;
+  RandomGenerator &random_device;
 
   // Method to determine if a vehicle is on a collision path to another.
   std::pair<bool, float> NegotiateCollision(const ActorId reference_vehicle_id,
@@ -90,7 +90,7 @@ public:
                  const TrackTraffic &track_traffic,
                  const Parameters &parameters,
                  CollisionFrame &output_array,
-                 RandomGeneratorMap &random_devices);
+                 RandomGenerator &random_device);
 
   void Update (const unsigned long index) override;
 

LibCarla/source/carla/trafficmanager/Constants.h

@@ -51,8 +51,8 @@ static const float HIGH_SPEED_HORIZON_RATE = 4.0f;
 } // namespace PathBufferUpdate
 
 namespace WaypointSelection {
-static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f;
-static const float TARGET_WAYPOINT_HORIZON_LENGTH = 1.0f;
+static const float TARGET_WAYPOINT_TIME_HORIZON = 0.3f;
+static const float MIN_TARGET_WAYPOINT_DISTANCE = 3.0f;
 static const float JUNCTION_LOOK_AHEAD = 5.0f;
 static const float SAFE_DISTANCE_AFTER_JUNCTION = 4.0f;
 static const float MIN_JUNCTION_LENGTH = 8.0f;
@@ -64,6 +64,10 @@ static const float MINIMUM_LANE_CHANGE_DISTANCE = 20.0f;
 static const float MAXIMUM_LANE_OBSTACLE_DISTANCE = 50.0f;
 static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f;
 static const float INTER_LANE_CHANGE_DISTANCE = 10.0f;
+static const float MIN_WPT_DISTANCE = 5.0f;
+static const float MAX_WPT_DISTANCE = 20.0f;
+static const float MIN_LANE_CHANGE_SPEED = 5.0f;
+static const float FIFTYPERC = 50.0f;
 } // namespace LaneChange
 
 namespace Collision {
@@ -80,7 +84,7 @@ static const float WALKER_TIME_EXTENSION = 1.5f;
 static const float SQUARE_ROOT_OF_TWO = 1.414f;
 static const float VERTICAL_OVERLAP_THRESHOLD = 4.0f;
 static const float EPSILON = 2.0f * std::numeric_limits<float>::epsilon();
-static const float MIN_REFERENCE_DISTANCE = 1.0f;
+static const float MIN_REFERENCE_DISTANCE = 0.5f;
 static const float MIN_VELOCITY_COLL_RADIUS = 2.0f;
 static const float VEL_EXT_FACTOR = 0.36f;
 } // namespace Collision
@@ -97,9 +101,10 @@ static const float MAX_GEODESIC_GRID_LENGTH = 20.0f;
 static const float MAP_RESOLUTION = 5.0f;
 static const float INV_MAP_RESOLUTION = 1.0f / MAP_RESOLUTION;
 static const double MAX_WPT_DISTANCE = MAP_RESOLUTION/2.0 + SQUARE(MAP_RESOLUTION);
-static const float MAX_WPT_RADIANS = 0.1745f;  // 10º
+static const float MAX_WPT_RADIANS = 0.087f;  // 5º
 static float const DELTA = 25.0f;
 static float const Z_DELTA = 500.0f;
+static float const STRAIGHT_DEG = 19.0f;
 } // namespace Map
 
 namespace TrafficLight {
@@ -128,9 +133,19 @@ static const float PERC_MAX_SLOWDOWN = 0.08f;
 static const float FOLLOW_LEAD_FACTOR = 2.0f;
 } // namespace MotionPlan
 
+namespace VehicleLight {
+static const float SUN_ALTITUDE_DEGREES_BEFORE_DAWN = 15.0f;
+static const float SUN_ALTITUDE_DEGREES_AFTER_SUNSET = 165.0f;
+static const float SUN_ALTITUDE_DEGREES_JUST_AFTER_DAWN = 35.0f;
+static const float SUN_ALTITUDE_DEGREES_JUST_BEFORE_SUNSET = 145.0f;
+static const float HEAVY_PRECIPITATION_THRESHOLD = 80.0f;
+static const float FOG_DENSITY_THRESHOLD = 20.0f;
+static const float MAX_DISTANCE_LIGHT_CHECK = 225.0f;
+} // namespace VehicleLight
+
 namespace PID {
 static const float MAX_THROTTLE = 0.85f;
-static const float MAX_BRAKE = 1.0f;
+static const float MAX_BRAKE = 0.7f;
 static const float MAX_STEERING = 0.8f;
 static const float MAX_STEERING_DIFF = 0.15f;
 static const float DT = 0.05f;

LibCarla/source/carla/trafficmanager/InMemoryMap.cpp

@@ -130,6 +130,7 @@ namespace traffic_manager {
       SimpleWaypointPtr wp = std::make_shared<SimpleWaypoint>(waypoint_ptr);
       wp->SetGeodesicGridId(cached_wp.geodesic_grid_id);
       wp->SetIsJunction(cached_wp.is_junction);
+      wp->SetRoadOption(static_cast<RoadOption>(cached_wp.road_option));
       dense_topology.push_back(wp);
     }
 
@@ -229,8 +230,7 @@ namespace traffic_manager {
     }
 
     // 3. Processing waypoints.
-    auto distance_squared =
-    [](cg::Location l1, cg::Location l2) {
+    auto distance_squared = [](cg::Location l1, cg::Location l2) {
       return cg::Math::DistanceSquared(l1, l2);
     };
     auto square = [](float input) {return std::pow(input, 2);};
@@ -284,17 +284,19 @@ namespace traffic_manager {
       // Placing intra-segment connections.
       cg::Location grid_edge_location = segment_waypoints.front()->GetLocation();
       for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
-
+        SimpleWaypointPtr current_waypoint = segment_waypoints.at(i);
+        SimpleWaypointPtr next_waypoint = segment_waypoints.at(i+1);
         // Assigning grid id.
-        if (distance_squared(grid_edge_location, segment_waypoints.at(i)->GetLocation()) >
+        if (distance_squared(grid_edge_location, current_waypoint->GetLocation()) >
         square(MAX_GEODESIC_GRID_LENGTH)) {
           ++geodesic_grid_id_counter;
-          grid_edge_location = segment_waypoints.at(i)->GetLocation();
+          grid_edge_location = current_waypoint->GetLocation();
         }
-        segment_waypoints.at(i)->SetGeodesicGridId(geodesic_grid_id_counter);
+        current_waypoint->SetGeodesicGridId(geodesic_grid_id_counter);
+
+        current_waypoint->SetNextWaypoint({next_waypoint});
+        next_waypoint->SetPreviousWaypoint({current_waypoint});
 
-        segment_waypoints.at(i)->SetNextWaypoint({segment_waypoints.at(i + 1)});
-        segment_waypoints.at(i + 1)->SetPreviousWaypoint({segment_waypoints.at(i)});
       }
       segment_waypoints.back()->SetGeodesicGridId(geodesic_grid_id_counter);
 
@@ -328,14 +330,14 @@ namespace traffic_manager {
     }
 
     // Linking lane change connections.
-    for (auto &simple_waypoint:dense_topology) {
-      if (!simple_waypoint->CheckJunction()) {
-        FindAndLinkLaneChange(simple_waypoint);
+    for (auto &swp : dense_topology) {
+      if (!swp->CheckJunction()) {
+        FindAndLinkLaneChange(swp);
       }
     }
 
     // Linking any unconnected segments.
-    for (auto &swp: dense_topology) {
+    for (auto &swp : dense_topology) {
       if (swp->GetNextWaypoint().empty()) {
         auto neighbour = swp->GetRightWaypoint();
         if (!neighbour) {
@@ -350,6 +352,9 @@ namespace traffic_manager {
         }
       }
     }
+
+    // Specifying a RoadOption for each SimpleWaypoint
+    SetUpRoadOption();
   }
 
   void InMemoryMap::SetUpSpatialTree() {
@@ -362,6 +367,95 @@ namespace traffic_manager {
     }
   }
 
+  void InMemoryMap::SetUpRoadOption() {
+    for (auto &swp : dense_topology) {
+      std::vector<SimpleWaypointPtr> next_waypoints = swp->GetNextWaypoint();
+      std::size_t next_swp_size = next_waypoints.size();
+
+      if (next_swp_size == 0) {
+        // No next waypoint means that this is an end of the road.
+        swp->SetRoadOption(RoadOption::RoadEnd);
+      }
+
+      else if (next_swp_size > 1 || (!swp->CheckJunction() && next_waypoints.front()->CheckJunction())) {
+        // To check if we are in an actual junction, and not on an highway, we try to see
+        // if there's a landmark nearby of type Traffic Light, Stop Sign or Yield Sign.
+
+        bool found_landmark= false;
+        if (next_swp_size <= 1) {
+
+          auto all_landmarks = swp->GetWaypoint()->GetAllLandmarksInDistance(15.0);
+
+          if (all_landmarks.empty()) {
+            // Landmark hasn't been found, this isn't a junction.
+            swp->SetRoadOption(RoadOption::LaneFollow);
+          } else {
+            for (auto &landmark : all_landmarks) {
+              auto landmark_type = landmark->GetType();
+              if (landmark_type == "1000001" || landmark_type == "206" || landmark_type == "205") {
+                // We found a landmark.
+                found_landmark= true;
+                break;
+              }
+            }
+            if (!found_landmark) {
+              swp->SetRoadOption(RoadOption::LaneFollow);
+            }
+          }
+        }
+
+        // If we did find a landmark, or we are in the other case, find all waypoints
+        // in the junction and assign the correct RoadOption.
+        if (found_landmark || next_swp_size > 1) {
+          swp->SetRoadOption(RoadOption::LaneFollow);
+          for (auto &next_swp : next_waypoints) {
+            std::vector<SimpleWaypointPtr> traversed_waypoints;
+            SimpleWaypointPtr junction_end_waypoint;
+
+            if (next_swp_size > 1) {
+              junction_end_waypoint = next_swp;
+            } else {
+              junction_end_waypoint = next_waypoints.front();
+            }
+
+            while (junction_end_waypoint->CheckJunction()){
+              traversed_waypoints.push_back(junction_end_waypoint);
+              std::vector<SimpleWaypointPtr> temp = junction_end_waypoint->GetNextWaypoint();
+              if (temp.empty()) {
+                break;
+              }
+              junction_end_waypoint = temp.front();
+            }
+
+            // Calculate the angle between the first and the last point of the junction.
+            int16_t current_angle = static_cast<int16_t>(traversed_waypoints.front()->GetTransform().rotation.yaw);
+            int16_t junction_end_angle = static_cast<int16_t>(traversed_waypoints.back()->GetTransform().rotation.yaw);
+            int16_t diff_angle = (junction_end_angle - current_angle) % 360;
+            bool straight = (diff_angle < STRAIGHT_DEG && diff_angle > -STRAIGHT_DEG) ||
+                  (diff_angle > 360-STRAIGHT_DEG && diff_angle <= 360) ||
+                  (diff_angle < -360+STRAIGHT_DEG && diff_angle >= -360);
+            bool right = (diff_angle >= STRAIGHT_DEG && diff_angle <= 180) ||
+                (diff_angle <= -180 && diff_angle >= -360+STRAIGHT_DEG);
+
+            auto assign_option = [](RoadOption ro, std::vector<SimpleWaypointPtr> traversed_waypoints) {
+              for (auto &twp : traversed_waypoints) {
+                  twp->SetRoadOption(ro);
+              }
+            };
+
+            // Assign RoadOption according to the angle.
+            if (straight) assign_option(RoadOption::Straight, traversed_waypoints);
+            else if (right) assign_option(RoadOption::Right, traversed_waypoints);
+            else assign_option(RoadOption::Left, traversed_waypoints);
+          }
+        }
+      }
+      else if (next_swp_size == 1 && swp->GetRoadOption() == RoadOption::Void) {
+        swp->SetRoadOption(RoadOption::LaneFollow);
+      }
+    }
+  }
+
   SimpleWaypointPtr InMemoryMap::GetWaypoint(const cg::Location loc) const {
 
     Point3D query_point(loc.x, loc.y, loc.z);

LibCarla/source/carla/trafficmanager/InMemoryMap.h

@@ -75,8 +75,7 @@ namespace bgi = boost::geometry::index;
     //bool Load(const std::string& filename);
     bool Load(const std::vector<uint8_t>& content);
 
-    /// This method constructs the local map with a resolution of
-    /// sampling_resolution.
+    /// This method constructs the local map with a resolution of sampling_resolution.
     void SetUp();
 
     /// This method returns the closest waypoint to a given location on the map.
@@ -85,8 +84,7 @@ namespace bgi = boost::geometry::index;
     /// This method returns n waypoints in an delta area with a certain distance from the ego vehicle.
     NodeList GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const;
 
-    /// This method returns the full list of discrete samples of the map in the
-    /// local cache.
+    /// This method returns the full list of discrete samples of the map in the local cache.
     NodeList GetDenseTopology() const;
 
     std::string GetMapName();
@@ -98,6 +96,7 @@ namespace bgi = boost::geometry::index;
 
     void SetUpDenseTopology();
     void SetUpSpatialTree();
+    void SetUpRoadOption();
 
     /// This method is used to find and place lane change links.
     void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);

LibCarla/source/carla/trafficmanager/LocalizationStage.cpp

@@ -10,6 +10,8 @@ using namespace constants::PathBufferUpdate;
 using namespace constants::LaneChange;
 using namespace constants::WaypointSelection;
 using namespace constants::SpeedThreshold;
+using namespace constants::Collision;
+using namespace constants::MotionPlan;
 
 LocalizationStage::LocalizationStage(
   const std::vector<ActorId> &vehicle_id_list,
@@ -20,7 +22,7 @@ LocalizationStage::LocalizationStage(
   Parameters &parameters,
   std::vector<ActorId>& marked_for_removal,
   LocalizationFrame &output_array,
-  RandomGeneratorMap &random_devices)
+  RandomGenerator &random_device)
     : vehicle_id_list(vehicle_id_list),
     buffer_map(buffer_map),
     simulation_state(simulation_state),
@@ -29,7 +31,7 @@ LocalizationStage::LocalizationStage(
     parameters(parameters),
     marked_for_removal(marked_for_removal),
     output_array(output_array),
-    random_devices(random_devices) {}
+    random_device(random_device){}
 
 void LocalizationStage::Update(const unsigned long index) {
 
@@ -93,10 +95,11 @@ void LocalizationStage::Update(const unsigned long index) {
       }
     }
 
-    // Purge waypoints too far from the front of the buffer.
+    // Purge waypoints too far from the front of the buffer, but not if it has reached a junction.
     while (!is_at_junction_entrance
            && !waypoint_buffer.empty()
-           && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square) {
+           && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square + horizon_square
+           && !waypoint_buffer.back()->CheckJunction()) {
       PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
     }
   }
@@ -112,21 +115,38 @@ void LocalizationStage::Update(const unsigned long index) {
   bool force_lane_change = lane_change_info.change_lane;
   bool lane_change_direction = lane_change_info.direction;
 
-  if (!force_lane_change) {
-    float perc_keep_right = parameters.GetKeepRightPercentage(actor_id);
-    if (perc_keep_right >= 0.0f && perc_keep_right >= random_devices.at(actor_id).next()) {
+  // Apply parameters for keep right rule and random lane changes.
+  if (!force_lane_change && vehicle_speed > MIN_LANE_CHANGE_SPEED){
+    const float perc_keep_right = parameters.GetKeepRightPercentage(actor_id);
+    const float perc_random_leftlanechange = parameters.GetRandomLeftLaneChangePercentage(actor_id);
+    const float perc_random_rightlanechange = parameters.GetRandomRightLaneChangePercentage(actor_id);
+    const bool is_keep_right = perc_keep_right > random_device.next();
+    const bool is_random_left_change = perc_random_leftlanechange >= random_device.next();
+    const bool is_random_right_change = perc_random_rightlanechange >= random_device.next();
+
+    // Determine which of the parameters we should apply.
+    if (is_keep_right || is_random_right_change) {
       force_lane_change = true;
       lane_change_direction = true;
     }
+    if (is_random_left_change) {
+      if (!force_lane_change) {
+        force_lane_change = true;
+        lane_change_direction = false;
+      } else {
+        // Both a left and right lane changes are forced. Choose between one of them.
+        lane_change_direction = FIFTYPERC > random_device.next();
+      }
+    }
   }
 
   const SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
   const float lane_change_distance = SQUARE(std::max(10.0f * vehicle_speed, INTER_LANE_CHANGE_DISTANCE));
 
-  bool recently_not_executed_lane_change = last_lane_change_location.find(actor_id) == last_lane_change_location.end();
+  bool recently_not_executed_lane_change = last_lane_change_swpt.find(actor_id) == last_lane_change_swpt.end();
   bool done_with_previous_lane_change = true;
   if (!recently_not_executed_lane_change) {
-    float distance_frm_previous = cg::Math::DistanceSquared(last_lane_change_location.at(actor_id), vehicle_location);
+    float distance_frm_previous = cg::Math::DistanceSquared(last_lane_change_swpt.at(actor_id)->GetLocation(), vehicle_location);
     done_with_previous_lane_change = distance_frm_previous > lane_change_distance;
   }
   bool auto_or_force_lane_change = parameters.GetAutoLaneChange(actor_id) || force_lane_change;
@@ -140,10 +160,10 @@ void LocalizationStage::Update(const unsigned long index) {
                                                            force_lane_change, lane_change_direction);
 
     if (change_over_point != nullptr) {
-      if (last_lane_change_location.find(actor_id) != last_lane_change_location.end()) {
-        last_lane_change_location.at(actor_id) = vehicle_location;
+      if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
+        last_lane_change_swpt.at(actor_id) = change_over_point;
       } else {
-        last_lane_change_location.insert({actor_id, vehicle_location});
+        last_lane_change_swpt.insert({actor_id, change_over_point});
       }
       auto number_of_pops = waypoint_buffer.size();
       for (uint64_t j = 0u; j < number_of_pops; ++j) {
@@ -153,27 +173,40 @@ void LocalizationStage::Update(const unsigned long index) {
     }
   }
 
-  // Populating the buffer.
-  while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
+  Path imported_path = parameters.GetCustomPath(actor_id);
+  Route imported_actions = parameters.GetImportedRoute(actor_id);
+  // We are effectively importing a path.
+  if (!imported_path.empty()) {
+
+    ImportPath(imported_path, waypoint_buffer, actor_id, horizon_square);
+
+  } else if (!imported_actions.empty()) {
+
+    ImportRoute(imported_actions, waypoint_buffer, actor_id, horizon_square);
 
-    SimpleWaypointPtr furthest_waypoint = waypoint_buffer.back();
-    std::vector<SimpleWaypointPtr> next_waypoints = furthest_waypoint->GetNextWaypoint();
-    uint64_t selection_index = 0u;
-    // Pseudo-randomized path selection if found more than one choice.
-    if (next_waypoints.size() > 1) {
-      double r_sample = random_devices.at(actor_id).next();
-      selection_index = static_cast<uint64_t>(r_sample*next_waypoints.size()*0.01);
-    } else if (next_waypoints.size() == 0) {
-      if (!parameters.GetOSMMode()) {
-        std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
-      }
-      marked_for_removal.push_back(actor_id);
-      break;
-    }
-    SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
-    PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
   }
 
+  // Populating the buffer through randomly chosen waypoints.
+  else {
+    while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
+      SimpleWaypointPtr furthest_waypoint = waypoint_buffer.back();
+      std::vector<SimpleWaypointPtr> next_waypoints = furthest_waypoint->GetNextWaypoint();
+      uint64_t selection_index = 0u;
+      // Pseudo-randomized path selection if found more than one choice.
+      if (next_waypoints.size() > 1) {
+        double r_sample = random_device.next();
+        selection_index = static_cast<uint64_t>(r_sample*next_waypoints.size()*0.01);
+      } else if (next_waypoints.size() == 0) {
+        if (!parameters.GetOSMMode()) {
+          std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
+        }
+        marked_for_removal.push_back(actor_id);
+        break;
+      }
+      SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
+      PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
+    }
+  }
   ExtendAndFindSafeSpace(actor_id, is_at_junction_entrance, waypoint_buffer);
 
   // Editing output array
@@ -282,12 +315,12 @@ void LocalizationStage::ExtendAndFindSafeSpace(const ActorId actor_id,
 }
 
 void LocalizationStage::RemoveActor(ActorId actor_id) {
-    last_lane_change_location.erase(actor_id);
+    last_lane_change_swpt.erase(actor_id);
     vehicles_at_junction.erase(actor_id);
 }
 
 void LocalizationStage::Reset() {
-  last_lane_change_location.clear();
+  last_lane_change_swpt.clear();
   vehicles_at_junction.clear();
 }
 
@@ -400,11 +433,11 @@ SimpleWaypointPtr LocalizationStage::AssignLaneChange(const ActorId actor_id,
     }
 
     if (change_over_point != nullptr) {
-      const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, 3.0f, 20.0f);
-      const auto starting_point = change_over_point;
+      const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, MIN_WPT_DISTANCE, MAX_WPT_DISTANCE);
+      const SimpleWaypointPtr starting_point = change_over_point;
       while (change_over_point->DistanceSquared(starting_point) < SQUARE(change_over_distance) &&
              !change_over_point->CheckJunction()) {
-        change_over_point = change_over_point->GetNextWaypoint()[0];
+        change_over_point = change_over_point->GetNextWaypoint().front();
       }
     }
   }
@@ -412,5 +445,215 @@ SimpleWaypointPtr LocalizationStage::AssignLaneChange(const ActorId actor_id,
   return change_over_point;
 }
 
+void LocalizationStage::ImportPath(Path &imported_path, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
+    // Remove the waypoints already added to the path, except for the first.
+    if (parameters.GetUploadPath(actor_id)) {
+      auto number_of_pops = waypoint_buffer.size();
+      for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
+        PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
+      }
+      // We have successfully imported the path. Remove it from the list of paths to be imported.
+      parameters.RemoveUploadPath(actor_id, false);
+    }
+
+    // Get the latest imported waypoint. and find its closest waypoint in TM's InMemoryMap.
+    cg::Location latest_imported = imported_path.front();
+    SimpleWaypointPtr imported = local_map->GetWaypoint(latest_imported);
+
+    // We need to generate a path compatible with TM's waypoints.
+    while (!imported_path.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
+      // Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
+      SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
+
+      // Try to link the latest_waypoint to the imported waypoint.
+      std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
+      uint64_t selection_index = 0u;
+
+      // Choose correct path.
+      if (next_waypoints.size() > 1) {
+        const float imported_road_id = imported->GetWaypoint()->GetRoadId();
+        float min_distance = std::numeric_limits<float>::infinity();
+        for (uint64_t k = 0u; k < next_waypoints.size(); ++k) {
+          SimpleWaypointPtr junction_end_point = next_waypoints.at(k);
+          while (!junction_end_point->CheckJunction()) {
+            junction_end_point = junction_end_point->GetNextWaypoint().front();
+          }
+          while (junction_end_point->CheckJunction()) {
+            junction_end_point = junction_end_point->GetNextWaypoint().front();
+          }
+          while (next_waypoints.at(k)->DistanceSquared(junction_end_point) < 50.0f) {
+            junction_end_point = junction_end_point->GetNextWaypoint().front();
+          }
+          float jep_road_id = junction_end_point->GetWaypoint()->GetRoadId();
+          if (jep_road_id == imported_road_id) {
+            selection_index = k;
+            break;
+          }
+          float distance = junction_end_point->DistanceSquared(imported);
+          if (distance < min_distance) {
+            min_distance = distance;
+            selection_index = k;
+          }
+        }
+      } else if (next_waypoints.size() == 0) {
+        if (!parameters.GetOSMMode()) {
+          std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
+        }
+        marked_for_removal.push_back(actor_id);
+        break;
+      }
+      SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
+      PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
+
+      // Remove the imported waypoint from the path if it's close to the last one.
+      if (next_wp_selection->DistanceSquared(imported) < 30.0f) {
+        imported_path.erase(imported_path.begin());
+        PushWaypoint(actor_id, track_traffic, waypoint_buffer, imported);
+        latest_imported = imported_path.front();
+        imported = local_map->GetWaypoint(latest_imported);
+      }
+    }
+    if (imported_path.empty()) {
+      // Once we are done, check if we can clear the structure.
+      parameters.RemoveUploadPath(actor_id, true);
+    } else {
+      // Otherwise, update the structure with the waypoints that we still need to import.
+      parameters.UpdateUploadPath(actor_id, imported_path);
+    }
+}
+
+void LocalizationStage::ImportRoute(Route &imported_actions, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
+
+    if (parameters.GetUploadRoute(actor_id)) {
+      auto number_of_pops = waypoint_buffer.size();
+      for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
+        PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
+      }
+      // We have successfully imported the route. Remove it from the list of routes to be imported.
+      parameters.RemoveImportedRoute(actor_id, false);
+    }
+
+    RoadOption next_road_option = static_cast<RoadOption>(imported_actions.front());
+    while (!imported_actions.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
+      // Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
+      SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
+      RoadOption latest_road_option = latest_waypoint->GetRoadOption();
+      // Try to link the latest_waypoint to the correct next RouteOption.
+      std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
+      uint16_t selection_index = 0u;
+      if (next_waypoints.size() > 1) {
+        for (uint16_t i=0; i<next_waypoints.size(); ++i) {
+          if (next_waypoints.at(i)->GetRoadOption() == next_road_option) {
+            selection_index = i;
+            break;
+          } else {
+            if (i == next_waypoints.size() - 1) {
+              std::cout << "We couldn't find the RoadOption you were looking for. This route might diverge from the one expected." << std::endl;
+            }
+          }
+        }
+      } else if (next_waypoints.size() == 0) {
+        if (!parameters.GetOSMMode()) {
+          std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
+        }
+        marked_for_removal.push_back(actor_id);
+        break;
+      }
+
+      SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
+      PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
+
+      // If we are switching to a new RoadOption, it means the current one is already fully imported.
+      if (latest_road_option != next_wp_selection->GetRoadOption() && next_road_option == next_wp_selection->GetRoadOption()) {
+        imported_actions.erase(imported_actions.begin());
+        next_road_option = static_cast<RoadOption>(imported_actions.front());
+      }
+    }
+    if (imported_actions.empty()) {
+      // Once we are done, check if we can clear the structure.
+      parameters.RemoveImportedRoute(actor_id, true);
+    } else {
+      // Otherwise, update the structure with the waypoints that we still need to import.
+      parameters.UpdateImportedRoute(actor_id, imported_actions);
+    }
+}
+
+Action LocalizationStage::ComputeNextAction(const ActorId& actor_id) {
+  auto waypoint_buffer = buffer_map.at(actor_id);
+  auto next_action = std::make_pair(RoadOption::LaneFollow, waypoint_buffer.back()->GetWaypoint());
+  bool is_lane_change = false;
+  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
+    // A lane change is happening.
+    is_lane_change = true;
+    const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
+    const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
+    bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
+    if (left_heading) next_action = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
+    else next_action = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
+  }
+  for (auto &swpt : waypoint_buffer) {
+    RoadOption road_opt = swpt->GetRoadOption();
+    if (road_opt != RoadOption::LaneFollow) {
+      if (!is_lane_change) {
+        // No lane change in sight, we can assume this will be the next action.
+        return std::make_pair(road_opt, swpt->GetWaypoint());
+      } else {
+        // A lane change will happen as well as another action, we need to figure out which one will happen first.
+        cg::Location lane_change = last_lane_change_swpt.at(actor_id)->GetLocation();
+        cg::Location actual_location = simulation_state.GetLocation(actor_id);
+        auto distance_lane_change = cg::Math::DistanceSquared(actual_location, lane_change);
+        auto distance_other_action = cg::Math::DistanceSquared(actual_location, swpt->GetLocation());
+        if (distance_lane_change < distance_other_action) return next_action;
+        else return std::make_pair(road_opt, swpt->GetWaypoint());
+      }
+    }
+  }
+  return next_action;
+}
+
+ActionBuffer LocalizationStage::ComputeActionBuffer(const ActorId& actor_id) {
+
+  auto waypoint_buffer = buffer_map.at(actor_id);
+  ActionBuffer action_buffer;
+  Action lane_change;
+  bool is_lane_change = false;
+  SimpleWaypointPtr buffer_front = waypoint_buffer.front();
+  RoadOption last_road_opt = buffer_front->GetRoadOption();
+  action_buffer.push_back(std::make_pair(last_road_opt, buffer_front->GetWaypoint()));
+  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
+    // A lane change is happening.
+    is_lane_change = true;
+    const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
+    const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
+    bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
+    if (left_heading) lane_change = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
+    else lane_change = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
+  }
+  for (auto &wpt : waypoint_buffer) {
+    RoadOption current_road_opt = wpt->GetRoadOption();
+    if (current_road_opt != last_road_opt) {
+      action_buffer.push_back(std::make_pair(current_road_opt, wpt->GetWaypoint()));
+      last_road_opt = current_road_opt;
+    }
+  }
+  if (is_lane_change) {
+    // Insert the lane change action in the appropriate part of the action buffer.
+    auto distance_lane_change = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), lane_change.second->GetTransform().location);
+    for (uint16_t i = 0; i < action_buffer.size(); ++i) {
+      auto distance_action = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), waypoint_buffer.at(i)->GetLocation());
+      // If the waypoint related to the next action is further away from the one of the lane change, insert lane change action here.
+      // If we reached the end of the buffer, place the action at the end.
+      if (i == action_buffer.size()-1) {
+        action_buffer.push_back(lane_change);
+        break;
+      } else if (distance_action > distance_lane_change) {
+        action_buffer.insert(action_buffer.begin()+i, lane_change);
+        break;
+      }
+    }
+  }
+  return action_buffer;
+}
+
 } // namespace traffic_manager
 } // namespace carla

LibCarla/source/carla/trafficmanager/LocalizationStage.h

@@ -18,7 +18,12 @@ namespace traffic_manager {
 namespace cc = carla::client;
 
 using LocalMapPtr = std::shared_ptr<InMemoryMap>;
-using LaneChangeLocationMap = std::unordered_map<ActorId, cg::Location>;
+using LaneChangeSWptMap = std::unordered_map<ActorId, SimpleWaypointPtr>;
+using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
+using Action = std::pair<RoadOption, WaypointPtr>;
+using ActionBuffer = std::vector<Action>;
+using Path = std::vector<cg::Location>;
+using Route = std::vector<uint8_t>;
 
 /// This class has functionality to maintain a horizon of waypoints ahead
 /// of the vehicle for it to follow.
@@ -35,23 +40,31 @@ private:
   // Array of vehicles marked by stages for removal.
   std::vector<ActorId>& marked_for_removal;
   LocalizationFrame &output_array;
-  LaneChangeLocationMap last_lane_change_location;
+  LaneChangeSWptMap last_lane_change_swpt;
   ActorIdSet vehicles_at_junction;
   using SimpleWaypointPair = std::pair<SimpleWaypointPtr, SimpleWaypointPtr>;
   std::unordered_map<ActorId, SimpleWaypointPair> vehicles_at_junction_entrance;
-  RandomGeneratorMap &random_devices;
+  RandomGenerator &random_device;
 
   SimpleWaypointPtr AssignLaneChange(const ActorId actor_id,
                                      const cg::Location vehicle_location,
                                      const float vehicle_speed,
                                      bool force, bool direction);
 
-  void DrawBuffer(Buffer &buffer);
-
   void ExtendAndFindSafeSpace(const ActorId actor_id,
                               const bool is_at_junction_entrance,
                               Buffer &waypoint_buffer);
 
+  void ImportPath(Path &imported_path,
+                  Buffer &waypoint_buffer,
+                  const ActorId actor_id,
+                  const float horizon_square);
+
+  void ImportRoute(Route &imported_actions,
+                  Buffer &waypoint_buffer,
+                  const ActorId actor_id,
+                  const float horizon_square);
+
 public:
   LocalizationStage(const std::vector<ActorId> &vehicle_id_list,
                     BufferMap &buffer_map,
@@ -61,13 +74,18 @@ public:
                     Parameters &parameters,
                     std::vector<ActorId>& marked_for_removal,
                     LocalizationFrame &output_array,
-                    RandomGeneratorMap &random_devices);
+                    RandomGenerator &random_device);
 
   void Update(const unsigned long index) override;
 
   void RemoveActor(const ActorId actor_id) override;
 
   void Reset() override;
+
+  Action ComputeNextAction(const ActorId &actor_id);
+
+  ActionBuffer ComputeActionBuffer(const ActorId& actor_id);
+
 };
 
 } // namespace traffic_manager

LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp

@@ -61,7 +61,7 @@ TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &targe
   SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
   const SimpleWaypointPtr &buffer_front = waypoint_buffer.front();
   uint64_t startPosn = static_cast<uint64_t>(std::fabs(target_point_distance * INV_MAP_RESOLUTION));
-  uint64_t index = 0;
+  uint64_t index = startPosn;
   /// Condition to determine forward or backward scanning of waypoint buffer.
 
   if (startPosn < waypoint_buffer.size()) {

LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp

@@ -41,7 +41,7 @@ MotionPlanStage::MotionPlanStage(
   const TLFrame &tl_frame,
   const cc::World &world,
   ControlFrame &output_array,
-  RandomGeneratorMap &random_devices,
+  RandomGenerator &random_device,
   const LocalMapPtr &local_map)
     : vehicle_id_list(vehicle_id_list),
     simulation_state(simulation_state),
@@ -57,7 +57,7 @@ MotionPlanStage::MotionPlanStage(
     tl_frame(tl_frame),
     world(world),
     output_array(output_array),
-    random_devices(random_devices),
+    random_device(random_device),
     local_map(local_map) {
 
       // Adding structure to avoid retrieving traffic lights when checking for landmarks.
@@ -111,7 +111,7 @@ void MotionPlanStage::Update(const unsigned long index) {
     double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
 
     if (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT) {
-      float random_sample = (static_cast<float>(random_devices.at(actor_id).next())*dilate_factor) + lower_bound;
+      float random_sample = (static_cast<float>(random_device.next())*dilate_factor) + lower_bound;
       NodeList teleport_waypoint_list = local_map->GetWaypointsInDelta(hero_location, ATTEMPTS_TO_TELEPORT, random_sample);
       if (!teleport_waypoint_list.empty()) {
         for (auto &teleport_waypoint : teleport_waypoint_list) {
@@ -157,7 +157,7 @@ void MotionPlanStage::Update(const unsigned long index) {
       ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
 
       const float target_point_distance = std::max(vehicle_speed * TARGET_WAYPOINT_TIME_HORIZON,
-                                                  TARGET_WAYPOINT_HORIZON_LENGTH);
+                                                  MIN_TARGET_WAYPOINT_DISTANCE);
       const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
       const cg::Location target_location = target_waypoint->GetLocation();
       float dot_product = DeviationDotProduct(vehicle_location, vehicle_heading, target_location);
@@ -215,7 +215,6 @@ void MotionPlanStage::Update(const unsigned long index) {
       current_state.steer = actuation_signal.steer;
       StateEntry &state = pid_state_map.at(actor_id);
       state = current_state;
-
     }
     // For physics-less vehicles, determine position and orientation for teleportation.
     else {

LibCarla/source/carla/trafficmanager/MotionPlanStage.h

@@ -42,7 +42,7 @@ private:
   std::unordered_map<ActorId, cc::Timestamp> teleportation_instance;
   ControlFrame &output_array;
   cc::Timestamp current_timestamp;
-  RandomGeneratorMap &random_devices;
+  RandomGenerator &random_device;
   const LocalMapPtr &local_map;
   TLMap tl_map;
 
@@ -83,7 +83,7 @@ public:
                   const TLFrame &tl_frame,
                   const cc::World &world,
                   ControlFrame &output_array,
-                  RandomGeneratorMap &random_devices,
+                  RandomGenerator &random_device,
                   const LocalMapPtr &local_map);
 
   void Update(const unsigned long index);

LibCarla/source/carla/trafficmanager/Parameters.cpp

@@ -90,6 +90,25 @@ void Parameters::SetKeepRightPercentage(const ActorPtr &actor, const float perce
   perc_keep_right.AddEntry(entry);
 }
 
+void Parameters::SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) {
+
+  const auto entry = std::make_pair(actor->GetId(), percentage);
+  perc_random_left.AddEntry(entry);
+}
+
+void Parameters::SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) {
+
+  const auto entry = std::make_pair(actor->GetId(), percentage);
+  perc_random_right.AddEntry(entry);
+
+}
+
+void Parameters::SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update) {
+
+  const auto entry = std::make_pair(actor->GetId(), do_update);
+  auto_update_vehicle_lights.AddEntry(entry);
+}
+
 void Parameters::SetAutoLaneChange(const ActorPtr &actor, const bool enable) {
 
   const auto entry = std::make_pair(actor->GetId(), enable);
@@ -153,6 +172,48 @@ void Parameters::SetOSMMode(const bool mode_switch) {
   osm_mode.store(mode_switch);
 }
 
+void Parameters::SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) {
+  const auto entry = std::make_pair(actor->GetId(), path);
+  custom_path.AddEntry(entry);
+  const auto entry2 = std::make_pair(actor->GetId(), empty_buffer);
+  upload_path.AddEntry(entry2);
+}
+
+void Parameters::RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
+  if (!remove_path) {
+    upload_path.RemoveEntry(actor_id);
+  } else {
+    custom_path.RemoveEntry(actor_id);
+  }
+}
+
+void Parameters::UpdateUploadPath(const ActorId &actor_id, const Path path) {
+  custom_path.RemoveEntry(actor_id);
+  const auto entry = std::make_pair(actor_id, path);
+  custom_path.AddEntry(entry);
+}
+
+void Parameters::SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) {
+  const auto entry = std::make_pair(actor->GetId(), route);
+  custom_route.AddEntry(entry);
+  const auto entry2 = std::make_pair(actor->GetId(), empty_buffer);
+  upload_route.AddEntry(entry2);
+}
+
+void Parameters::RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
+  if (!remove_path) {
+    upload_route.RemoveEntry(actor_id);
+  } else {
+    custom_route.RemoveEntry(actor_id);
+  }
+}
+
+void Parameters::UpdateImportedRoute(const ActorId &actor_id, const Route route) {
+  custom_route.RemoveEntry(actor_id);
+  const auto entry = std::make_pair(actor_id, route);
+  custom_route.AddEntry(entry);
+}
+
 //////////////////////////////////// GETTERS //////////////////////////////////
 
 float Parameters::GetHybridPhysicsRadius() const {
@@ -212,7 +273,27 @@ float Parameters::GetKeepRightPercentage(const ActorId &actor_id) {
     percentage = perc_keep_right.GetValue(actor_id);
   }
 
-  perc_keep_right.RemoveEntry(actor_id);
+  return percentage;
+}
+
+float Parameters::GetRandomLeftLaneChangePercentage(const ActorId &actor_id) {
+
+  float percentage = -1.0f;
+
+  if (perc_random_left.Contains(actor_id)) {
+    percentage = perc_random_left.GetValue(actor_id);
+  }
+
+  return percentage;
+}
+
+float Parameters::GetRandomRightLaneChangePercentage(const ActorId &actor_id) {
+
+  float percentage = -1.0f;
+
+  if (perc_random_right.Contains(actor_id)) {
+    percentage = perc_random_right.GetValue(actor_id);
+  }
 
   return percentage;
 }
@@ -273,6 +354,16 @@ float Parameters::GetPercentageIgnoreWalkers(const ActorId &actor_id) const {
   return percentage;
 }
 
+bool Parameters::GetUpdateVehicleLights(const ActorId &actor_id) const {
+  bool do_update = false;
+
+  if (auto_update_vehicle_lights.Contains(actor_id)) {
+    do_update = auto_update_vehicle_lights.GetValue(actor_id);
+  }
+
+  return do_update;
+}
+
 float Parameters::GetPercentageIgnoreVehicles(const ActorId &actor_id) const {
 
   float percentage = 0.0f;
@@ -310,5 +401,51 @@ bool Parameters::GetOSMMode() const {
   return osm_mode.load();
 }
 
+bool Parameters::GetUploadPath(const ActorId &actor_id) const {
+
+  bool custom_path_bool = false;
+
+  if (upload_path.Contains(actor_id)) {
+    custom_path_bool = upload_path.GetValue(actor_id);
+  }
+
+  return custom_path_bool;
+}
+
+Path Parameters::GetCustomPath(const ActorId &actor_id) const {
+
+  Path custom_path_import;
+
+  if (custom_path.Contains(actor_id)) {
+    custom_path_import = custom_path.GetValue(actor_id);
+  }
+
+  return custom_path_import;
+}
+
+
+bool Parameters::GetUploadRoute(const ActorId &actor_id) const {
+
+  bool custom_route_bool = false;
+
+  if (upload_route.Contains(actor_id)) {
+    custom_route_bool = upload_route.GetValue(actor_id);
+  }
+
+  return custom_route_bool;
+}
+
+Route Parameters::GetImportedRoute(const ActorId &actor_id) const {
+
+  Route custom_route_import;
+
+  if (custom_route.Contains(actor_id)) {
+    custom_route_import = custom_route.GetValue(actor_id);
+  }
+
+  return custom_route_import;
+}
+
+
 } // namespace traffic_manager
 } // namespace carla

LibCarla/source/carla/trafficmanager/Parameters.h

@@ -9,6 +9,7 @@
 #include <atomic>
 #include <chrono>
 #include <random>
+#include <unordered_map>
 
 #include "carla/client/Actor.h"
 #include "carla/client/Vehicle.h"
@@ -25,6 +26,8 @@ namespace cc = carla::client;
 namespace cg = carla::geom;
 using ActorPtr = carla::SharedPtr<cc::Actor>;
 using ActorId = carla::ActorId;
+using Path = std::vector<cg::Location>;
+using Route = std::vector<uint8_t>;
 
 struct ChangeLaneInfo {
   bool change_lane = false;
@@ -56,6 +59,12 @@ private:
   AtomicMap<ActorId, float> perc_ignore_vehicles;
   /// Map containing % of keep right rule.
   AtomicMap<ActorId, float> perc_keep_right;
+  /// Map containing % of random left lane change.
+  AtomicMap<ActorId, float> perc_random_left;
+  /// Map containing % of random right lane change.
+  AtomicMap<ActorId, float> perc_random_right;
+  /// Map containing the automatic vehicle lights update flag
+  AtomicMap<ActorId, bool> auto_update_vehicle_lights;
   /// Synchronous mode switch.
   std::atomic<bool> synchronous_mode{false};
   /// Distance margin
@@ -76,6 +85,14 @@ private:
   std::atomic<float> hybrid_physics_radius {70.0};
   /// Parameter specifying Open Street Map mode.
   std::atomic<bool> osm_mode {true};
+  /// Parameter specifying if importing a custom path.
+  AtomicMap<ActorId, bool> upload_path;
+  /// Structure to hold all custom paths.
+  AtomicMap<ActorId, Path> custom_path;
+  /// Parameter specifying if importing a custom route.
+  AtomicMap<ActorId, bool> upload_route;
+  /// Structure to hold all custom routes.
+  AtomicMap<ActorId, Route> custom_route;
 
 public:
   Parameters();
@@ -120,9 +137,18 @@ public:
   /// Method to set % to ignore any vehicle.
   void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc);
 
-  /// Method to set probabilistic preference to keep on the right lane.
+  /// Method to set % to keep on the right lane.
   void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
 
+  /// Method to set % to randomly do a left lane change.
+  void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage);
+
+  /// Method to set % to randomly do a right lane change.
+  void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage);
+
+  /// Method to set the automatic vehicle light state update flag.
+  void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update);
+
   /// Method to set the distance to leading vehicle for all registered vehicles.
   void SetGlobalDistanceToLeadingVehicle(const float dist);
 
@@ -150,6 +176,24 @@ public:
   /// Method to set limits for boundaries when respawning vehicles.
   void SetMaxBoundaries(const float lower, const float upper);
 
+  /// Method to set our own imported path.
+  void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer);
+
+  /// Method to remove a list of points.
+  void RemoveUploadPath(const ActorId &actor_id, const bool remove_path);
+
+  /// Method to update an already set list of points.
+  void UpdateUploadPath(const ActorId &actor_id, const Path path);
+
+  /// Method to set our own imported route.
+  void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer);
+
+  /// Method to remove a route.
+  void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path);
+
+  /// Method to update an already set route.
+  void UpdateImportedRoute(const ActorId &actor_id, const Route route);
+
   ///////////////////////////////// GETTERS /////////////////////////////////////
 
   /// Method to retrieve hybrid physics radius.
@@ -167,6 +211,12 @@ public:
   /// Method to query percentage probability of keep right rule for a vehicle.
   float GetKeepRightPercentage(const ActorId &actor_id);
 
+  /// Method to query percentage probability of a random right lane change for a vehicle.
+  float GetRandomLeftLaneChangePercentage(const ActorId &actor_id);
+
+  /// Method to query percentage probability of a random left lane change for a vehicle.
+  float GetRandomRightLaneChangePercentage(const ActorId &actor_id);
+
   /// Method to query auto lane change rule for a vehicle.
   bool GetAutoLaneChange(const ActorId &actor_id) const;
 
@@ -185,6 +235,9 @@ public:
   /// Method to get % to ignore any walker.
   float GetPercentageIgnoreWalkers(const ActorId &actor_id) const;
 
+  /// Method to get if the vehicle lights should be updates automatically
+  bool GetUpdateVehicleLights(const ActorId &actor_id) const;
+
   /// Method to get synchronous mode.
   bool GetSynchronousMode() const;
 
@@ -206,6 +259,18 @@ public:
   /// Method to get Open Street Map mode.
   bool GetOSMMode() const;
 
+  /// Method to get if we are uploading a path.
+  bool GetUploadPath(const ActorId &actor_id) const;
+
+  /// Method to get a custom path.
+  Path GetCustomPath(const ActorId &actor_id) const;
+
+  /// Method to get if we are uploading a route.
+  bool GetUploadRoute(const ActorId &actor_id) const;
+
+  /// Method to get a custom route.
+  Route GetImportedRoute(const ActorId &actor_id) const;
+
   /// Synchronous mode time out variable.
   std::chrono::duration<double, std::milli> synchronous_time_out;
 };

LibCarla/source/carla/trafficmanager/RandomGenerator.h

@@ -18,7 +18,5 @@ private:
     std::uniform_real_distribution<double> dist;
 };
 
-using RandomGeneratorMap = std::unordered_map<carla::rpc::ActorId, RandomGenerator>;
-
 } // namespace traffic_manager
 } // namespace carla

LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp

@@ -134,5 +134,13 @@ namespace traffic_manager {
     return waypoint->GetTransform();
   }
 
+  void SimpleWaypoint::SetRoadOption(RoadOption _road_option) {
+    road_option = _road_option;
+  }
+
+  RoadOption SimpleWaypoint::GetRoadOption() {
+    return road_option;
+  }
+
 } // namespace traffic_manager
 } // namespace carla

LibCarla/source/carla/trafficmanager/SimpleWaypoint.h

@@ -22,6 +22,16 @@ namespace traffic_manager {
   namespace cg = carla::geom;
   using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
   using GeoGridId = carla::road::JuncId;
+  enum class RoadOption : uint8_t {
+    Void = 0,
+    Left = 1,
+    Right = 2,
+    Straight = 3,
+    LaneFollow = 4,
+    ChangeLaneLeft = 5,
+    ChangeLaneRight = 6,
+    RoadEnd = 7
+  };
 
   /// This is a simple wrapper class on Carla's waypoint object.
   /// The class is used to represent discrete samples of the world map.
@@ -41,6 +51,8 @@ namespace traffic_manager {
     SimpleWaypointPtr next_left_waypoint;
     /// Pointer to right lane change waypoint.
     SimpleWaypointPtr next_right_waypoint;
+    /// RoadOption for the actual waypoint.
+    RoadOption road_option = RoadOption::Void;
     /// Integer placing the waypoint into a geodesic grid.
     GeoGridId geodesic_grid_id = 0;
     // Boolean to hold if the waypoint belongs to a junction
@@ -118,6 +130,10 @@ namespace traffic_manager {
 
     /// Return transform object for the current waypoint.
     cg::Transform GetTransform() const;
+
+    // Accessor methods for road option.
+    void SetRoadOption(RoadOption _road_option);
+    RoadOption GetRoadOption();
   };
 
 } // namespace traffic_manager

LibCarla/source/carla/trafficmanager/TrafficLightStage.cpp

@@ -17,14 +17,14 @@ TrafficLightStage::TrafficLightStage(
   const Parameters &parameters,
   const cc::World &world,
   TLFrame &output_array,
-  RandomGeneratorMap &random_devices)
+  RandomGenerator &random_device)
   : vehicle_id_list(vehicle_id_list),
     simulation_state(simulation_state),
     buffer_map(buffer_map),
     parameters(parameters),
     world(world),
     output_array(output_array),
-    random_devices(random_devices) {}
+    random_device(random_device) {}
 
 void TrafficLightStage::Update(const unsigned long index) {
   bool traffic_light_hazard = false;
@@ -46,7 +46,7 @@ void TrafficLightStage::Update(const unsigned long index) {
     if (is_at_traffic_light &&
         traffic_light_state != TLS::Green &&
         traffic_light_state != TLS::Off &&
-        parameters.GetPercentageRunningLight(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
+        parameters.GetPercentageRunningLight(ego_actor_id) <= random_device.next()) {
 
       traffic_light_hazard = true;
     }
@@ -55,7 +55,7 @@ void TrafficLightStage::Update(const unsigned long index) {
             !is_at_traffic_light &&
             traffic_light_state != TLS::Green &&
             traffic_light_state != TLS::Off &&
-            parameters.GetPercentageRunningSign(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
+            parameters.GetPercentageRunningSign(ego_actor_id) <= random_device.next()) {
 
       traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_timestamp);
     }

LibCarla/source/carla/trafficmanager/TrafficLightStage.h

@@ -26,7 +26,7 @@ private:
   /// Map containing the previous junction visited by a vehicle.
   std::unordered_map<ActorId, JunctionID> vehicle_last_junction;
   TLFrame &output_array;
-  RandomGeneratorMap &random_devices;
+  RandomGenerator &random_device;
   cc::Timestamp current_timestamp;
 
   bool HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id,
@@ -39,7 +39,7 @@ public:
                     const Parameters &parameters,
                     const cc::World &world,
                     TLFrame &output_array,
-                    RandomGeneratorMap &random_devices);
+                    RandomGenerator &random_device);
 
   void Update(const unsigned long index) override;
 

LibCarla/source/carla/trafficmanager/TrafficManager.h

@@ -65,6 +65,54 @@ public:
     }
   }
 
+  /// Method to set our own imported path.
+  void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      tm_ptr->SetCustomPath(actor, path, empty_buffer);
+    }
+  }
+
+  /// Method to remove a path.
+  void RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      tm_ptr->RemoveUploadPath(actor_id, remove_path);
+    }
+  }
+
+  /// Method to update an already set path.
+  void UpdateUploadPath(const ActorId &actor_id, const Path path) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      tm_ptr->UpdateUploadPath(actor_id, path);
+    }
+  }
+
+  /// Method to set our own imported route.
+  void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      tm_ptr->SetImportedRoute(actor, route, empty_buffer);
+    }
+  }
+
+  /// Method to remove a route.
+  void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      tm_ptr->RemoveImportedRoute(actor_id, remove_path);
+    }
+  }
+
+  /// Method to update an already set route.
+  void UpdateImportedRoute(const ActorId &actor_id, const Route route) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      tm_ptr->UpdateImportedRoute(actor_id, route);
+    }
+  }
+
   /// Method to set if we are automatically respawning vehicles.
   void SetRespawnDormantVehicles(const bool mode_switch) {
     TrafficManagerBase* tm_ptr = GetTM(_port);
@@ -138,6 +186,14 @@ public:
     }
   }
 
+  /// Set the automatic management of the vehicle lights
+  void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update){
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if(tm_ptr != nullptr){
+      tm_ptr->SetUpdateVehicleLights(actor, do_update);
+    }
+  }
+
   /// Method to set collision detection rules between vehicles.
   void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) {
     TrafficManagerBase* tm_ptr = GetTM(_port);
@@ -237,7 +293,7 @@ public:
     }
   }
 
-  /// Method to set probabilistic preference to keep on the right lane.
+  /// Method to set % to keep on the right lane.
   void SetKeepRightPercentage(const ActorPtr &actor, const float percentage) {
     TrafficManagerBase* tm_ptr = GetTM(_port);
     if(tm_ptr != nullptr){
@@ -245,6 +301,22 @@ public:
     }
   }
 
+  /// Method to set % to randomly do a left lane change.
+  void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if(tm_ptr != nullptr){
+      tm_ptr->SetRandomLeftLaneChangePercentage(actor, percentage);
+    }
+  }
+
+  /// Method to set % to randomly do a right lane change.
+  void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) {
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if(tm_ptr != nullptr){
+      tm_ptr->SetRandomRightLaneChangePercentage(actor, percentage);
+    }
+  }
+
   /// Method to set randomization seed.
   void SetRandomDeviceSeed(const uint64_t seed) {
     TrafficManagerBase* tm_ptr = GetTM(_port);
@@ -255,6 +327,28 @@ public:
 
   void ShutDown();
 
+  /// Method to get the next action.
+  Action GetNextAction(const ActorId &actor_id) {
+    Action next_action;
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      next_action = tm_ptr->GetNextAction(actor_id);
+      return next_action;
+    }
+    return next_action;
+  }
+
+  /// Method to get the action buffer.
+  ActionBuffer GetActionBuffer(const ActorId &actor_id) {
+    ActionBuffer action_buffer;
+    TrafficManagerBase* tm_ptr = GetTM(_port);
+    if (tm_ptr != nullptr) {
+      action_buffer = tm_ptr->GetActionBuffer(actor_id);
+      return action_buffer;
+    }
+    return action_buffer;
+  }
+
 private:
 
   void CreateTrafficManagerServer(

LibCarla/source/carla/trafficmanager/TrafficManagerBase.h

@@ -8,11 +8,18 @@
 
 #include <memory>
 #include "carla/client/Actor.h"
+#include "carla/trafficmanager/SimpleWaypoint.h"
 
 namespace carla {
 namespace traffic_manager {
 
 using ActorPtr = carla::SharedPtr<carla::client::Actor>;
+using Path = std::vector<cg::Location>;
+using Route = std::vector<uint8_t>;
+using WaypointPtr = carla::SharedPtr<carla::client::Waypoint>;
+using Action = std::pair<RoadOption, WaypointPtr>;
+using ActionBuffer = std::vector<Action>;
+
 
 /// The function of this class is to integrate all the various stages of
 /// the traffic manager appropriately using messengers.
@@ -51,6 +58,9 @@ public:
   /// If less than 0, it's a % increase.
   virtual void SetGlobalPercentageSpeedDifference(float const percentage) = 0;
 
+  /// Method to set the automatic management of the vehicle lights
+  virtual void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update) = 0;
+
   /// Method to set collision detection rules between vehicles.
   virtual void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) = 0;
 
@@ -92,9 +102,15 @@ public:
   /// Method to set Global Distance to Leading Vehicle.
   virtual void SetGlobalDistanceToLeadingVehicle(const float dist) = 0;
 
-  /// Method to set probabilistic preference to keep on the right lane.
+  /// Method to set % to keep on the right lane.
   virtual void SetKeepRightPercentage(const ActorPtr &actor,const float percentage) = 0;
 
+  /// Method to set % to randomly do a left lane change.
+  virtual void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) = 0;
+
+  /// Method to set % to randomly do a right lane change.
+  virtual void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) = 0;
+
   /// Method to set hybrid physics mode.
   virtual void SetHybridPhysicsMode(const bool mode_switch) = 0;
 
@@ -107,6 +123,24 @@ public:
   /// Method to set Open Street Map mode.
   virtual void SetOSMMode(const bool mode_switch) = 0;
 
+  /// Method to set our own imported path.
+  virtual void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) = 0;
+
+  /// Method to remove a path.
+  virtual void RemoveUploadPath(const ActorId &actor_id, const bool remove_path) = 0;
+
+  /// Method to update an already set path.
+  virtual void UpdateUploadPath(const ActorId &actor_id, const Path path) = 0;
+
+  /// Method to set our own imported route.
+  virtual void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) = 0;
+
+  /// Method to remove a route.
+  virtual void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) = 0;
+
+  /// Method to update an already set route.
+  virtual void UpdateImportedRoute(const ActorId &actor_id, const Route route) = 0;
+
   /// Method to set automatic respawn of dormant vehicles.
   virtual void SetRespawnDormantVehicles(const bool mode_switch) = 0;
 
@@ -116,6 +150,12 @@ public:
   /// Method to set limits for boundaries when respawning vehicles.
   virtual void SetMaxBoundaries(const float lower, const float upper) = 0;
 
+  /// Method to get the vehicle's next action.
+  virtual Action GetNextAction(const ActorId &actor_id) = 0;
+
+  /// Method to get the vehicle's action buffer.
+  virtual ActionBuffer GetActionBuffer(const ActorId &actor_id) = 0;
+
   virtual void ShutDown() = 0;
 
 protected:

LibCarla/source/carla/trafficmanager/TrafficManagerClient.h

@@ -88,6 +88,12 @@ public:
     _client->call("set_global_percentage_speed_difference", percentage);
   }
 
+  /// 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);
+    _client->call("update_vehicle_lights", std::move(_actor), do_update);
+  }
+
   /// Method to set collision detection rules between vehicles.
   void SetCollisionDetection(const carla::rpc::Actor &reference_actor, const carla::rpc::Actor &other_actor, const bool detect_collision) {
     DEBUG_ASSERT(_client != nullptr);
@@ -169,10 +175,22 @@ public:
     _client->call("set_global_distance_to_leading_vehicle",distance);
   }
 
-  /// Method to set probabilistic preference to keep on the right lane.
+  /// Method to set % to keep on the right lane.
   void SetKeepRightPercentage(const carla::rpc::Actor &actor, const float percentage) {
     DEBUG_ASSERT(_client != nullptr);
-    _client->call("set_percentage_keep_right_rule", actor, percentage);
+    _client->call("keep_right_rule_percentage", actor, percentage);
+  }
+
+  /// Method to set % to randomly do a left lane change.
+  void SetRandomLeftLaneChangePercentage(const carla::rpc::Actor &actor, const float percentage) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("random_left_lanechange_percentage", actor, percentage);
+  }
+
+  /// Method to set % to randomly do a right lane change.
+  void SetRandomRightLaneChangePercentage(const carla::rpc::Actor &actor, const float percentage) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("random_right_lanechange_percentage", actor, percentage);
   }
 
   /// Method to set hybrid physics mode.
@@ -199,6 +217,42 @@ public:
     _client->call("set_osm_mode", mode_switch);
   }
 
+  /// Method to set our own imported path.
+  void SetCustomPath(const carla::rpc::Actor &actor, const Path path, const bool empty_buffer) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("set_path", actor, path, empty_buffer);
+  }
+
+  /// Method to remove a list of points.
+  void RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("remove_custom_path", actor_id, remove_path);
+  }
+
+  /// Method to update an already set list of points.
+  void UpdateUploadPath(const ActorId &actor_id, const Path path) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("update_custom_path", actor_id, path);
+  }
+
+  /// Method to set our own imported route.
+  void SetImportedRoute(const carla::rpc::Actor &actor, const Route route, const bool empty_buffer) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("set_imported_route", actor, route, empty_buffer);
+  }
+
+  /// Method to remove a route.
+  void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("remove_imported_route", actor_id, remove_path);
+  }
+
+  /// Method to update an already set list of points.
+  void UpdateImportedRoute(const ActorId &actor_id, const Route route) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("update_imported_route", actor_id, route);
+  }
+
   /// Method to set automatic respawn of dormant vehicles.
   void SetRespawnDormantVehicles(const bool mode_switch) {
     DEBUG_ASSERT(_client != nullptr);
@@ -217,6 +271,20 @@ public:
     _client->call("set_max_boundaries", lower, upper);
   }
 
+  /// Method to get the vehicle's next action.
+  Action GetNextAction(const ActorId &actor_id) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("get_next_action", actor_id);
+    return Action();
+  }
+
+  /// Method to get the vehicle's action buffer.
+  ActionBuffer GetActionBuffer(const ActorId &actor_id) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("get_all_actions", actor_id);
+    return ActionBuffer();
+  }
+
   void ShutDown() {
     DEBUG_ASSERT(_client != nullptr);
     _client->call("shut_down");

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp

@@ -42,7 +42,7 @@ TrafficManagerLocal::TrafficManagerLocal(
                                          parameters,
                                          marked_for_removal,
                                          localization_frame,
-                                         random_devices)),
+                                         random_device)),
 
     collision_stage(CollisionStage(vehicle_id_list,
                                    simulation_state,
@@ -50,7 +50,7 @@ TrafficManagerLocal::TrafficManagerLocal(
                                    track_traffic,
                                    parameters,
                                    collision_frame,
-                                   random_devices)),
+                                   random_device)),
 
     traffic_light_stage(TrafficLightStage(vehicle_id_list,
                                           simulation_state,
@@ -58,7 +58,7 @@ TrafficManagerLocal::TrafficManagerLocal(
                                           parameters,
                                           world,
                                           tl_frame,
-                                          random_devices)),
+                                          random_device)),
 
     motion_plan_stage(MotionPlanStage(vehicle_id_list,
                                       simulation_state,
@@ -74,9 +74,15 @@ TrafficManagerLocal::TrafficManagerLocal(
                                       tl_frame,
                                       world,
                                       control_frame,
-                                      random_devices,
+                                      random_device,
                                       local_map)),
 
+    vehicle_light_stage(VehicleLightStage(vehicle_id_list,
+                                          buffer_map,
+                                          parameters,
+                                          world,
+                                          control_frame)),
+
     alsm(ALSM(registered_vehicles,
               buffer_map,
               track_traffic,
@@ -89,7 +95,7 @@ TrafficManagerLocal::TrafficManagerLocal(
               collision_stage,
               traffic_light_stage,
               motion_plan_stage,
-              random_devices)),
+              vehicle_light_stage)),
 
     server(TrafficManagerServer(RPCportTM, static_cast<carla::traffic_manager::TrafficManagerBase *>(this))) {
 
@@ -177,16 +183,11 @@ void TrafficManagerLocal::Run() {
     // Updating simulation state, actor life cycle and performing necessary cleanup.
     alsm.Update();
 
-
     // Re-allocating inter-stage communication frames based on changed number of registered vehicles.
     int current_registered_vehicles_state = registered_vehicles.GetState();
     unsigned long number_of_vehicles = vehicle_id_list.size();
     if (registered_vehicles_state != current_registered_vehicles_state || number_of_vehicles != registered_vehicles.Size()) {
-
       vehicle_id_list = registered_vehicles.GetIDList();
-
-      std::sort(vehicle_id_list.begin(), vehicle_id_list.end());
-
       number_of_vehicles = vehicle_id_list.size();
 
       // Reserve more space if needed.
@@ -210,10 +211,14 @@ void TrafficManagerLocal::Run() {
     tl_frame.clear();
     tl_frame.resize(number_of_vehicles);
     control_frame.clear();
+    // Reserve two frames for each vehicle: one for the ApplyVehicleControl command,
+    // and one for the optional SetVehicleLightState command
+    control_frame.reserve(2 * number_of_vehicles);
+    // Resize to accomodate at least all ApplyVehicleControl commands,
+    // that will be inserted by the motion_plan_stage stage.
     control_frame.resize(number_of_vehicles);
 
     // Run core operation stages.
-
     for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
       localization_stage.Update(index);
     }
@@ -221,9 +226,11 @@ void TrafficManagerLocal::Run() {
       collision_stage.Update(index);
     }
     collision_stage.ClearCycleCache();
+    vehicle_light_stage.UpdateWorldInfo();
     for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
       traffic_light_stage.Update(index);
       motion_plan_stage.Update(index);
+      vehicle_light_stage.Update(index);
     }
 
     registration_lock.unlock();
@@ -273,7 +280,6 @@ void TrafficManagerLocal::Stop() {
   track_traffic.Clear();
   previous_update_instance = chr::system_clock::now();
   current_reserved_capacity = 0u;
-  random_devices.clear();
 
   simulation_state.Reset();
   localization_stage.Reset();
@@ -300,7 +306,6 @@ void TrafficManagerLocal::Release() {
 }
 
 void TrafficManagerLocal::Reset() {
-
   Release();
   episode_proxy = episode_proxy.Lock()->GetCurrentEpisode();
   world = cc::World(episode_proxy);
@@ -311,14 +316,6 @@ void TrafficManagerLocal::Reset() {
 void TrafficManagerLocal::RegisterVehicles(const std::vector<ActorPtr> &vehicle_list) {
   std::lock_guard<std::mutex> registration_lock(registration_mutex);
   registered_vehicles.Insert(vehicle_list);
-  for (const ActorPtr &vehicle: vehicle_list) {
-    if (!is_custom_seed) {
-      seed = vehicle->GetId() + seed;
-    } else {
-      seed = 1 + seed;
-    }
-    random_devices.insert({vehicle->GetId(), RandomGenerator(seed)});
-  }
 }
 
 void TrafficManagerLocal::UnregisterVehicles(const std::vector<ActorPtr> &actor_list) {
@@ -337,6 +334,11 @@ void TrafficManagerLocal::SetGlobalPercentageSpeedDifference(const float percent
   parameters.SetGlobalPercentageSpeedDifference(percentage);
 }
 
+/// Method to set the automatic management of the vehicle lights
+void TrafficManagerLocal::SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update) {
+  parameters.SetUpdateVehicleLights(actor, do_update);
+}
+
 void TrafficManagerLocal::SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision) {
   parameters.SetCollisionDetection(reference_actor, other_actor, detect_collision);
 }
@@ -377,6 +379,14 @@ void TrafficManagerLocal::SetKeepRightPercentage(const ActorPtr &actor, const fl
   parameters.SetKeepRightPercentage(actor, percentage);
 }
 
+void TrafficManagerLocal::SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) {
+  parameters.SetRandomLeftLaneChangePercentage(actor, percentage);
+}
+
+void TrafficManagerLocal::SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) {
+  parameters.SetRandomRightLaneChangePercentage(actor, percentage);
+}
+
 void TrafficManagerLocal::SetHybridPhysicsMode(const bool mode_switch) {
   parameters.SetHybridPhysicsMode(mode_switch);
 }
@@ -389,6 +399,30 @@ void TrafficManagerLocal::SetOSMMode(const bool mode_switch) {
   parameters.SetOSMMode(mode_switch);
 }
 
+void TrafficManagerLocal::SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) {
+  parameters.SetCustomPath(actor, path, empty_buffer);
+}
+
+void TrafficManagerLocal::RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
+  parameters.RemoveUploadPath(actor_id, remove_path);
+}
+
+void TrafficManagerLocal::UpdateUploadPath(const ActorId &actor_id, const Path path) {
+  parameters.UpdateUploadPath(actor_id, path);
+}
+
+void TrafficManagerLocal::SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) {
+  parameters.SetImportedRoute(actor, route, empty_buffer);
+}
+
+void TrafficManagerLocal::RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
+  parameters.RemoveImportedRoute(actor_id, remove_path);
+}
+
+void TrafficManagerLocal::UpdateImportedRoute(const ActorId &actor_id, const Route route) {
+  parameters.UpdateImportedRoute(actor_id, route);
+}
+
 void TrafficManagerLocal::SetRespawnDormantVehicles(const bool mode_switch) {
   parameters.SetRespawnDormantVehicles(mode_switch);
 }
@@ -401,6 +435,14 @@ void TrafficManagerLocal::SetMaxBoundaries(const float lower, const float upper)
   parameters.SetMaxBoundaries(lower, upper);
 }
 
+Action TrafficManagerLocal::GetNextAction(const ActorId &actor_id) {
+  return localization_stage.ComputeNextAction(actor_id);
+}
+
+ActionBuffer TrafficManagerLocal::GetActionBuffer(const ActorId &actor_id) {
+  return localization_stage.ComputeActionBuffer(actor_id);
+}
+
 bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) {
   for (auto &elem : tl_to_freeze) {
     if (!elem->IsFrozen() || elem->GetState() != TLS::Red) {
@@ -433,7 +475,7 @@ std::vector<ActorId> TrafficManagerLocal::GetRegisteredVehiclesIDs() {
 
 void TrafficManagerLocal::SetRandomDeviceSeed(const uint64_t _seed) {
   seed = _seed;
-  is_custom_seed = true;
+  random_device = RandomGenerator(seed);
   world.ResetAllTrafficLights();
 }
 

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h

@@ -93,6 +93,7 @@ private:
   CollisionStage collision_stage;
   TrafficLightStage traffic_light_stage;
   MotionPlanStage motion_plan_stage;
+  VehicleLightStage vehicle_light_stage;
   ALSM alsm;
   /// Traffic manager server instance.
   TrafficManagerServer server;
@@ -108,11 +109,10 @@ private:
   std::condition_variable step_end_trigger;
   /// Single worker thread for sequential execution of sub-components.
   std::unique_ptr<std::thread> worker_thread;
-  /// Structure holding random devices per vehicle.
-  RandomGeneratorMap random_devices;
   /// Randomization seed.
   uint64_t seed {static_cast<uint64_t>(time(NULL))};
-  bool is_custom_seed {false};
+  /// Structure holding random devices per vehicle.
+  RandomGenerator random_device = RandomGenerator(seed);
   std::vector<ActorId> marked_for_removal;
   /// Mutex to prevent vehicle registration during frame array re-allocation.
   std::mutex registration_mutex;
@@ -165,6 +165,9 @@ public:
   /// If less than 0, it's a % increase.
   void SetGlobalPercentageSpeedDifference(float const percentage);
 
+  /// Method to set the automatic management of the vehicle lights
+  void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update);
+
   /// Method to set collision detection rules between vehicles.
   void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision);
 
@@ -210,9 +213,15 @@ public:
   /// the Global leading vehicle.
   void SetGlobalDistanceToLeadingVehicle(const float distance);
 
-  /// Method to set probabilistic preference to keep on the right lane.
+  /// Method to set % to keep on the right lane.
   void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
 
+  /// Method to set % to randomly do a left lane change.
+  void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage);
+
+  /// Method to set % to randomly do a right lane change.
+  void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage);
+
   /// Method to set hybrid physics mode.
   void SetHybridPhysicsMode(const bool mode_switch);
 
@@ -225,15 +234,39 @@ public:
   /// Method to set Open Street Map mode.
   void SetOSMMode(const bool mode_switch);
 
+  /// Method to set our own imported path.
+  void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer);
+
+  /// Method to remove a list of points.
+  void RemoveUploadPath(const ActorId &actor_id, const bool remove_path);
+
+  /// Method to update an already set list of points.
+  void UpdateUploadPath(const ActorId &actor_id, const Path path);
+
+  /// Method to set our own imported route.
+  void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer);
+
+  /// Method to remove a route.
+  void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path);
+
+  /// Method to update an already set route.
+  void UpdateImportedRoute(const ActorId &actor_id, const Route route);
+
   /// Method to set automatic respawn of dormant vehicles.
   void SetRespawnDormantVehicles(const bool mode_switch);
 
-  // Method to set boundaries to respawn of dormant vehicles.
+  /// Method to set boundaries to respawn of dormant vehicles.
   void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound);
 
-  // Method to set limits for boundaries when respawning dormant vehicles.
+  /// Method to set limits for boundaries when respawning dormant vehicles.
   void SetMaxBoundaries(const float lower, const float upper);
 
+  /// Method to get the vehicle's next action.
+  Action GetNextAction(const ActorId &actor_id);
+
+  /// Method to get the vehicle's action buffer.
+  ActionBuffer GetActionBuffer(const ActorId &actor_id);
+
   void ShutDown() {};
 };
 

LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp

@@ -115,6 +115,12 @@ void TrafficManagerRemote::SetGlobalPercentageSpeedDifference(const float percen
   client.SetGlobalPercentageSpeedDifference(percentage);
 }
 
+void TrafficManagerRemote::SetUpdateVehicleLights(const ActorPtr &_actor, const bool do_update) {
+  carla::rpc::Actor actor(_actor->Serialize());
+
+  client.SetUpdateVehicleLights(actor, do_update);
+}
+
 void TrafficManagerRemote::SetCollisionDetection(const ActorPtr &_reference_actor, const ActorPtr &_other_actor, const bool detect_collision) {
   carla::rpc::Actor reference_actor(_reference_actor->Serialize());
   carla::rpc::Actor other_actor(_other_actor->Serialize());
@@ -175,6 +181,18 @@ void TrafficManagerRemote::SetKeepRightPercentage(const ActorPtr &_actor, const
   client.SetKeepRightPercentage(actor, percentage);
 }
 
+void TrafficManagerRemote::SetRandomLeftLaneChangePercentage(const ActorPtr &_actor, const float percentage) {
+  carla::rpc::Actor actor(_actor->Serialize());
+
+  client.SetRandomLeftLaneChangePercentage(actor, percentage);
+}
+
+void TrafficManagerRemote::SetRandomRightLaneChangePercentage(const ActorPtr &_actor, const float percentage) {
+  carla::rpc::Actor actor(_actor->Serialize());
+
+  client.SetRandomRightLaneChangePercentage(actor, percentage);
+}
+
 void TrafficManagerRemote::SetHybridPhysicsMode(const bool mode_switch) {
   client.SetHybridPhysicsMode(mode_switch);
 }
@@ -187,6 +205,34 @@ void TrafficManagerRemote::SetOSMMode(const bool mode_switch) {
   client.SetOSMMode(mode_switch);
 }
 
+void TrafficManagerRemote::SetCustomPath(const ActorPtr &_actor, const Path path, const bool empty_buffer) {
+  carla::rpc::Actor actor(_actor->Serialize());
+
+  client.SetCustomPath(actor, path, empty_buffer);
+}
+
+void TrafficManagerRemote::RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
+  client.RemoveUploadPath(actor_id, remove_path);
+}
+
+void TrafficManagerRemote::UpdateUploadPath(const ActorId &actor_id, const Path path) {
+  client.UpdateUploadPath(actor_id, path);
+}
+
+void TrafficManagerRemote::SetImportedRoute(const ActorPtr &_actor, const Route route, const bool empty_buffer) {
+  carla::rpc::Actor actor(_actor->Serialize());
+
+  client.SetImportedRoute(actor, route, empty_buffer);
+}
+
+void TrafficManagerRemote::RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
+  client.RemoveImportedRoute(actor_id, remove_path);
+}
+
+void TrafficManagerRemote::UpdateImportedRoute(const ActorId &actor_id, const Route route) {
+  client.UpdateImportedRoute(actor_id, route);
+}
+
 void TrafficManagerRemote::SetRespawnDormantVehicles(const bool mode_switch) {
   client.SetRespawnDormantVehicles(mode_switch);
 }
@@ -211,6 +257,14 @@ void TrafficManagerRemote::SetSynchronousModeTimeOutInMiliSecond(double time) {
   client.SetSynchronousModeTimeOutInMiliSecond(time);
 }
 
+Action TrafficManagerRemote::GetNextAction(const ActorId &actor_id) {
+  return client.GetNextAction(actor_id);
+}
+
+ActionBuffer TrafficManagerRemote::GetActionBuffer(const ActorId &actor_id) {
+  return client.GetActionBuffer(actor_id);
+}
+
 bool TrafficManagerRemote::SynchronousTick() {
   return false;
 }

LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h

@@ -19,6 +19,9 @@ namespace carla {
 namespace traffic_manager {
 
 using ActorPtr = carla::SharedPtr<carla::client::Actor>;
+using Path = std::vector<cg::Location>;
+using Route = std::vector<uint8_t>;
+
 
 /// The function of this class is to integrate all the various stages of
 /// the traffic manager appropriately using messengers.
@@ -58,6 +61,9 @@ public:
   /// If less than 0, it's a % increase.
   void SetGlobalPercentageSpeedDifference(float const percentage);
 
+  /// Method to set the automatic management of the vehicle lights
+  void SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update);
+
   /// Method to set collision detection rules between vehicles.
   void SetCollisionDetection(const ActorPtr &reference_actor, const ActorPtr &other_actor, const bool detect_collision);
 
@@ -93,9 +99,15 @@ public:
   /// Method to set Tick timeout for synchronous execution.
   void SetSynchronousModeTimeOutInMiliSecond(double time);
 
-  /// Method to set probabilistic preference to keep on the right lane.
+  /// Method to set % to keep on the right lane.
   void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
 
+  /// Method to set % to randomly do a left lane change.
+  void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage);
+
+  /// Method to set % to randomly do a right lane change.
+  void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage);
+
   /// Method to set hybrid physics mode.
   void SetHybridPhysicsMode(const bool mode_switch);
 
@@ -105,6 +117,24 @@ public:
   /// Method to set Open Street Map mode.
   void SetOSMMode(const bool mode_switch);
 
+  /// Method to set our own imported path.
+  void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer);
+
+  /// Method to remove a path.
+  void RemoveUploadPath(const ActorId &actor_id, const bool remove_path);
+
+  /// Method to update an already set path.
+  void UpdateUploadPath(const ActorId &actor_id, const Path path);
+
+  /// Method to set our own imported route.
+  void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer);
+
+  /// Method to remove a route.
+  void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path);
+
+  /// Method to update an already set route.
+  void UpdateImportedRoute(const ActorId &actor_id, const Route route);
+
   /// Method to set automatic respawn of dormant vehicles.
   void SetRespawnDormantVehicles(const bool mode_switch);
 
@@ -116,6 +146,12 @@ public:
 
   virtual void ShutDown();
 
+  /// Method to get the vehicle's next action.
+  Action GetNextAction(const ActorId &actor_id);
+
+  /// Method to get the vehicle's action buffer.
+  ActionBuffer GetActionBuffer(const ActorId &actor_id);
+
   /// Method to provide synchronous tick
   bool SynchronousTick();
 

LibCarla/source/carla/trafficmanager/TrafficManagerServer.h

@@ -19,6 +19,8 @@ namespace carla {
 namespace traffic_manager {
 
 using ActorPtr = carla::SharedPtr<carla::client::Actor>;
+using Path = std::vector<cg::Location>;
+using Route = std::vector<uint8_t>;
 
 using namespace constants::Networking;
 
@@ -92,6 +94,11 @@ public:
         tm->SetPercentageSpeedDifference(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
       });
 
+      /// Method to set the automatic management of the vehicle lights
+      server->bind("update_vehicle_lights", [=](carla::rpc::Actor actor, const bool do_update) {
+        tm->SetUpdateVehicleLights(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), do_update);
+      });
+
       /// Method to set a global % decrease in velocity with respect to the speed limit.
       /// If less than 0, it's a % increase.
       server->bind("set_global_percentage_speed_difference", [=](const float percentage) {
@@ -138,21 +145,31 @@ public:
         tm->SetPercentageRunningSign(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
       });
 
-        /// Method to specify the % chance of ignoring collisions with any walker.
+      /// Method to specify the % chance of ignoring collisions with any walker.
       server->bind("set_percentage_ignore_walkers", [=](carla::rpc::Actor actor, const float percentage) {
         tm->SetPercentageIgnoreWalkers(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
       });
 
-        /// Method to specify the % chance of ignoring collisions with any vehicle.
+      /// Method to specify the % chance of ignoring collisions with any vehicle.
       server->bind("set_percentage_ignore_vehicles", [=](carla::rpc::Actor actor, const float percentage) {
         tm->SetPercentageIgnoreVehicles(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
       });
 
-      /// Method to specify the % chance of ignoring collisions with any vehicle.
+      /// Method to set % to keep on the right lane.
       server->bind("set_percentage_keep_right_rule", [=](carla::rpc::Actor actor, const float percentage) {
         tm->SetKeepRightPercentage(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
       });
 
+      /// Method to set % to randomly do a left lane change.
+      server->bind("set_percentage_random_left_lanechange", [=](carla::rpc::Actor actor, const float percentage) {
+        tm->SetRandomLeftLaneChangePercentage(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
+      });
+
+      /// Method to set % to randomly do a right lane change.
+      server->bind("set_percentage_random_right_lanechange", [=](carla::rpc::Actor actor, const float percentage) {
+        tm->SetRandomRightLaneChangePercentage(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);
@@ -165,7 +182,37 @@ public:
 
       /// Method to set hybrid physics radius.
       server->bind("set_osm_mode", [=](const bool mode_switch) {
-        tm->SetHybridPhysicsRadius(mode_switch);
+        tm->SetOSMMode(mode_switch);
+      });
+
+      /// Method to set our own imported path.
+      server->bind("set_path", [=](carla::rpc::Actor actor, const Path path, const bool empty_buffer) {
+        tm->SetCustomPath(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), path, empty_buffer);
+      });
+
+      /// Method to remove a list of points.
+      server->bind("remove_custom_path", [=](const ActorId actor_id, const bool remove_path) {
+        tm->RemoveUploadPath(actor_id, remove_path);
+      });
+
+       /// Method to update an already set list of points.
+      server->bind("update_custom_path", [=](const ActorId actor_id, const Path path) {
+        tm->UpdateUploadPath(actor_id, path);
+      });
+
+      /// Method to set our own imported route.
+      server->bind("set_imported_route", [=](carla::rpc::Actor actor, const Route route, const bool empty_buffer) {
+        tm->SetImportedRoute(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), route, empty_buffer);
+      });
+
+      /// Method to remove a route.
+      server->bind("remove_imported_route", [=](const ActorId actor_id, const bool remove_path) {
+        tm->RemoveImportedRoute(actor_id, remove_path);
+      });
+
+      /// Method to update an already set list of points.
+      server->bind("update_imported_route", [=](const ActorId actor_id, const Route route) {
+        tm->UpdateImportedRoute(actor_id, route);
       });
 
       /// Method to set respawn dormant vehicles mode.
@@ -178,6 +225,16 @@ public:
         tm->SetBoundariesRespawnDormantVehicles(lower_bound, upper_bound);
       });
 
+      /// Method to get the vehicle's next action.
+      server->bind("get_next_action", [=](const ActorId actor_id) {
+        tm->GetNextAction(actor_id);
+      });
+
+      /// Method to get the vehicle's action buffer.
+      server->bind("get_all_actions", [=](const ActorId actor_id) {
+        tm->GetActionBuffer(actor_id);
+      });
+
       server->bind("shut_down", [=]() {
         tm->Release();
       });

LibCarla/source/carla/trafficmanager/VehicleLightStage.cpp

@@ -0,0 +1,156 @@
+
+#include "carla/trafficmanager/Constants.h"
+#include "carla/trafficmanager/LocalizationUtils.h"
+
+#include "carla/trafficmanager/VehicleLightStage.h"
+
+namespace carla {
+namespace traffic_manager {
+
+using namespace constants::VehicleLight;
+
+VehicleLightStage::VehicleLightStage(
+  const std::vector<ActorId> &vehicle_id_list,
+  const BufferMap &buffer_map,
+  const Parameters &parameters,
+  const cc::World &world,
+  ControlFrame& control_frame)
+  : vehicle_id_list(vehicle_id_list),
+    buffer_map(buffer_map),
+    parameters(parameters),
+    world(world),
+    control_frame(control_frame) {}
+
+void VehicleLightStage::UpdateWorldInfo() {
+  // Get the global weather and all the vehicle light states at once
+  all_light_states = world.GetVehiclesLightStates();
+  weather = world.GetWeather();
+}
+
+void VehicleLightStage::Update(const unsigned long index) {
+  ActorId actor_id = vehicle_id_list.at(index);
+
+  if (!parameters.GetUpdateVehicleLights(actor_id))
+    return; // this vehicle is not set to have automatic lights update
+
+  rpc::VehicleLightState::flag_type light_states = uint32_t(-1);
+  bool brake_lights = false;
+  bool left_turn_indicator = false;
+  bool right_turn_indicator = false;
+  bool position = false;
+  bool low_beam = false;
+  bool high_beam = false;
+  bool fog_lights = false;
+
+  // search the current light state of the vehicle
+  for (auto&& vls : all_light_states) {
+    if (vls.first == actor_id) {
+      light_states = vls.second;
+      break;
+    }
+  }
+
+  // Determine if the vehicle is truning left or right by checking the close waypoints
+
+  const Buffer& waypoint_buffer = buffer_map.at(actor_id);
+  cg::Location front_location = waypoint_buffer.front()->GetLocation();
+
+  for (const SimpleWaypointPtr& waypoint : waypoint_buffer) {
+    if (waypoint->CheckJunction()) {
+      RoadOption target_ro = waypoint->GetRoadOption();
+      if (target_ro == RoadOption::Left) left_turn_indicator = true;
+      else if (target_ro == RoadOption::Right) right_turn_indicator = true;
+      break;
+    }
+    if (cg::Math::DistanceSquared(front_location, waypoint->GetLocation()) > MAX_DISTANCE_LIGHT_CHECK) {
+      break;
+    }
+  }
+
+  // Determine brake light state
+  for (size_t cc = 0; cc < control_frame.size(); cc++) {
+    if (auto* maybe_ctrl = boost::variant2::get_if<carla::rpc::Command::ApplyVehicleControl>(&control_frame[cc].command)) {
+      carla::rpc::Command::ApplyVehicleControl& ctrl = *maybe_ctrl;
+      if (ctrl.actor == actor_id) {
+        brake_lights = (ctrl.control.brake > 0.5); // hard braking, avoid blinking for throttle control
+        break;
+      }
+    }
+  }
+
+  // Determine position, fog and beams
+
+  // Turn on beams & positions from sunset to dawn
+  if (weather.sun_altitude_angle < SUN_ALTITUDE_DEGREES_BEFORE_DAWN ||
+      weather.sun_altitude_angle >SUN_ALTITUDE_DEGREES_AFTER_SUNSET)
+  {
+    position = true;
+    low_beam = true;
+  }
+  else if (weather.sun_altitude_angle < SUN_ALTITUDE_DEGREES_JUST_AFTER_DAWN ||
+           weather.sun_altitude_angle > SUN_ALTITUDE_DEGREES_JUST_BEFORE_SUNSET)
+  {
+    position = true;
+  }
+  // Turn on lights under heavy rain
+  if (weather.precipitation > HEAVY_PRECIPITATION_THRESHOLD) {
+    position = true;
+    low_beam = true;
+  }
+  // Turn on fog lights
+  if (weather.fog_density > FOG_DENSITY_THRESHOLD) {
+    position = true;
+    low_beam = true;
+    fog_lights = true;
+  }
+
+  // Determine the new vehicle light state
+  rpc::VehicleLightState::flag_type new_light_states = light_states;
+  if (brake_lights)
+    new_light_states |= rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::Brake);
+  else
+    new_light_states &= ~rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::Brake);
+
+  if (left_turn_indicator)
+    new_light_states |= rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::LeftBlinker);
+  else
+    new_light_states &= ~rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::LeftBlinker);
+
+  if (right_turn_indicator)
+    new_light_states |= rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::RightBlinker);
+  else
+    new_light_states &= ~rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::RightBlinker);
+
+  if (position)
+    new_light_states |= rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::Position);
+  else
+    new_light_states &= ~rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::Position);
+
+  if (low_beam)
+    new_light_states |= rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::LowBeam);
+  else
+    new_light_states &= ~rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::LowBeam);
+
+  if (high_beam)
+    new_light_states |= rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::HighBeam);
+  else
+    new_light_states &= ~rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::HighBeam);
+
+  if (fog_lights)
+    new_light_states |= rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::Fog);
+  else
+    new_light_states &= ~rpc::VehicleLightState::flag_type(rpc::VehicleLightState::LightState::Fog);
+
+  // Update the vehicle light state if it has changed
+  if (new_light_states != light_states)
+    control_frame.push_back(carla::rpc::Command::SetVehicleLightState(actor_id, new_light_states));
+}
+
+void VehicleLightStage::RemoveActor(const ActorId) {
+}
+
+void VehicleLightStage::Reset() {
+}
+
+} // namespace traffic_manager
+} // namespace carla

LibCarla/source/carla/trafficmanager/VehicleLightStage.h

@@ -0,0 +1,44 @@
+
+#pragma once
+
+#include "carla/trafficmanager/DataStructures.h"
+#include "carla/trafficmanager/Parameters.h"
+#include "carla/trafficmanager/RandomGenerator.h"
+#include "carla/trafficmanager/SimulationState.h"
+#include "carla/trafficmanager/Stage.h"
+
+namespace carla {
+namespace traffic_manager {
+
+/// This class has functionality for turning on/off the vehicle lights
+/// according to the current vehicle state and its surrounding environment.
+class VehicleLightStage: Stage {
+private:
+  const std::vector<ActorId> &vehicle_id_list;
+  const BufferMap &buffer_map;
+  const Parameters &parameters;
+  const cc::World &world;
+  ControlFrame& control_frame;
+  /// All vehicle light states
+  rpc::VehicleLightStateList all_light_states;
+  /// Current weather parameters
+  rpc::WeatherParameters weather;
+
+public:
+  VehicleLightStage(const std::vector<ActorId> &vehicle_id_list,
+                    const BufferMap &buffer_map,
+                    const Parameters &parameters,
+                    const cc::World &world,
+                    ControlFrame& control_frame);
+
+  void UpdateWorldInfo();
+
+  void Update(const unsigned long index) override;
+
+  void RemoveActor(const ActorId actor_id) override;
+
+  void Reset() override;
+};
+
+} // namespace traffic_manager
+} // namespace carla

LibCarla/source/test/common/test_msgpack.cpp

@@ -74,22 +74,22 @@ TEST(msgpack, actor) {
 TEST(msgpack, variant) {
   using mp = carla::MsgPack;
 
-  boost::variant<bool, float, std::string> var;
+  boost::variant2::variant<bool, float, std::string> var;
 
   var = true;
   auto result = mp::UnPack<decltype(var)>(mp::Pack(var));
-  ASSERT_EQ(result.which(), 0);
-  ASSERT_EQ(boost::get<bool>(result), true);
+  ASSERT_EQ(result.index(), 0);
+  ASSERT_EQ(boost::variant2::get<bool>(result), true);
 
   var = 42.0f;
   result = mp::UnPack<decltype(var)>(mp::Pack(var));
-  ASSERT_EQ(result.which(), 1);
-  ASSERT_EQ(boost::get<float>(result), 42.0f);
+  ASSERT_EQ(result.index(), 1);
+  ASSERT_EQ(boost::variant2::get<float>(result), 42.0f);
 
   var = std::string("hola!");
   result = mp::UnPack<decltype(var)>(mp::Pack(var));
-  ASSERT_EQ(result.which(), 2);
-  ASSERT_EQ(boost::get<std::string>(result), "hola!");
+  ASSERT_EQ(result.index(), 2);
+  ASSERT_EQ(boost::variant2::get<std::string>(result), "hola!");
 }
 
 TEST(msgpack, optional) {

LibCarla/source/test/common/test_streaming.cpp

@@ -55,12 +55,12 @@ TEST(streaming, low_level_sending_strings) {
 
   io_context_running io;
 
-  Server<tcp::Server> srv(io.service, TESTING_PORT);
+  carla::streaming::low_level::Server<tcp::Server> srv(io.service, TESTING_PORT);
   srv.SetTimeout(1s);
 
   auto stream = srv.MakeStream();
 
-  Client<tcp::Client> c;
+  carla::streaming::low_level::Client<tcp::Client> c;
   c.Subscribe(io.service, stream.token(), [&](auto message) {
     ++message_count;
     ASSERT_EQ(message.size(), message_text.size());
@@ -90,10 +90,10 @@ TEST(streaming, low_level_unsubscribing) {
 
   io_context_running io;
 
-  Server<tcp::Server> srv(io.service, TESTING_PORT);
+  carla::streaming::low_level::Server<tcp::Server> srv(io.service, TESTING_PORT);
   srv.SetTimeout(1s);
 
-  Client<tcp::Client> c;
+  carla::streaming::low_level::Client<tcp::Client> c;
   for (auto n = 0u; n < 10u; ++n) {
     auto stream = srv.MakeStream();
     std::atomic_size_t message_count{0u};

PythonAPI/carla/agents/navigation/basic_agent.py

@@ -15,7 +15,7 @@ from shapely.geometry import Polygon
 
 from agents.navigation.local_planner import LocalPlanner
 from agents.navigation.global_route_planner import GlobalRoutePlanner
-from agents.tools.misc import get_speed, is_within_distance, get_trafficlight_trigger_location
+from agents.tools.misc import get_speed, is_within_distance, get_trafficlight_trigger_location, compute_distance
 
 
 class BasicAgent(object):
@@ -167,7 +167,7 @@ class BasicAgent(object):
 
         # Check for possible vehicle obstacles
         max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed
-        affected_by_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance)
+        affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance)
         if affected_by_vehicle:
             hazard_detected = True
 
@@ -249,7 +249,7 @@ class BasicAgent(object):
 
         return (False, None)
 
-    def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None):
+    def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_angle_th=90, low_angle_th=0, lane_offset=0):
         """
         Method to check if there is a vehicle in front of the agent blocking its path.
 
@@ -259,7 +259,7 @@ class BasicAgent(object):
                 If None, the base threshold value is used
         """
         if self._ignore_vehicles:
-            return (False, None)
+            return (False, None, -1)
 
         if not vehicle_list:
             vehicle_list = self._world.get_actors().filter("*vehicle*")
@@ -270,6 +270,10 @@ class BasicAgent(object):
         ego_transform = self._vehicle.get_transform()
         ego_wpt = self._map.get_waypoint(self._vehicle.get_location())
 
+        # Get the right offset
+        if ego_wpt.lane_id < 0 and lane_offset != 0:
+            lane_offset *= -1
+
         # Get the transform of the front of the ego
         ego_forward_vector = ego_transform.get_forward_vector()
         ego_extent = self._vehicle.bounding_box.extent.x
@@ -286,11 +290,11 @@ class BasicAgent(object):
             # Simplified version for outside junctions
             if not ego_wpt.is_junction or not target_wpt.is_junction:
 
-                if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id:
+                if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id  + lane_offset:
                     next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0]
                     if not next_wpt:
                         continue
-                    if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id:
+                    if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id  + lane_offset:
                         continue
 
                 target_forward_vector = target_transform.get_forward_vector()
@@ -301,8 +305,8 @@ class BasicAgent(object):
                     y=target_extent * target_forward_vector.y,
                 )
 
-                if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [0, 90]):
-                    return (True, target_vehicle)
+                if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [low_angle_th, up_angle_th]):
+                    return (True, target_vehicle, compute_distance(target_transform.location, ego_transform.location))
 
             # Waypoints aren't reliable, check the proximity of the vehicle to the route
             else:
@@ -327,7 +331,7 @@ class BasicAgent(object):
 
                 if len(route_bb) < 3:
                     # 2 points don't create a polygon, nothing to check
-                    return (False, None)
+                    return (False, None, -1)
                 ego_polygon = Polygon(route_bb)
 
                 # Compare the two polygons
@@ -344,8 +348,8 @@ class BasicAgent(object):
                     target_polygon = Polygon(target_list)
 
                     if ego_polygon.intersects(target_polygon):
-                        return (True, target_vehicle)
+                        return (True, target_vehicle, compute_distance(target_vehicle.get_location(), ego_location))
 
-                return (False, None)
+                return (False, None, -1)
 
-        return (False, None)
+        return (False, None, -1)

PythonAPI/carla/agents/navigation/behavior_agent.py

@@ -81,62 +81,6 @@ class BehaviorAgent(BasicAgent):
         if self._incoming_direction is None:
             self._incoming_direction = RoadOption.LANEFOLLOW
 
-    def _vehicle_obstacle_detected(self, vehicle_list, proximity_th, up_angle_th, low_angle_th=0, lane_offset=0):
-        """
-        Check if a given vehicle is an obstacle in our way. To this end we take
-        into account the road and lane the target vehicle is on and run a
-        geometry test to check if the target vehicle is under a certain distance
-        in front of our ego vehicle. We also check the next waypoint, just to be
-        sure there's not a sudden road id change.
-
-        WARNING: This method is an approximation that could fail for very large
-        vehicles, which center is actually on a different lane but their
-        extension falls within the ego vehicle lane. Also, make sure to remove
-        the ego vehicle from the list. Lane offset is set to +1 for right lanes
-        and -1 for left lanes, but this has to be inverted if lane values are
-        negative.
-
-            :param vehicle_list: list of potential obstacle to check
-            :param proximity_th: threshold for the agent to be alerted of
-            a possible collision
-            :param up_angle_th: upper threshold for angle
-            :param low_angle_th: lower threshold for angle
-            :param lane_offset: for right and left lane changes
-            :return: a tuple given by (bool_flag, vehicle, distance), where:
-            - bool_flag is True if there is a vehicle ahead blocking us
-                   and False otherwise
-            - vehicle is the blocker object itself
-            - distance is the meters separating the two vehicles
-        """
-        ego_transform = self._vehicle.get_transform()
-        ego_location = ego_transform.location
-        ego_wpt = self._map.get_waypoint(ego_location)
-
-        # Get the right offset
-        if ego_wpt.lane_id < 0 and lane_offset != 0:
-            lane_offset *= -1
-
-        for target_vehicle in vehicle_list:
-
-            target_transform = target_vehicle.get_transform()
-            target_location = target_transform.location
-            # If the object is not in our next or current lane it's not an obstacle
-
-            target_wpt = self._map.get_waypoint(target_location)
-            if target_wpt.road_id != ego_wpt.road_id or \
-                    target_wpt.lane_id != ego_wpt.lane_id + lane_offset:
-                next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=5)[0]
-                if target_wpt.road_id != next_wpt.road_id or \
-                        target_wpt.lane_id != next_wpt.lane_id + lane_offset:
-                    continue
-
-            if is_within_distance(target_transform, ego_transform,
-                                  proximity_th, [low_angle_th, up_angle_th]):
-
-                return (True, target_vehicle, compute_distance(target_location, ego_location))
-
-        return (False, None, -1)
-
     def traffic_light_manager(self):
         """
         This method is in charge of behaviors for red lights.

PythonAPI/carla/setup.py

@@ -162,7 +162,7 @@ with open("README.md") as f:
 
 setup(
     name='carla',
-    version='0.9.12',
+    version='0.9.13',
     package_dir={'': 'source'},
     packages=['carla'],
     ext_modules=get_libcarla_extensions(),

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -165,6 +165,7 @@ void export_actor() {
   class_<cc::Vehicle, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Vehicle>>("Vehicle",
       no_init)
       .def("apply_control", &cc::Vehicle::ApplyControl, (arg("control")))
+      .def("apply_ackermann_control", &cc::Vehicle::ApplyAckermannControl, (arg("control")))
       .def("get_control", &cc::Vehicle::GetControl)
       .def("set_light_state", &cc::Vehicle::SetLightState, (arg("light_state")))
       .def("open_door", &cc::Vehicle::OpenDoor, (arg("door_idx")))
@@ -174,6 +175,8 @@ void export_actor() {
       .def("get_light_state", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetLightState))
       .def("apply_physics_control", &cc::Vehicle::ApplyPhysicsControl, (arg("physics_control")))
       .def("get_physics_control", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetPhysicsControl))
+      .def("apply_ackermann_controller_settings", &cc::Vehicle::ApplyAckermannControllerSettings, (arg("settings")))
+      .def("get_ackermann_controller_settings", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetAckermannControllerSettings))
       .def("set_autopilot", CALL_WITHOUT_GIL_2(cc::Vehicle, SetAutopilot, bool, uint16_t), (arg("enabled") = true, arg("tm_port") = ctm::TM_DEFAULT_PORT))
       .def("show_debug_telemetry", &cc::Vehicle::ShowDebugTelemetry, (arg("enabled") = true))
       .def("get_speed_limit", &cc::Vehicle::GetSpeedLimit)
@@ -188,8 +191,13 @@ void export_actor() {
 
   class_<cc::Walker, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Walker>>("Walker", no_init)
       .def("apply_control", &ApplyControl<cr::WalkerControl>, (arg("control")))
-      .def("apply_control", &ApplyControl<cr::WalkerBoneControl>, (arg("control")))
       .def("get_control", &cc::Walker::GetWalkerControl)
+      .def("get_bones", &cc::Walker::GetBonesTransform)
+      .def("set_bones", &cc::Walker::SetBonesTransform, (arg("bones")))
+      .def("blend_pose", &cc::Walker::BlendPose, (arg("blend")))
+      .def("show_pose", &cc::Walker::ShowPose)
+      .def("hide_pose", &cc::Walker::HidePose)
+      .def("get_pose_from_animation", &cc::Walker::GetPoseFromAnimation)
       .def(self_ns::str(self_ns::self))
   ;
 

PythonAPI/carla/source/libcarla/AdRss.cpp

@@ -93,6 +93,12 @@ static void RegisterActorConstellationCallback(carla::client::RssSensor &self, b
   self.RegisterActorConstellationCallback(callback_function);
 }
 
+static void Stop(carla::client::RssSensor &self) {
+  // ensure the RssSensor is stopped with GIL released to sync on processing lock
+  carla::PythonUtil::ReleaseGIL unlock;
+  self.Stop();
+}
+
 void export_ad() {
   using namespace boost::python;
 
@@ -186,6 +192,7 @@ void export_ad_rss() {
       .add_property("pedestrian_dynamics", &GetPedestrianDynamics, &cc::RssSensor::SetPedestrianDynamics)
       .add_property("road_boundaries_mode", &GetRoadBoundariesMode, &cc::RssSensor::SetRoadBoundariesMode)
       .add_property("routing_targets", &GetRoutingTargets)
+      .def("stop", &Stop)
       .def("register_actor_constellation_callback", &RegisterActorConstellationCallback, (arg("callback")))
       .def("append_routing_target", &cc::RssSensor::AppendRoutingTarget, (arg("routing_target")))
       .def("reset_routing_targets", &cc::RssSensor::ResetRoutingTargets)
@@ -199,5 +206,6 @@ void export_ad_rss() {
       .def(init<>())
       .def("restrict_vehicle_control", &carla::rss::RssRestrictor::RestrictVehicleControl,
            (arg("vehicle_control"), arg("proper_response"), arg("ego_dynamics_on_route"), arg("vehicle_physics")))
+      .def("set_log_level", &carla::rss::RssRestrictor::SetLogLevel, (arg("log_level")))
       .def(self_ns::str(self_ns::self));
 }

PythonAPI/carla/source/libcarla/Blueprint.cpp

@@ -103,6 +103,17 @@ void export_blueprint() {
     .def(self_ns::str(self_ns::self))
   ;
 
+  class_<crpc::FloatColor>("FloatColor")
+    .def(init<float, float, float, float>(
+        (arg("r")=0, arg("g")=0.f, arg("b")=0.f, arg("a")=1.0f)))
+    .def_readwrite("r", &crpc::FloatColor::r)
+    .def_readwrite("g", &crpc::FloatColor::g)
+    .def_readwrite("b", &crpc::FloatColor::b)
+    .def_readwrite("a", &crpc::FloatColor::a)
+    .def("__eq__", &crpc::FloatColor::operator==)
+    .def("__ne__", &crpc::FloatColor::operator!=)
+  ;
+
   class_<csd::OpticalFlowPixel>("OpticalFlowPixel")
     .def(init<float, float>(
         (arg("x")=0, arg("y")=0)))

PythonAPI/carla/source/libcarla/Client.cpp

@@ -86,24 +86,22 @@ static auto ApplyBatchCommandsSync(
         bool autopilotValue = false;
 
         CommandType::CommandType& cmd_type = cmds[i].command;
-        const boost::typeindex::type_info& cmd_type_info = cmd_type.type();
 
         // check SpawnActor command
-        if (cmd_type_info == typeid(carla::rpc::Command::SpawnActor)) {
+        if (const auto *maybe_spawn_actor_cmd = boost::variant2::get_if<carla::rpc::Command::SpawnActor>(&cmd_type)) {
           // check inside 'do_after'
-          auto &spawn = boost::get<carla::rpc::Command::SpawnActor>(cmd_type);
-          for (auto &cmd : spawn.do_after) {
-            if (cmd.command.type() == typeid(carla::rpc::Command::SetAutopilot)) {
-              tm_port = boost::get<carla::rpc::Command::SetAutopilot>(cmd.command).tm_port;
-              autopilotValue = boost::get<carla::rpc::Command::SetAutopilot>(cmd.command).enabled;
+          for (auto &cmd : maybe_spawn_actor_cmd->do_after) {
+            if (const auto *maybe_set_autopilot_command = boost::variant2::get_if<carla::rpc::Command::SetAutopilot>(&cmd.command)) {
+              tm_port = maybe_set_autopilot_command->tm_port;
+              autopilotValue = maybe_set_autopilot_command->enabled;
               isAutopilot = true;
             }
           }
         }
         // check SetAutopilot command
-        else if (cmd_type_info == typeid(carla::rpc::Command::SetAutopilot)) {
-          tm_port = boost::get<carla::rpc::Command::SetAutopilot>(cmd_type).tm_port;
-          autopilotValue = boost::get<carla::rpc::Command::SetAutopilot>(cmd_type).enabled;
+        else if (const auto *maybe_set_autopilot_command = boost::variant2::get_if<carla::rpc::Command::SetAutopilot>(&cmd_type)) {
+          tm_port = maybe_set_autopilot_command->tm_port;
+          autopilotValue = maybe_set_autopilot_command->enabled;
           isAutopilot = true;
         }
 
@@ -156,10 +154,17 @@ static auto ApplyBatchCommandsSync(
   vehicles_to_enable.shrink_to_fit();
   vehicles_to_disable.shrink_to_fit();
 
+  // Ensure the TM always receives the same vector by sorting the elements
+  std::vector<carla::traffic_manager::ActorPtr> sorted_vehicle_to_enable = vehicles_to_enable;
+  std::sort(sorted_vehicle_to_enable.begin(), sorted_vehicle_to_enable.end(), [](carla::traffic_manager::ActorPtr &a, carla::traffic_manager::ActorPtr &b) {return a->GetId() < b->GetId(); });
+
+  std::vector<carla::traffic_manager::ActorPtr> sorted_vehicle_to_disable = vehicles_to_disable;
+  std::sort(sorted_vehicle_to_disable.begin(), sorted_vehicle_to_disable.end(), [](carla::traffic_manager::ActorPtr &a, carla::traffic_manager::ActorPtr &b) {return a->GetId() < b->GetId(); });
+
   // check if any autopilot command was sent
-  if (vehicles_to_enable.size() || vehicles_to_disable.size()) {
-    self.GetInstanceTM(tm_port).RegisterVehicles(vehicles_to_enable);
-    self.GetInstanceTM(tm_port).UnregisterVehicles(vehicles_to_disable);
+  if (sorted_vehicle_to_enable.size() || sorted_vehicle_to_disable.size()) {
+    self.GetInstanceTM(tm_port).RegisterVehicles(sorted_vehicle_to_enable);
+    self.GetInstanceTM(tm_port).UnregisterVehicles(sorted_vehicle_to_disable);
   }
 
   return result;

PythonAPI/carla/source/libcarla/Commands.cpp

@@ -101,6 +101,13 @@ void export_commands() {
     .def_readwrite("control", &cr::Command::ApplyVehicleControl::control)
   ;
 
+  class_<cr::Command::ApplyVehicleAckermannControl>("ApplyVehicleAckermannControl")
+    .def("__init__", &command_impl::CustomInit<ActorPtr, cr::VehicleAckermannControl>, (arg("actor"), arg("control")))
+    .def(init<cr::ActorId, cr::VehicleAckermannControl>((arg("actor_id"), arg("control"))))
+    .def_readwrite("actor_id", &cr::Command::ApplyVehicleAckermannControl::actor)
+    .def_readwrite("control", &cr::Command::ApplyVehicleAckermannControl::control)
+  ;
+
   class_<cr::Command::ApplyWalkerControl>("ApplyWalkerControl")
     .def("__init__", &command_impl::CustomInit<ActorPtr, cr::WalkerControl>, (arg("actor"), arg("control")))
     .def(init<cr::ActorId, cr::WalkerControl>((arg("actor_id"), arg("control"))))
@@ -211,6 +218,7 @@ void export_commands() {
   implicitly_convertible<cr::Command::SpawnActor, cr::Command>();
   implicitly_convertible<cr::Command::DestroyActor, cr::Command>();
   implicitly_convertible<cr::Command::ApplyVehicleControl, cr::Command>();
+  implicitly_convertible<cr::Command::ApplyVehicleAckermannControl, cr::Command>();
   implicitly_convertible<cr::Command::ApplyWalkerControl, cr::Command>();
   implicitly_convertible<cr::Command::ApplyVehiclePhysicsControl, cr::Command>();
   implicitly_convertible<cr::Command::ApplyTransform, cr::Command>();

PythonAPI/carla/source/libcarla/Control.cpp

@@ -5,11 +5,13 @@
 // This work is licensed under the terms of the MIT license.
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
+#include <carla/rpc/VehicleAckermannControl.h>
 #include <carla/rpc/VehicleControl.h>
 #include <carla/rpc/VehiclePhysicsControl.h>
 #include <carla/rpc/WheelPhysicsControl.h>
 #include <carla/rpc/WalkerControl.h>
-#include <carla/rpc/WalkerBoneControl.h>
+#include <carla/rpc/WalkerBoneControlIn.h>
+#include <carla/rpc/WalkerBoneControlOut.h>
 
 #include <ostream>
 
@@ -31,6 +33,15 @@ namespace rpc {
     return out;
   }
 
+  std::ostream &operator<<(std::ostream &out, const VehicleAckermannControl &control) {
+    out << "VehicleAckermannControl(steer=" << std::to_string(control.steer)
+        << ", steer_speed=" << std::to_string(control.steer_speed)
+        << ", speed=" << std::to_string(control.speed)
+        << ", acceleration=" << std::to_string(control.acceleration)
+        << ", jerk=" << std::to_string(control.jerk) << ')';
+    return out;
+  }
+
   std::ostream &operator<<(std::ostream &out, const WalkerControl &control) {
     out << "WalkerControl(direction=" << control.direction
         << ", speed=" << std::to_string(control.speed)
@@ -38,8 +49,8 @@ namespace rpc {
     return out;
   }
 
-  std::ostream &operator<<(std::ostream &out, const WalkerBoneControl &control) {
-    out << "WalkerBoneControl(bone_transforms(";
+  std::ostream &operator<<(std::ostream &out, const WalkerBoneControlIn &control) {
+    out << "WalkerBoneControlIn(bone_transforms(";
     for (auto bone_transform : control.bone_transforms) {
       out << "(name="  << bone_transform.first
           << ", transform=" << bone_transform.second << ')';
@@ -48,6 +59,21 @@ namespace rpc {
     return out;
   }
 
+  std::ostream &operator<<(std::ostream &out, const BoneTransformDataOut &data) {
+    out << "BoneTransformDataOut(name="  << data.bone_name << ", world=" << data.world << ", component=" << data.component << ", relative=" << data.relative << ')';
+    return out;
+  }
+
+  std::ostream &operator<<(std::ostream &out, const WalkerBoneControlOut &control) {
+    out << "WalkerBoneControlOut(bone_transforms(";
+    for (auto bone_transform : control.bone_transforms) {
+      out << "(name="  << bone_transform.bone_name
+          << ", world=" << bone_transform.world << ", component=" << bone_transform.component << ", relative=" << bone_transform.relative << ')';
+    }
+    out << "))";
+    return out;
+  }
+
   std::ostream &operator<<(std::ostream &out, const GearPhysicsControl &control) {
     out << "GearPhysicsControl(ratio=" << std::to_string(control.ratio)
         << ", down_ratio=" << std::to_string(control.down_ratio)
@@ -89,6 +115,17 @@ namespace rpc {
     << ", use_sweep_wheel_collision=" << control.use_sweep_wheel_collision << ')';
     return out;
   }
+
+  std::ostream &operator<<(std::ostream &out, const AckermannControllerSettings &settings) {
+    out << "AckermannControllerSettings(speed_kp=" << std::to_string(settings.speed_kp)
+        << ", speed_ki=" << std::to_string(settings.speed_ki)
+        << ", speed_kd=" << std::to_string(settings.speed_kd)
+        << ", accel_kp=" << std::to_string(settings.accel_kp)
+        << ", accel_ki=" << std::to_string(settings.accel_ki)
+        << ", accel_kd=" << std::to_string(settings.accel_kd)  << ')';
+    return out;
+  }
+
 } // namespace rpc
 } // namespace carla
 
@@ -111,16 +148,16 @@ static auto GetVectorOfVector2DFromList(const boost::python::list &list) {
 }
 
 static auto GetVectorOfBoneTransformFromList(const boost::python::list &list) {
-  std::vector<carla::rpc::BoneTransformData> v;
+  std::vector<carla::rpc::BoneTransformDataIn> v;
 
   auto length = boost::python::len(list);
   v.reserve(static_cast<size_t>(length));
   for (auto i = 0u; i < length; ++i) {
-    boost::python::extract<carla::rpc::BoneTransformData> ext(list[i]);
+    boost::python::extract<carla::rpc::BoneTransformDataIn> ext(list[i]);
     if (ext.check()) {
       v.push_back(ext);
     } else {
-      v.push_back(carla::rpc::BoneTransformData{
+      v.push_back(carla::rpc::BoneTransformDataIn{
         boost::python::extract<std::string>(list[i][0u]),
         boost::python::extract<carla::geom::Transform>(list[i][1u])});
     }
@@ -227,18 +264,26 @@ boost::python::object VehiclePhysicsControl_init(boost::python::tuple args, boos
   return res;
 }
 
-static auto GetBonesTransform(const carla::rpc::WalkerBoneControl &self) {
-  const std::vector<carla::rpc::BoneTransformData> &bone_transform_data = self.bone_transforms;
+static auto GetBonesTransform(const carla::rpc::WalkerBoneControlIn &self) {
+  const std::vector<carla::rpc::BoneTransformDataIn> &bone_transform_data = self.bone_transforms;
   boost::python::object get_iter =
-      boost::python::iterator<const std::vector<carla::rpc::BoneTransformData>>();
+      boost::python::iterator<const std::vector<carla::rpc::BoneTransformDataIn>>();
   boost::python::object iter = get_iter(bone_transform_data);
   return boost::python::list(iter);
 }
 
-static void SetBonesTransform(carla::rpc::WalkerBoneControl &self, const boost::python::list &list) {
+static void SetBonesTransform(carla::rpc::WalkerBoneControlIn &self, const boost::python::list &list) {
   self.bone_transforms = GetVectorOfBoneTransformFromList(list);
 }
 
+static auto GetBonesTransformOut(const carla::rpc::WalkerBoneControlOut &self) {
+  const std::vector<carla::rpc::BoneTransformDataOut> &bone_transform_data = self.bone_transforms;
+  boost::python::object get_iter =
+      boost::python::iterator<const std::vector<carla::rpc::BoneTransformDataOut>>();
+  boost::python::object iter = get_iter(bone_transform_data);
+  return boost::python::list(iter);
+}
+
 boost::python::object WalkerBoneControl_init(boost::python::tuple args, boost::python::dict kwargs) {
   // Args names
   const uint32_t NUM_ARGUMENTS = 1;
@@ -291,6 +336,42 @@ void export_control() {
     .def(self_ns::str(self_ns::self))
   ;
 
+  class_<cr::VehicleAckermannControl>("VehicleAckermannControl")
+    .def(init<float, float, float, float, float>(
+      (arg("steer") = 0.0f,
+      arg("steer_speed") = 0.0f,
+      arg("speed") = 0.0f,
+      arg("acceleration") = 0.0f,
+      arg("jerk") = 0.0f)))
+    .def_readwrite("steer", &cr::VehicleAckermannControl::steer)
+    .def_readwrite("steer_speed", &cr::VehicleAckermannControl::steer_speed)
+    .def_readwrite("speed", &cr::VehicleAckermannControl::speed)
+    .def_readwrite("acceleration", &cr::VehicleAckermannControl::acceleration)
+    .def_readwrite("jerk", &cr::VehicleAckermannControl::jerk)
+    .def("__eq__", &cr::VehicleAckermannControl::operator==)
+    .def("__ne__", &cr::VehicleAckermannControl::operator!=)
+    .def(self_ns::str(self_ns::self))
+  ;
+
+  class_<cr::AckermannControllerSettings>("AckermannControllerSettings")
+    .def(init<float, float, float, float, float, float>(
+      (arg("speed_kp") = 0.0f,
+      arg("speed_ki") = 0.0f,
+      arg("speed_kd") = 0.0f,
+      arg("accel_kp") = 0.0f,
+      arg("accel_ki") = 0.0f,
+      arg("accel_kd") = 0.0f)))
+    .def_readwrite("speed_kp", &cr::AckermannControllerSettings::speed_kp)
+    .def_readwrite("speed_ki", &cr::AckermannControllerSettings::speed_ki)
+    .def_readwrite("speed_kd", &cr::AckermannControllerSettings::speed_kd)
+    .def_readwrite("accel_kp", &cr::AckermannControllerSettings::accel_kp)
+    .def_readwrite("accel_ki", &cr::AckermannControllerSettings::accel_ki)
+    .def_readwrite("accel_kd", &cr::AckermannControllerSettings::accel_kd)
+    .def("__eq__", &cr::AckermannControllerSettings::operator==)
+    .def("__ne__", &cr::AckermannControllerSettings::operator!=)
+    .def(self_ns::str(self_ns::self))
+  ;
+
   class_<cr::WalkerControl>("WalkerControl")
     .def(init<cg::Vector3D, float, bool>(
        (arg("direction") = cg::Vector3D{1.0f, 0.0f, 0.0f},
@@ -304,13 +385,51 @@ void export_control() {
     .def(self_ns::str(self_ns::self))
   ;
 
-  class_<cr::WalkerBoneControl>("WalkerBoneControl")
+  class_<cr::BoneTransformDataIn>("bone_transform")
+    .def(init<>())
+    .def_readwrite("name", &std::pair<std::string, cg::Transform>::first)
+    .def_readwrite("transform", &std::pair<std::string, cg::Transform>::second)
+    .def(self_ns::str(self_ns::self))
+  ;
+
+  class_<std::vector<cr::BoneTransformDataIn>>("vector_of_bones")
+    .def(init<>())
+    .def(boost::python::vector_indexing_suite<std::vector<cr::BoneTransformDataIn>>())
+    .def(self_ns::str(self_ns::self))
+  ;
+
+  class_<cr::BoneTransformDataOut>("bone_transform_out")
+    .def(init<>())
+    .def_readwrite("name", &cr::BoneTransformDataOut::bone_name)
+    .def_readwrite("world", &cr::BoneTransformDataOut::world)
+    .def_readwrite("component", &cr::BoneTransformDataOut::component)
+    .def_readwrite("relative", &cr::BoneTransformDataOut::relative)
+    .def(self_ns::str(self_ns::self))
+    .def("__eq__", &cr::BoneTransformDataOut::operator==)
+    .def("__ne__", &cr::BoneTransformDataOut::operator!=)
+  ;
+
+  class_<std::vector<cr::BoneTransformDataOut>>("vector_of_bones_out")
+    .def(init<>())
+    .def(boost::python::vector_indexing_suite<std::vector<cr::BoneTransformDataOut>>())
+    .def(self_ns::str(self_ns::self))
+  ;
+
+  class_<cr::WalkerBoneControlIn>("WalkerBoneControlIn")
     .def("__init__", raw_function(WalkerBoneControl_init))
     .def(init<>())
     .add_property("bone_transforms", &GetBonesTransform, &SetBonesTransform)
     .def(self_ns::str(self_ns::self))
   ;
 
+  class_<cr::WalkerBoneControlOut>("WalkerBoneControlOut")
+    .def("__init__", raw_function(WalkerBoneControl_init))
+    .def(init<>())
+    // .add_property("bone_transforms", &GetBonesTransformOut, &SetBonesTransformOut)
+    .add_property("bone_transforms", &GetBonesTransformOut)
+    .def(self_ns::str(self_ns::self))
+  ;
+
   class_<std::vector<cr::GearPhysicsControl>>("vector_of_gears")
       .def(boost::python::vector_indexing_suite<std::vector<cr::GearPhysicsControl>>())
       .def(self_ns::str(self_ns::self))

PythonAPI/carla/source/libcarla/SensorData.cpp

@@ -365,7 +365,7 @@ void export_sensor_data() {
   namespace css = carla::sensor::s11n;
 
   // Fake image returned from optical flow to color conversion
-  // fakes the regular image object
+  // fakes the regular image object. Only used for visual purposes
   class_<FakeImage>("FakeImage", no_init)
       .def(vector_indexing_suite<std::vector<uint8_t>>())
       .add_property("width", &FakeImage::Width)

PythonAPI/carla/source/libcarla/TrafficManager.cpp

@@ -6,11 +6,78 @@
 
 #include <chrono>
 #include <memory>
-
+#include <stdio.h>
 #include "carla/PythonUtil.h"
 #include "boost/python/suite/indexing/vector_indexing_suite.hpp"
 
 #include "carla/trafficmanager/TrafficManager.h"
+#include "carla/trafficmanager/SimpleWaypoint.h"
+
+using ActorPtr = carla::SharedPtr<carla::client::Actor>;
+using ActorId = carla::ActorId;
+
+const char* RoadOptionToString(carla::client::RoadOption value) {
+  switch (value)
+  {
+      case carla::client::RoadOption::Void:   return "Void";
+      case carla::client::RoadOption::Left:   return "Left";
+      case carla::client::RoadOption::Right:   return "Right";
+      case carla::client::RoadOption::Straight: return "Straight";
+      case carla::client::RoadOption::LaneFollow:   return "LaneFollow";
+      case carla::client::RoadOption::ChangeLaneLeft:   return "ChangeLaneLeft";
+      case carla::client::RoadOption::ChangeLaneRight: return "ChangeLaneRight";
+      case carla::client::RoadOption::RoadEnd:   return "RoadEnd";
+      default:      return "[Unknown RoadOption]";
+  }
+}
+
+std::vector<uint8_t> RoadOptionToUint(boost::python::list input) {
+  std::vector<uint8_t> route;
+  for (int i = 0; i < len(input); ++i) {
+    uint8_t val;
+    char* str = boost::python::extract<char*>(input[i]);
+    if (strcmp(str,"Void") == 0) val = 0u;
+    else if (strcmp(str,"Left") == 0) val = 1u;
+    else if (strcmp(str,"Right") == 0) val = 2u;
+    else if (strcmp(str,"Straight") == 0) val = 3u;
+    else if (strcmp(str,"LaneFollow") == 0) val = 4u;
+    else if (strcmp(str,"ChangeLaneLeft") == 0) val = 5u;
+    else if (strcmp(str,"ChangeLaneRight") == 0) val = 6u;
+    else if (strcmp(str,"RoadEnd") == 0) val = 7u;
+    else val = 10u;
+    route.push_back(val);
+  }
+  return route;
+}
+
+void InterSetCustomPath(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor, boost::python::list input, bool empty_buffer) {
+  self.SetCustomPath(actor, PythonLitstToVector<carla::geom::Location>(input), empty_buffer);
+}
+
+void InterSetImportedRoute(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor, boost::python::list input, bool empty_buffer) {
+  self.SetImportedRoute(actor, RoadOptionToUint(input), empty_buffer);
+}
+
+boost::python::list InterGetNextAction(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor_ptr) {
+  boost::python::list l;
+  auto next_action = self.GetNextAction(actor_ptr->GetId());
+  l.append(RoadOptionToString(next_action.first));
+  l.append(next_action.second);
+  return l;
+}
+
+boost::python::list InterGetActionBuffer(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor_ptr) {
+  boost::python::list l;
+  auto action_buffer = self.GetActionBuffer(actor_ptr->GetId());
+  for (auto &next_action : action_buffer) {
+    boost::python::list temp;
+    temp.append(RoadOptionToString(next_action.first));
+    temp.append(next_action.second);
+    l.append(temp);
+  }
+  return l;
+}
+
 
 void export_trafficmanager() {
   namespace cc = carla::client;
@@ -21,6 +88,7 @@ void export_trafficmanager() {
     .def("get_port", &ctm::TrafficManager::Port)
     .def("vehicle_percentage_speed_difference", &ctm::TrafficManager::SetPercentageSpeedDifference)
     .def("global_percentage_speed_difference", &ctm::TrafficManager::SetGlobalPercentageSpeedDifference)
+    .def("update_vehicle_lights", &ctm::TrafficManager::SetUpdateVehicleLights)
     .def("collision_detection", &ctm::TrafficManager::SetCollisionDetection)
     .def("force_lane_change", &ctm::TrafficManager::SetForceLaneChange)
     .def("auto_lane_change", &ctm::TrafficManager::SetAutoLaneChange)
@@ -30,13 +98,19 @@ void export_trafficmanager() {
     .def("ignore_lights_percentage", &ctm::TrafficManager::SetPercentageRunningLight)
     .def("ignore_signs_percentage", &ctm::TrafficManager::SetPercentageRunningSign)
     .def("set_global_distance_to_leading_vehicle", &ctm::TrafficManager::SetGlobalDistanceToLeadingVehicle)
-    .def("set_percentage_keep_right_rule", &ctm::TrafficManager::SetKeepRightPercentage)
+    .def("keep_right_rule_percentage", &ctm::TrafficManager::SetKeepRightPercentage)
+    .def("random_left_lanechange_percentage", &ctm::TrafficManager::SetRandomLeftLaneChangePercentage)
+    .def("random_right_lanechange_percentage", &ctm::TrafficManager::SetRandomRightLaneChangePercentage)
     .def("set_synchronous_mode", &ctm::TrafficManager::SetSynchronousMode)
     .def("set_hybrid_physics_mode", &ctm::TrafficManager::SetHybridPhysicsMode)
     .def("set_hybrid_physics_radius", &ctm::TrafficManager::SetHybridPhysicsRadius)
     .def("set_random_device_seed", &ctm::TrafficManager::SetRandomDeviceSeed)
     .def("set_osm_mode", &carla::traffic_manager::TrafficManager::SetOSMMode)
+    .def("set_path", &InterSetCustomPath, (arg("empty_buffer") = true))
+    .def("set_route", &InterSetImportedRoute, (arg("empty_buffer") = true))
     .def("set_respawn_dormant_vehicles", &carla::traffic_manager::TrafficManager::SetRespawnDormantVehicles)
     .def("set_boundaries_respawn_dormant_vehicles", &carla::traffic_manager::TrafficManager::SetBoundariesRespawnDormantVehicles)
+    .def("get_next_action", &InterGetNextAction)
+    .def("get_all_actions", &InterGetActionBuffer)
     .def("shut_down", &ctm::TrafficManager::ShutDown);
 }

PythonAPI/carla/source/libcarla/Weather.cpp

@@ -25,7 +25,8 @@ namespace rpc {
         << ", wetness=" << std::to_string(weather.wetness)
         << ", scattering_intensity=" << std::to_string(weather.scattering_intensity)
         << ", mie_scattering_scale=" << std::to_string(weather.mie_scattering_scale)
-        << ", rayleigh_scattering_scale=" << std::to_string(weather.rayleigh_scattering_scale) << ')';
+        << ", rayleigh_scattering_scale=" << std::to_string(weather.rayleigh_scattering_scale)
+        << ", dust_storm=" << std::to_string(weather.dust_storm) << ')';
     return out;
   }
 
@@ -37,7 +38,7 @@ void export_weather() {
   namespace cr = carla::rpc;
 
   auto cls = class_<cr::WeatherParameters>("WeatherParameters")
-    .def(init<float, float, float, float, float, float, float, float, float, float, float, float, float>(
+    .def(init<float, float, float, float, float, float, float, float, float, float, float, float, float, bool>(
         (arg("cloudiness")=0.0f,
          arg("precipitation")=0.0f,
          arg("precipitation_deposits")=0.0f,
@@ -50,7 +51,8 @@ void export_weather() {
          arg("wetness")=0.0f,
          arg("scattering_intensity")=0.0f,
          arg("mie_scattering_scale")=0.0f,
-         arg("rayleigh_scattering_scale")=0.0331f)))
+         arg("rayleigh_scattering_scale")=0.0331f,
+         arg("dust_storm")=false)))
     .def_readwrite("cloudiness", &cr::WeatherParameters::cloudiness)
     .def_readwrite("precipitation", &cr::WeatherParameters::precipitation)
     .def_readwrite("precipitation_deposits", &cr::WeatherParameters::precipitation_deposits)
@@ -64,6 +66,7 @@ void export_weather() {
     .def_readwrite("scattering_intensity", &cr::WeatherParameters::scattering_intensity)
     .def_readwrite("mie_scattering_scale", &cr::WeatherParameters::mie_scattering_scale)
     .def_readwrite("rayleigh_scattering_scale", &cr::WeatherParameters::rayleigh_scattering_scale)
+    .def_readwrite("dust_storm", &cr::WeatherParameters::dust_storm)
     .def("__eq__", &cr::WeatherParameters::operator==)
     .def("__ne__", &cr::WeatherParameters::operator!=)
     .def(self_ns::str(self_ns::self))
@@ -91,4 +94,5 @@ void export_weather() {
   cls.attr("SoftRainNight") = cr::WeatherParameters::SoftRainNight;
   cls.attr("MidRainyNight") = cr::WeatherParameters::MidRainyNight;
   cls.attr("HardRainNight") = cr::WeatherParameters::HardRainNight;
+  cls.attr("DustStorm") = cr::WeatherParameters::DustStorm;
 }

PythonAPI/carla/source/libcarla/World.cpp

@@ -125,6 +125,7 @@ void export_world() {
   namespace cc = carla::client;
   namespace cg = carla::geom;
   namespace cr = carla::rpc;
+  namespace csd = carla::sensor::data;
 
   class_<cc::Timestamp>("Timestamp")
     .def(init<size_t, double, double, double>(
@@ -245,6 +246,39 @@ void export_world() {
     .value("All", cr::MapLayer::All)
   ;
 
+  enum_<cr::MaterialParameter>("MaterialParameter")
+    .value("Normal", cr::MaterialParameter::Tex_Normal)
+    .value("AO_Roughness_Metallic_Emissive", cr::MaterialParameter::Tex_Ao_Roughness_Metallic_Emissive)
+    .value("Diffuse", cr::MaterialParameter::Tex_Diffuse)
+    .value("Emissive", cr::MaterialParameter::Tex_Emissive)
+  ;
+
+  class_<cr::TextureColor>("TextureColor")
+    .def(init<uint32_t, uint32_t>())
+    .add_property("width", &cr::TextureColor::GetWidth)
+    .add_property("height", &cr::TextureColor::GetHeight)
+    .def("set_dimensions", &cr::TextureColor::SetDimensions)
+    .def("get", +[](const cr::TextureColor &self, int x, int y) -> csd::Color{
+      return self.At(static_cast<uint32_t>(x), static_cast<uint32_t>(y));
+    })
+    .def("set", +[](cr::TextureColor &self, int x, int y, csd::Color& value) {
+      self.At(static_cast<uint32_t>(x), static_cast<uint32_t>(y)) = value;
+    })
+  ;
+
+  class_<cr::TextureFloatColor>("TextureFloatColor")
+    .def(init<uint32_t, uint32_t>())
+    .add_property("width", &cr::TextureFloatColor::GetWidth)
+    .add_property("height", &cr::TextureFloatColor::GetHeight)
+    .def("set_dimensions", &cr::TextureFloatColor::SetDimensions)
+    .def("get", +[](const cr::TextureFloatColor &self, int x, int y) -> cr::FloatColor{
+      return self.At(static_cast<uint32_t>(x), static_cast<uint32_t>(y));
+    })
+    .def("set", +[](cr::TextureFloatColor &self, int x, int y, cr::FloatColor& value) {
+      self.At(static_cast<uint32_t>(x), static_cast<uint32_t>(y)) = value;
+    })
+  ;
+
 #define SPAWN_ACTOR_WITHOUT_GIL(fn) +[]( \
         cc::World &self, \
         const cc::ActorBlueprint &blueprint, \
@@ -285,6 +319,7 @@ void export_world() {
     .def("remove_on_tick", &cc::World::RemoveOnTick, (arg("callback_id")))
     .def("tick", &Tick, (arg("seconds")=0.0))
     .def("set_pedestrians_cross_factor", CALL_WITHOUT_GIL_1(cc::World, SetPedestriansCrossFactor, float), (arg("percentage")))
+    .def("set_pedestrians_seed", CALL_WITHOUT_GIL_1(cc::World, SetPedestriansSeed, unsigned int), (arg("seed")))
     .def("get_traffic_sign", CONST_CALL_WITHOUT_GIL_1(cc::World, GetTrafficSign, cc::Landmark), arg("landmark"))
     .def("get_traffic_light", CONST_CALL_WITHOUT_GIL_1(cc::World, GetTrafficLight, cc::Landmark), arg("landmark"))
     .def("get_traffic_light_from_opendrive_id", CONST_CALL_WITHOUT_GIL_1(cc::World, GetTrafficLightFromOpenDRIVE, const carla::road::SignId&), arg("traffic_light_id"))
@@ -299,6 +334,19 @@ void export_world() {
     .def("cast_ray", CALL_RETURNING_LIST_2(cc::World, CastRay, cg::Location, cg::Location), (arg("initial_location"), arg("final_location")))
     .def("project_point", CALL_RETURNING_OPTIONAL_3(cc::World, ProjectPoint, cg::Location, cg::Vector3D, float), (arg("location"), arg("direction"), arg("search_distance")=10000.f))
     .def("ground_projection", CALL_RETURNING_OPTIONAL_2(cc::World, GroundProjection, cg::Location, float), (arg("location"), arg("search_distance")=10000.f))
+    .def("get_names_of_all_objects", CALL_RETURNING_LIST(cc::World, GetNamesOfAllObjects))
+    .def("apply_color_texture_to_object", &cc::World::ApplyColorTextureToObject, (arg("object_name"), arg("material_parameter"), arg("texture")))
+    .def("apply_float_color_texture_to_object", &cc::World::ApplyFloatColorTextureToObject, (arg("object_name"), arg("material_parameter"), arg("texture")))
+    .def("apply_textures_to_object", &cc::World::ApplyTexturesToObject, (arg("object_name"), arg("diffuse_texture"), arg("emissive_texture"), arg("normal_texture"), arg("ao_roughness_metallic_emissive_texture")))
+    .def("apply_color_texture_to_objects", +[](cc::World &self, boost::python::list &list, const cr::MaterialParameter& parameter, const cr::TextureColor& Texture) {
+        self.ApplyColorTextureToObjects(PythonLitstToVector<std::string>(list), parameter, Texture);
+      }, (arg("objects_name_list"), arg("material_parameter"), arg("texture")))
+    .def("apply_float_color_texture_to_objects", +[](cc::World &self, boost::python::list &list, const cr::MaterialParameter& parameter, const cr::TextureFloatColor& Texture) {
+        self.ApplyFloatColorTextureToObjects(PythonLitstToVector<std::string>(list), parameter, Texture);
+      }, (arg("objects_name_list"), arg("material_parameter"), arg("texture")))
+    .def("apply_textures_to_objects", +[](cc::World &self, boost::python::list &list, const cr::TextureColor& diffuse_texture, const cr::TextureFloatColor& emissive_texture, const cr::TextureFloatColor& normal_texture, const cr::TextureFloatColor& ao_roughness_metallic_emissive_texture) {
+        self.ApplyTexturesToObjects(PythonLitstToVector<std::string>(list), diffuse_texture, emissive_texture, normal_texture, ao_roughness_metallic_emissive_texture);
+      }, (arg("objects_name_list"), arg("diffuse_texture"), arg("emissive_texture"), arg("normal_texture"), arg("ao_roughness_metallic_emissive_texture")))
     .def(self_ns::str(self_ns::self))
   ;