CARLA
MotionPlanStage.cpp
Go to the documentation of this file.
1 // Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include <limits>
8 
12 
15 
17 
18 namespace carla {
19 namespace traffic_manager {
20 
21 using namespace constants::MotionPlan;
22 using namespace constants::WaypointSelection;
23 using namespace constants::SpeedThreshold;
24 
28 
30  const std::vector<ActorId> &vehicle_id_list,
31  SimulationState &simulation_state,
32  const Parameters &parameters,
33  const BufferMap &buffer_map,
34  TrackTraffic &track_traffic,
35  const std::vector<float> &urban_longitudinal_parameters,
36  const std::vector<float> &highway_longitudinal_parameters,
37  const std::vector<float> &urban_lateral_parameters,
38  const std::vector<float> &highway_lateral_parameters,
39  const LocalizationFrame &localization_frame,
40  const CollisionFrame&collision_frame,
41  const TLFrame &tl_frame,
42  const cc::World &world,
43  ControlFrame &output_array,
44  RandomGenerator &random_device,
45  const LocalMapPtr &local_map)
46  : vehicle_id_list(vehicle_id_list),
47  simulation_state(simulation_state),
48  parameters(parameters),
49  buffer_map(buffer_map),
50  track_traffic(track_traffic),
51  urban_longitudinal_parameters(urban_longitudinal_parameters),
52  highway_longitudinal_parameters(highway_longitudinal_parameters),
53  urban_lateral_parameters(urban_lateral_parameters),
54  highway_lateral_parameters(highway_lateral_parameters),
55  localization_frame(localization_frame),
56  collision_frame(collision_frame),
57  tl_frame(tl_frame),
58  world(world),
59  output_array(output_array),
60  random_device(random_device),
61  local_map(local_map) {}
62 
63 void MotionPlanStage::Update(const unsigned long index) {
64  const ActorId actor_id = vehicle_id_list.at(index);
65  const cg::Location vehicle_location = simulation_state.GetLocation(actor_id);
66  const cg::Vector3D vehicle_velocity = simulation_state.GetVelocity(actor_id);
67  const cg::Rotation vehicle_rotation = simulation_state.GetRotation(actor_id);
68  const float vehicle_speed = vehicle_velocity.Length();
69  const cg::Vector3D vehicle_heading = simulation_state.GetHeading(actor_id);
70  const bool vehicle_physics_enabled = simulation_state.IsPhysicsEnabled(actor_id);
71  const float vehicle_speed_limit = simulation_state.GetSpeedLimit(actor_id);
72  const Buffer &waypoint_buffer = buffer_map.at(actor_id);
73  const LocalizationData &localization = localization_frame.at(index);
74  const CollisionHazardData &collision_hazard = collision_frame.at(index);
75  const bool &tl_hazard = tl_frame.at(index);
77  StateEntry current_state;
78 
79  // Instanciating teleportation transform as current vehicle transform.
80  cg::Transform teleportation_transform = cg::Transform(vehicle_location, vehicle_rotation);
81 
82  // Get information about the hero location from the actor_id state.
83  cg::Location hero_location = track_traffic.GetHeroLocation();
84  bool is_hero_alive = hero_location != cg::Location(0, 0, 0);
85 
86  if (simulation_state.IsDormant(actor_id) && parameters.GetRespawnDormantVehicles() && is_hero_alive) {
87  // Flushing controller state for vehicle.
88  current_state = {current_timestamp,
89  0.0f, 0.0f,
90  0.0f};
91 
92  // Add entry to teleportation duration clock table if not present.
93  if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
94  teleportation_instance.insert({actor_id, current_timestamp});
95  }
96 
97  // Get lower and upper bound for teleporting vehicle.
100  float dilate_factor = (upper_bound-lower_bound)/100.0f;
101 
102  // Measuring time elapsed since last teleportation for the vehicle.
103  double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
104 
105  if (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT) {
106  float random_sample = (static_cast<float>(random_device.next())*dilate_factor) + lower_bound;
107  NodeList teleport_waypoint_list = local_map->GetWaypointsInDelta(hero_location, ATTEMPTS_TO_TELEPORT, random_sample);
108  if (!teleport_waypoint_list.empty()) {
109  for (auto &teleport_waypoint : teleport_waypoint_list) {
110  GeoGridId geogrid_id = teleport_waypoint->GetGeodesicGridId();
111  if (track_traffic.IsGeoGridFree(geogrid_id)) {
112  teleportation_transform = teleport_waypoint->GetTransform();
113  teleportation_transform.location.z += 0.5f;
114  track_traffic.AddTakenGrid(geogrid_id, actor_id);
115  break;
116  }
117  }
118  }
119  }
120  output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
121 
122  // Update the simulation state with the new transform of the vehicle after teleporting it.
123  KinematicState kinematic_state{teleportation_transform.location,
124  teleportation_transform.rotation,
125  vehicle_velocity, vehicle_speed_limit,
126  vehicle_physics_enabled, simulation_state.IsDormant(actor_id),
127  teleportation_transform.location};
128  simulation_state.UpdateKinematicState(actor_id, kinematic_state);
129  }
130 
131  else {
132 
133  // Target velocity for vehicle.
134  float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, vehicle_speed_limit) / 3.6f;
135 
136  // Algorithm to reduce speed near landmarks
137  float max_landmark_target_velocity = GetLandmarkTargetVelocity(*(waypoint_buffer.at(0)), vehicle_location, actor_id, max_target_velocity);
138 
139  // Algorithm to reduce speed near turns
140  float max_turn_target_velocity = GetTurnTargetVelocity(waypoint_buffer, max_target_velocity);
141  max_target_velocity = std::min(std::min(max_target_velocity, max_landmark_target_velocity), max_turn_target_velocity);
142 
143  // Collision handling and target velocity correction.
144  std::pair<bool, float> collision_response = CollisionHandling(collision_hazard, tl_hazard, vehicle_velocity,
145  vehicle_heading, max_target_velocity);
146  bool collision_emergency_stop = collision_response.first;
147  float dynamic_target_velocity = collision_response.second;
148 
149  // Don't enter junction if there isn't enough free space after the junction.
150  bool safe_after_junction = SafeAfterJunction(localization, tl_hazard, collision_emergency_stop);
151 
152  // In case of collision or traffic light hazard.
153  bool emergency_stop = tl_hazard || collision_emergency_stop || !safe_after_junction;
154 
155  if (vehicle_physics_enabled && !simulation_state.IsDormant(actor_id)) {
156  ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
157 
158  const float target_point_distance = std::max(vehicle_speed * TARGET_WAYPOINT_TIME_HORIZON,
160  const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
161  cg::Location target_location = target_waypoint->GetLocation();
162 
163  float offset = parameters.GetLaneOffset(actor_id);
164  auto right_vector = target_waypoint->GetTransform().GetRightVector();
165  auto offset_location = cg::Location(cg::Vector3D(offset*right_vector.x, offset*right_vector.y, 0.0f));
166  target_location = target_location + offset_location;
167 
168  float dot_product = DeviationDotProduct(vehicle_location, vehicle_heading, target_location);
169  float cross_product = DeviationCrossProduct(vehicle_location, vehicle_heading, target_location);
170  dot_product = acos(dot_product) / PI;
171  if (cross_product < 0.0f) {
172  dot_product *= -1.0f;
173  }
174  const float angular_deviation = dot_product;
175  const float velocity_deviation = (dynamic_target_velocity - vehicle_speed) / dynamic_target_velocity;
176  // If previous state for vehicle not found, initialize state entry.
177  if (pid_state_map.find(actor_id) == pid_state_map.end()) {
178  const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f};
179  pid_state_map.insert({actor_id, initial_state});
180  }
181 
182  // Retrieving the previous state.
183  traffic_manager::StateEntry previous_state;
184  previous_state = pid_state_map.at(actor_id);
185 
186  // Select PID parameters.
187  std::vector<float> longitudinal_parameters;
188  std::vector<float> lateral_parameters;
189  if (vehicle_speed > HIGHWAY_SPEED) {
190  longitudinal_parameters = highway_longitudinal_parameters;
191  lateral_parameters = highway_lateral_parameters;
192  } else {
193  longitudinal_parameters = urban_longitudinal_parameters;
194  lateral_parameters = urban_lateral_parameters;
195  }
196 
197  // If physics is enabled for the vehicle, use PID controller.
198  // State update for vehicle.
199  current_state = {current_timestamp, angular_deviation, velocity_deviation, 0.0f};
200 
201  // Controller actuation.
202  actuation_signal = PID::RunStep(current_state, previous_state,
203  longitudinal_parameters, lateral_parameters);
204 
205  if (emergency_stop) {
206  actuation_signal.throttle = 0.0f;
207  actuation_signal.brake = 1.0f;
208  }
209 
210  // Constructing the actuation signal.
211 
212  carla::rpc::VehicleControl vehicle_control;
213  vehicle_control.throttle = actuation_signal.throttle;
214  vehicle_control.brake = actuation_signal.brake;
215  vehicle_control.steer = actuation_signal.steer;
216 
217  output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
218 
219  // Updating PID state.
220  current_state.steer = actuation_signal.steer;
221  StateEntry &state = pid_state_map.at(actor_id);
222  state = current_state;
223  }
224  // For physics-less vehicles, determine position and orientation for teleportation.
225  else {
226  // Flushing controller state for vehicle.
227  current_state = {current_timestamp,
228  0.0f, 0.0f,
229  0.0f};
230 
231  // Add entry to teleportation duration clock table if not present.
232  if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
233  teleportation_instance.insert({actor_id, current_timestamp});
234  }
235 
236  // Measuring time elapsed since last teleportation for the vehicle.
237  double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
238 
239  // Find a location ahead of the vehicle for teleportation to achieve intended velocity.
240  if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT)) {
241 
242  // Target displacement magnitude to achieve target velocity.
243  const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT_FL;
244  SimpleWaypointPtr teleport_target = waypoint_buffer.front();
245  cg::Transform target_base_transform = teleport_target->GetTransform();
246  cg::Location target_base_location = target_base_transform.location;
247  cg::Vector3D target_heading = target_base_transform.GetForwardVector();
248  cg::Vector3D correct_heading = (target_base_location - vehicle_location).MakeSafeUnitVector(EPSILON);
249 
250  if (vehicle_location.Distance(target_base_location) < target_displacement) {
251  cg::Location teleportation_location = vehicle_location + cg::Location(target_heading.MakeSafeUnitVector(EPSILON) * target_displacement);
252  teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
253  }
254  else {
255  cg::Location teleportation_location = vehicle_location + cg::Location(correct_heading * target_displacement);
256  teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
257  }
258  // In case of an emergency stop, stay in the same location.
259  // Also, teleport only once every dt in asynchronous mode.
260  } else {
261  teleportation_transform = cg::Transform(vehicle_location, simulation_state.GetRotation(actor_id));
262  }
263  // Constructing the actuation signal.
264  output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
265  simulation_state.UpdateKinematicHybridEndLocation(actor_id, teleportation_transform.location);
266  }
267  }
268 }
269 
271  const bool tl_hazard,
272  const bool collision_emergency_stop) {
273 
274  SimpleWaypointPtr junction_end_point = localization.junction_end_point;
275  SimpleWaypointPtr safe_point = localization.safe_point;
276 
277  bool safe_after_junction = true;
278  if (!tl_hazard && !collision_emergency_stop
279  && localization.is_at_junction_entrance
280  && junction_end_point != nullptr && safe_point != nullptr
281  && junction_end_point->DistanceSquared(safe_point) > SQUARE(MIN_SAFE_INTERVAL_LENGTH)) {
282 
283  ActorIdSet passing_safe_point = track_traffic.GetPassingVehicles(safe_point->GetId());
284  ActorIdSet passing_junction_end_point = track_traffic.GetPassingVehicles(junction_end_point->GetId());
285  cg::Location mid_point = (junction_end_point->GetLocation() + safe_point->GetLocation())/2.0f;
286 
287  // Only check for vehicles that have the safe point in their passing waypoint, but not
288  // the junction end point.
289  ActorIdSet difference;
290  std::set_difference(passing_safe_point.begin(), passing_safe_point.end(),
291  passing_junction_end_point.begin(), passing_junction_end_point.end(),
292  std::inserter(difference, difference.begin()));
293  if (difference.size() > 0) {
294  for (const ActorId &blocking_id: difference) {
295  cg::Location blocking_actor_location = simulation_state.GetLocation(blocking_id);
296  if (cg::Math::DistanceSquared(blocking_actor_location, mid_point) < SQUARE(MAX_JUNCTION_BLOCK_DISTANCE)
298  safe_after_junction = false;
299  break;
300  }
301  }
302  }
303  }
304 
305  return safe_after_junction;
306 }
307 
308 std::pair<bool, float> MotionPlanStage::CollisionHandling(const CollisionHazardData &collision_hazard,
309  const bool tl_hazard,
310  const cg::Vector3D vehicle_velocity,
311  const cg::Vector3D vehicle_heading,
312  const float max_target_velocity) {
313  bool collision_emergency_stop = false;
314  float dynamic_target_velocity = max_target_velocity;
315  const float vehicle_speed = vehicle_velocity.Length();
316 
317  if (collision_hazard.hazard && !tl_hazard) {
318  const ActorId other_actor_id = collision_hazard.hazard_actor_id;
319  const cg::Vector3D other_velocity = simulation_state.GetVelocity(other_actor_id);
320  const float vehicle_relative_speed = (vehicle_velocity - other_velocity).Length();
321  const float available_distance_margin = collision_hazard.available_distance_margin;
322 
323  const float other_speed_along_heading = cg::Math::Dot(other_velocity, vehicle_heading);
324 
325  // Consider collision avoidance decisions only if there is positive relative velocity
326  // of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle).
327  if (vehicle_relative_speed > EPSILON_RELATIVE_SPEED) {
328  // If other vehicle is approaching lead vehicle and lead vehicle is further
329  // than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
330  float follow_lead_distance = FOLLOW_LEAD_FACTOR * vehicle_speed + MIN_FOLLOW_LEAD_DISTANCE;
331  if (available_distance_margin > follow_lead_distance) {
332  // Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE
333  // by maintaining a relative speed of other_speed_along_heading
334  dynamic_target_velocity = other_speed_along_heading;
335  }
336  // If vehicle is approaching a lead vehicle and the lead vehicle is further
337  // than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE.
338  else if (available_distance_margin > CRITICAL_BRAKING_MARGIN) {
339  // Then follow the lead vehicle by acquiring it's speed along current heading.
340  dynamic_target_velocity = std::max(other_speed_along_heading, RELATIVE_APPROACH_SPEED);
341  } else {
342  // If lead vehicle closer than CRITICAL_BRAKING_MARGIN, initiate emergency stop.
343  collision_emergency_stop = true;
344  }
345  }
346  if (available_distance_margin < CRITICAL_BRAKING_MARGIN) {
347  collision_emergency_stop = true;
348  }
349  }
350 
351  float max_gradual_velocity = PERC_MAX_SLOWDOWN * vehicle_speed;
352  if (dynamic_target_velocity < vehicle_speed - max_gradual_velocity) {
353  // Don't slow more than PERC_MAX_SLOWDOWN per frame.
354  dynamic_target_velocity = vehicle_speed - max_gradual_velocity;
355  }
356  dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity);
357 
358  return {collision_emergency_stop, dynamic_target_velocity};
359 }
360 
362  const cg::Location vehicle_location,
363  const ActorId actor_id,
364  float max_target_velocity) {
365 
366  auto const max_distance = LANDMARK_DETECTION_TIME * max_target_velocity;
367 
368  float landmark_target_velocity = std::numeric_limits<float>::max();
369 
370  auto all_landmarks = waypoint.GetWaypoint()->GetAllLandmarksInDistance(max_distance, false);
371 
372  for (auto &landmark: all_landmarks) {
373 
374  auto landmark_location = landmark->GetWaypoint()->GetTransform().location;
375  auto landmark_type = landmark->GetType();
376  auto distance = landmark_location.Distance(vehicle_location);
377 
378  if (distance > max_distance) {
379  continue;
380  }
381 
382  float minimum_velocity = max_target_velocity;
383  if (landmark_type == "1000001") { // Traffic light
384  minimum_velocity = TL_TARGET_VELOCITY;
385  } else if (landmark_type == "206") { // Stop
386  minimum_velocity = STOP_TARGET_VELOCITY;
387  } else if (landmark_type == "205") { // Yield
388  minimum_velocity = YIELD_TARGET_VELOCITY;
389  } else if (landmark_type == "274") { // Speed limit
390  float value = static_cast<float>(landmark->GetValue()) / 3.6f;
391  value = parameters.GetVehicleTargetVelocity(actor_id, value);
392  minimum_velocity = (value < max_target_velocity) ? value : max_target_velocity;
393  } else {
394  continue;
395  }
396 
397  float v = std::max(((max_target_velocity - minimum_velocity) / max_distance) * distance + minimum_velocity, minimum_velocity);
398  landmark_target_velocity = std::min(landmark_target_velocity, v);
399  }
400 
401  return landmark_target_velocity;
402 }
403 
404 float MotionPlanStage::GetTurnTargetVelocity(const Buffer &waypoint_buffer,
405  float max_target_velocity) {
406 
407  if (waypoint_buffer.size() < 3) {
408  return max_target_velocity;
409  }
410  else {
411  const SimpleWaypointPtr first_waypoint = waypoint_buffer.front();
412  const SimpleWaypointPtr last_waypoint = waypoint_buffer.back();
413  const SimpleWaypointPtr middle_waypoint = waypoint_buffer.at(static_cast<uint16_t>(waypoint_buffer.size() / 2));
414 
415  float radius = GetThreePointCircleRadius(first_waypoint->GetLocation(),
416  middle_waypoint->GetLocation(),
417  last_waypoint->GetLocation());
418 
419  // Return the max velocity at the turn
420  return std::sqrt(radius * FRICTION * GRAVITY);
421  }
422 }
423 
425  cg::Location middle_location,
426  cg::Location last_location) {
427 
428  float x1 = first_location.x;
429  float y1 = first_location.y;
430  float x2 = middle_location.x;
431  float y2 = middle_location.y;
432  float x3 = last_location.x;
433  float y3 = last_location.y;
434 
435  float x12 = x1 - x2;
436  float x13 = x1 - x3;
437  float y12 = y1 - y2;
438  float y13 = y1 - y3;
439  float y31 = y3 - y1;
440  float y21 = y2 - y1;
441  float x31 = x3 - x1;
442  float x21 = x2 - x1;
443 
444  float sx13 = x1 * x1 - x3 * x3;
445  float sy13 = y1 * y1 - y3 * y3;
446  float sx21 = x2 * x2 - x1 * x1;
447  float sy21 = y2 * y2 - y1 * y1;
448 
449  float f_denom = 2 * (y31 * x12 - y21 * x13);
450  if (f_denom == 0) {
451  return std::numeric_limits<float>::max();
452  }
453  float f = (sx13 * x12 + sy13 * x12 + sx21 * x13 + sy21 * x13) / f_denom;
454 
455  float g_denom = 2 * (x31 * y12 - x21 * y13);
456  if (g_denom == 0) {
457  return std::numeric_limits<float>::max();
458  }
459  float g = (sx13 * y12 + sy13 * y12 + sx21 * y13 + sy21 * y13) / g_denom;
460 
461  float c = - (x1 * x1 + y1 * y1) - 2 * g * x1 - 2 * f * y1;
462  float h = -g;
463  float k = -f;
464 
465  return std::sqrt(h * h + k * k - c);
466 }
467 
468 void MotionPlanStage::RemoveActor(const ActorId actor_id) {
469  pid_state_map.erase(actor_id);
470  teleportation_instance.erase(actor_id);
471 }
472 
474  pid_state_map.clear();
475  teleportation_instance.clear();
476 }
477 
478 } // namespace traffic_manager
479 } // namespace carla
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
std::vector< carla::rpc::Command > ControlFrame
float GetVehicleTargetVelocity(const ActorId &actor_id, const float speed_limit) const
Method to query target velocity for a vehicle.
Definition: Parameters.cpp:253
const std::vector< float > highway_longitudinal_parameters
void Update(const unsigned long index)
float GetLaneOffset(const ActorId &actor_id) const
Method to query lane offset for a vehicle.
Definition: Parameters.cpp:266
const std::vector< ActorId > & vehicle_id_list
std::vector< bool > TLFrame
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...
float SquaredLength() const
Definition: geom/Vector3D.h:45
float GetLandmarkTargetVelocity(const SimpleWaypoint &waypoint, const cg::Location vehicle_location, const ActorId actor_id, float max_target_velocity)
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_set< ActorId > ActorIdSet
std::deque< std::shared_ptr< SimpleWaypoint > > Buffer
void RemoveActor(const ActorId actor_id)
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:70
std::unordered_map< ActorId, cc::Timestamp > teleportation_instance
std::unordered_map< carla::ActorId, Buffer > BufferMap
Vector3D GetForwardVector() const
float Length() const
Definition: geom/Vector3D.h:49
void UpdateKinematicState(ActorId actor_id, KinematicState state)
cg::Location GetLocation(const ActorId actor_id) const
bool GetRespawnDormantVehicles() const
Method to retrieve if we are automatically respawning vehicles.
Definition: Parameters.cpp:416
geom::Location Location
Definition: rpc/Location.h:14
const Timestamp & GetTimestamp() const
Get timestamp of this snapshot.
Definition: WorldSnapshot.h:34
std::unordered_map< ActorId, StateEntry > pid_state_map
Structure to hold the actuation signals.
bool IsDormant(const ActorId actor_id) const
cg::Rotation GetRotation(const ActorId actor_id) const
float GetSpeedLimit(const ActorId actor_id) const
float GetTurnTargetVelocity(const Buffer &waypoint_buffer, float max_target_velocity)
#define SQUARE(a)
This file contains various constants used in traffic manager arranged into sensible namespaces for re...
Definition: Constants.h:13
double min(double v1, double v2)
Definition: Simplify.h:294
This is a simple wrapper class on Carla&#39;s waypoint object.
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...
float GetThreePointCircleRadius(cg::Location first_location, cg::Location middle_location, cg::Location last_location)
std::vector< SimpleWaypointPtr > NodeList
Definition: InMemoryMap.h:47
crd::JuncId GeoGridId
Definition: InMemoryMap.h:48
bool GetSynchronousMode() const
Method to get synchronous mode.
Definition: Parameters.cpp:245
const std::vector< float > highway_lateral_parameters
const LocalizationFrame & localization_frame
const std::vector< float > urban_longitudinal_parameters
bool SafeAfterJunction(const LocalizationData &localization, const bool tl_hazard, const bool collision_emergency_stop)
cg::Vector3D GetHeading(const ActorId actor_id) const
float GetUpperBoundaryRespawnDormantVehicles() const
Method to retrieve maximum distance from hero vehicle when respawning vehicles.
Definition: Parameters.cpp:426
bool IsPhysicsEnabled(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
auto Distance(const Location &loc) const
Definition: geom/Location.h:48
float GetLowerBoundaryRespawnDormantVehicles() const
Method to retrieve minimum distance from hero vehicle when respawning vehicles.
Definition: Parameters.cpp:421
Structure to hold the controller state.
void UpdateKinematicHybridEndLocation(ActorId actor_id, cg::Location location)
void AddTakenGrid(const GeoGridId geogrid_id, const ActorId actor_id)
ActuationSignal RunStep(StateEntry present_state, StateEntry previous_state, const std::vector< float > &longitudinal_parameters, const std::vector< float > &lateral_parameters)
This function calculates the actuation signals based on the resent state change of the vehicle to min...
Definition: PIDController.h:27
cg::Location GetHeroLocation() const
bool IsGeoGridFree(const GeoGridId geogrid_id) const
ActorIdSet GetPassingVehicles(uint64_t waypoint_id) const
std::pair< bool, float > CollisionHandling(const CollisionHazardData &collision_hazard, const bool tl_hazard, const cg::Vector3D ego_velocity, const cg::Vector3D ego_heading, const float max_target_velocity)
cg::Vector3D GetVelocity(const ActorId actor_id) const
Vector3D MakeSafeUnitVector(const float epsilon) const
Definition: geom/Vector3D.h:72
WorldSnapshot GetSnapshot() const
Return a snapshot of the world at this moment.
Definition: World.cpp:94
const std::vector< float > urban_lateral_parameters
double elapsed_seconds
Simulated seconds elapsed since the beginning of the current episode.
Definition: Timestamp.h:33
geom::Transform Transform
Definition: rpc/Transform.h:16
WaypointPtr GetWaypoint() const
Returns a carla::shared_ptr to carla::waypoint.
MotionPlanStage(const std::vector< ActorId > &vehicle_id_list, SimulationState &simulation_state, const Parameters &parameters, const BufferMap &buffer_map, TrackTraffic &track_traffic, const std::vector< float > &urban_longitudinal_parameters, const std::vector< float > &highway_longitudinal_parameters, const std::vector< float > &urban_lateral_parameters, const std::vector< float > &highway_lateral_parameters, const LocalizationFrame &localization_frame, const CollisionFrame &collision_frame, const TLFrame &tl_frame, const cc::World &world, ControlFrame &output_array, RandomGenerator &random_device, const LocalMapPtr &local_map)
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
Definition: road/Map.cpp:40
std::vector< CollisionHazardData > CollisionFrame