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