CARLA
TrafficLightStage.cpp
Go to the documentation of this file.
1 
4 
6 
7 namespace carla {
8 namespace traffic_manager {
9 
12 
14  const std::vector<ActorId> &vehicle_id_list,
15  const SimulationState &simulation_state,
16  const BufferMap &buffer_map,
17  const Parameters &parameters,
18  const cc::World &world,
19  TLFrame &output_array,
20  RandomGeneratorMap &random_devices)
21  : vehicle_id_list(vehicle_id_list),
22  simulation_state(simulation_state),
23  buffer_map(buffer_map),
24  parameters(parameters),
25  world(world),
26  output_array(output_array),
27  random_devices(random_devices) {}
28 
29 void TrafficLightStage::Update(const unsigned long index) {
30  bool traffic_light_hazard = false;
31 
32  const ActorId ego_actor_id = vehicle_id_list.at(index);
33  if (!simulation_state.IsDormant(ego_actor_id)) {
34  const Buffer &waypoint_buffer = buffer_map.at(ego_actor_id);
35  const SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
36 
37  const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId();
39 
40  const TrafficLightState tl_state = simulation_state.GetTLS(ego_actor_id);
41  const TLS traffic_light_state = tl_state.tl_state;
42  const bool is_at_traffic_light = tl_state.at_traffic_light;
43 
44  // We determine to stop if the current position of the vehicle is not a
45  // junction and there is a red or yellow light.
46  if (is_at_traffic_light &&
47  traffic_light_state != TLS::Green &&
48  traffic_light_state != TLS::Off &&
49  parameters.GetPercentageRunningLight(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
50 
51  traffic_light_hazard = true;
52  }
53  // Handle entry negotiation at non-signalised junction.
54  else if (look_ahead_point->CheckJunction() &&
55  !is_at_traffic_light &&
56  traffic_light_state != TLS::Green &&
57  traffic_light_state != TLS::Off &&
58  parameters.GetPercentageRunningSign(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
59 
60  traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_timestamp);
61  }
62  }
63  output_array.at(index) = traffic_light_hazard;
64 }
65 
66 bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id,
67  cc::Timestamp timestamp) {
68 
69  bool traffic_light_hazard = false;
70 
71  if (vehicle_last_junction.find(ego_actor_id) == vehicle_last_junction.end()) {
72  // Initializing new map entry for the actor with an arbitrary and different junction id.
73  vehicle_last_junction.insert({ego_actor_id, junction_id + 1});
74  }
75 
76  if (vehicle_last_junction.at(ego_actor_id) != junction_id) {
77  vehicle_last_junction.at(ego_actor_id) = junction_id;
78 
79  // Check if the vehicle has an outdated ticket or needs a new one.
80  bool need_to_issue_new_ticket = false;
81  if (vehicle_last_ticket.find(ego_actor_id) == vehicle_last_ticket.end()) {
82 
83  need_to_issue_new_ticket = true;
84  } else {
85 
86  const cc::Timestamp &previous_ticket = vehicle_last_ticket.at(ego_actor_id);
87  const double diff = timestamp.elapsed_seconds - previous_ticket.elapsed_seconds;
89  need_to_issue_new_ticket = true;
90  }
91  }
92 
93  // If new ticket is needed for the vehicle, then query the junction
94  // ticket map and update the map value to the new ticket value.
95  if (need_to_issue_new_ticket) {
96  if (junction_last_ticket.find(junction_id) != junction_last_ticket.end()) {
97 
98  cc::Timestamp &last_ticket = junction_last_ticket.at(junction_id);
99  const double diff = timestamp.elapsed_seconds - last_ticket.elapsed_seconds;
100  if (diff > 0.0) {
102  } else {
104  }
105  } else {
106  cc::Timestamp &new_ticket = timestamp;
108  junction_last_ticket.insert({junction_id, new_ticket});
109  }
110  if (vehicle_last_ticket.find(ego_actor_id) != vehicle_last_ticket.end()) {
111  vehicle_last_ticket.at(ego_actor_id) = junction_last_ticket.at(junction_id);
112  } else {
113  vehicle_last_ticket.insert({ego_actor_id, junction_last_ticket.at(junction_id)});
114  }
115  }
116  }
117 
118  // If current time is behind ticket time, then do not enter junction.
119  const cc::Timestamp current_ticket = vehicle_last_ticket.at(ego_actor_id);
120  const double diff = current_ticket.elapsed_seconds - timestamp.elapsed_seconds;
121  if (diff > 0.0) {
122  traffic_light_hazard = true;
123  }
124 
125  return traffic_light_hazard;
126 }
127 
129  vehicle_last_ticket.erase(actor_id);
130  vehicle_last_junction.erase(actor_id);
131 }
132 
134  vehicle_last_ticket.clear();
135  vehicle_last_junction.clear();
136  junction_last_ticket.clear();
137 }
138 
139 } // namespace traffic_manager
140 } // namespace carla
float GetPercentageRunningLight(const ActorId &actor_id) const
Method to get % to run any traffic light.
Definition: Parameters.cpp:243
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
std::vector< bool > TLFrame
static const double DOUBLE_NO_SIGNAL_PASSTHROUGH_INTERVAL
Definition: Constants.h:107
TrafficLightState GetTLS(const ActorId actor_id) const
This class holds the state of all the vehicles in the simlation.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
std::unordered_map< JunctionID, cc::Timestamp > junction_last_ticket
Map containing the previous time ticket issued for junctions.
std::unordered_map< carla::rpc::ActorId, RandomGenerator > RandomGeneratorMap
std::deque< std::shared_ptr< SimpleWaypoint > > Buffer
TrafficLightStage(const std::vector< ActorId > &vehicle_id_list, const SimulationState &Simulation_state, const BufferMap &buffer_map, const Parameters &parameters, const cc::World &world, TLFrame &output_array, RandomGeneratorMap &random_devices)
void RemoveActor(const ActorId actor_id) override
std::unordered_map< carla::ActorId, Buffer > BufferMap
void Update(const unsigned long index) override
const std::vector< ActorId > & vehicle_id_list
const Timestamp & GetTimestamp() const
Get timestamp of this snapshot.
Definition: WorldSnapshot.h:34
bool IsDormant(const ActorId actor_id) const
carla::road::JuncId JunctionID
carla::ActorId ActorId
float GetPercentageRunningSign(const ActorId &actor_id) const
Method to get % to run any traffic light.
Definition: Parameters.cpp:254
std::unordered_map< ActorId, cc::Timestamp > vehicle_last_ticket
Map containing the time ticket issued for vehicles.
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
bool HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id, cc::Timestamp timestamp)
std::unordered_map< ActorId, JunctionID > vehicle_last_junction
Map containing the previous junction visited by a vehicle.
WorldSnapshot GetSnapshot() const
Return a snapshot of the world at this moment.
Definition: World.cpp:94
double elapsed_seconds
Simulated seconds elapsed since the beginning of the current episode.
Definition: Timestamp.h:33