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  const Buffer &waypoint_buffer = buffer_map.at(ego_actor_id);
34  const SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
35 
36  const JunctionID junction_id = look_ahead_point->GetWaypoint()->GetJunctionId();
38 
39  const TrafficLightState tl_state = simulation_state.GetTLS(ego_actor_id);
40  const TLS traffic_light_state = tl_state.tl_state;
41  const bool is_at_traffic_light = tl_state.at_traffic_light;
42 
43  // We determine to stop if the current position of the vehicle is not a
44  // junction and there is a red or yellow light.
45  if (is_at_traffic_light &&
46  traffic_light_state != TLS::Green &&
47  traffic_light_state != TLS::Off &&
48  parameters.GetPercentageRunningLight(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
49 
50  traffic_light_hazard = true;
51  }
52  // Handle entry negotiation at non-signalised junction.
53  else if (look_ahead_point->CheckJunction() &&
54  !is_at_traffic_light &&
55  traffic_light_state != TLS::Green &&
56  traffic_light_state != TLS::Off &&
57  parameters.GetPercentageRunningSign(ego_actor_id) <= random_devices.at(ego_actor_id).next()) {
58 
59  traffic_light_hazard = HandleNonSignalisedJunction(ego_actor_id, junction_id, current_timestamp);
60  }
61 
62  output_array.at(index) = traffic_light_hazard;
63 }
64 
65 bool TrafficLightStage::HandleNonSignalisedJunction(const ActorId ego_actor_id, const JunctionID junction_id,
66  cc::Timestamp timestamp) {
67 
68  bool traffic_light_hazard = false;
69 
70  if (vehicle_last_junction.find(ego_actor_id) == vehicle_last_junction.end()) {
71  // Initializing new map entry for the actor with
72  // 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
95  // and update the map value to the new ticket value.
96  if (need_to_issue_new_ticket) {
97  if (junction_last_ticket.find(junction_id) != junction_last_ticket.end()) {
98 
99  cc::Timestamp &last_ticket = junction_last_ticket.at(junction_id);
100  const double diff = timestamp.elapsed_seconds - last_ticket.elapsed_seconds;
101  if (diff > 0.0) {
103  } else {
105  }
106  } else {
107  cc::Timestamp &new_ticket = timestamp;
109  junction_last_ticket.insert({junction_id, new_ticket});
110  }
111  if (vehicle_last_ticket.find(ego_actor_id) != vehicle_last_ticket.end()) {
112  vehicle_last_ticket.at(ego_actor_id) = junction_last_ticket.at(junction_id);
113  } else {
114  vehicle_last_ticket.insert({ego_actor_id, junction_last_ticket.at(junction_id)});
115  }
116  }
117  }
118 
119  // If current time is behind ticket time, then do not enter junction.
120  const cc::Timestamp current_ticket = vehicle_last_ticket.at(ego_actor_id);
121  const double diff = current_ticket.elapsed_seconds - timestamp.elapsed_seconds;
122  if (diff > 0.0) {
123  traffic_light_hazard = true;
124  }
125 
126  return traffic_light_hazard;
127 }
128 
130  vehicle_last_ticket.erase(actor_id);
131  vehicle_last_junction.erase(actor_id);
132 }
133 
135  vehicle_last_ticket.clear();
136  vehicle_last_junction.clear();
137  junction_last_ticket.clear();
138 }
139 
140 } // namespace traffic_manager
141 } // namespace carla
float GetPercentageRunningLight(const ActorId &actor_id) const
Method to get % to run any traffic light.
Definition: Parameters.cpp:227
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:103
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:99
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
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:238
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:87
double elapsed_seconds
Simulated seconds elapsed since the beginning of the current episode.
Definition: Timestamp.h:33