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 using namespace constants::SpeedThreshold;
13 using namespace constants::Collision;
14 using namespace constants::MotionPlan;
15 
17  const std::vector<ActorId> &vehicle_id_list,
18  BufferMap &buffer_map,
19  const SimulationState &simulation_state,
20  TrackTraffic &track_traffic,
21  const LocalMapPtr &local_map,
22  Parameters &parameters,
23  std::vector<ActorId>& marked_for_removal,
24  LocalizationFrame &output_array,
25  RandomGeneratorMap &random_devices)
26  : vehicle_id_list(vehicle_id_list),
27  buffer_map(buffer_map),
28  simulation_state(simulation_state),
29  track_traffic(track_traffic),
30  local_map(local_map),
31  parameters(parameters),
32  marked_for_removal(marked_for_removal),
33  output_array(output_array),
34  random_devices(random_devices){}
35 
36 void LocalizationStage::Update(const unsigned long index) {
37 
38  const ActorId actor_id = vehicle_id_list.at(index);
39  const cg::Location vehicle_location = simulation_state.GetLocation(actor_id);
40  const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
41  const cg::Vector3D vehicle_velocity_vector = simulation_state.GetVelocity(actor_id);
42  const float vehicle_speed = vehicle_velocity_vector.Length();
43 
44  // Speed dependent waypoint horizon length.
45  float horizon_length = std::max(vehicle_speed * HORIZON_RATE, MINIMUM_HORIZON_LENGTH);
46  if (vehicle_speed > HIGHWAY_SPEED) {
47  horizon_length = std::max(vehicle_speed * HIGH_SPEED_HORIZON_RATE, MINIMUM_HORIZON_LENGTH);
48  }
49  const float horizon_square = SQUARE(horizon_length);
50 
51  if (buffer_map.find(actor_id) == buffer_map.end()) {
52  buffer_map.insert({actor_id, Buffer()});
53  }
54  Buffer &waypoint_buffer = buffer_map.at(actor_id);
55 
56  // Clear buffer if vehicle is too far from the first waypoint in the buffer.
57  if (!waypoint_buffer.empty() &&
58  cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(),
59  vehicle_location) > SQUARE(MAX_START_DISTANCE)) {
60 
61  auto number_of_pops = waypoint_buffer.size();
62  for (uint64_t j = 0u; j < number_of_pops; ++j) {
63  PopWaypoint(actor_id, track_traffic, waypoint_buffer);
64  }
65  }
66 
67  bool is_at_junction_entrance = false;
68  if (!waypoint_buffer.empty()) {
69  // Purge passed waypoints.
70  float dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation());
71  while (dot_product <= 0.0f && !waypoint_buffer.empty()) {
72  PopWaypoint(actor_id, track_traffic, waypoint_buffer);
73  if (!waypoint_buffer.empty()) {
74  dot_product = DeviationDotProduct(vehicle_location, heading_vector, waypoint_buffer.front()->GetLocation());
75  }
76  }
77 
78  if (!waypoint_buffer.empty()) {
79  // Determine if the vehicle is at the entrance of a junction.
80  SimpleWaypointPtr look_ahead_point = GetTargetWaypoint(waypoint_buffer, JUNCTION_LOOK_AHEAD).first;
81  SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
82  bool front_waypoint_junction = front_waypoint->CheckJunction();
83  is_at_junction_entrance = !front_waypoint_junction && look_ahead_point->CheckJunction();
84  if (!is_at_junction_entrance) {
85  std::vector<SimpleWaypointPtr> last_passed_waypoints = front_waypoint->GetPreviousWaypoint();
86  if (last_passed_waypoints.size() == 1) {
87  is_at_junction_entrance = !last_passed_waypoints.front()->CheckJunction() && front_waypoint_junction;
88  }
89  }
90  if (is_at_junction_entrance
91  // Exception for roundabout in Town03.
92  && local_map->GetMapName() == "Carla/Maps/Town03"
93  && vehicle_location.SquaredLength() < SQUARE(30)) {
94  is_at_junction_entrance = false;
95  }
96  }
97 
98  // Purge waypoints too far from the front of the buffer, but not if it has reached a junction.
99  while (!is_at_junction_entrance
100  && !waypoint_buffer.empty()
101  && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square + horizon_square
102  && !waypoint_buffer.back()->CheckJunction()) {
103  PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
104  }
105  }
106 
107  // Initializing buffer if it is empty.
108  if (waypoint_buffer.empty()) {
109  SimpleWaypointPtr closest_waypoint = local_map->GetWaypoint(vehicle_location);
110  PushWaypoint(actor_id, track_traffic, waypoint_buffer, closest_waypoint);
111  }
112 
113  // Assign a lane change.
114  const ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(actor_id);
115  bool force_lane_change = lane_change_info.change_lane;
116  bool lane_change_direction = lane_change_info.direction;
117 
118  // Apply parameters for keep right rule and random lane changes.
119  if (!force_lane_change && vehicle_speed > MIN_LANE_CHANGE_SPEED){
120  const float perc_keep_right = parameters.GetKeepRightPercentage(actor_id);
121  const float perc_random_leftlanechange = parameters.GetRandomLeftLaneChangePercentage(actor_id);
122  const float perc_random_rightlanechange = parameters.GetRandomRightLaneChangePercentage(actor_id);
123  const bool is_keep_right = perc_keep_right > random_devices.at(actor_id).next();
124  const bool is_random_left_change = perc_random_leftlanechange >= random_devices.at(actor_id).next();
125  const bool is_random_right_change = perc_random_rightlanechange >= random_devices.at(actor_id).next();
126 
127  // Determine which of the parameters we should apply.
128  if (is_keep_right || is_random_right_change) {
129  force_lane_change = true;
130  lane_change_direction = true;
131  }
132  if (is_random_left_change) {
133  if (!force_lane_change) {
134  force_lane_change = true;
135  lane_change_direction = false;
136  } else {
137  // Both a left and right lane changes are forced. Choose between one of them.
138  lane_change_direction = FIFTYPERC > random_devices.at(actor_id).next();
139  }
140  }
141  }
142 
143  const SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
144  const float lane_change_distance = SQUARE(std::max(10.0f * vehicle_speed, INTER_LANE_CHANGE_DISTANCE));
145 
146  bool recently_not_executed_lane_change = last_lane_change_swpt.find(actor_id) == last_lane_change_swpt.end();
147  bool done_with_previous_lane_change = true;
148  if (!recently_not_executed_lane_change) {
149  float distance_frm_previous = cg::Math::DistanceSquared(last_lane_change_swpt.at(actor_id)->GetLocation(), vehicle_location);
150  done_with_previous_lane_change = distance_frm_previous > lane_change_distance;
151  }
152  bool auto_or_force_lane_change = parameters.GetAutoLaneChange(actor_id) || force_lane_change;
153  bool front_waypoint_not_junction = !front_waypoint->CheckJunction();
154 
155  if (auto_or_force_lane_change
156  && front_waypoint_not_junction
157  && (recently_not_executed_lane_change || done_with_previous_lane_change)) {
158 
159  SimpleWaypointPtr change_over_point = AssignLaneChange(actor_id, vehicle_location, vehicle_speed,
160  force_lane_change, lane_change_direction);
161 
162  if (change_over_point != nullptr) {
163  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
164  last_lane_change_swpt.at(actor_id) = change_over_point;
165  } else {
166  last_lane_change_swpt.insert({actor_id, change_over_point});
167  }
168  auto number_of_pops = waypoint_buffer.size();
169  for (uint64_t j = 0u; j < number_of_pops; ++j) {
170  PopWaypoint(actor_id, track_traffic, waypoint_buffer);
171  }
172  PushWaypoint(actor_id, track_traffic, waypoint_buffer, change_over_point);
173  }
174  }
175 
176  Path imported_path = parameters.GetCustomPath(actor_id);
177  Route imported_actions = parameters.GetImportedRoute(actor_id);
178  // We are effectively importing a path.
179  if (!imported_path.empty()) {
180 
181  ImportPath(imported_path, waypoint_buffer, actor_id, horizon_square);
182 
183  } else if (!imported_actions.empty()) {
184 
185  ImportRoute(imported_actions, waypoint_buffer, actor_id, horizon_square);
186 
187  }
188 
189  // Populating the buffer through randomly chosen waypoints.
190  else {
191  while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
192  SimpleWaypointPtr furthest_waypoint = waypoint_buffer.back();
193  std::vector<SimpleWaypointPtr> next_waypoints = furthest_waypoint->GetNextWaypoint();
194  uint64_t selection_index = 0u;
195  // Pseudo-randomized path selection if found more than one choice.
196  if (next_waypoints.size() > 1) {
197  double r_sample = random_devices.at(actor_id).next();
198  selection_index = static_cast<uint64_t>(r_sample*next_waypoints.size()*0.01);
199  } else if (next_waypoints.size() == 0) {
200  if (!parameters.GetOSMMode()) {
201  std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
202  }
203  marked_for_removal.push_back(actor_id);
204  break;
205  }
206  SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
207  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
208  }
209  }
210  ExtendAndFindSafeSpace(actor_id, is_at_junction_entrance, waypoint_buffer);
211 
212  // Editing output array
213  LocalizationData &output = output_array.at(index);
214  output.is_at_junction_entrance = is_at_junction_entrance;
215 
216  if (is_at_junction_entrance) {
217  const SimpleWaypointPair &safe_space_end_points = vehicles_at_junction_entrance.at(actor_id);
218  output.junction_end_point = safe_space_end_points.first;
219  output.safe_point = safe_space_end_points.second;
220  } else {
221  output.junction_end_point = nullptr;
222  output.safe_point = nullptr;
223  }
224 
225  // Updating geodesic grid position for actor.
226  track_traffic.UpdateGridPosition(actor_id, waypoint_buffer);
227 }
228 
230  const bool is_at_junction_entrance,
231  Buffer &waypoint_buffer) {
232 
233  SimpleWaypointPtr junction_end_point = nullptr;
234  SimpleWaypointPtr safe_point_after_junction = nullptr;
235 
236  if (is_at_junction_entrance
238 
239  bool entered_junction = false;
240  bool past_junction = false;
241  bool safe_point_found = false;
242  SimpleWaypointPtr current_waypoint = nullptr;
243  SimpleWaypointPtr junction_begin_point = nullptr;
244  float safe_distance_squared = SQUARE(SAFE_DISTANCE_AFTER_JUNCTION);
245 
246  // Scanning existing buffer points.
247  for (unsigned long i = 0u; i < waypoint_buffer.size() && !safe_point_found; ++i) {
248  current_waypoint = waypoint_buffer.at(i);
249  if (!entered_junction && current_waypoint->CheckJunction()) {
250  entered_junction = true;
251  junction_begin_point = current_waypoint;
252  }
253  if (entered_junction && !past_junction && !current_waypoint->CheckJunction()) {
254  past_junction = true;
255  junction_end_point = current_waypoint;
256  }
257  if (past_junction && junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared) {
258  safe_point_found = true;
259  safe_point_after_junction = current_waypoint;
260  }
261  }
262 
263  // Extend buffer if safe point not found.
264  if (!safe_point_found) {
265  bool abort = false;
266 
267  while (!past_junction && !abort) {
268  NodeList next_waypoints = current_waypoint->GetNextWaypoint();
269  if (!next_waypoints.empty()) {
270  current_waypoint = next_waypoints.front();
271  PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
272  if (!current_waypoint->CheckJunction()) {
273  past_junction = true;
274  junction_end_point = current_waypoint;
275  }
276  } else {
277  abort = true;
278  }
279  }
280 
281  while (!safe_point_found && !abort) {
282  std::vector<SimpleWaypointPtr> next_waypoints = current_waypoint->GetNextWaypoint();
283  if ((junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared)
284  || next_waypoints.size() > 1
285  || current_waypoint->CheckJunction()) {
286 
287  safe_point_found = true;
288  safe_point_after_junction = current_waypoint;
289  } else {
290  if (!next_waypoints.empty()) {
291  current_waypoint = next_waypoints.front();
292  PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
293  } else {
294  abort = true;
295  }
296  }
297  }
298  }
299 
300  if (junction_end_point != nullptr &&
301  safe_point_after_junction != nullptr &&
302  junction_begin_point->DistanceSquared(junction_end_point) < SQUARE(MIN_JUNCTION_LENGTH)) {
303 
304  junction_end_point = nullptr;
305  safe_point_after_junction = nullptr;
306  }
307 
308  vehicles_at_junction_entrance.insert({actor_id, {junction_end_point, safe_point_after_junction}});
309  }
310  else if (!is_at_junction_entrance
312 
313  vehicles_at_junction_entrance.erase(actor_id);
314  }
315 }
316 
318  last_lane_change_swpt.erase(actor_id);
319  vehicles_at_junction.erase(actor_id);
320 }
321 
323  last_lane_change_swpt.clear();
324  vehicles_at_junction.clear();
325 }
326 
328  const cg::Location vehicle_location,
329  const float vehicle_speed,
330  bool force, bool direction) {
331 
332  // Waypoint representing the new starting point for the waypoint buffer
333  // due to lane change. Remains nullptr if lane change not viable.
334  SimpleWaypointPtr change_over_point = nullptr;
335 
336  // Retrieve waypoint buffer for current vehicle.
337  const Buffer &waypoint_buffer = buffer_map.at(actor_id);
338 
339  // Check buffer is not empty.
340  if (!waypoint_buffer.empty()) {
341  // Get the left and right waypoints for the current closest waypoint.
342  const SimpleWaypointPtr &current_waypoint = waypoint_buffer.front();
343  const SimpleWaypointPtr left_waypoint = current_waypoint->GetLeftWaypoint();
344  const SimpleWaypointPtr right_waypoint = current_waypoint->GetRightWaypoint();
345 
346  // Retrieve vehicles with overlapping waypoint buffers with current vehicle.
347  const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
348 
349  // Find immediate in-lane obstacle and check if any are too close to initiate lane change.
350  bool obstacle_too_close = false;
351  float minimum_squared_distance = std::numeric_limits<float>::infinity();
352  ActorId obstacle_actor_id = 0u;
353  for (auto i = blocking_vehicles.begin();
354  i != blocking_vehicles.end() && !obstacle_too_close && !force;
355  ++i) {
356  const ActorId &other_actor_id = *i;
357  // Find vehicle in buffer map and check if it's buffer is not empty.
358  if (buffer_map.find(other_actor_id) != buffer_map.end() && !buffer_map.at(other_actor_id).empty()) {
359  const Buffer &other_buffer = buffer_map.at(other_actor_id);
360  const SimpleWaypointPtr &other_current_waypoint = other_buffer.front();
361  const cg::Location other_location = other_current_waypoint->GetLocation();
362 
363  const cg::Vector3D reference_heading = current_waypoint->GetForwardVector();
364  cg::Vector3D reference_to_other = other_location - current_waypoint->GetLocation();
365  const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector();
366 
367  WaypointPtr current_raw_waypoint = current_waypoint->GetWaypoint();
368  WaypointPtr other_current_raw_waypoint = other_current_waypoint->GetWaypoint();
369  // Check both vehicles are not in junction,
370  // Check if the other vehicle is in front of the current vehicle,
371  // Check if the two vehicles have acceptable angular deviation between their headings.
372  if (!current_waypoint->CheckJunction()
373  && !other_current_waypoint->CheckJunction()
374  && other_current_raw_waypoint->GetRoadId() == current_raw_waypoint->GetRoadId()
375  && other_current_raw_waypoint->GetLaneId() == current_raw_waypoint->GetLaneId()
376  && cg::Math::Dot(reference_heading, reference_to_other) > 0.0f
377  && cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) {
378  float squared_distance = cg::Math::DistanceSquared(vehicle_location, other_location);
379  // Abort if the obstacle is too close.
380  if (squared_distance > SQUARE(MINIMUM_LANE_CHANGE_DISTANCE)) {
381  // Remember if the new vehicle is closer.
382  if (squared_distance < minimum_squared_distance && squared_distance < SQUARE(MAXIMUM_LANE_OBSTACLE_DISTANCE)) {
383  minimum_squared_distance = squared_distance;
384  obstacle_actor_id = other_actor_id;
385  }
386  } else {
387  obstacle_too_close = true;
388  }
389  }
390  }
391  }
392 
393  // If a valid immediate obstacle found.
394  if (!obstacle_too_close && obstacle_actor_id != 0u && !force) {
395  const Buffer &other_buffer = buffer_map.at(obstacle_actor_id);
396  const SimpleWaypointPtr &other_current_waypoint = other_buffer.front();
397  const auto other_neighbouring_lanes = {other_current_waypoint->GetLeftWaypoint(),
398  other_current_waypoint->GetRightWaypoint()};
399 
400  // Flags reflecting whether adjacent lanes are free near the obstacle.
401  bool distant_left_lane_free = false;
402  bool distant_right_lane_free = false;
403 
404  // Check if the neighbouring lanes near the obstructing vehicle are free of other vehicles.
405  bool left_right = true;
406  for (auto &candidate_lane_wp : other_neighbouring_lanes) {
407  if (candidate_lane_wp != nullptr &&
408  track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0) {
409 
410  if (left_right)
411  distant_left_lane_free = true;
412  else
413  distant_right_lane_free = true;
414  }
415  left_right = !left_right;
416  }
417 
418  // Based on what lanes are free near the obstacle,
419  // find the change over point with no vehicles passing through them.
420  if (distant_right_lane_free && right_waypoint != nullptr
421  && track_traffic.GetPassingVehicles(right_waypoint->GetId()).size() == 0) {
422  change_over_point = right_waypoint;
423  } else if (distant_left_lane_free && left_waypoint != nullptr
424  && track_traffic.GetPassingVehicles(left_waypoint->GetId()).size() == 0) {
425  change_over_point = left_waypoint;
426  }
427  } else if (force) {
428  if (direction && right_waypoint != nullptr) {
429  change_over_point = right_waypoint;
430  } else if (!direction && left_waypoint != nullptr) {
431  change_over_point = left_waypoint;
432  }
433  }
434 
435  if (change_over_point != nullptr) {
436  const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, MIN_WPT_DISTANCE, MAX_WPT_DISTANCE);
437  const SimpleWaypointPtr starting_point = change_over_point;
438  while (change_over_point->DistanceSquared(starting_point) < SQUARE(change_over_distance) &&
439  !change_over_point->CheckJunction()) {
440  change_over_point = change_over_point->GetNextWaypoint().front();
441  }
442  }
443  }
444 
445  return change_over_point;
446 }
447 
448 void LocalizationStage::ImportPath(Path &imported_path, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
449  // Remove the waypoints already added to the path, except for the first.
450  if (parameters.GetUploadPath(actor_id)) {
451  auto number_of_pops = waypoint_buffer.size();
452  for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
453  PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
454  }
455  // We have successfully imported the path. Remove it from the list of paths to be imported.
456  parameters.RemoveUploadPath(actor_id, false);
457  }
458 
459  // Get the latest imported waypoint. and find its closest waypoint in TM's InMemoryMap.
460  cg::Location latest_imported = imported_path.front();
461  SimpleWaypointPtr imported = local_map->GetWaypoint(latest_imported);
462 
463  // We need to generate a path compatible with TM's waypoints.
464  while (!imported_path.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
465  // Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
466  SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
467 
468  // Try to link the latest_waypoint to the imported waypoint.
469  std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
470  uint64_t selection_index = 0u;
471 
472  // Choose correct path.
473  if (next_waypoints.size() > 1) {
474  const float imported_road_id = imported->GetWaypoint()->GetRoadId();
475  float min_distance = std::numeric_limits<float>::infinity();
476  for (uint64_t k = 0u; k < next_waypoints.size(); ++k) {
477  SimpleWaypointPtr junction_end_point = next_waypoints.at(k);
478  while (!junction_end_point->CheckJunction()) {
479  junction_end_point = junction_end_point->GetNextWaypoint().front();
480  }
481  while (junction_end_point->CheckJunction()) {
482  junction_end_point = junction_end_point->GetNextWaypoint().front();
483  }
484  while (next_waypoints.at(k)->DistanceSquared(junction_end_point) < 50.0f) {
485  junction_end_point = junction_end_point->GetNextWaypoint().front();
486  }
487  float jep_road_id = junction_end_point->GetWaypoint()->GetRoadId();
488  if (jep_road_id == imported_road_id) {
489  selection_index = k;
490  break;
491  }
492  float distance = junction_end_point->DistanceSquared(imported);
493  if (distance < min_distance) {
494  min_distance = distance;
495  selection_index = k;
496  }
497  }
498  } else if (next_waypoints.size() == 0) {
499  if (!parameters.GetOSMMode()) {
500  std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
501  }
502  marked_for_removal.push_back(actor_id);
503  break;
504  }
505  SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
506  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
507 
508  // Remove the imported waypoint from the path if it's close to the last one.
509  if (next_wp_selection->DistanceSquared(imported) < 30.0f) {
510  imported_path.erase(imported_path.begin());
511  PushWaypoint(actor_id, track_traffic, waypoint_buffer, imported);
512  latest_imported = imported_path.front();
513  imported = local_map->GetWaypoint(latest_imported);
514  }
515  }
516  if (imported_path.empty()) {
517  // Once we are done, check if we can clear the structure.
518  parameters.RemoveUploadPath(actor_id, true);
519  } else {
520  // Otherwise, update the structure with the waypoints that we still need to import.
521  parameters.UpdateUploadPath(actor_id, imported_path);
522  }
523 }
524 
525 void LocalizationStage::ImportRoute(Route &imported_actions, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
526 
527  if (parameters.GetUploadRoute(actor_id)) {
528  auto number_of_pops = waypoint_buffer.size();
529  for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
530  PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
531  }
532  // We have successfully imported the route. Remove it from the list of routes to be imported.
533  parameters.RemoveImportedRoute(actor_id, false);
534  }
535 
536  RoadOption next_road_option = static_cast<RoadOption>(imported_actions.front());
537  while (!imported_actions.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
538  // Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
539  SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
540  RoadOption latest_road_option = latest_waypoint->GetRoadOption();
541  // Try to link the latest_waypoint to the correct next RouteOption.
542  std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
543  uint16_t selection_index = 0u;
544  if (next_waypoints.size() > 1) {
545  for (uint16_t i=0; i<next_waypoints.size(); ++i) {
546  if (next_waypoints.at(i)->GetRoadOption() == next_road_option) {
547  selection_index = i;
548  break;
549  } else {
550  if (i == next_waypoints.size() - 1) {
551  std::cout << "We couldn't find the RoadOption you were looking for. This route might diverge from the one expected." << std::endl;
552  }
553  }
554  }
555  } else if (next_waypoints.size() == 0) {
556  if (!parameters.GetOSMMode()) {
557  std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
558  }
559  marked_for_removal.push_back(actor_id);
560  break;
561  }
562 
563  SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
564  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
565 
566  // If we are switching to a new RoadOption, it means the current one is already fully imported.
567  if (latest_road_option != next_wp_selection->GetRoadOption() && next_road_option == next_wp_selection->GetRoadOption()) {
568  imported_actions.erase(imported_actions.begin());
569  next_road_option = static_cast<RoadOption>(imported_actions.front());
570  }
571  }
572  if (imported_actions.empty()) {
573  // Once we are done, check if we can clear the structure.
574  parameters.RemoveImportedRoute(actor_id, true);
575  } else {
576  // Otherwise, update the structure with the waypoints that we still need to import.
577  parameters.UpdateImportedRoute(actor_id, imported_actions);
578  }
579 }
580 
582  auto waypoint_buffer = buffer_map.at(actor_id);
583  auto next_action = std::make_pair(RoadOption::LaneFollow, waypoint_buffer.back()->GetWaypoint());
584  bool is_lane_change = false;
585  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
586  // A lane change is happening.
587  is_lane_change = true;
588  const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
589  const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
590  bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
591  if (left_heading) next_action = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
592  else next_action = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
593  }
594  for (auto &swpt : waypoint_buffer) {
595  RoadOption road_opt = swpt->GetRoadOption();
596  if (road_opt != RoadOption::LaneFollow) {
597  if (!is_lane_change) {
598  // No lane change in sight, we can assume this will be the next action.
599  return std::make_pair(road_opt, swpt->GetWaypoint());
600  } else {
601  // A lane change will happen as well as another action, we need to figure out which one will happen first.
602  cg::Location lane_change = last_lane_change_swpt.at(actor_id)->GetLocation();
603  cg::Location actual_location = simulation_state.GetLocation(actor_id);
604  auto distance_lane_change = cg::Math::DistanceSquared(actual_location, lane_change);
605  auto distance_other_action = cg::Math::DistanceSquared(actual_location, swpt->GetLocation());
606  if (distance_lane_change < distance_other_action) return next_action;
607  else return std::make_pair(road_opt, swpt->GetWaypoint());
608  }
609  }
610  }
611  return next_action;
612 }
613 
615 
616  auto waypoint_buffer = buffer_map.at(actor_id);
617  ActionBuffer action_buffer;
618  Action lane_change;
619  bool is_lane_change = false;
620  SimpleWaypointPtr buffer_front = waypoint_buffer.front();
621  RoadOption last_road_opt = buffer_front->GetRoadOption();
622  action_buffer.push_back(std::make_pair(last_road_opt, buffer_front->GetWaypoint()));
623  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
624  // A lane change is happening.
625  is_lane_change = true;
626  const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
627  const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
628  bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
629  if (left_heading) lane_change = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
630  else lane_change = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
631  }
632  for (auto &wpt : waypoint_buffer) {
633  RoadOption current_road_opt = wpt->GetRoadOption();
634  if (current_road_opt != last_road_opt) {
635  action_buffer.push_back(std::make_pair(current_road_opt, wpt->GetWaypoint()));
636  last_road_opt = current_road_opt;
637  }
638  }
639  if (is_lane_change) {
640  // Insert the lane change action in the appropriate part of the action buffer.
641  auto distance_lane_change = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), lane_change.second->GetTransform().location);
642  for (uint16_t i = 0; i < action_buffer.size(); ++i) {
643  auto distance_action = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), waypoint_buffer.at(i)->GetLocation());
644  // If the waypoint related to the next action is further away from the one of the lane change, insert lane change action here.
645  // If we reached the end of the buffer, place the action at the end.
646  if (i == action_buffer.size()-1) {
647  action_buffer.push_back(lane_change);
648  break;
649  } else if (distance_action > distance_lane_change) {
650  action_buffer.insert(action_buffer.begin()+i, lane_change);
651  break;
652  }
653  }
654  }
655  return action_buffer;
656 }
657 
658 } // namespace traffic_manager
659 } // namespace carla
float GetRandomRightLaneChangePercentage(const ActorId &actor_id)
Method to query percentage probability of a random left lane change for a vehicle.
Definition: Parameters.cpp:290
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
static const float MAXIMUM_LANE_OBSTACLE_DISTANCE
Definition: Constants.h:64
void PopWaypoint(ActorId actor_id, TrackTraffic &track_traffic, Buffer &buffer, bool front_or_back)
Action ComputeNextAction(const ActorId &actor_id)
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE
Definition: Constants.h:65
Route GetImportedRoute(const ActorId &actor_id) const
Method to get a custom route.
Definition: Parameters.cpp:438
std::unordered_map< ActorId, SimpleWaypointPair > vehicles_at_junction_entrance
bool GetUploadRoute(const ActorId &actor_id) const
Method to get if we are uploading a route.
Definition: Parameters.cpp:427
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:399
float SquaredLength() const
Definition: geom/Vector3D.h:45
std::vector< cg::Location > Path
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:301
std::shared_ptr< InMemoryMap > LocalMapPtr
Definition: ALSM.h:36
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:70
std::unordered_map< carla::ActorId, Buffer > BufferMap
float Length() const
Definition: geom/Vector3D.h:49
cg::Location GetLocation(const ActorId actor_id) const
bool GetUploadPath(const ActorId &actor_id) const
Method to get if we are uploading a path.
Definition: Parameters.cpp:404
void UpdateUploadPath(const ActorId &actor_id, const Path path)
Method to update an already set list of points.
Definition: Parameters.cpp:190
void ImportPath(Path &imported_path, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square)
float GetKeepRightPercentage(const ActorId &actor_id)
Method to query percentage probability of keep right rule for a vehicle.
Definition: Parameters.cpp:268
std::pair< RoadOption, WaypointPtr > Action
void UpdateImportedRoute(const ActorId &actor_id, const Route route)
Method to update an already set route.
Definition: Parameters.cpp:211
#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
carla::SharedPtr< cc::Waypoint > WaypointPtr
Definition: InMemoryMap.h:38
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const
std::vector< uint8_t > Route
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:62
std::vector< LocalizationData > LocalizationFrame
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
void ImportRoute(Route &imported_actions, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square)
void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path)
Method to remove a route.
Definition: Parameters.cpp:203
std::pair< SimpleWaypointPtr, SimpleWaypointPtr > SimpleWaypointPair
void RemoveUploadPath(const ActorId &actor_id, const bool remove_path)
Method to remove a list of points.
Definition: Parameters.cpp:182
std::vector< Action > ActionBuffer
float GetRandomLeftLaneChangePercentage(const ActorId &actor_id)
Method to query percentage probability of a random right lane change for a vehicle.
Definition: Parameters.cpp:279
ActorIdSet GetPassingVehicles(uint64_t waypoint_id) const
cg::Vector3D GetVelocity(const ActorId actor_id) const
ActionBuffer ComputeActionBuffer(const ActorId &actor_id)
ChangeLaneInfo GetForceLaneChange(const ActorId &actor_id)
Method to query lane change command for a vehicle.
Definition: Parameters.cpp:255
Path GetCustomPath(const ActorId &actor_id) const
Method to get a custom path.
Definition: Parameters.cpp:415