CARLA
TrafficLightStage.cpp
Go to the documentation of this file.
1 
4 
6 
7 namespace carla {
8 namespace traffic_manager {
9 
14 
16  const std::vector<ActorId> &vehicle_id_list,
17  const SimulationState &simulation_state,
18  const BufferMap &buffer_map,
19  const Parameters &parameters,
20  const cc::World &world,
21  TLFrame &output_array,
22  RandomGenerator &random_device)
23  : vehicle_id_list(vehicle_id_list),
24  simulation_state(simulation_state),
25  buffer_map(buffer_map),
26  parameters(parameters),
27  world(world),
28  output_array(output_array),
29  random_device(random_device) {}
30 
31 void TrafficLightStage::Update(const unsigned long index) {
32  bool traffic_light_hazard = false;
33 
34  const ActorId ego_actor_id = vehicle_id_list.at(index);
35  if (!simulation_state.IsDormant(ego_actor_id)) {
36  const Buffer &waypoint_buffer = buffer_map.at(ego_actor_id);
37  const SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
38 
39  const Junction junction = look_ahead_point->GetWaypoint()->GetJunction();
41 
42  const TrafficLightState tl_state = simulation_state.GetTLS(ego_actor_id);
43  const TLS traffic_light_state = tl_state.tl_state;
44  const bool is_at_traffic_light = tl_state.at_traffic_light;
45 
46  // We determine to stop if the vehicle found a traffic light in yellow / red.
47  if (is_at_traffic_light &&
48  traffic_light_state != TLS::Green &&
49  traffic_light_state != TLS::Off &&
51 
52  traffic_light_hazard = true;
53  }
54  // The vehicle is currently at a non signalized junction, so handle its priorities.
55  // Don't use the next condition as bounding boxes might switch to green
56  else if (vehicle_last_junction.find(ego_actor_id) != vehicle_last_junction.end())
57  {
58  if (!look_ahead_point->CheckJunction()) {
59  // Very close to the junction exit, forget about it.
60  RemoveActor(ego_actor_id);
61  }
62  else {
63  auto junction_id = junction->GetId();
64  if (exiting_vehicles_map.find(junction_id) != exiting_vehicles_map.end()) {
65  auto& exiting_vehicles = exiting_vehicles_map.at(junction_id);
66  if (std::find(exiting_vehicles.begin(), exiting_vehicles.end(), ego_actor_id) != exiting_vehicles.end()) {
67  // The vehicle is exitting the junction.
68  traffic_light_hazard = false;
69  }
70  else {
71  // Vehicle entering the junction, handle it
72  traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction, waypoint_buffer, current_timestamp);
73  }
74  }
75  }
76  }
77  else if (look_ahead_point->CheckJunction() &&
78  !is_at_traffic_light &&
79  traffic_light_state != TLS::Green &&
81 
82  AddActorToNonSignalisedJunction(ego_actor_id, junction);
83  traffic_light_hazard = true;
84  }
85  }
86  output_array.at(index) = traffic_light_hazard;
87 }
88 
89 void TrafficLightStage::AddActorToNonSignalisedJunction(const ActorId ego_actor_id, const Junction junction) {
90 
91  auto junction_id = junction->GetId();
92 
93  if (entering_vehicles_map.find(junction_id) == entering_vehicles_map.end()) {
94  // Initializing new map entry for the junction with an empty actor deque.
95  std::deque<ActorId> entry_deque;
96  std::deque<ActorId> exit_deque;
97  entering_vehicles_map.insert({junction_id, entry_deque});
98  exiting_vehicles_map.insert({junction_id, exit_deque});
99  }
100 
101  auto& entering_vehicles = entering_vehicles_map.at(junction_id);
102  if (std::find(entering_vehicles.begin(), entering_vehicles.end(), ego_actor_id) == entering_vehicles.end()){
103  // Initializing new actor entry to the junction maps.
104  entering_vehicles.push_back(ego_actor_id);
105  if (vehicle_last_junction.find(ego_actor_id) != vehicle_last_junction.end()) {
106  // The actor was entering another junction, so remove all of its stored data
107  RemoveActor(ego_actor_id);
108  }
109  vehicle_last_junction.insert({ego_actor_id, junction_id});
110  }
111 }
112 
113 
114 bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, const Junction junction,
115  const Buffer &waypoint_buffer, cc::Timestamp timestamp) {
116 
117  bool traffic_light_hazard = false;
118  auto junction_id = junction->GetId();
119 
120  auto& entering_vehicles = entering_vehicles_map.at(junction_id);
121 
122  if (vehicle_stop_time.find(ego_actor_id) == vehicle_stop_time.end()) {
123  // Ensure the vehicle stops before doing anything else
125  vehicle_stop_time.insert({ego_actor_id, timestamp});
126  }
127  traffic_light_hazard = true;
128  }
129 
130  else if (entering_vehicles.front() == ego_actor_id) {
131  auto entry_elapsed_seconds = vehicle_stop_time.at(ego_actor_id).elapsed_seconds;
132  if (timestamp.elapsed_seconds - entry_elapsed_seconds < MINIMUM_STOP_TIME) {
133  // Wait at least the minimum amount of time before entering the junction
134  traffic_light_hazard = true;
135  }
136  else {
137  // Track the first actor until it has passed the mid-point
138  cg::Transform actor_transform = waypoint_buffer.front()->GetTransform();
139  cg::Vector3D forward_vec = actor_transform.GetForwardVector();
140  cg::Vector3D to_center_vec = junction->GetBoundingBox().location - actor_transform.location;
141 
142  if (cg::Math::Dot(forward_vec, to_center_vec) < EXIT_JUNCTION_THRESHOLD) {
143  // Remove it from the entry data, letting the next one enter it
144  entering_vehicles.pop_front();
145  vehicle_stop_time.erase(ego_actor_id);
146  exiting_vehicles_map.at(junction_id).push_back(ego_actor_id);
147  }
148  }
149 
150  } else {
151  // Only one vehicle can be entering the junction, so stop the rest.
152  traffic_light_hazard = true;
153  }
154  return traffic_light_hazard;
155 }
156 
158  if (vehicle_last_junction.find(actor_id) != vehicle_last_junction.end()) {
159  auto junction_id = vehicle_last_junction.at(actor_id);
160 
161  auto& entering_vehicles = entering_vehicles_map.at(junction_id);
162  auto ent_index = std::find(entering_vehicles.begin(), entering_vehicles.end(), actor_id);
163  if (ent_index != entering_vehicles.end()) {
164  entering_vehicles.erase(ent_index);
165  }
166 
167  auto& exiting_vehicles = exiting_vehicles_map.at(junction_id);
168  auto ext_index = std::find(exiting_vehicles.begin(), exiting_vehicles.end(), actor_id);
169  if (ext_index != exiting_vehicles.end()) {
170  exiting_vehicles.erase(ext_index);
171  }
172 
173  if (vehicle_stop_time.find(actor_id) != vehicle_stop_time.end()) {
174  vehicle_stop_time.erase(actor_id);
175  }
176 
177  vehicle_last_junction.erase(actor_id);
178  }
179 }
180 
182  entering_vehicles_map.clear();
183  exiting_vehicles_map.clear();
184  vehicle_last_junction.clear();
185  vehicle_stop_time.clear();
186 }
187 
188 } // namespace traffic_manager
189 } // namespace carla
float GetPercentageRunningLight(const ActorId &actor_id) const
Method to get % to run any traffic light.
Definition: Parameters.cpp:338
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
void AddActorToNonSignalisedJunction(const ActorId ego_actor_id, const Junction junction)
Initialized the vehicle to the non-signalized junction maps.
std::vector< bool > TLFrame
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::deque< std::shared_ptr< SimpleWaypoint > > Buffer
void RemoveActor(const ActorId actor_id) override
std::unordered_map< carla::ActorId, Buffer > BufferMap
Vector3D GetForwardVector() const
float Length() const
Definition: geom/Vector3D.h:49
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::ActorId ActorId
std::unordered_map< JunctionID, std::deque< ActorId > > exiting_vehicles_map
Map containing the vehicles exiting a specific junction.
std::unordered_map< JunctionID, std::deque< ActorId > > entering_vehicles_map
Variables used to handle non signalized junctions.
float GetPercentageRunningSign(const ActorId &actor_id) const
Method to get % to run any traffic light.
Definition: Parameters.cpp:349
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, RandomGenerator &random_device)
static auto Dot(const Vector3D &a, const Vector3D &b)
Definition: Math.h:62
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
std::unordered_map< ActorId, JunctionID > vehicle_last_junction
Map linking the vehicles with their current junction. Used for easy access to the previous two maps...
cg::Vector3D GetVelocity(const ActorId actor_id) const
WorldSnapshot GetSnapshot() const
Return a snapshot of the world at this moment.
Definition: World.cpp:94
bool HandleNonSignalisedJunction(const ActorId ego_actor_id, const Junction junction, const Buffer &waypoint_buffer, cc::Timestamp timestamp)
This controls all vehicle&#39;s interactions at non signalized junctions.
double elapsed_seconds
Simulated seconds elapsed since the beginning of the current episode.
Definition: Timestamp.h:33
std::unordered_map< ActorId, cc::Timestamp > vehicle_stop_time
Map containing the timestamp at which the actor first stopped at a stop sign.
carla::SharedPtr< carla::client::Junction > Junction