Vehicle lights state in traffic manager (#4717)

* added vehicle lights stage to the traffic manager under the dev branch

* update CHANGELOG to lastest version and added vehicle lights stage for automatically turn on-off lights

* added vehicle lights stage to the traffic manager under the dev branch

* removed commented code

* cleanups to the code, adding some comments to explain how the trafficmanagerlocal handles the command buffer

* fixing missing reference in ALSM constructor, disabling vehicle light state for dormant vehicles

* bugfix in vehicle_id_list access, added fix to avoid blinking brake lights due to throttle control

* requested changes in the vehicle light stage code

* missing namespace qualifier for constants

* Added explicit method to enable the vehicle lights management by the TM, on a per-vehicle basis. Changed generate_traffic.py to use the new vehicle lights stage instead of just setting a fixed, constant vehicle light status. Docs are updated, accordingly.

Co-authored-by: npunito <npunito>
Co-authored-by: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com>

CHANGELOG.md

@@ -6,6 +6,7 @@
   * 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 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`
 
 ## 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.auto_update_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:

LibCarla/source/carla/trafficmanager/ALSM.cpp

@@ -27,6 +27,7 @@ ALSM::ALSM(
   CollisionStage &collision_stage,
   TrafficLightStage &traffic_light_stage,
   MotionPlanStage &motion_plan_stage,
+  VehicleLightStage &vehicle_light_stage,
   RandomGeneratorMap &random_devices)
   : registered_vehicles(registered_vehicles),
     buffer_map(buffer_map),
@@ -40,6 +41,7 @@ ALSM::ALSM(
     collision_stage(collision_stage),
     traffic_light_stage(traffic_light_stage),
     motion_plan_stage(motion_plan_stage),
+    vehicle_light_stage(vehicle_light_stage),
     random_devices(random_devices) {}
 
 void ALSM::Update() {
@@ -378,6 +380,7 @@ void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) {
     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,6 +60,7 @@ 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;
@@ -102,6 +104,7 @@ public:
        CollisionStage &collision_stage,
        TrafficLightStage &traffic_light_stage,
        MotionPlanStage &motion_plan_stage,
+       VehicleLightStage &vehicle_light_stage,
        RandomGeneratorMap &random_devices);
 
   void Update();

LibCarla/source/carla/trafficmanager/Constants.h

@@ -128,6 +128,15 @@ 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;
+} // namespace VehicleLight
+
 namespace PID {
 static const float MAX_THROTTLE = 0.85f;
 static const float MAX_BRAKE = 1.0f;

LibCarla/source/carla/trafficmanager/Parameters.cpp

@@ -90,6 +90,11 @@ void Parameters::SetKeepRightPercentage(const ActorPtr &actor, const float perce
   perc_keep_right.AddEntry(entry);
 }
 
+void Parameters::SetUpdateVehicleLightState(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);
@@ -273,6 +278,16 @@ float Parameters::GetPercentageIgnoreWalkers(const ActorId &actor_id) const {
   return percentage;
 }
 
+bool Parameters::GetUpdateVehicleLightState(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;

LibCarla/source/carla/trafficmanager/Parameters.h

@@ -56,6 +56,8 @@ private:
   AtomicMap<ActorId, float> perc_ignore_vehicles;
   /// Map containing % of keep right rule.
   AtomicMap<ActorId, float> perc_keep_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
@@ -123,6 +125,9 @@ public:
   /// Method to set probabilistic preference to keep on the right lane.
   void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
 
+  /// Method to set the automatic vehicle light state update flag.
+  void SetUpdateVehicleLightState(const ActorPtr &actor, const bool do_update);
+
   /// Method to set the distance to leading vehicle for all registered vehicles.
   void SetGlobalDistanceToLeadingVehicle(const float dist);
 
@@ -185,6 +190,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 GetUpdateVehicleLightState(const ActorId &actor_id) const;
+
   /// Method to get synchronous mode.
   bool GetSynchronousMode() const;
 

LibCarla/source/carla/trafficmanager/TrafficManager.h

@@ -138,6 +138,14 @@ public:
     }
   }
 
+  /// Set the automatical 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);

LibCarla/source/carla/trafficmanager/TrafficManagerBase.h

@@ -51,6 +51,9 @@ public:
   /// If less than 0, it's a % increase.
   virtual void SetGlobalPercentageSpeedDifference(float const percentage) = 0;
 
+  /// Method to set the automatical 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;
 

LibCarla/source/carla/trafficmanager/TrafficManagerClient.h

@@ -88,6 +88,12 @@ public:
     _client->call("set_global_percentage_speed_difference", percentage);
   }
 
+  /// Method to set the automatical management of the vehicle lights
+  void SetUpdateVehicleLights(const carla::rpc::Actor &_actor, const bool do_update) {
+    DEBUG_ASSERT(_client != nullptr);
+    _client->call("auto_update_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);

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp

@@ -77,6 +77,13 @@ TrafficManagerLocal::TrafficManagerLocal(
                                       random_devices,
                                       local_map)),
 
+    vehicle_light_stage(VehicleLightStage(vehicle_id_list,
+                                          simulation_state,
+                                          buffer_map,
+                                          parameters,
+                                          world,
+                                          control_frame)),
+
     alsm(ALSM(registered_vehicles,
               buffer_map,
               track_traffic,
@@ -89,6 +96,7 @@ TrafficManagerLocal::TrafficManagerLocal(
               collision_stage,
               traffic_light_stage,
               motion_plan_stage,
+              vehicle_light_stage,
               random_devices)),
 
     server(TrafficManagerServer(RPCportTM, static_cast<carla::traffic_manager::TrafficManagerBase *>(this))) {
@@ -210,10 +218,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 +233,11 @@ void TrafficManagerLocal::Run() {
       collision_stage.Update(index);
     }
     collision_stage.ClearCycleCache();
+    vehicle_light_stage.ClearCycleCache();
     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();
@@ -337,6 +351,11 @@ void TrafficManagerLocal::SetGlobalPercentageSpeedDifference(const float percent
   parameters.SetGlobalPercentageSpeedDifference(percentage);
 }
 
+/// Method to set the automatical management of the vehicle lights
+void TrafficManagerLocal::SetUpdateVehicleLights(const ActorPtr &actor, const bool do_update) {
+  parameters.SetUpdateVehicleLightState(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);
 }

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;
@@ -165,6 +166,9 @@ public:
   /// If less than 0, it's a % increase.
   void SetGlobalPercentageSpeedDifference(float const percentage);
 
+  /// Method to set the automatical 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);
 

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());

LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h

@@ -58,6 +58,9 @@ public:
   /// If less than 0, it's a % increase.
   void SetGlobalPercentageSpeedDifference(float const percentage);
 
+  /// Method to set the automatical 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);
 

LibCarla/source/carla/trafficmanager/VehicleLightStage.cpp

@@ -0,0 +1,178 @@
+
+#include "carla/trafficmanager/Constants.h"
+#include "carla/trafficmanager/LocalizationUtils.h"
+
+#include "carla/trafficmanager/VehicleLightStage.h"
+
+namespace carla {
+namespace traffic_manager {
+
+VehicleLightStage::VehicleLightStage(
+  const std::vector<ActorId> &vehicle_id_list,
+  const SimulationState &simulation_state,
+  const BufferMap &buffer_map,
+  const Parameters &parameters,
+  const cc::World &world,
+  ControlFrame& control_frame)
+  : vehicle_id_list(vehicle_id_list),
+    simulation_state(simulation_state),
+    buffer_map(buffer_map),
+    parameters(parameters),
+    world(world),
+    control_frame(control_frame) {}
+
+void VehicleLightStage::ClearCycleCache() {
+  // 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 id = vehicle_id_list.at(index);
+  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;
+
+  if (!parameters.GetUpdateVehicleLightState(id))
+    return; // this vehicle is not set to have automatic lights update
+
+  // search the current light state of the vehicle
+  for (auto&& vls : all_light_states) {
+    if (vls.first == id) {
+      light_states = vls.second;
+      break;
+    }
+  }
+
+  cg::Vector3D actor_vec = simulation_state.GetHeading(id);
+
+  // Recover the planned waypoints for this vehicle
+  if (buffer_map.count(id) == 1) {
+    const Buffer& waypoint_deque = buffer_map.at(id);
+    // Find the next intersection (if any) to decide to turn on the blinkers
+    for (const SimpleWaypointPtr& swpp : waypoint_deque) {
+      WaypointPtr wptr = swpp->GetWaypoint();
+      if (!wptr->IsJunction())
+        continue;
+
+      // Get the end of the junction road segment
+      std::vector<WaypointPtr> next_wptrs = wptr -> GetNextUntilLaneEnd(2);
+      if(next_wptrs.empty())
+        break;
+      wptr = next_wptrs.back();
+      cg::Vector3D next_road_vec = wptr->GetTransform().GetForwardVector();
+      cg::Vector3D up_vec(0, 0, 1);
+      float dot_prod = actor_vec.x*next_road_vec.x +
+                        actor_vec.y*next_road_vec.y +
+                        actor_vec.z*next_road_vec.z;
+      cg::Vector3D cross_prod(actor_vec.y*up_vec.z - actor_vec.z*up_vec.y,
+                              actor_vec.z*up_vec.x - actor_vec.x*up_vec.z,
+                              actor_vec.x*up_vec.y - actor_vec.y*up_vec.x);
+
+      float dot_prod_left = cross_prod.x*next_road_vec.x +
+                            cross_prod.y*next_road_vec.y +
+                            cross_prod.z*next_road_vec.z;
+
+      // Determine if the vehicle is truning left or right
+      if(dot_prod < 0.5) {
+        if(dot_prod_left > 0.5)
+          left_turn_indicator = true;
+        if(dot_prod_left < -0.5)
+          right_turn_indicator = true;
+      }
+      break;
+    }
+  }
+
+  // Determine brake light state
+  for (size_t cc = 0; cc < control_frame.size(); cc++) {
+    if (control_frame[cc].command.type() == typeid(carla::rpc::Command::ApplyVehicleControl)) {
+      carla::rpc::Command::ApplyVehicleControl& ctrl = boost::get<carla::rpc::Command::ApplyVehicleControl>(control_frame[cc].command);
+      if (ctrl.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 < constants::VehicleLight::SUN_ALTITUDE_DEGREES_BEFORE_DAWN ||
+      weather.sun_altitude_angle > constants::VehicleLight::SUN_ALTITUDE_DEGREES_AFTER_SUNSET)
+  {
+    position = true;
+    low_beam = true;
+  }
+  else if (weather.sun_altitude_angle < constants::VehicleLight::SUN_ALTITUDE_DEGREES_JUST_AFTER_DAWN ||
+           weather.sun_altitude_angle > constants::VehicleLight::SUN_ALTITUDE_DEGREES_JUST_BEFORE_SUNSET)
+  {
+    position = true;
+  }
+  // Turn on lights under heavy rain
+  if (weather.precipitation > constants::VehicleLight::HEAVY_PRECIPITATION_THRESHOLD) {
+    position = true;
+    low_beam = true;
+  }
+  // Turn on fog lights
+  if (weather.fog_density > constants::VehicleLight::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(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,46 @@
+
+#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 SimulationState &simulation_state;
+  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 SimulationState &simulation_state,
+                    const BufferMap &buffer_map,
+                    const Parameters &parameters,
+                    const cc::World &world,
+                    ControlFrame& control_frame);
+
+  void ClearCycleCache();
+
+  void Update(const unsigned long index) override;
+
+  void RemoveActor(const ActorId actor_id) override;
+
+  void Reset() override;
+};
+
+} // namespace traffic_manager
+} // namespace carla

PythonAPI/carla/source/libcarla/TrafficManager.cpp

@@ -21,6 +21,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("auto_update_lights", &ctm::TrafficManager::SetUpdateVehicleLights)
     .def("collision_detection", &ctm::TrafficManager::SetCollisionDetection)
     .def("force_lane_change", &ctm::TrafficManager::SetForceLaneChange)
     .def("auto_lane_change", &ctm::TrafficManager::SetAutoLaneChange)

PythonAPI/docs/client.yml

@@ -481,6 +481,21 @@
 
         Default is 30. Exceeding a speed limit can be done using negative percentages.
     # --------------------------------------
+    - def_name: auto_update_lights
+      params:
+      - param_name: actor
+        type: carla.Actor
+        doc: >
+          Vehicle whose lights status is being changed.
+      - param_name: do_update
+        type: bool
+        doc: >
+          If __True__ the traffic manager will manage the vehicle lights for the specified vehicle.
+      doc: >
+        Sets if the Traffic Manager is responsible of updating the vehicle lights, or not.
+
+        Default is __False__. The traffic manager will not change the vehicle light status of a vehicle, unless its auto_update_status is st to __True__.
+    # --------------------------------------
     - def_name: get_port
       params:
       return: uint16

PythonAPI/examples/generate_traffic.py

@@ -126,7 +126,7 @@ def main():
         '--car-lights-on',
         action='store_true',
         default=False,
-        help='Enable car lights')
+        help='Enable automatic car light management')
     argparser.add_argument(
         '--hero',
         action='store_true',
@@ -214,7 +214,6 @@ def main():
         # @todo cannot import these directly.
         SpawnActor = carla.command.SpawnActor
         SetAutopilot = carla.command.SetAutopilot
-        SetVehicleLightState = carla.command.SetVehicleLightState
         FutureActor = carla.command.FutureActor
 
         # --------------
@@ -238,15 +237,9 @@ def main():
             else:
                 blueprint.set_attribute('role_name', 'autopilot')
 
-            # prepare the light state of the cars to spawn
-            light_state = vls.NONE
-            if args.car_lights_on:
-                light_state = vls.Position | vls.LowBeam | vls.LowBeam
-
             # spawn the cars and set their autopilot and light state all together
             batch.append(SpawnActor(blueprint, transform)
-                .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))
-                .then(SetVehicleLightState(FutureActor, light_state)))
+                .then(SetAutopilot(FutureActor, True, traffic_manager.get_port())))
 
         for response in client.apply_batch_sync(batch, synchronous_master):
             if response.error:
@@ -254,6 +247,12 @@ def main():
             else:
                 vehicles_list.append(response.actor_id)
 
+        # Set automatic vehicle lights update if specified
+        if args.car_lights_on:
+            all_vehicle_actors = world.get_actors(vehicles_list)
+            for actor in all_vehicle_actors:
+                traffic_manager.auto_update_lights(actor, True)
+
         # -------------
         # Spawn Walkers
         # -------------