CARLA
LocalizationStage.cpp
Go to the documentation of this file.
1 
3 
5 
6 namespace carla {
7 namespace traffic_manager {
8 
9 using namespace constants::PathBufferUpdate;
10 using namespace constants::LaneChange;
11 using namespace constants::WaypointSelection;
12 
14  const std::vector<ActorId> &vehicle_id_list,
15  BufferMap &buffer_map,
16  const SimulationState &simulation_state,
17  TrackTraffic &track_traffic,
18  const LocalMapPtr &local_map,
19  Parameters &parameters,
20  std::vector<ActorId>& marked_for_removal,
21  LocalizationFrame &output_array,
22  RandomGeneratorMap &random_devices)
23  : vehicle_id_list(vehicle_id_list),
24  buffer_map(buffer_map),
25  simulation_state(simulation_state),
26  track_traffic(track_traffic),
27  local_map(local_map),
28  parameters(parameters),
29  marked_for_removal(marked_for_removal),
30  output_array(output_array),
31  random_devices(random_devices) {}
32 
33 void LocalizationStage::Update(const unsigned long index) {
34 
35  const ActorId actor_id = vehicle_id_list.at(index);
36  const cg::Location vehicle_location = simulation_state.GetLocation(actor_id);
37  const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
38  const cg::Vector3D vehicle_velocity_vector = simulation_state.GetVelocity(actor_id);
39  const float vehicle_speed = vehicle_velocity_vector.Length();
40 
41  // Speed dependent waypoint horizon length.
42  float horizon_length = vehicle_speed * HORIZON_RATE + MINIMUM_HORIZON_LENGTH;
43  const float horizon_square = SQUARE(horizon_length);
44 
45  if (buffer_map.find(actor_id) == buffer_map.end()) {
46  buffer_map.insert({actor_id, Buffer()});
47  }
48  Buffer &waypoint_buffer = buffer_map.at(actor_id);
49 
50  // Clear buffer if vehicle is too far from the first waypoint in the buffer.
51  if (!waypoint_buffer.empty() &&
52  cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(),
53  vehicle_location) > SQUARE(MAX_START_DISTANCE)) {
54 
55  auto number_of_pops = waypoint_buffer.size();
56  for (uint64_t j = 0u; j < number_of_pops; ++j) {
57  PopWaypoint(actor_id, track_traffic, waypoint_buffer);
58  }
59  }
60 
61  bool is_at_junction_entrance = false;
62  if (!waypoint_buffer.empty()) {
63  // Purge passed waypoints.
64  float dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation());
65  while (dot_product <= 0.0f && !waypoint_buffer.empty()) {
66  PopWaypoint(actor_id, track_traffic, waypoint_buffer);
67  if (!waypoint_buffer.empty()) {
68  dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation());
69  }
70  }
71 
72  if (!waypoint_buffer.empty()) {
73  // Determine if the vehicle is at the entrance of a junction.
74  SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
75  SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
76  bool front_waypoint_junction = front_waypoint->CheckJunction();
77  is_at_junction_entrance = !front_waypoint_junction && look_ahead_point->CheckJunction();
78  if (!is_at_junction_entrance) {
79  std::vector<SimpleWaypointPtr> last_passed_waypoints = front_waypoint->GetPreviousWaypoint();
80  if (last_passed_waypoints.size() == 1) {
81  is_at_junction_entrance = !last_passed_waypoints.front()->CheckJunction() && front_waypoint_junction;
82  }
83  }
84  if (is_at_junction_entrance
85  // Exception for roundabout in Town03.
86  && local_map->GetMapName() == "Carla/Maps/Town03"
87  && vehicle_location.SquaredLength() < SQUARE(30)) {
88  is_at_junction_entrance = false;
89  }
90  }
91 
92  // Purge waypoints too far from the front of the buffer.
93  while (!is_at_junction_entrance
94  && !waypoint_buffer.empty()
95  && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square) {
96  PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
97  }
98  }
99 
100  // Initializing buffer if it is empty.
101  if (waypoint_buffer.empty()) {
102  SimpleWaypointPtr closest_waypoint = local_map->GetWaypoint(vehicle_location);
103  PushWaypoint(actor_id, track_traffic, waypoint_buffer, closest_waypoint);
104  }
105 
106  // Assign a lane change.
107  const ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(actor_id);
108  bool force_lane_change = lane_change_info.change_lane;
109  bool lane_change_direction = lane_change_info.direction;
110 
111  if (!force_lane_change) {
112  float perc_keep_right = parameters.GetKeepRightPercentage(actor_id);
113  if (perc_keep_right >= 0.0f && perc_keep_right >= random_devices.at(actor_id).next()) {
114  force_lane_change = true;
115  lane_change_direction = true;
116  }
117  }
118 
119  const SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
120  const float lane_change_distance = SQUARE(std::max(10.0f * vehicle_speed, INTER_LANE_CHANGE_DISTANCE));
121 
122  bool recently_not_executed_lane_change = last_lane_change_location.find(actor_id) == last_lane_change_location.end();
123  bool done_with_previous_lane_change = true;
124  if (!recently_not_executed_lane_change) {
125  float distance_frm_previous = cg::Math::DistanceSquared(last_lane_change_location.at(actor_id), vehicle_location);
126  done_with_previous_lane_change = distance_frm_previous > lane_change_distance;
127  }
128  bool auto_or_force_lane_change = parameters.GetAutoLaneChange(actor_id) || force_lane_change;
129  bool front_waypoint_not_junction = !front_waypoint->CheckJunction();
130 
131  if (auto_or_force_lane_change
132  && front_waypoint_not_junction
133  && (recently_not_executed_lane_change || done_with_previous_lane_change)) {
134 
135  SimpleWaypointPtr change_over_point = AssignLaneChange(actor_id, vehicle_location, vehicle_speed,
136  force_lane_change, lane_change_direction);
137 
138  if (change_over_point != nullptr) {
139  if (last_lane_change_location.find(actor_id) != last_lane_change_location.end()) {
140  last_lane_change_location.at(actor_id) = vehicle_location;
141  } else {
142  last_lane_change_location.insert({actor_id, vehicle_location});
143  }
144  auto number_of_pops = waypoint_buffer.size();
145  for (uint64_t j = 0u; j < number_of_pops; ++j) {
146  PopWaypoint(actor_id, track_traffic, waypoint_buffer);
147  }
148  PushWaypoint(actor_id, track_traffic, waypoint_buffer, change_over_point);
149  }
150  }
151 
152  // Populating the buffer.
153  while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
154 
155  SimpleWaypointPtr furthest_waypoint = waypoint_buffer.back();
156  std::vector<SimpleWaypointPtr> next_waypoints = furthest_waypoint->GetNextWaypoint();
157  uint64_t selection_index = 0u;
158  // Pseudo-randomized path selection if found more than one choice.
159  if (next_waypoints.size() > 1) {
160  double r_sample = random_devices.at(actor_id).next();
161  selection_index = static_cast<uint64_t>(r_sample*next_waypoints.size()*0.01);
162  } else if (next_waypoints.size() == 0) {
163  if (!parameters.GetOSMMode()) {
164  std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
165  }
166  marked_for_removal.push_back(actor_id);
167  break;
168  }
169  SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
170  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
171  }
172 
173  ExtendAndFindSafeSpace(actor_id, is_at_junction_entrance, waypoint_buffer);
174 
175  // Editing output array
176  LocalizationData &output = output_array.at(index);
177  output.is_at_junction_entrance = is_at_junction_entrance;
178 
179  if (is_at_junction_entrance) {
180  const SimpleWaypointPair &safe_space_end_points = vehicles_at_junction_entrance.at(actor_id);
181  output.junction_end_point = safe_space_end_points.first;
182  output.safe_point = safe_space_end_points.second;
183  } else {
184  output.junction_end_point = nullptr;
185  output.safe_point = nullptr;
186  }
187 
188  // Updating geodesic grid position for actor.
189  track_traffic.UpdateGridPosition(actor_id, waypoint_buffer);
190 }
191 
193  const bool is_at_junction_entrance,
194  Buffer &waypoint_buffer) {
195 
196  SimpleWaypointPtr junction_end_point = nullptr;
197  SimpleWaypointPtr safe_point_after_junction = nullptr;
198 
199  if (is_at_junction_entrance
201 
202  bool entered_junction = false;
203  bool past_junction = false;
204  bool safe_point_found = false;
205  SimpleWaypointPtr current_waypoint = nullptr;
206  SimpleWaypointPtr junction_begin_point = nullptr;
207  float safe_distance_squared = SQUARE(SAFE_DISTANCE_AFTER_JUNCTION);
208 
209  // Scanning existing buffer points.
210  for (unsigned long i = 0u; i < waypoint_buffer.size() && !safe_point_found; ++i) {
211  current_waypoint = waypoint_buffer.at(i);
212  if (!entered_junction && current_waypoint->CheckJunction()) {
213  entered_junction = true;
214  junction_begin_point = current_waypoint;
215  }
216  if (entered_junction && !past_junction && !current_waypoint->CheckJunction()) {
217  past_junction = true;
218  junction_end_point = current_waypoint;
219  }
220  if (past_junction && junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared) {
221  safe_point_found = true;
222  safe_point_after_junction = current_waypoint;
223  }
224  }
225 
226  // Extend buffer if safe point not found.
227  if (!safe_point_found) {
228  bool abort = false;
229 
230  while (!past_junction && !abort) {
231  NodeList next_waypoints = current_waypoint->GetNextWaypoint();
232  if (!next_waypoints.empty()) {
233  current_waypoint = next_waypoints.front();
234  PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
235  if (!current_waypoint->CheckJunction()) {
236  past_junction = true;
237  junction_end_point = current_waypoint;
238  }
239  } else {
240  abort = true;
241  }
242  }
243 
244  while (!safe_point_found && !abort) {
245  std::vector<SimpleWaypointPtr> next_waypoints = current_waypoint->GetNextWaypoint();
246  if ((junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared)
247  || next_waypoints.size() > 1
248  || current_waypoint->CheckJunction()) {
249 
250  safe_point_found = true;
251  safe_point_after_junction = current_waypoint;
252  } else {
253  if (!next_waypoints.empty()) {
254  current_waypoint = next_waypoints.front();
255  PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
256  } else {
257  abort = true;
258  }
259  }
260  }
261  }
262 
263  if (junction_end_point != nullptr &&
264  safe_point_after_junction != nullptr &&
265  junction_begin_point->DistanceSquared(junction_end_point) < SQUARE(MIN_JUNCTION_LENGTH)) {
266 
267  junction_end_point = nullptr;
268  safe_point_after_junction = nullptr;
269  }
270 
271  vehicles_at_junction_entrance.insert({actor_id, {junction_end_point, safe_point_after_junction}});
272  }
273  else if (!is_at_junction_entrance
275 
276  vehicles_at_junction_entrance.erase(actor_id);
277  }
278 }
279 
281  last_lane_change_location.erase(actor_id);
282  vehicles_at_junction.erase(actor_id);
283 }
284 
287  vehicles_at_junction.clear();
288 }
289 
291  const cg::Location vehicle_location,
292  const float vehicle_speed,
293  bool force, bool direction) {
294 
295  // Waypoint representing the new starting point for the waypoint buffer
296  // due to lane change. Remains nullptr if lane change not viable.
297  SimpleWaypointPtr change_over_point = nullptr;
298 
299  // Retrieve waypoint buffer for current vehicle.
300  const Buffer &waypoint_buffer = buffer_map.at(actor_id);
301 
302  // Check buffer is not empty.
303  if (!waypoint_buffer.empty()) {
304  // Get the left and right waypoints for the current closest waypoint.
305  const SimpleWaypointPtr &current_waypoint = waypoint_buffer.front();
306  const SimpleWaypointPtr left_waypoint = current_waypoint->GetLeftWaypoint();
307  const SimpleWaypointPtr right_waypoint = current_waypoint->GetRightWaypoint();
308 
309  // Retrieve vehicles with overlapping waypoint buffers with current vehicle.
310  const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
311 
312  // Find immediate in-lane obstacle and check if any are too close to initiate lane change.
313  bool obstacle_too_close = false;
314  float minimum_squared_distance = std::numeric_limits<float>::infinity();
315  ActorId obstacle_actor_id = 0u;
316  for (auto i = blocking_vehicles.begin();
317  i != blocking_vehicles.end() && !obstacle_too_close && !force;
318  ++i) {
319  const ActorId &other_actor_id = *i;
320  // Find vehicle in buffer map and check if it's buffer is not empty.
321  if (buffer_map.find(other_actor_id) != buffer_map.end() && !buffer_map.at(other_actor_id).empty()) {
322  const Buffer &other_buffer = buffer_map.at(other_actor_id);
323  const SimpleWaypointPtr &other_current_waypoint = other_buffer.front();
324  const cg::Location other_location = other_current_waypoint->GetLocation();
325 
326  const cg::Vector3D reference_heading = current_waypoint->GetForwardVector();
327  cg::Vector3D reference_to_other = other_location - current_waypoint->GetLocation();
328  const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector();
329 
330  WaypointPtr current_raw_waypoint = current_waypoint->GetWaypoint();
331  WaypointPtr other_current_raw_waypoint = other_current_waypoint->GetWaypoint();
332  // Check both vehicles are not in junction,
333  // Check if the other vehicle is in front of the current vehicle,
334  // Check if the two vehicles have acceptable angular deviation between their headings.
335  if (!current_waypoint->CheckJunction()
336  && !other_current_waypoint->CheckJunction()
337  && other_current_raw_waypoint->GetRoadId() == current_raw_waypoint->GetRoadId()
338  && other_current_raw_waypoint->GetLaneId() == current_raw_waypoint->GetLaneId()
339  && cg::Math::Dot(reference_heading, reference_to_other) > 0.0f
340  && cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) {
341  float squared_distance = cg::Math::DistanceSquared(vehicle_location, other_location);
342  // Abort if the obstacle is too close.
343  if (squared_distance > SQUARE(MINIMUM_LANE_CHANGE_DISTANCE)) {
344  // Remember if the new vehicle is closer.
345  if (squared_distance < minimum_squared_distance && squared_distance < SQUARE(MAXIMUM_LANE_OBSTACLE_DISTANCE)) {
346  minimum_squared_distance = squared_distance;
347  obstacle_actor_id = other_actor_id;
348  }
349  } else {
350  obstacle_too_close = true;
351  }
352  }
353  }
354  }
355 
356  // If a valid immediate obstacle found.
357  if (!obstacle_too_close && obstacle_actor_id != 0u && !force) {
358  const Buffer &other_buffer = buffer_map.at(obstacle_actor_id);
359  const SimpleWaypointPtr &other_current_waypoint = other_buffer.front();
360  const auto other_neighbouring_lanes = {other_current_waypoint->GetLeftWaypoint(),
361  other_current_waypoint->GetRightWaypoint()};
362 
363  // Flags reflecting whether adjacent lanes are free near the obstacle.
364  bool distant_left_lane_free = false;
365  bool distant_right_lane_free = false;
366 
367  // Check if the neighbouring lanes near the obstructing vehicle are free of other vehicles.
368  bool left_right = true;
369  for (auto &candidate_lane_wp : other_neighbouring_lanes) {
370  if (candidate_lane_wp != nullptr &&
371  track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0) {
372 
373  if (left_right)
374  distant_left_lane_free = true;
375  else
376  distant_right_lane_free = true;
377  }
378  left_right = !left_right;
379  }
380 
381  // Based on what lanes are free near the obstacle,
382  // find the change over point with no vehicles passing through them.
383  if (distant_right_lane_free && right_waypoint != nullptr
384  && track_traffic.GetPassingVehicles(right_waypoint->GetId()).size() == 0) {
385  change_over_point = right_waypoint;
386  } else if (distant_left_lane_free && left_waypoint != nullptr
387  && track_traffic.GetPassingVehicles(left_waypoint->GetId()).size() == 0) {
388  change_over_point = left_waypoint;
389  }
390  } else if (force) {
391  if (direction && right_waypoint != nullptr) {
392  change_over_point = right_waypoint;
393  } else if (!direction && left_waypoint != nullptr) {
394  change_over_point = left_waypoint;
395  }
396  }
397 
398  if (change_over_point != nullptr) {
399  const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, 3.0f, 20.0f);
400  const auto starting_point = change_over_point;
401  while (change_over_point->DistanceSquared(starting_point) < SQUARE(change_over_distance) &&
402  !change_over_point->CheckJunction()) {
403  change_over_point = change_over_point->GetNextWaypoint()[0];
404  }
405  }
406  }
407 
408  return change_over_point;
409 }
410 
411 } // namespace traffic_manager
412 } // namespace carla
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
static const float MAXIMUM_LANE_OBSTACLE_DISTANCE
Definition: Constants.h:63
void PopWaypoint(ActorId actor_id, TrackTraffic &track_traffic, Buffer &buffer, bool front_or_back)
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE
Definition: Constants.h:64
std::unordered_map< ActorId, SimpleWaypointPair > vehicles_at_junction_entrance
float DeviationDotProduct(const cg::Location &reference_location, const cg::Vector3D &heading_vector, const cg::Location &target_location)
Returns the dot product between the vehicle&#39;s heading vector and the vector along the direction to th...
bool GetOSMMode() const
Method to get Open Street Map mode.
Definition: Parameters.cpp:308
float SquaredLength() const
Definition: geom/Vector3D.h:45
static T Clamp(T a, T min=T(0), T max=T(1))
Definition: Math.h:49
void ExtendAndFindSafeSpace(const ActorId actor_id, const bool is_at_junction_entrance, Buffer &waypoint_buffer)
void UpdateGridPosition(const ActorId actor_id, const Buffer &buffer)
bool GetAutoLaneChange(const ActorId &actor_id) const
Method to query auto lane change rule for a vehicle.
Definition: Parameters.cpp:220
std::shared_ptr< InMemoryMap > LocalMapPtr
Definition: ALSM.h:35
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< carla::rpc::ActorId, RandomGenerator > RandomGeneratorMap
LocalizationStage(const std::vector< ActorId > &vehicle_id_list, BufferMap &buffer_map, const SimulationState &simulation_state, TrackTraffic &track_traffic, const LocalMapPtr &local_map, Parameters &parameters, std::vector< ActorId > &marked_for_removal, LocalizationFrame &output_array, RandomGeneratorMap &random_devices)
void PushWaypoint(ActorId actor_id, TrackTraffic &track_traffic, Buffer &buffer, SimpleWaypointPtr &waypoint)
std::deque< std::shared_ptr< SimpleWaypoint > > Buffer
const std::vector< ActorId > & vehicle_id_list
SimpleWaypointPtr AssignLaneChange(const ActorId actor_id, const cg::Location vehicle_location, const float vehicle_speed, bool force, bool direction)
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:66
std::unordered_map< carla::ActorId, Buffer > BufferMap
float Length() const
Definition: geom/Vector3D.h:49
carla::SharedPtr< cc::Waypoint > WaypointPtr
Definition: InMemoryMap.h:38
cg::Location GetLocation(const ActorId actor_id) const
float GetKeepRightPercentage(const ActorId &actor_id)
Method to query percentage probability of keep right rule for a vehicle.
Definition: Parameters.cpp:207
#define SQUARE(a)
This file contains various constants used in traffic manager arranged into sensible namespaces for re...
Definition: Constants.h:13
carla::ActorId ActorId
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const
std::vector< SimpleWaypointPtr > NodeList
Definition: InMemoryMap.h:40
void RemoveActor(const ActorId actor_id) override
void Update(const unsigned long index) override
cg::Vector3D GetHeading(const ActorId actor_id) const
static auto Dot(const Vector3D &a, const Vector3D &b)
Definition: Math.h:58
std::vector< LocalizationData > LocalizationFrame
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
std::pair< SimpleWaypointPtr, SimpleWaypointPtr > SimpleWaypointPair
ActorIdSet GetPassingVehicles(uint64_t waypoint_id) const
cg::Vector3D GetVelocity(const ActorId actor_id) const
ChangeLaneInfo GetForceLaneChange(const ActorId &actor_id)
Method to query lane change command for a vehicle.
Definition: Parameters.cpp:194