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  const 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  RandomGeneratorMap &random_devices,
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_devices(random_devices),
61  local_map(local_map) {
62 
63  // Adding structure to avoid retrieving traffic lights when checking for landmarks.
64  std::vector<SharedPtr<cc::Landmark>> traffic_lights = world.GetMap()->GetAllLandmarksOfType("1000001");
65  for (auto &tl : traffic_lights) {
66  std::string landmark_id = tl->GetId();
67  SharedPtr<cc::Actor> actor = world.GetTrafficLight(*tl);
68  tl_map.insert({landmark_id, actor});
69  }
70  }
71 
72 void MotionPlanStage::Update(const unsigned long index) {
73  const ActorId actor_id = vehicle_id_list.at(index);
74  const cg::Location vehicle_location = simulation_state.GetLocation(actor_id);
75  const cg::Vector3D vehicle_velocity = simulation_state.GetVelocity(actor_id);
76  const cg::Rotation vehicle_rotation = simulation_state.GetRotation(actor_id);
77  const float vehicle_speed = vehicle_velocity.Length();
78  const cg::Vector3D vehicle_heading = simulation_state.GetHeading(actor_id);
79  const bool vehicle_physics_enabled = simulation_state.IsPhysicsEnabled(actor_id);
80  const Buffer &waypoint_buffer = buffer_map.at(actor_id);
81  const LocalizationData &localization = localization_frame.at(index);
82  const CollisionHazardData &collision_hazard = collision_frame.at(index);
83  const bool &tl_hazard = tl_frame.at(index);
85  StateEntry current_state;
86 
87  // Instanciating teleportation transform as current vehicle transform.
88  cg::Transform teleportation_transform = cg::Transform(vehicle_location, vehicle_rotation);
89 
90  // Get information about the hero location from the actor_id state.
91  cg::Location hero_location = track_traffic.GetHeroLocation();
92  bool is_hero_alive = hero_location != cg::Location(0, 0, 0);
93 
94  if (simulation_state.IsDormant(actor_id) && parameters.GetRespawnDormantVehicles() && is_hero_alive) {
95  // Flushing controller state for vehicle.
96  current_state = {current_timestamp,
97  0.0f, 0.0f,
98  0.0f};
99 
100  // Add entry to teleportation duration clock table if not present.
101  if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
102  teleportation_instance.insert({actor_id, current_timestamp});
103  }
104 
105  // Get lower and upper bound for teleporting vehicle.
108  float dilate_factor = (upper_bound-lower_bound)/100.0f;
109 
110  // Measuring time elapsed since last teleportation for the vehicle.
111  double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
112 
113  if (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT) {
114  float random_sample = (static_cast<float>(random_devices.at(actor_id).next())*dilate_factor) + lower_bound;
115  NodeList teleport_waypoint_list = local_map->GetWaypointsInDelta(hero_location, ATTEMPTS_TO_TELEPORT, random_sample);
116  if (!teleport_waypoint_list.empty()) {
117  for (auto &teleport_waypoint : teleport_waypoint_list) {
118  GeoGridId geogrid_id = teleport_waypoint->GetGeodesicGridId();
119  if (track_traffic.IsGeoGridFree(geogrid_id)) {
120  teleportation_transform = teleport_waypoint->GetTransform();
121  teleportation_transform.location.z += 0.5f;
122  track_traffic.AddTakenGrid(geogrid_id, actor_id);
123  break;
124  }
125  }
126  }
127  }
128  output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
129  }
130 
131  else {
132 
133  // Target velocity for vehicle.
134  const float vehicle_speed_limit = simulation_state.GetSpeedLimit(actor_id);
135  float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, vehicle_speed_limit) / 3.6f;
136 
137  // Algorithm to reduce speed near landmarks
138  float max_landmark_target_velocity = GetLandmarkTargetVelocity(*(waypoint_buffer.at(0)), vehicle_location, actor_id, max_target_velocity);
139 
140  // Algorithm to reduce speed near turns
141  float max_turn_target_velocity = GetTurnTargetVelocity(waypoint_buffer, max_target_velocity);
142  max_target_velocity = std::min(std::min(max_target_velocity, max_landmark_target_velocity), max_turn_target_velocity);
143 
144  // Collision handling and target velocity correction.
145  std::pair<bool, float> collision_response = CollisionHandling(collision_hazard, tl_hazard, vehicle_velocity,
146  vehicle_heading, max_target_velocity);
147  bool collision_emergency_stop = collision_response.first;
148  float dynamic_target_velocity = collision_response.second;
149 
150  // Don't enter junction if there isn't enough free space after the junction.
151  bool safe_after_junction = SafeAfterJunction(localization, tl_hazard, collision_emergency_stop);
152 
153  // In case of collision or traffic light hazard.
154  bool emergency_stop = tl_hazard || collision_emergency_stop || !safe_after_junction;
155 
156  if (vehicle_physics_enabled && !simulation_state.IsDormant(actor_id)) {
157  ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
158 
159  const float target_point_distance = std::max(vehicle_speed * TARGET_WAYPOINT_TIME_HORIZON,
161  const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
162  const cg::Location target_location = target_waypoint->GetLocation();
163  float dot_product = DeviationDotProduct(vehicle_location, vehicle_heading, target_location);
164  float cross_product = DeviationCrossProduct(vehicle_location, vehicle_heading, target_location);
165  dot_product = acos(dot_product) / PI;
166  if (cross_product < 0.0f) {
167  dot_product *= -1.0f;
168  }
169  const float angular_deviation = dot_product;
170  const float velocity_deviation = (dynamic_target_velocity - vehicle_speed) / dynamic_target_velocity;
171  // If previous state for vehicle not found, initialize state entry.
172  if (pid_state_map.find(actor_id) == pid_state_map.end()) {
173  const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f};
174  pid_state_map.insert({actor_id, initial_state});
175  }
176 
177  // Retrieving the previous state.
178  traffic_manager::StateEntry previous_state;
179  previous_state = pid_state_map.at(actor_id);
180 
181  // Select PID parameters.
182  std::vector<float> longitudinal_parameters;
183  std::vector<float> lateral_parameters;
184  if (vehicle_speed > HIGHWAY_SPEED) {
185  longitudinal_parameters = highway_longitudinal_parameters;
186  lateral_parameters = highway_lateral_parameters;
187  } else {
188  longitudinal_parameters = urban_longitudinal_parameters;
189  lateral_parameters = urban_lateral_parameters;
190  }
191 
192  // If physics is enabled for the vehicle, use PID controller.
193  // State update for vehicle.
194  current_state = {current_timestamp, angular_deviation, velocity_deviation, 0.0f};
195 
196  // Controller actuation.
197  actuation_signal = PID::RunStep(current_state, previous_state,
198  longitudinal_parameters, lateral_parameters);
199 
200  if (emergency_stop) {
201  actuation_signal.throttle = 0.0f;
202  actuation_signal.brake = 1.0f;
203  }
204 
205  // Constructing the actuation signal.
206 
207  carla::rpc::VehicleControl vehicle_control;
208  vehicle_control.throttle = actuation_signal.throttle;
209  vehicle_control.brake = actuation_signal.brake;
210  vehicle_control.steer = actuation_signal.steer;
211 
212  output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
213 
214  // Updating PID state.
215  current_state.steer = actuation_signal.steer;
216  StateEntry &state = pid_state_map.at(actor_id);
217  state = current_state;
218 
219  }
220  // For physics-less vehicles, determine position and orientation for teleportation.
221  else {
222  // Flushing controller state for vehicle.
223  current_state = {current_timestamp,
224  0.0f, 0.0f,
225  0.0f};
226 
227  // Add entry to teleportation duration clock table if not present.
228  if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
229  teleportation_instance.insert({actor_id, current_timestamp});
230  }
231 
232  // Measuring time elapsed since last teleportation for the vehicle.
233  double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
234 
235  // Find a location ahead of the vehicle for teleportation to achieve intended velocity.
236  if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT)) {
237 
238  // Target displacement magnitude to achieve target velocity.
239  const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT_FL;
240  SimpleWaypointPtr teleport_target = waypoint_buffer.front();
241  cg::Transform target_base_transform = teleport_target->GetTransform();
242  cg::Location target_base_location = target_base_transform.location;
243  cg::Vector3D target_heading = target_base_transform.GetForwardVector();
244  cg::Vector3D correct_heading = (target_base_location - vehicle_location).MakeSafeUnitVector(EPSILON);
245 
246  if (vehicle_location.Distance(target_base_location) < target_displacement) {
247  cg::Location teleportation_location = vehicle_location + cg::Location(target_heading.MakeSafeUnitVector(EPSILON) * target_displacement);
248  teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
249  }
250  else {
251  cg::Location teleportation_location = vehicle_location + cg::Location(correct_heading * target_displacement);
252  teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
253  }
254  // In case of an emergency stop, stay in the same location.
255  // Also, teleport only once every dt in asynchronous mode.
256  } else {
257  teleportation_transform = cg::Transform(vehicle_location, simulation_state.GetRotation(actor_id));
258  }
259  // Constructing the actuation signal.
260  output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
261  }
262  }
263 }
264 
266  const bool tl_hazard,
267  const bool collision_emergency_stop) {
268 
269  SimpleWaypointPtr junction_end_point = localization.junction_end_point;
270  SimpleWaypointPtr safe_point = localization.safe_point;
271 
272  bool safe_after_junction = true;
273  if (!tl_hazard && !collision_emergency_stop
274  && localization.is_at_junction_entrance
275  && junction_end_point != nullptr && safe_point != nullptr
276  && junction_end_point->DistanceSquared(safe_point) > SQUARE(MIN_SAFE_INTERVAL_LENGTH)) {
277 
278  ActorIdSet passing_safe_point = track_traffic.GetPassingVehicles(safe_point->GetId());
279  ActorIdSet passing_junction_end_point = track_traffic.GetPassingVehicles(junction_end_point->GetId());
280  cg::Location mid_point = (junction_end_point->GetLocation() + safe_point->GetLocation())/2.0f;
281 
282  // Only check for vehicles that have the safe point in their passing waypoint, but not
283  // the junction end point.
284  ActorIdSet difference;
285  std::set_difference(passing_safe_point.begin(), passing_safe_point.end(),
286  passing_junction_end_point.begin(), passing_junction_end_point.end(),
287  std::inserter(difference, difference.begin()));
288  if (difference.size() > 0) {
289  for (const ActorId &blocking_id: difference) {
290  cg::Location blocking_actor_location = simulation_state.GetLocation(blocking_id);
291  if (cg::Math::DistanceSquared(blocking_actor_location, mid_point) < SQUARE(MAX_JUNCTION_BLOCK_DISTANCE)
293  safe_after_junction = false;
294  break;
295  }
296  }
297  }
298  }
299 
300  return safe_after_junction;
301 }
302 
303 std::pair<bool, float> MotionPlanStage::CollisionHandling(const CollisionHazardData &collision_hazard,
304  const bool tl_hazard,
305  const cg::Vector3D vehicle_velocity,
306  const cg::Vector3D vehicle_heading,
307  const float max_target_velocity) {
308  bool collision_emergency_stop = false;
309  float dynamic_target_velocity = max_target_velocity;
310  const float vehicle_speed = vehicle_velocity.Length();
311 
312  if (collision_hazard.hazard && !tl_hazard) {
313  const ActorId other_actor_id = collision_hazard.hazard_actor_id;
314  const cg::Vector3D other_velocity = simulation_state.GetVelocity(other_actor_id);
315  const float vehicle_relative_speed = (vehicle_velocity - other_velocity).Length();
316  const float available_distance_margin = collision_hazard.available_distance_margin;
317 
318  const float other_speed_along_heading = cg::Math::Dot(other_velocity, vehicle_heading);
319 
320  // Consider collision avoidance decisions only if there is positive relative velocity
321  // of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle).
322  if (vehicle_relative_speed > EPSILON_RELATIVE_SPEED) {
323  // If other vehicle is approaching lead vehicle and lead vehicle is further
324  // than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
325  float follow_lead_distance = FOLLOW_LEAD_FACTOR * vehicle_speed + MIN_FOLLOW_LEAD_DISTANCE;
326  if (available_distance_margin > follow_lead_distance) {
327  // Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE
328  // by maintaining a relative speed of other_speed_along_heading
329  dynamic_target_velocity = other_speed_along_heading;
330  }
331  // If vehicle is approaching a lead vehicle and the lead vehicle is further
332  // than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE.
333  else if (available_distance_margin > CRITICAL_BRAKING_MARGIN) {
334  // Then follow the lead vehicle by acquiring it's speed along current heading.
335  dynamic_target_velocity = std::max(other_speed_along_heading, RELATIVE_APPROACH_SPEED);
336  } else {
337  // If lead vehicle closer than CRITICAL_BRAKING_MARGIN, initiate emergency stop.
338  collision_emergency_stop = true;
339  }
340  }
341  if (available_distance_margin < CRITICAL_BRAKING_MARGIN) {
342  collision_emergency_stop = true;
343  }
344  }
345 
346  float max_gradual_velocity = PERC_MAX_SLOWDOWN * vehicle_speed;
347  if (dynamic_target_velocity < vehicle_speed - max_gradual_velocity) {
348  // Don't slow more than PERC_MAX_SLOWDOWN per frame.
349  dynamic_target_velocity = vehicle_speed - max_gradual_velocity;
350  }
351  dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity);
352 
353  return {collision_emergency_stop, dynamic_target_velocity};
354 }
355 
357  const cg::Location vehicle_location,
358  const ActorId actor_id,
359  float max_target_velocity) {
360 
361  auto const max_distance = LANDMARK_DETECTION_TIME * max_target_velocity;
362 
363  float landmark_target_velocity = std::numeric_limits<float>::max();
364 
365  auto all_landmarks = waypoint.GetWaypoint()->GetAllLandmarksInDistance(max_distance, false);
366 
367  for (auto &landmark: all_landmarks) {
368 
369  auto landmark_location = landmark->GetWaypoint()->GetTransform().location;
370  auto landmark_type = landmark->GetType();
371  auto distance = landmark_location.Distance(vehicle_location);
372 
373  if (distance > max_distance) {
374  continue;
375  }
376 
377  float minimum_velocity = max_target_velocity;
378  if (landmark_type == "1000001") { // Traffic light
379  auto landmark_id = landmark->GetId();
380  if (tl_map.find(landmark_id) != tl_map.end()) {
381  auto actor = tl_map.at(landmark_id);
382  if (actor != nullptr) {
383 
384  cc::TrafficLight* tl = static_cast<cc::TrafficLight*>(actor.get());
385  auto state = tl->GetState();
386 
388  minimum_velocity = TL_GREEN_TARGET_VELOCITY;
390  minimum_velocity = TL_RED_TARGET_VELOCITY;
391  } else if (state == carla::rpc::TrafficLightState::Unknown){
392  minimum_velocity = TL_UNKNOWN_TARGET_VELOCITY;
393  } else {
394  // Traffic light is off
395  continue;
396  }
397  } else {
398  // It is a traffic light, but it's not present in our structure
399  minimum_velocity = TL_UNKNOWN_TARGET_VELOCITY;
400  }
401  } else {
402  // It is a traffic light, but it's not present in our structure
403  minimum_velocity = TL_UNKNOWN_TARGET_VELOCITY;
404  }
405  } else if (landmark_type == "206") { // Stop
406  minimum_velocity = STOP_TARGET_VELOCITY;
407  } else if (landmark_type == "205") { // Yield
408  minimum_velocity = YIELD_TARGET_VELOCITY;
409  } else if (landmark_type == "274") { // Speed limit
410  float value = static_cast<float>(landmark->GetValue()) / 3.6f;
411  value = parameters.GetVehicleTargetVelocity(actor_id, value);
412  minimum_velocity = (value < max_target_velocity) ? value : max_target_velocity;
413  } else {
414  continue;
415  }
416 
417  float v = std::max(((max_target_velocity - minimum_velocity) / max_distance) * distance + minimum_velocity, minimum_velocity);
418  landmark_target_velocity = std::min(landmark_target_velocity, v);
419  }
420 
421  return landmark_target_velocity;
422 }
423 
424 float MotionPlanStage::GetTurnTargetVelocity(const Buffer &waypoint_buffer,
425  float max_target_velocity) {
426 
427  if (waypoint_buffer.size() < 3) {
428  return max_target_velocity;
429  }
430  else {
431  const SimpleWaypointPtr first_waypoint = waypoint_buffer.front();
432  const SimpleWaypointPtr last_waypoint = waypoint_buffer.back();
433  const SimpleWaypointPtr middle_waypoint = waypoint_buffer.at(static_cast<uint16_t>(waypoint_buffer.size() / 2));
434 
435  float radius = GetThreePointCircleRadius(first_waypoint->GetLocation(),
436  middle_waypoint->GetLocation(),
437  last_waypoint->GetLocation());
438 
439  // Return the max velocity at the turn
440  return std::sqrt(radius * FRICTION * GRAVITY);
441  }
442 }
443 
445  cg::Location middle_location,
446  cg::Location last_location) {
447 
448  float x1 = first_location.x;
449  float y1 = first_location.y;
450  float x2 = middle_location.x;
451  float y2 = middle_location.y;
452  float x3 = last_location.x;
453  float y3 = last_location.y;
454 
455  float x12 = x1 - x2;
456  float x13 = x1 - x3;
457  float y12 = y1 - y2;
458  float y13 = y1 - y3;
459  float y31 = y3 - y1;
460  float y21 = y2 - y1;
461  float x31 = x3 - x1;
462  float x21 = x2 - x1;
463 
464  float sx13 = x1 * x1 - x3 * x3;
465  float sy13 = y1 * y1 - y3 * y3;
466  float sx21 = x2 * x2 - x1 * x1;
467  float sy21 = y2 * y2 - y1 * y1;
468 
469  float f_denom = 2 * (y31 * x12 - y21 * x13);
470  if (f_denom == 0) {
471  return std::numeric_limits<float>::max();
472  }
473  float f = (sx13 * x12 + sy13 * x12 + sx21 * x13 + sy21 * x13) / f_denom;
474 
475  float g_denom = 2 * (x31 * y12 - x21 * y13);
476  if (g_denom == 0) {
477  return std::numeric_limits<float>::max();
478  }
479  float g = (sx13 * y12 + sy13 * y12 + sx21 * y13 + sy21 * y13) / g_denom;
480 
481  float c = - (x1 * x1 + y1 * y1) - 2 * g * x1 - 2 * f * y1;
482  float h = -g;
483  float k = -f;
484 
485  return std::sqrt(h * h + k * k - c);
486 }
487 
488 void MotionPlanStage::RemoveActor(const ActorId actor_id) {
489  pid_state_map.erase(actor_id);
490  teleportation_instance.erase(actor_id);
491 }
492 
494  pid_state_map.clear();
495  teleportation_instance.clear();
496 }
497 
498 } // namespace traffic_manager
499 } // 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:171
const std::vector< float > highway_longitudinal_parameters
void Update(const unsigned long index)
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)
MotionPlanStage(const std::vector< ActorId > &vehicle_id_list, const 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, RandomGeneratorMap &random_devices, const LocalMapPtr &local_map)
boost::shared_ptr< T > SharedPtr
Use this SharedPtr (boost::shared_ptr) to keep compatibility with boost::python, but it would be nice...
Definition: Memory.h:20
std::shared_ptr< InMemoryMap > LocalMapPtr
Definition: ALSM.h:35
This class holds the state of all the vehicles in the simlation.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
SharedPtr< Map > GetMap() const
Return the map that describes this world.
Definition: World.cpp:24
std::unordered_map< carla::rpc::ActorId, RandomGenerator > RandomGeneratorMap
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
cg::Location GetLocation(const ActorId actor_id) const
const SimulationState & simulation_state
bool GetRespawnDormantVehicles() const
Method to retrieve if we are automatically respawning vehicles.
Definition: Parameters.cpp:292
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
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:40
crd::JuncId GeoGridId
Definition: InMemoryMap.h:41
bool GetSynchronousMode() const
Method to get synchronous mode.
Definition: Parameters.cpp:163
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
rpc::TrafficLightState GetState() const
Return the current state of the traffic light.
float GetUpperBoundaryRespawnDormantVehicles() const
Method to retrieve maximum distance from hero vehicle when respawning vehicles.
Definition: Parameters.cpp:302
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:42
float GetLowerBoundaryRespawnDormantVehicles() const
Method to retrieve minimum distance from hero vehicle when respawning vehicles.
Definition: Parameters.cpp:297
Structure to hold the controller state.
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:64
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
SharedPtr< Actor > GetTrafficLight(const Landmark &landmark) const
Definition: World.cpp:179
geom::Transform Transform
Definition: rpc/Transform.h:16
WaypointPtr GetWaypoint() const
Returns a carla::shared_ptr to carla::waypoint.
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:31
std::vector< CollisionHazardData > CollisionFrame