CARLA
MotionPlanStage.cpp
Go to the documentation of this file.
1 
4 
6 
7 namespace carla {
8 namespace traffic_manager {
9 
10 using namespace constants::MotionPlan;
11 using namespace constants::WaypointSelection;
12 using namespace constants::SpeedThreshold;
13 
16 
18  const std::vector<ActorId> &vehicle_id_list,
19  const SimulationState &simulation_state,
20  const Parameters &parameters,
21  const BufferMap &buffer_map,
22  const TrackTraffic &track_traffic,
23  const std::vector<float> &urban_longitudinal_parameters,
24  const std::vector<float> &highway_longitudinal_parameters,
25  const std::vector<float> &urban_lateral_parameters,
26  const std::vector<float> &highway_lateral_parameters,
27  const LocalizationFrame &localization_frame,
28  const CollisionFrame&collision_frame,
29  const TLFrame &tl_frame,
30  const cc::World &world,
31  ControlFrame &output_array)
32  : vehicle_id_list(vehicle_id_list),
33  simulation_state(simulation_state),
34  parameters(parameters),
35  buffer_map(buffer_map),
36  track_traffic(track_traffic),
37  urban_longitudinal_parameters(urban_longitudinal_parameters),
38  highway_longitudinal_parameters(highway_longitudinal_parameters),
39  urban_lateral_parameters(urban_lateral_parameters),
40  highway_lateral_parameters(highway_lateral_parameters),
41  localization_frame(localization_frame),
42  collision_frame(collision_frame),
43  tl_frame(tl_frame),
44  world(world),
45  output_array(output_array) {}
46 
47 void MotionPlanStage::Update(const unsigned long index) {
48  const ActorId actor_id = vehicle_id_list.at(index);
49  const cg::Location ego_location = simulation_state.GetLocation(actor_id);
50  const cg::Vector3D ego_velocity = simulation_state.GetVelocity(actor_id);
51  const float ego_speed = ego_velocity.Length();
52  const cg::Vector3D ego_heading = simulation_state.GetHeading(actor_id);
53  const bool ego_physics_enabled = simulation_state.IsPhysicsEnabled(actor_id);
54  const Buffer &waypoint_buffer = buffer_map.at(actor_id);
55  const LocalizationData &localization = localization_frame.at(index);
56  const CollisionHazardData &collision_hazard = collision_frame.at(index);
57  const bool &tl_hazard = tl_frame.at(index);
58 
59  const float target_point_distance = std::max(ego_speed * TARGET_WAYPOINT_TIME_HORIZON,
61  const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
62  const cg::Location target_location = target_waypoint->GetLocation();
63  float dot_product = DeviationDotProduct(ego_location, ego_heading, target_location);
64  float cross_product = DeviationCrossProduct(ego_location, ego_heading, target_location);
65  dot_product = 1.0f - dot_product;
66  if (cross_product < 0.0f) {
67  dot_product *= -1.0f;
68  }
69  const float current_deviation = dot_product;
71 
72  // If previous state for vehicle not found, initialize state entry.
73  if (pid_state_map.find(actor_id) == pid_state_map.end()) {
74  const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f, 0.0f};
75  pid_state_map.insert({actor_id, initial_state});
76  }
77 
78  // Retrieving the previous state.
79  traffic_manager::StateEntry previous_state;
80  previous_state = pid_state_map.at(actor_id);
81 
82  // Select PID parameters.
83  std::vector<float> longitudinal_parameters;
84  std::vector<float> lateral_parameters;
85  if (ego_speed > HIGHWAY_SPEED) {
86  longitudinal_parameters = highway_longitudinal_parameters;
87  lateral_parameters = highway_lateral_parameters;
88  } else {
89  longitudinal_parameters = urban_longitudinal_parameters;
90  lateral_parameters = urban_lateral_parameters;
91  }
92 
93  // Target velocity for vehicle.
94  const float ego_speed_limit = simulation_state.GetSpeedLimit(actor_id);
95  float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, ego_speed_limit) / 3.6f;
96 
97  // Collision handling and target velocity correction.
98  std::pair<bool, float> collision_response = CollisionHandling(collision_hazard, tl_hazard, ego_velocity,
99  ego_heading, max_target_velocity);
100  bool collision_emergency_stop = collision_response.first;
101  float dynamic_target_velocity = collision_response.second;
102 
103  // Don't enter junction if there isn't enough free space after the junction.
104  bool safe_after_junction = SafeAfterJunction(localization, tl_hazard, collision_emergency_stop);
105 
106  // In case of collision or traffic light hazard.
107  bool emergency_stop = tl_hazard || collision_emergency_stop || !safe_after_junction;
108 
109  ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
110  cg::Transform teleportation_transform;
111 
112  // If physics is enabled for the vehicle, use PID controller.
113  StateEntry current_state;
114  if (ego_physics_enabled) {
115 
116  // State update for vehicle.
117  current_state = PID::StateUpdate(previous_state, ego_speed, dynamic_target_velocity,
118  current_deviation, current_timestamp);
119 
120  // Controller actuation.
121  actuation_signal = PID::RunStep(current_state, previous_state,
122  longitudinal_parameters, lateral_parameters);
123 
124  if (emergency_stop) {
125 
126  current_state.deviation_integral = 0.0f;
127  current_state.velocity_integral = 0.0f;
128  actuation_signal.throttle = 0.0f;
129  actuation_signal.brake = 1.0f;
130  }
131  }
132  // For physics-less vehicles, determine position and orientation for teleportation.
133  else {
134  // Flushing controller state for vehicle.
135  current_state = {current_timestamp,
136  0.0f, 0.0f,
137  0.0f, 0.0f};
138 
139  // Add entry to teleportation duration clock table if not present.
140  if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
141  teleportation_instance.insert({actor_id, current_timestamp});
142  }
143 
144  // Measuring time elapsed since last teleportation for the vehicle.
145  double elapsed_time = current_timestamp.elapsed_seconds - teleportation_instance.at(actor_id).elapsed_seconds;
146 
147  // Find a location ahead of the vehicle for teleportation to achieve intended velocity.
148  if (!emergency_stop && (parameters.GetSynchronousMode() || elapsed_time > HYBRID_MODE_DT)) {
149 
150  // Target displacement magnitude to achieve target velocity.
151  const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT_FL;
152  const SimpleWaypointPtr teleport_target_waypoint = GetTargetWaypoint(waypoint_buffer, target_displacement).first;
153 
154  // Construct target transform to accurately achieve desired velocity.
155  float missing_displacement = 0.0f;
156  const float base_displacement = teleport_target_waypoint->Distance(ego_location);
157  if (base_displacement < target_displacement) {
158  missing_displacement = target_displacement - base_displacement;
159  }
160  cg::Transform target_base_transform = teleport_target_waypoint->GetTransform();
161  cg::Location target_base_location = target_base_transform.location;
162  cg::Vector3D target_heading = target_base_transform.GetForwardVector();
163  cg::Location teleportation_location = target_base_location + cg::Location(target_heading * missing_displacement);
164  teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
165  }
166  // In case of an emergency stop, stay in the same location.
167  // Also, teleport only once every dt in asynchronous mode.
168  else {
169  teleportation_transform = cg::Transform(ego_location, simulation_state.GetRotation(actor_id));
170  }
171  }
172 
173  // Updating PID state.
174  StateEntry &state = pid_state_map.at(actor_id);
175  state = current_state;
176 
177  // Constructing the actuation signal.
178  if (ego_physics_enabled) {
179  carla::rpc::VehicleControl vehicle_control;
180  vehicle_control.throttle = actuation_signal.throttle;
181  vehicle_control.brake = actuation_signal.brake;
182  vehicle_control.steer = actuation_signal.steer;
183 
184  output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
185  } else {
186  output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
187  }
188 }
189 
191  const bool tl_hazard,
192  const bool collision_emergency_stop) {
193 
194  SimpleWaypointPtr junction_end_point = localization.junction_end_point;
195  SimpleWaypointPtr safe_point = localization.safe_point;
196 
197  bool safe_after_junction = true;
198 
199  if (!tl_hazard && !collision_emergency_stop
200  && localization.is_at_junction_entrance
201  && junction_end_point != nullptr && safe_point != nullptr
202  && junction_end_point->DistanceSquared(safe_point) > SQUARE(MIN_SAFE_INTERVAL_LENGTH)) {
203 
204  ActorIdSet initial_set = track_traffic.GetPassingVehicles(junction_end_point->GetId());
205  float safe_interval_length_squared = junction_end_point->DistanceSquared(safe_point);
206  cg::Location mid_point = (junction_end_point->GetLocation() + safe_point->GetLocation())/2.0f;
207 
208  // Scan through the safe interval and find if any vehicles are present in it
209  // by finding their occupied waypoints.
210  for (SimpleWaypointPtr current_waypoint = junction_end_point;
211  current_waypoint->DistanceSquared(junction_end_point) < safe_interval_length_squared && safe_after_junction;
212  current_waypoint = current_waypoint->GetNextWaypoint().front()) {
213 
214  ActorIdSet current_set = track_traffic.GetPassingVehicles(current_waypoint->GetId());
215  ActorIdSet difference;
216  std::set_difference(current_set.begin(), current_set.end(),
217  initial_set.begin(), initial_set.end(),
218  std::inserter(difference, difference.begin()));
219  if (difference.size() > 0) {
220  for (const ActorId &blocking_id: difference) {
221  cg::Location blocking_actor_location = simulation_state.GetLocation(blocking_id);
222  if (cg::Math::DistanceSquared(blocking_actor_location, mid_point) < SQUARE(MAX_JUNCTION_BLOCK_DISTANCE)
224  safe_after_junction = false;
225  }
226  }
227  }
228  }
229  }
230 
231  return safe_after_junction;
232 }
233 
234 std::pair<bool, float> MotionPlanStage::CollisionHandling(const CollisionHazardData &collision_hazard,
235  const bool tl_hazard,
236  const cg::Vector3D ego_velocity,
237  const cg::Vector3D ego_heading,
238  const float max_target_velocity) {
239  bool collision_emergency_stop = false;
240  float dynamic_target_velocity = max_target_velocity;
241 
242  if (collision_hazard.hazard && !tl_hazard) {
243  const ActorId other_actor_id = collision_hazard.hazard_actor_id;
244  const cg::Vector3D other_velocity = simulation_state.GetVelocity(other_actor_id);
245  const float ego_relative_speed = (ego_velocity - other_velocity).Length();
246  const float available_distance_margin = collision_hazard.available_distance_margin;
247 
248  const float other_speed_along_heading = cg::Math::Dot(other_velocity, ego_heading);
249 
250  // Consider collision avoidance decisions only if there is positive relative velocity
251  // of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle).
252  if (ego_relative_speed > EPSILON_RELATIVE_SPEED) {
253  // If other vehicle is approaching lead vehicle and lead vehicle is further
254  // than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
255  float follow_lead_distance = ego_relative_speed * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE;
256  if (available_distance_margin > follow_lead_distance) {
257  // Then reduce the gap between the vehicles till FOLLOW_LEAD_DISTANCE
258  // by maintaining a relative speed of RELATIVE_APPROACH_SPEED
259  dynamic_target_velocity = other_speed_along_heading + RELATIVE_APPROACH_SPEED;
260  }
261  // If vehicle is approaching a lead vehicle and the lead vehicle is further
262  // than CRITICAL_BRAKING_MARGIN but closer than FOLLOW_LEAD_DISTANCE.
263  else if (available_distance_margin > CRITICAL_BRAKING_MARGIN) {
264  // Then follow the lead vehicle by acquiring it's speed along current heading.
265  dynamic_target_velocity = std::max(other_speed_along_heading, RELATIVE_APPROACH_SPEED);
266  } else {
267  // If lead vehicle closer than CRITICAL_BRAKING_MARGIN, initiate emergency stop.
268  collision_emergency_stop = true;
269  }
270  }
271  if (available_distance_margin < CRITICAL_BRAKING_MARGIN) {
272  collision_emergency_stop = true;
273  }
274  }
275 
276  dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity);
277 
278  return {collision_emergency_stop, dynamic_target_velocity};
279 }
280 
281 void MotionPlanStage::RemoveActor(const ActorId actor_id) {
282  pid_state_map.erase(actor_id);
283  teleportation_instance.erase(actor_id);
284 }
285 
287  pid_state_map.clear();
288  teleportation_instance.clear();
289 }
290 
291 } // namespace traffic_manager
292 } // 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:155
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
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_set< ActorId > ActorIdSet
StateEntry StateUpdate(StateEntry previous_state, float current_velocity, float target_velocity, float angular_deviation, cc::Timestamp current_time)
This function calculates the present state of the vehicle including the accumulated integral componen...
Definition: PIDController.h:27
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:66
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
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.
cg::Rotation GetRotation(const ActorId actor_id) const
float GetSpeedLimit(const ActorId actor_id) const
#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...
bool GetSynchronousMode() const
Method to get synchronous mode.
Definition: Parameters.cpp:147
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
bool IsPhysicsEnabled(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
Structure to hold the controller state.
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:52
MotionPlanStage(const std::vector< ActorId > &vehicle_id_list, const SimulationState &simulation_state, const Parameters &parameters, const BufferMap &buffer_map, const 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)
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
WorldSnapshot GetSnapshot() const
Return a snapshot of the world at this moment.
Definition: World.cpp:87
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
std::vector< CollisionHazardData > CollisionFrame