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  RandomGenerator &random_device)
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_device(random_device){}
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_device.next();
124  const bool is_random_left_change = perc_random_leftlanechange >= random_device.next();
125  const bool is_random_right_change = perc_random_rightlanechange >= random_device.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_device.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  if (done_with_previous_lane_change) last_lane_change_swpt.erase(actor_id);
152  }
153  bool auto_or_force_lane_change = parameters.GetAutoLaneChange(actor_id) || force_lane_change;
154  bool front_waypoint_not_junction = !front_waypoint->CheckJunction();
155 
156  if (auto_or_force_lane_change
157  && front_waypoint_not_junction
158  && (recently_not_executed_lane_change || done_with_previous_lane_change)) {
159 
160  SimpleWaypointPtr change_over_point = AssignLaneChange(actor_id, vehicle_location, vehicle_speed,
161  force_lane_change, lane_change_direction);
162 
163  if (change_over_point != nullptr) {
164  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
165  last_lane_change_swpt.at(actor_id) = change_over_point;
166  } else {
167  last_lane_change_swpt.insert({actor_id, change_over_point});
168  }
169  auto number_of_pops = waypoint_buffer.size();
170  for (uint64_t j = 0u; j < number_of_pops; ++j) {
171  PopWaypoint(actor_id, track_traffic, waypoint_buffer);
172  }
173  PushWaypoint(actor_id, track_traffic, waypoint_buffer, change_over_point);
174  }
175  }
176 
177  Path imported_path = parameters.GetCustomPath(actor_id);
178  Route imported_actions = parameters.GetImportedRoute(actor_id);
179  // We are effectively importing a path.
180  if (!imported_path.empty()) {
181 
182  ImportPath(imported_path, waypoint_buffer, actor_id, horizon_square);
183 
184  } else if (!imported_actions.empty()) {
185 
186  ImportRoute(imported_actions, waypoint_buffer, actor_id, horizon_square);
187 
188  }
189 
190  // Populating the buffer through randomly chosen waypoints.
191  else {
192  while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
193  SimpleWaypointPtr furthest_waypoint = waypoint_buffer.back();
194  std::vector<SimpleWaypointPtr> next_waypoints = furthest_waypoint->GetNextWaypoint();
195  uint64_t selection_index = 0u;
196  // Pseudo-randomized path selection if found more than one choice.
197  if (next_waypoints.size() > 1) {
198  double r_sample = random_device.next();
199  selection_index = static_cast<uint64_t>(r_sample*next_waypoints.size()*0.01);
200  } else if (next_waypoints.size() == 0) {
201  if (!parameters.GetOSMMode()) {
202  std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
203  }
204  marked_for_removal.push_back(actor_id);
205  break;
206  }
207  SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
208  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
209  if (next_wp_selection->GetId() == waypoint_buffer.front()->GetId()){
210  // Found a loop, stop. Don't use zero distance as there can be two waypoints at the same location
211  break;
212  }
213  }
214  }
215  ExtendAndFindSafeSpace(actor_id, is_at_junction_entrance, waypoint_buffer);
216 
217  // Editing output array
218  LocalizationData &output = output_array.at(index);
219  output.is_at_junction_entrance = is_at_junction_entrance;
220 
221  if (is_at_junction_entrance) {
222  const SimpleWaypointPair &safe_space_end_points = vehicles_at_junction_entrance.at(actor_id);
223  output.junction_end_point = safe_space_end_points.first;
224  output.safe_point = safe_space_end_points.second;
225  } else {
226  output.junction_end_point = nullptr;
227  output.safe_point = nullptr;
228  }
229 
230  // Updating geodesic grid position for actor.
231  track_traffic.UpdateGridPosition(actor_id, waypoint_buffer);
232 }
233 
235  const bool is_at_junction_entrance,
236  Buffer &waypoint_buffer) {
237 
238  SimpleWaypointPtr junction_end_point = nullptr;
239  SimpleWaypointPtr safe_point_after_junction = nullptr;
240 
241  if (is_at_junction_entrance
243 
244  bool entered_junction = false;
245  bool past_junction = false;
246  bool safe_point_found = false;
247  SimpleWaypointPtr current_waypoint = nullptr;
248  SimpleWaypointPtr junction_begin_point = nullptr;
249  float safe_distance_squared = SQUARE(SAFE_DISTANCE_AFTER_JUNCTION);
250 
251  // Scanning existing buffer points.
252  for (unsigned long i = 0u; i < waypoint_buffer.size() && !safe_point_found; ++i) {
253  current_waypoint = waypoint_buffer.at(i);
254  if (!entered_junction && current_waypoint->CheckJunction()) {
255  entered_junction = true;
256  junction_begin_point = current_waypoint;
257  }
258  if (entered_junction && !past_junction && !current_waypoint->CheckJunction()) {
259  past_junction = true;
260  junction_end_point = current_waypoint;
261  }
262  if (past_junction && junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared) {
263  safe_point_found = true;
264  safe_point_after_junction = current_waypoint;
265  }
266  }
267 
268  // Extend buffer if safe point not found.
269  if (!safe_point_found) {
270  bool abort = false;
271 
272  while (!past_junction && !abort) {
273  NodeList next_waypoints = current_waypoint->GetNextWaypoint();
274  if (!next_waypoints.empty()) {
275  current_waypoint = next_waypoints.front();
276  PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
277  if (!current_waypoint->CheckJunction()) {
278  past_junction = true;
279  junction_end_point = current_waypoint;
280  }
281  } else {
282  abort = true;
283  }
284  }
285 
286  while (!safe_point_found && !abort) {
287  std::vector<SimpleWaypointPtr> next_waypoints = current_waypoint->GetNextWaypoint();
288  if ((junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared)
289  || next_waypoints.size() > 1
290  || current_waypoint->CheckJunction()) {
291 
292  safe_point_found = true;
293  safe_point_after_junction = current_waypoint;
294  } else {
295  if (!next_waypoints.empty()) {
296  current_waypoint = next_waypoints.front();
297  PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
298  } else {
299  abort = true;
300  }
301  }
302  }
303  }
304 
305  if (junction_end_point != nullptr &&
306  safe_point_after_junction != nullptr &&
307  junction_begin_point->DistanceSquared(junction_end_point) < SQUARE(MIN_JUNCTION_LENGTH)) {
308 
309  junction_end_point = nullptr;
310  safe_point_after_junction = nullptr;
311  }
312 
313  vehicles_at_junction_entrance.insert({actor_id, {junction_end_point, safe_point_after_junction}});
314  }
315  else if (!is_at_junction_entrance
317 
318  vehicles_at_junction_entrance.erase(actor_id);
319  }
320 }
321 
323  last_lane_change_swpt.erase(actor_id);
324  vehicles_at_junction.erase(actor_id);
325 }
326 
328  last_lane_change_swpt.clear();
329  vehicles_at_junction.clear();
330 }
331 
333  const cg::Location vehicle_location,
334  const float vehicle_speed,
335  bool force, bool direction) {
336 
337  // Waypoint representing the new starting point for the waypoint buffer
338  // due to lane change. Remains nullptr if lane change not viable.
339  SimpleWaypointPtr change_over_point = nullptr;
340 
341  // Retrieve waypoint buffer for current vehicle.
342  const Buffer &waypoint_buffer = buffer_map.at(actor_id);
343 
344  // Check buffer is not empty.
345  if (!waypoint_buffer.empty()) {
346  // Get the left and right waypoints for the current closest waypoint.
347  const SimpleWaypointPtr &current_waypoint = waypoint_buffer.front();
348  const SimpleWaypointPtr left_waypoint = current_waypoint->GetLeftWaypoint();
349  const SimpleWaypointPtr right_waypoint = current_waypoint->GetRightWaypoint();
350 
351  // Retrieve vehicles with overlapping waypoint buffers with current vehicle.
352  const auto blocking_vehicles = track_traffic.GetOverlappingVehicles(actor_id);
353 
354  // Find immediate in-lane obstacle and check if any are too close to initiate lane change.
355  bool obstacle_too_close = false;
356  float minimum_squared_distance = std::numeric_limits<float>::infinity();
357  ActorId obstacle_actor_id = 0u;
358  for (auto i = blocking_vehicles.begin();
359  i != blocking_vehicles.end() && !obstacle_too_close && !force;
360  ++i) {
361  const ActorId &other_actor_id = *i;
362  // Find vehicle in buffer map and check if it's buffer is not empty.
363  if (buffer_map.find(other_actor_id) != buffer_map.end() && !buffer_map.at(other_actor_id).empty()) {
364  const Buffer &other_buffer = buffer_map.at(other_actor_id);
365  const SimpleWaypointPtr &other_current_waypoint = other_buffer.front();
366  const cg::Location other_location = other_current_waypoint->GetLocation();
367 
368  const cg::Vector3D reference_heading = current_waypoint->GetForwardVector();
369  cg::Vector3D reference_to_other = other_location - current_waypoint->GetLocation();
370  const cg::Vector3D other_heading = other_current_waypoint->GetForwardVector();
371 
372  WaypointPtr current_raw_waypoint = current_waypoint->GetWaypoint();
373  WaypointPtr other_current_raw_waypoint = other_current_waypoint->GetWaypoint();
374  // Check both vehicles are not in junction,
375  // Check if the other vehicle is in front of the current vehicle,
376  // Check if the two vehicles have acceptable angular deviation between their headings.
377  if (!current_waypoint->CheckJunction()
378  && !other_current_waypoint->CheckJunction()
379  && other_current_raw_waypoint->GetRoadId() == current_raw_waypoint->GetRoadId()
380  && other_current_raw_waypoint->GetLaneId() == current_raw_waypoint->GetLaneId()
381  && cg::Math::Dot(reference_heading, reference_to_other) > 0.0f
382  && cg::Math::Dot(reference_heading, other_heading) > MAXIMUM_LANE_OBSTACLE_CURVATURE) {
383  float squared_distance = cg::Math::DistanceSquared(vehicle_location, other_location);
384  // Abort if the obstacle is too close.
385  if (squared_distance > SQUARE(MINIMUM_LANE_CHANGE_DISTANCE)) {
386  // Remember if the new vehicle is closer.
387  if (squared_distance < minimum_squared_distance && squared_distance < SQUARE(MAXIMUM_LANE_OBSTACLE_DISTANCE)) {
388  minimum_squared_distance = squared_distance;
389  obstacle_actor_id = other_actor_id;
390  }
391  } else {
392  obstacle_too_close = true;
393  }
394  }
395  }
396  }
397 
398  // If a valid immediate obstacle found.
399  if (!obstacle_too_close && obstacle_actor_id != 0u && !force) {
400  const Buffer &other_buffer = buffer_map.at(obstacle_actor_id);
401  const SimpleWaypointPtr &other_current_waypoint = other_buffer.front();
402  const auto other_neighbouring_lanes = {other_current_waypoint->GetLeftWaypoint(),
403  other_current_waypoint->GetRightWaypoint()};
404 
405  // Flags reflecting whether adjacent lanes are free near the obstacle.
406  bool distant_left_lane_free = false;
407  bool distant_right_lane_free = false;
408 
409  // Check if the neighbouring lanes near the obstructing vehicle are free of other vehicles.
410  bool left_right = true;
411  for (auto &candidate_lane_wp : other_neighbouring_lanes) {
412  if (candidate_lane_wp != nullptr &&
413  track_traffic.GetPassingVehicles(candidate_lane_wp->GetId()).size() == 0) {
414 
415  if (left_right)
416  distant_left_lane_free = true;
417  else
418  distant_right_lane_free = true;
419  }
420  left_right = !left_right;
421  }
422 
423  // Based on what lanes are free near the obstacle,
424  // find the change over point with no vehicles passing through them.
425  if (distant_right_lane_free && right_waypoint != nullptr
426  && track_traffic.GetPassingVehicles(right_waypoint->GetId()).size() == 0) {
427  change_over_point = right_waypoint;
428  } else if (distant_left_lane_free && left_waypoint != nullptr
429  && track_traffic.GetPassingVehicles(left_waypoint->GetId()).size() == 0) {
430  change_over_point = left_waypoint;
431  }
432  } else if (force) {
433  if (direction && right_waypoint != nullptr) {
434  change_over_point = right_waypoint;
435  } else if (!direction && left_waypoint != nullptr) {
436  change_over_point = left_waypoint;
437  }
438  }
439 
440  if (change_over_point != nullptr) {
441  const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, MIN_WPT_DISTANCE, MAX_WPT_DISTANCE);
442  const SimpleWaypointPtr starting_point = change_over_point;
443  while (change_over_point->DistanceSquared(starting_point) < SQUARE(change_over_distance) &&
444  !change_over_point->CheckJunction()) {
445  change_over_point = change_over_point->GetNextWaypoint().front();
446  }
447  }
448  }
449 
450  return change_over_point;
451 }
452 
453 void LocalizationStage::ImportPath(Path &imported_path, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
454  // Remove the waypoints already added to the path, except for the first.
455  if (parameters.GetUploadPath(actor_id)) {
456  auto number_of_pops = waypoint_buffer.size();
457  for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
458  PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
459  }
460  // We have successfully imported the path. Remove it from the list of paths to be imported.
461  parameters.RemoveUploadPath(actor_id, false);
462  }
463 
464  // Get the latest imported waypoint. and find its closest waypoint in TM's InMemoryMap.
465  cg::Location latest_imported = imported_path.front();
466  SimpleWaypointPtr imported = local_map->GetWaypoint(latest_imported);
467 
468  // We need to generate a path compatible with TM's waypoints.
469  while (!imported_path.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
470  // Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
471  SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
472 
473  // Try to link the latest_waypoint to the imported waypoint.
474  std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
475  uint64_t selection_index = 0u;
476 
477  // Choose correct path.
478  if (next_waypoints.size() > 1) {
479  const float imported_road_id = imported->GetWaypoint()->GetRoadId();
480  float min_distance = std::numeric_limits<float>::infinity();
481  for (uint64_t k = 0u; k < next_waypoints.size(); ++k) {
482  SimpleWaypointPtr junction_end_point = next_waypoints.at(k);
483  while (!junction_end_point->CheckJunction()) {
484  junction_end_point = junction_end_point->GetNextWaypoint().front();
485  }
486  while (junction_end_point->CheckJunction()) {
487  junction_end_point = junction_end_point->GetNextWaypoint().front();
488  }
489  while (next_waypoints.at(k)->DistanceSquared(junction_end_point) < 50.0f) {
490  junction_end_point = junction_end_point->GetNextWaypoint().front();
491  }
492  float jep_road_id = junction_end_point->GetWaypoint()->GetRoadId();
493  if (jep_road_id == imported_road_id) {
494  selection_index = k;
495  break;
496  }
497  float distance = junction_end_point->DistanceSquared(imported);
498  if (distance < min_distance) {
499  min_distance = distance;
500  selection_index = k;
501  }
502  }
503  } else if (next_waypoints.size() == 0) {
504  if (!parameters.GetOSMMode()) {
505  std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
506  }
507  marked_for_removal.push_back(actor_id);
508  break;
509  }
510  SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
511 
512  // Remove the imported waypoint from the path if it's close to the last one.
513  if (next_wp_selection->DistanceSquared(imported) < 30.0f) {
514  imported_path.erase(imported_path.begin());
515  std::vector<SimpleWaypointPtr> possible_waypoints = next_wp_selection->GetNextWaypoint();
516  if (std::find(possible_waypoints.begin(), possible_waypoints.end(), imported) != possible_waypoints.end()) {
517  // If the lane is changing, only push the new waypoint
518  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
519  }
520  PushWaypoint(actor_id, track_traffic, waypoint_buffer, imported);
521  latest_imported = imported_path.front();
522  imported = local_map->GetWaypoint(latest_imported);
523  } else {
524  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
525  }
526  }
527  if (imported_path.empty()) {
528  // Once we are done, check if we can clear the structure.
529  parameters.RemoveUploadPath(actor_id, true);
530  } else {
531  // Otherwise, update the structure with the waypoints that we still need to import.
532  parameters.UpdateUploadPath(actor_id, imported_path);
533  }
534 }
535 
536 void LocalizationStage::ImportRoute(Route &imported_actions, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
537 
538  if (parameters.GetUploadRoute(actor_id)) {
539  auto number_of_pops = waypoint_buffer.size();
540  for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
541  PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
542  }
543  // We have successfully imported the route. Remove it from the list of routes to be imported.
544  parameters.RemoveImportedRoute(actor_id, false);
545  }
546 
547  RoadOption next_road_option = static_cast<RoadOption>(imported_actions.front());
548  while (!imported_actions.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
549  // Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
550  SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
551  RoadOption latest_road_option = latest_waypoint->GetRoadOption();
552  // Try to link the latest_waypoint to the correct next RouteOption.
553  std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
554  uint16_t selection_index = 0u;
555  if (next_waypoints.size() > 1) {
556  for (uint16_t i=0; i<next_waypoints.size(); ++i) {
557  if (next_waypoints.at(i)->GetRoadOption() == next_road_option) {
558  selection_index = i;
559  break;
560  } else {
561  if (i == next_waypoints.size() - 1) {
562  std::cout << "We couldn't find the RoadOption you were looking for. This route might diverge from the one expected." << std::endl;
563  }
564  }
565  }
566  } else if (next_waypoints.size() == 0) {
567  if (!parameters.GetOSMMode()) {
568  std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
569  }
570  marked_for_removal.push_back(actor_id);
571  break;
572  }
573 
574  SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
575  PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
576 
577  // If we are switching to a new RoadOption, it means the current one is already fully imported.
578  if (latest_road_option != next_wp_selection->GetRoadOption() && next_road_option == next_wp_selection->GetRoadOption()) {
579  imported_actions.erase(imported_actions.begin());
580  next_road_option = static_cast<RoadOption>(imported_actions.front());
581  }
582  }
583  if (imported_actions.empty()) {
584  // Once we are done, check if we can clear the structure.
585  parameters.RemoveImportedRoute(actor_id, true);
586  } else {
587  // Otherwise, update the structure with the waypoints that we still need to import.
588  parameters.UpdateImportedRoute(actor_id, imported_actions);
589  }
590 }
591 
593  auto waypoint_buffer = buffer_map.at(actor_id);
594  auto next_action = std::make_pair(RoadOption::LaneFollow, waypoint_buffer.back()->GetWaypoint());
595  bool is_lane_change = false;
596  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
597  // A lane change is happening.
598  is_lane_change = true;
599  const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
600  const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
601  bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
602  if (left_heading) next_action = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
603  else next_action = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
604  }
605  for (auto &swpt : waypoint_buffer) {
606  RoadOption road_opt = swpt->GetRoadOption();
607  if (road_opt != RoadOption::LaneFollow) {
608  if (!is_lane_change) {
609  // No lane change in sight, we can assume this will be the next action.
610  return std::make_pair(road_opt, swpt->GetWaypoint());
611  } else {
612  // A lane change will happen as well as another action, we need to figure out which one will happen first.
613  cg::Location lane_change = last_lane_change_swpt.at(actor_id)->GetLocation();
614  cg::Location actual_location = simulation_state.GetLocation(actor_id);
615  auto distance_lane_change = cg::Math::DistanceSquared(actual_location, lane_change);
616  auto distance_other_action = cg::Math::DistanceSquared(actual_location, swpt->GetLocation());
617  if (distance_lane_change < distance_other_action) return next_action;
618  else return std::make_pair(road_opt, swpt->GetWaypoint());
619  }
620  }
621  }
622  return next_action;
623 }
624 
626 
627  auto waypoint_buffer = buffer_map.at(actor_id);
628  ActionBuffer action_buffer;
629  Action lane_change;
630  bool is_lane_change = false;
631  SimpleWaypointPtr buffer_front = waypoint_buffer.front();
632  RoadOption last_road_opt = buffer_front->GetRoadOption();
633  action_buffer.push_back(std::make_pair(last_road_opt, buffer_front->GetWaypoint()));
634  if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
635  // A lane change is happening.
636  is_lane_change = true;
637  const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
638  const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
639  bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
640  if (left_heading) lane_change = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
641  else lane_change = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
642  }
643  for (auto &wpt : waypoint_buffer) {
644  RoadOption current_road_opt = wpt->GetRoadOption();
645  if (current_road_opt != last_road_opt) {
646  action_buffer.push_back(std::make_pair(current_road_opt, wpt->GetWaypoint()));
647  last_road_opt = current_road_opt;
648  }
649  }
650  if (is_lane_change) {
651  // Insert the lane change action in the appropriate part of the action buffer.
652  auto distance_lane_change = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), lane_change.second->GetTransform().location);
653  for (uint16_t i = 0; i < action_buffer.size(); ++i) {
654  auto distance_action = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), waypoint_buffer.at(i)->GetLocation());
655  // If the waypoint related to the next action is further away from the one of the lane change, insert lane change action here.
656  // If we reached the end of the buffer, place the action at the end.
657  if (i == action_buffer.size()-1) {
658  action_buffer.push_back(lane_change);
659  break;
660  } else if (distance_action > distance_lane_change) {
661  action_buffer.insert(action_buffer.begin()+i, lane_change);
662  break;
663  }
664  }
665  }
666  return action_buffer;
667 }
668 
669 } // namespace traffic_manager
670 } // 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:304
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:452
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:441
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:413
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:315
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
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:418
void UpdateUploadPath(const ActorId &actor_id, const Path path)
Method to update an already set list of points.
Definition: Parameters.cpp:202
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:282
std::pair< RoadOption, WaypointPtr > Action
void UpdateImportedRoute(const ActorId &actor_id, const Route route)
Method to update an already set route.
Definition: Parameters.cpp:223
#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:215
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:194
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, RandomGenerator &random_device)
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:293
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:269
Path GetCustomPath(const ActorId &actor_id) const
Method to get a custom path.
Definition: Parameters.cpp:429