CARLA
RssCheck.cpp
Go to the documentation of this file.
1 // Copyright (c) 2019-2020 Intel Corporation
2 //
3 // This work is licensed under the terms of the MIT license.
4 // For a copy, see <https://opensource.org/licenses/MIT>.
5 
6 #include "carla/rss/RssCheck.h"
7 
8 #include <spdlog/sinks/stdout_color_sinks.h>
9 #ifdef RSS_USE_TBB
10 #include <tbb/tbb.h>
11 #endif
12 #include <ad/map/access/Logging.hpp>
13 #include <ad/map/access/Operation.hpp>
14 #include <ad/map/intersection/Intersection.hpp>
15 #include <ad/map/lane/Operation.hpp>
16 #include <ad/map/match/AdMapMatching.hpp>
17 #include <ad/map/match/MapMatchedOperation.hpp>
18 #include <ad/map/route/LaneIntervalOperation.hpp>
19 #include <ad/map/route/Operation.hpp>
20 #include <ad/map/route/Planning.hpp>
21 #include <ad/rss/map/Logging.hpp>
22 #include <ad/rss/map/RssObjectConversion.hpp>
23 #include <ad/rss/map/RssObjectData.hpp>
24 #include <ad/rss/map/RssSceneCreator.hpp>
25 #include <ad/rss/state/RssStateOperation.hpp>
26 #include <chrono>
27 #include <tuple>
28 
29 #include "carla/client/Map.h"
31 #include "carla/client/Vehicle.h"
32 #include "carla/client/Walker.h"
33 #include "carla/client/Waypoint.h"
34 
35 #define DEBUG_TIMING 0
36 
37 namespace carla {
38 namespace rss {
39 
40 void printRoute(std::string const &route_descr, ::ad::map::route::FullRoute const &route) {
41  std::cout << route_descr << std::endl;
42  for (auto road_segment : route.roadSegments) {
43  for (auto lane_segment : road_segment.drivableLaneSegments) {
44  std::cout << "(" << static_cast<uint64_t>(lane_segment.laneInterval.laneId) << " | " << std::setprecision(2)
45  << static_cast<double>(lane_segment.laneInterval.start) << ":"
46  << static_cast<double>(lane_segment.laneInterval.end) << ") ";
47  }
48  std::cout << std::endl;
49  }
50 }
51 
52 // constants for deg-> rad conversion PI / 180
53 constexpr float to_radians = static_cast<float>(M_PI) / 180.0f;
54 
56  : time_since_epoch_check_start_ms(0.),
57  time_since_epoch_check_end_ms(0.),
58  ego_speed(0.),
59  min_stopping_distance(0.),
60  ego_center({0., 0., 0.}),
61  ego_heading(0.),
62  ego_heading_change(0.),
63  ego_steering_angle(0.),
64  ego_center_within_route(false),
65  crossing_border(false),
66  route_heading(0.),
67  route_nominal_center({0., 0., 0.}),
68  heading_diff(0.),
69  route_speed_lat(0.),
70  route_speed_lon(0.),
71  route_accel_lat(0.),
72  route_accel_lon(0.),
73  avg_route_accel_lat(0.),
74  avg_route_accel_lon(0.) {
75  timestamp.elapsed_seconds = 0.;
76 }
77 
78 std::shared_ptr<spdlog::logger> getLogger() {
79  static auto logger = spdlog::stdout_color_mt("RssCheck");
80  return logger;
81 }
82 
83 std::shared_ptr<spdlog::logger> getTimingLogger() {
84  static auto logger = spdlog::stdout_color_mt("RssCheckTiming");
85  return logger;
86 }
87 
88 ::ad::rss::world::RssDynamics RssCheck::GetDefaultVehicleDynamics() {
89  ::ad::rss::world::RssDynamics default_ego_vehicle_dynamics;
90  default_ego_vehicle_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(3.5);
91  default_ego_vehicle_dynamics.alphaLon.brakeMax = ::ad::physics::Acceleration(-8.);
92  default_ego_vehicle_dynamics.alphaLon.brakeMin = ::ad::physics::Acceleration(-4.);
93  default_ego_vehicle_dynamics.alphaLon.brakeMinCorrect = ::ad::physics::Acceleration(-3);
94  default_ego_vehicle_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.2);
95  default_ego_vehicle_dynamics.alphaLat.brakeMin = ::ad::physics::Acceleration(-0.8);
96  default_ego_vehicle_dynamics.lateralFluctuationMargin = ::ad::physics::Distance(0.1);
97  default_ego_vehicle_dynamics.responseTime = ::ad::physics::Duration(1.0);
98  default_ego_vehicle_dynamics.maxSpeedOnAcceleration = ::ad::physics::Speed(100.);
99  default_ego_vehicle_dynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0);
100  default_ego_vehicle_dynamics.unstructuredSettings.driveAwayMaxAngle = ad::physics::Angle(2.4);
101  default_ego_vehicle_dynamics.unstructuredSettings.vehicleYawRateChange = ad::physics::AngularAcceleration(0.3);
102  default_ego_vehicle_dynamics.unstructuredSettings.vehicleMinRadius = ad::physics::Distance(3.5);
103  default_ego_vehicle_dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = ad::physics::Duration(0.2);
104  return default_ego_vehicle_dynamics;
105 }
106 
107 ::ad::rss::world::RssDynamics RssCheck::GetDefaultPedestrianDynamics() {
108  ::ad::rss::world::RssDynamics default_pedestrian_dynamics;
109  default_pedestrian_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(2.0);
110  default_pedestrian_dynamics.alphaLon.brakeMax = ::ad::physics::Acceleration(-4.);
111  default_pedestrian_dynamics.alphaLon.brakeMin = ::ad::physics::Acceleration(-2.);
112  default_pedestrian_dynamics.alphaLon.brakeMinCorrect = ::ad::physics::Acceleration(-2.);
113  default_pedestrian_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.001);
114  default_pedestrian_dynamics.alphaLat.brakeMin = ::ad::physics::Acceleration(-0.001);
115  default_pedestrian_dynamics.lateralFluctuationMargin = ::ad::physics::Distance(0.1);
116  default_pedestrian_dynamics.responseTime = ::ad::physics::Duration(0.5);
117  default_pedestrian_dynamics.maxSpeedOnAcceleration = ::ad::physics::Speed(10.);
118  default_pedestrian_dynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0);
119  default_pedestrian_dynamics.unstructuredSettings.driveAwayMaxAngle = ad::physics::Angle(2.4);
120  default_pedestrian_dynamics.unstructuredSettings.vehicleYawRateChange = ad::physics::AngularAcceleration(0.3);
121  default_pedestrian_dynamics.unstructuredSettings.vehicleMinRadius = ad::physics::Distance(3.5);
122  default_pedestrian_dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = ad::physics::Duration(0.2);
123 
124  return default_pedestrian_dynamics;
125 }
126 
127 RssCheck::RssCheck(float maximum_steering_angle)
129  _logger = getLogger();
131  _timing_logger->set_level(spdlog::level::off);
132 
133  SetLogLevel(spdlog::level::warn);
134  SetMapLogLevel(spdlog::level::warn);
135 
138  // set the response time of others vehicles to 2 seconds; the rest stays the same
139  _default_actor_constellation_callback_other_vehicle_dynamics.responseTime = ::ad::physics::Duration(2.0);
141 
142  // Create a default callback.
144  [this](carla::SharedPtr<::carla::rss::ActorConstellationData> actor_constellation_data)
146  ::carla::rss::ActorConstellationResult actor_constellation_result;
147 
148  actor_constellation_result.rss_calculation_mode = ::ad::rss::map::RssMode::NotRelevant;
149  actor_constellation_result.restrict_speed_limit_mode =
150  ::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode::IncreasedSpeedLimit10;
151  actor_constellation_result.actor_object_type = ad::rss::world::ObjectType::Invalid;
154 
155  if (actor_constellation_data->other_actor != nullptr) {
156  if (boost::dynamic_pointer_cast<carla::client::Walker>(actor_constellation_data->other_actor) != nullptr) {
157  actor_constellation_result.rss_calculation_mode = ::ad::rss::map::RssMode::Unstructured;
158  actor_constellation_result.actor_object_type = ad::rss::world::ObjectType::Pedestrian;
160  } else if (boost::dynamic_pointer_cast<carla::client::Vehicle>(actor_constellation_data->other_actor) !=
161  nullptr) {
162  actor_constellation_result.rss_calculation_mode = ::ad::rss::map::RssMode::Structured;
163  actor_constellation_result.actor_object_type = ad::rss::world::ObjectType::OtherVehicle;
164 
165  if (GetSpeed(*actor_constellation_data->other_actor) == ::ad::physics::Speed(0.)) {
166  /*
167  special handling for vehicles standing still
168  to cope with not yet implemented lateral intersection checks in core RSS implementation
169  if the other is standing still, we don't assume that he will accelerate
170  otherwise if standing at the intersection the acceleration within reaction time
171  will allow to enter the intersection which current RSS implementation will immediately consider
172  as dangerous
173  */
174  actor_constellation_result.actor_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(0.);
175  }
176  }
177  }
178 
179  /* since the ego vehicle is controlled manually, it is easy possible that the ego vehicle
180  accelerates far more in lateral direction than the ego_dynamics indicate
181  in an automated vehicle this would be considered by the low-level controller when the RSS restriction
182  is taken into account properly
183  but the simple RSS restrictor within CARLA is not able to do so...
184  So we should at least tell RSS about the fact that we acceleration more than this
185  to be able to react on this
186  */
187  auto const abs_avg_route_accel_lat = std::fabs(actor_constellation_data->ego_dynamics_on_route.avg_route_accel_lat);
188  if (abs_avg_route_accel_lat > actor_constellation_result.ego_vehicle_dynamics.alphaLat.accelMax) {
189  actor_constellation_result.ego_vehicle_dynamics.alphaLat.accelMax =
190  std::min(ad::physics::Acceleration(20.), abs_avg_route_accel_lat);
191  }
192  return actor_constellation_result;
193  };
194 
195  // set the default dynamics
197 
198  _logger->debug("RssCheck with default actor constellation callback created");
199 }
200 
201 RssCheck::RssCheck(float maximum_steering_angle,
202  ActorConstellationCallbackFunctionType rss_actor_constellation_callback,
203  carla::SharedPtr<carla::client::Actor> const &carla_ego_actor)
204  : _maximum_steering_angle(maximum_steering_angle),
205  _actor_constellation_callback(rss_actor_constellation_callback),
207  _logger = getLogger();
209  _timing_logger->set_level(spdlog::level::off);
210 
211  SetLogLevel(spdlog::level::warn);
212  SetMapLogLevel(spdlog::level::warn);
213 
214  _carla_rss_state.ego_match_object = GetMatchObject(carla_ego_actor, ::ad::physics::Distance(2.0));
216 
217  _logger->debug("RssCheck created");
218 }
219 
221 
222 void RssCheck::SetLogLevel(const spdlog::level::level_enum &log_level) {
223  spdlog::set_level(log_level);
224  _logger->set_level(log_level);
225 }
226 
227 void RssCheck::SetMapLogLevel(const spdlog::level::level_enum &map_log_level) {
228  ::ad::map::access::getLogger()->set_level(map_log_level);
229  ::ad::rss::map::getLogger()->set_level(map_log_level);
230 }
231 
232 const ::ad::rss::world::RssDynamics &RssCheck::GetDefaultActorConstellationCallbackEgoVehicleDynamics() const {
234 }
235 
237  const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics) {
239 }
240 
241 const ::ad::rss::world::RssDynamics &RssCheck::GetDefaultActorConstellationCallbackOtherVehicleDynamics() const {
243 }
244 
246  const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) {
248 }
249 
250 const ::ad::rss::world::RssDynamics &RssCheck::GetDefaultActorConstellationCallbackPedestrianDynamics() const {
252 }
253 
255  const ::ad::rss::world::RssDynamics &pedestrian_dynamics) {
257 }
258 
260  return _road_boundaries_mode;
261 }
262 
264  _road_boundaries_mode = road_boundaries_mode;
265 }
266 
268  _routing_targets_to_append.push_back(
269  ::ad::map::point::createENUPoint(routing_target.location.x, -1. * routing_target.location.y, 0.));
270 }
271 
272 const std::vector<::carla::geom::Transform> RssCheck::GetRoutingTargets() const {
273  std::vector<::carla::geom::Transform> routing_targets;
274  if (withinValidInputRange(_routing_targets)) {
275  for (auto const &target : _routing_targets) {
276  ::carla::geom::Transform routing_target;
277  routing_target.location.x = static_cast<float>(target.x);
278  routing_target.location.y = static_cast<float>(-target.y);
279  routing_target.location.z = 0.f;
280  routing_targets.push_back(routing_target);
281  }
282  }
283  return routing_targets;
284 }
285 
287  _routing_targets.clear();
289 }
290 
292  _logger->debug("Dropping Route:: {}", _carla_rss_state.ego_route);
293  _carla_rss_state.ego_route = ::ad::map::route::FullRoute();
294 }
295 
298  carla::SharedPtr<carla::client::Actor> const &carla_ego_actor,
299  ::ad::rss::state::ProperResponse &output_response,
300  ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot,
301  ::ad::rss::situation::SituationSnapshot &output_situation_snapshot,
302  ::ad::rss::world::WorldModel &output_world_model,
303  EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route) {
304  bool result = false;
305  try {
306  double const time_since_epoch_check_start_ms =
307  std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
308 #if DEBUG_TIMING
309  std::cout << "--- time: " << timestamp.frame << ", " << timestamp.elapsed_seconds << std::endl;
310  auto t_start = std::chrono::high_resolution_clock::now();
311  auto t_end = std::chrono::high_resolution_clock::now();
312  std::cout << "-> SC " << std::chrono::duration<double, std::milli>(t_end - t_start).count() << " start checkObjects"
313  << std::endl;
314 #endif
315 
316  const auto carla_ego_vehicle = boost::dynamic_pointer_cast<carla::client::Vehicle>(carla_ego_actor);
317  if (carla_ego_vehicle == nullptr) {
318  _logger->error("RSS Sensor only support vehicles as ego.");
319  }
320 
321 #if DEBUG_TIMING
322  t_end = std::chrono::high_resolution_clock::now();
323  std::cout << "-> ME " << std::chrono::duration<double, std::milli>(t_end - t_start).count()
324  << " before MapMatching" << std::endl;
325 #endif
326 
327  // allow the vehicle to be at least 2.0 m away form the route to not lose
328  // the contact to the route
329  auto const ego_match_object = GetMatchObject(carla_ego_actor, ::ad::physics::Distance(2.0));
330 
331  if (::ad::map::point::isValid(_carla_rss_state.ego_match_object.enuPosition.centerPoint, false)) {
332  // check for bigger position jumps of the ego vehicle
333  auto const travelled_distance = ::ad::map::point::distance(
334  _carla_rss_state.ego_match_object.enuPosition.centerPoint, ego_match_object.enuPosition.centerPoint);
335  if (travelled_distance > ::ad::physics::Distance(10.)) {
336  _logger->warn("Jump in ego vehicle position detected {} -> {}! Force reroute!",
337  _carla_rss_state.ego_match_object.enuPosition.centerPoint,
338  ego_match_object.enuPosition.centerPoint);
339  DropRoute();
340  }
341  }
342 
343  _carla_rss_state.ego_match_object = ego_match_object;
344 
345  _logger->trace("MapMatch:: {}", _carla_rss_state.ego_match_object);
346 
347 #if DEBUG_TIMING
348  t_end = std::chrono::high_resolution_clock::now();
349  std::cout << "-> ME " << std::chrono::duration<double, std::milli>(t_end - t_start).count()
350  << " after ego MapMatching" << std::endl;
351 #endif
352 
354 
355 #if DEBUG_TIMING
356  t_end = std::chrono::high_resolution_clock::now();
357  std::cout << "-> RU " << std::chrono::duration<double, std::milli>(t_end - t_start).count()
358  << " after route update " << std::endl;
359 #endif
360 
362  timestamp, time_since_epoch_check_start_ms, *carla_ego_vehicle, _carla_rss_state.ego_match_object,
365 
367 
368  CreateWorldModel(timestamp, *actors, *carla_ego_vehicle, _carla_rss_state);
369 
370 #if DEBUG_TIMING
371  t_end = std::chrono::high_resolution_clock::now();
372  std::cout << "-> WM " << std::chrono::duration<double, std::milli>(t_end - t_start).count()
373  << " after create world model " << std::endl;
374 #endif
375 
376  result = PerformCheck(_carla_rss_state);
377 
378 #if DEBUG_TIMING
379  t_end = std::chrono::high_resolution_clock::now();
380  std::cout << "-> CH " << std::chrono::duration<double, std::milli>(t_end - t_start).count() << " end RSS check"
381  << std::endl;
382 #endif
383 
385 
386 #if DEBUG_TIMING
387  t_end = std::chrono::high_resolution_clock::now();
388  std::cout << "-> AN " << std::chrono::duration<double, std::milli>(t_end - t_start).count()
389  << " end analyze results" << std::endl;
390 #endif
391 
393  std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
394 
395  // store result
396  output_response = _carla_rss_state.proper_response;
397  output_rss_state_snapshot = _carla_rss_state.rss_state_snapshot;
398  output_situation_snapshot = _carla_rss_state.situation_snapshot;
399  output_world_model = _carla_rss_state.world_model;
400  output_rss_ego_dynamics_on_route = _carla_rss_state.ego_dynamics_on_route;
402  _logger->debug("===== ROUTE NOT SAFE =====");
403  } else {
404  _logger->debug("===== ROUTE SAFE =====");
405  }
406 
407 #if DEBUG_TIMING
408  t_end = std::chrono::high_resolution_clock::now();
409  std::cout << "-> EC " << std::chrono::duration<double, std::milli>(t_end - t_start).count() << " end check objects"
410  << std::endl;
411 #endif
412  } catch (...) {
413  _logger->error("Exception -> Check failed");
414  }
415  return result;
416 }
417 
419  ::ad::physics::Distance const &sampling_distance) const {
420  ::ad::map::match::Object match_object;
421 
422  auto const vehicle_transform = actor->GetTransform();
423  match_object.enuPosition.centerPoint.x = ::ad::map::point::ENUCoordinate(vehicle_transform.location.x);
424  match_object.enuPosition.centerPoint.y = ::ad::map::point::ENUCoordinate(-1. * vehicle_transform.location.y);
425  match_object.enuPosition.centerPoint.z = ::ad::map::point::ENUCoordinate(0.); // vehicle_transform.location.z;
426  match_object.enuPosition.heading =
427  ::ad::map::point::createENUHeading(-1 * vehicle_transform.rotation.yaw * to_radians);
428 
429  auto const vehicle = boost::dynamic_pointer_cast<carla::client::Vehicle>(actor);
430  auto const walker = boost::dynamic_pointer_cast<carla::client::Walker>(actor);
431  if (vehicle != nullptr) {
432  const auto &bounding_box = vehicle->GetBoundingBox();
433  match_object.enuPosition.dimension.length = ::ad::physics::Distance(2 * bounding_box.extent.x);
434  match_object.enuPosition.dimension.width = ::ad::physics::Distance(2 * bounding_box.extent.y);
435  match_object.enuPosition.dimension.height = ::ad::physics::Distance(2 * bounding_box.extent.z);
436  } else if (walker != nullptr) {
437  const auto &bounding_box = walker->GetBoundingBox();
438  match_object.enuPosition.dimension.length = ::ad::physics::Distance(2 * bounding_box.extent.x);
439  match_object.enuPosition.dimension.width = ::ad::physics::Distance(2 * bounding_box.extent.y);
440  match_object.enuPosition.dimension.height = ::ad::physics::Distance(2 * bounding_box.extent.z);
441  } else {
442  _logger->error("Could not get bounding box of actor {}", actor->GetId());
443  }
444  match_object.enuPosition.enuReferencePoint = ::ad::map::access::getENUReferencePoint();
445 
446  ::ad::map::match::AdMapMatching map_matching;
447  match_object.mapMatchedBoundingBox =
448  map_matching.getMapMatchedBoundingBox(match_object.enuPosition, sampling_distance);
449 
450  return match_object;
451 }
452 
453 ::ad::physics::Speed RssCheck::GetSpeed(carla::client::Actor const &actor) const {
454  auto velocity = actor.GetVelocity();
455  auto const actor_transform = actor.GetTransform();
456  actor_transform.rotation.InverseRotateVector(velocity);
457 
458  ::ad::physics::Speed speed(std::sqrt(velocity.x * velocity.x + velocity.y * velocity.y));
459  if (velocity.x < 0.) {
460  speed = -speed;
461  }
462 
463  return speed;
464 }
465 
466 ::ad::physics::AngularVelocity RssCheck::GetHeadingChange(carla::client::Actor const &actor) const {
467  auto const angular_velocity = actor.GetAngularVelocity();
468  ::ad::physics::AngularVelocity heading_change(-1. * angular_velocity.z * to_radians);
469  return heading_change;
470 }
471 
472 ::ad::physics::Angle RssCheck::GetSteeringAngle(carla::client::Vehicle const &actor) const {
473  auto const steer_ratio = actor.GetControl().steer;
474  ::ad::physics::Angle steering_angle(-1 * _maximum_steering_angle * steer_ratio);
475  return steering_angle;
476 }
477 
478 void RssCheck::UpdateRoute(CarlaRssState &carla_rss_state) {
479  _logger->trace("Update route start: {}", carla_rss_state.ego_route);
480 
481  // remove the parts of the route already taken, try to prepend route sections
482  // (i.e. when driving backwards)
483  // try to ensure that the back of the vehicle is still within the route to
484  // support orientation calculation
485  ::ad::map::point::ParaPointList all_lane_matches;
486  for (auto reference_point :
487  {::ad::map::match::ObjectReferencePoints::RearRight, ::ad::map::match::ObjectReferencePoints::RearLeft}) {
488  auto const &reference_position =
489  carla_rss_state.ego_match_object.mapMatchedBoundingBox.referencePointPositions[size_t(reference_point)];
490  auto const para_points = ::ad::map::match::getParaPoints(reference_position);
491  all_lane_matches.insert(all_lane_matches.end(), para_points.begin(), para_points.end());
492  }
493 
494  auto shorten_route_result = ::ad::map::route::shortenRoute(
495  all_lane_matches, carla_rss_state.ego_route,
496  ::ad::map::route::ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute);
497  if (shorten_route_result == ::ad::map::route::ShortenRouteResult::SucceededIntersectionNotCut) {
498  shorten_route_result = ::ad::map::route::ShortenRouteResult::Succeeded;
499  }
500 
501  bool routing_target_check_finished = false;
502  while ((!_routing_targets.empty()) && (!routing_target_check_finished)) {
503  auto const next_target = _routing_targets.front();
504  auto const &distance_to_next_target =
505  ::ad::map::point::distance(next_target, carla_rss_state.ego_match_object.enuPosition.centerPoint);
506  if (distance_to_next_target < ::ad::physics::Distance(3.)) {
507  _routing_targets.erase(_routing_targets.begin());
508  _logger->debug("Next target reached: {}; remaining targets: {}", next_target, _routing_targets);
509  } else {
510  routing_target_check_finished = true;
511  }
512  }
513 
514  bool reroute_required = false;
515  if (!_routing_targets_to_append.empty()) {
516  reroute_required = true;
519  _logger->debug("Appending new routing targets: {}; resulting targets: {}", _routing_targets_to_append,
522  }
523 
524  ::ad::physics::Distance const route_target_length(50.);
525 
526  if ((!reroute_required) && (shorten_route_result == ::ad::map::route::ShortenRouteResult::Succeeded)) {
527  std::vector<::ad::map::route::FullRoute> additional_routes;
528  auto const route_valid =
529  ::ad::map::route::extendRouteToDistance(carla_rss_state.ego_route, route_target_length, additional_routes);
530 
531  if (route_valid) {
532  if (additional_routes.size() > 0u) {
533  // take a random extension to the route
534  std::size_t route_index = static_cast<std::size_t>(std::rand()) % (additional_routes.size() + 1);
535  if (route_index < additional_routes.size()) {
536  // we decided for one of the additional routes
537  _logger->debug("Additional Routes: {}->{}", additional_routes.size(), route_index);
538  carla_rss_state.ego_route = additional_routes[route_index];
539  } else {
540  // we decided for the extension within route, nothing to be done
541  _logger->debug("Additional Routes: expand current");
542  }
543  }
544  } else {
545  reroute_required = true;
546  }
547  } else {
548  // on all other results we recreate the route
549  reroute_required = true;
550  }
551 
552  // create the route if required
553  if (reroute_required) {
554  // try to create routes
555  std::vector<::ad::map::route::FullRoute> all_new_routes;
556  for (const auto &position :
557  carla_rss_state.ego_match_object.mapMatchedBoundingBox
558  .referencePointPositions[int32_t(::ad::map::match::ObjectReferencePoints::Center)]) {
559  auto start_point = position.lanePoint.paraPoint;
560  auto projected_start_point = start_point;
561  if (!::ad::map::lane::isHeadingInLaneDirection(start_point,
562  carla_rss_state.ego_match_object.enuPosition.heading)) {
563  _logger->debug("EgoVehicle heading in opposite lane direction");
564  if (::ad::map::lane::projectPositionToLaneInHeadingDirection(
565  start_point, carla_rss_state.ego_match_object.enuPosition.heading, projected_start_point)) {
566  _logger->debug("Projected to lane {}", projected_start_point.laneId);
567  }
568  }
569  _logger->debug("Route start_point: {}, projected_start_point: {}", start_point, projected_start_point);
570  auto routing_start_point = ::ad::map::route::planning::createRoutingPoint(
571  projected_start_point, carla_rss_state.ego_match_object.enuPosition.heading);
572  if (!_routing_targets.empty() && ::ad::map::point::isValid(_routing_targets)) {
573  auto new_route = ::ad::map::route::planning::planRoute(routing_start_point, _routing_targets,
574  ::ad::map::route::RouteCreationMode::AllRoutableLanes);
575  all_new_routes.push_back(new_route);
576  } else {
577  auto new_routes = ::ad::map::route::planning::predictRoutesOnDistance(
578  routing_start_point, route_target_length, ::ad::map::route::RouteCreationMode::AllRoutableLanes);
579 
580  for (const auto &new_route : new_routes) {
581  // extend route with all lanes
582  all_new_routes.push_back(new_route);
583  }
584  }
585  }
586 
587  _logger->debug("New routes: {}", all_new_routes.size());
588 
589  if (!all_new_routes.empty()) {
590  // take a random route
591  std::size_t route_index = static_cast<std::size_t>(std::rand()) % (all_new_routes.size());
592  carla_rss_state.ego_route = all_new_routes[route_index];
593  }
594  }
595 
596  _logger->trace("Update route result: {}", carla_rss_state.ego_route);
597 }
598 
600  carla::client::Timestamp const &current_timestamp, double const &time_since_epoch_check_start_ms,
601  carla::client::Vehicle const &carla_vehicle, ::ad::map::match::Object match_object,
602  ::ad::map::route::FullRoute const &route, ::ad::rss::world::RssDynamics const &default_ego_vehicle_dynamics,
603  EgoDynamicsOnRoute const &last_dynamics) const {
604  EgoDynamicsOnRoute new_dynamics;
605  new_dynamics.timestamp = current_timestamp;
606  new_dynamics.time_since_epoch_check_start_ms = time_since_epoch_check_start_ms;
607  new_dynamics.ego_speed = GetSpeed(carla_vehicle);
608  new_dynamics.ego_center = match_object.enuPosition.centerPoint;
609  new_dynamics.ego_heading = match_object.enuPosition.heading;
610  new_dynamics.ego_heading_change = GetHeadingChange(carla_vehicle);
611  new_dynamics.ego_steering_angle = GetSteeringAngle(carla_vehicle);
612 
613  auto object_route =
614  ::ad::map::route::getRouteSection(match_object, route, ::ad::map::route::RouteSectionCreationMode::AllRouteLanes);
615  auto border = ::ad::map::route::getENUBorderOfRoute(object_route);
616  new_dynamics.route_heading = ::ad::map::lane::getENUHeading(border, match_object.enuPosition.centerPoint);
617 
618  auto const object_center = ::ad::map::route::findCenterWaypoint(match_object, object_route);
619  if (object_center.isValid()) {
620  auto lane_center_point = object_center.queryPosition;
621  auto lane_center_point_enu = ::ad::map::lane::getENULanePoint(lane_center_point);
622  if (std::fabs(new_dynamics.route_heading) > ::ad::map::point::ENUHeading(M_PI)) {
623  // if the actual center point is already outside, try to use this extended
624  // object center for the route heading calculation
625  new_dynamics.route_heading = ::ad::map::lane::getENUHeading(border, lane_center_point_enu);
626  }
627 
628  if (object_center.laneSegmentIterator->laneInterval.wrongWay) {
629  // driving on the wrong lane, so we have to project to nominal route
630  // direction
631  ::ad::map::lane::projectPositionToLaneInHeadingDirection(lane_center_point, new_dynamics.route_heading,
632  lane_center_point);
633  lane_center_point_enu = ::ad::map::lane::getENULanePoint(lane_center_point);
634  }
635  new_dynamics.route_nominal_center = lane_center_point_enu;
636 
637  } else {
638  // the ego vehicle is completely outside the route, so we can't update the
639  // values
640  new_dynamics.route_nominal_center = last_dynamics.route_nominal_center;
641  new_dynamics.route_heading = last_dynamics.route_heading;
642  }
643 
644  new_dynamics.heading_diff =
645  ::ad::map::point::normalizeENUHeading(new_dynamics.route_heading - new_dynamics.ego_heading);
646  new_dynamics.route_speed_lon =
647  std::fabs(std::cos(static_cast<double>(new_dynamics.heading_diff))) * new_dynamics.ego_speed;
648  new_dynamics.route_speed_lat = std::sin(static_cast<double>(new_dynamics.heading_diff)) * new_dynamics.ego_speed;
649 
650  bool keep_last_acceleration = true;
651  if (last_dynamics.timestamp.elapsed_seconds > 0.) {
652  ::ad::physics::Duration const delta_time(current_timestamp.elapsed_seconds -
653  last_dynamics.timestamp.elapsed_seconds);
654  if (delta_time > ::ad::physics::Duration(0.0001)) {
655  try {
656  new_dynamics.route_accel_lat = (new_dynamics.route_speed_lat - last_dynamics.route_speed_lat) / delta_time;
657  new_dynamics.avg_route_accel_lat =
658  ((last_dynamics.avg_route_accel_lat * 2.) + new_dynamics.route_accel_lat) / 3.;
659  new_dynamics.route_accel_lon = (new_dynamics.route_speed_lon - last_dynamics.route_speed_lon) / delta_time;
660  new_dynamics.avg_route_accel_lon =
661  ((last_dynamics.avg_route_accel_lon * 2.) + new_dynamics.route_accel_lon) / 3.;
662 
663  if (new_dynamics.avg_route_accel_lat == ::ad::physics::Acceleration(0.)) {
664  // prevent from underrun
665  new_dynamics.avg_route_accel_lat = ::ad::physics::Acceleration(0.);
666  }
667  if (new_dynamics.avg_route_accel_lon == ::ad::physics::Acceleration(0.)) {
668  // prevent from underrun
669  new_dynamics.avg_route_accel_lon = ::ad::physics::Acceleration(0.);
670  }
671  keep_last_acceleration = false;
672  } catch (...) {
673  }
674  }
675  }
676 
677  if (keep_last_acceleration) {
678  new_dynamics.route_accel_lat = last_dynamics.route_accel_lat;
679  new_dynamics.avg_route_accel_lat = last_dynamics.avg_route_accel_lat;
680  new_dynamics.route_accel_lon = last_dynamics.route_accel_lon;
681  new_dynamics.avg_route_accel_lon = last_dynamics.avg_route_accel_lon;
682  }
683 
684  // check if the center point (and only the center point) is still found on the
685  // route
686  ::ad::map::point::ParaPointList in_lane_matches;
687  for (auto &match_position : match_object.mapMatchedBoundingBox
688  .referencePointPositions[int32_t(::ad::map::match::ObjectReferencePoints::Center)]) {
689  if (match_position.type == ::ad::map::match::MapMatchedPositionType::LANE_IN) {
690  in_lane_matches.push_back(match_position.lanePoint.paraPoint);
691  }
692  }
693  auto const object_in_lane_center = ::ad::map::route::findNearestWaypoint(in_lane_matches, route);
694  new_dynamics.ego_center_within_route = object_in_lane_center.isValid();
695  // evaluated by AnalyseResults
696  new_dynamics.crossing_border = false;
697 
698  // calculate the ego stopping distance, to be able to reduce the effort
699 
700  ::ad::rss::map::RssObjectData ego_object_data;
701  ego_object_data.id = ::ad::rss::world::ObjectId(0u);
702  ego_object_data.type = ::ad::rss::world::ObjectType::EgoVehicle;
703  ego_object_data.matchObject = match_object;
704  ego_object_data.speed = new_dynamics.ego_speed;
705  ego_object_data.yawRate = new_dynamics.ego_heading_change;
706  ego_object_data.steeringAngle = new_dynamics.ego_steering_angle;
707  ego_object_data.rssDynamics = default_ego_vehicle_dynamics;
708 
709  ad::rss::map::RssObjectConversion object_conversion(ego_object_data);
710  if (!object_conversion.calculateMinStoppingDistance(new_dynamics.min_stopping_distance)) {
711  _logger->error(
712  "CalculateEgoDynamicsOnRoute: calculation of min stopping distance "
713  "failed. Setting to 100. ({} {} {} {})",
714  match_object, new_dynamics.ego_speed, new_dynamics.ego_speed, new_dynamics.ego_heading_change,
715  default_ego_vehicle_dynamics);
716  new_dynamics.min_stopping_distance = ::ad::physics::Distance(100.);
717  }
718 
719  _logger->trace("CalculateEgoDynamicsOnRoute: route-section {} -> dynamics: {}", object_route, new_dynamics);
720  return new_dynamics;
721 }
722 
724  ::ad::map::match::Object other_match_object;
725  carla::SharedPtr<ActorConstellationData> default_constellation_data{
726  new ActorConstellationData{carla_rss_state.ego_match_object, carla_rss_state.ego_route,
727  carla_rss_state.ego_dynamics_on_route, other_match_object, nullptr}};
728  auto const default_constellation_result = _actor_constellation_callback(default_constellation_data);
729  carla_rss_state.default_ego_vehicle_dynamics = default_constellation_result.ego_vehicle_dynamics;
730 }
731 
732 ::ad::map::landmark::LandmarkIdSet RssCheck::GetGreenTrafficLightsOnRoute(
733  std::vector<SharedPtr<carla::client::TrafficLight>> const &traffic_lights,
734  ::ad::map::route::FullRoute const &route) const {
735  ::ad::map::landmark::LandmarkIdSet green_traffic_lights;
736 
737  auto next_intersection = ::ad::map::intersection::Intersection::getNextIntersectionOnRoute(route);
738  if (next_intersection &&
739  (next_intersection->intersectionType() == ::ad::map::intersection::IntersectionType::TrafficLight)) {
740  // try to guess the the relevant traffic light with the rule: nearest
741  // traffic light in respect to the incoming lane.
742  // @todo: when OpenDrive maps have the traffic lights incorporated, we only
743  // have to fill all green traffic lights into the green_traffic_lights list
744  auto incoming_lanes = next_intersection->incomingLanesOnRoute();
745  // since our route spans the whole street, we have to filter out the
746  // incoming lanes with wrong way flag
747  auto incoming_lanes_iter = incoming_lanes.begin();
748  while (incoming_lanes_iter != incoming_lanes.end()) {
749  auto find_waypoint = ::ad::map::route::findWaypoint(*incoming_lanes_iter, route);
750  if (find_waypoint.isValid() && find_waypoint.laneSegmentIterator->laneInterval.wrongWay) {
751  incoming_lanes_iter = incoming_lanes.erase(incoming_lanes_iter);
752  } else {
753  incoming_lanes_iter++;
754  }
755  }
756 
757  ::ad::map::match::AdMapMatching traffic_light_map_matching;
758  bool found_relevant_traffic_light = false;
759  for (const auto &traffic_light : traffic_lights) {
760  auto traffic_light_state = traffic_light->GetState();
761  carla::geom::BoundingBox trigger_bounding_box = traffic_light->GetTriggerVolume();
762 
763  auto traffic_light_transform = traffic_light->GetTransform();
764  auto trigger_box_location = trigger_bounding_box.location;
765  traffic_light_transform.TransformPoint(trigger_box_location);
766 
767  ::ad::map::point::ENUPoint trigger_box_position;
768  trigger_box_position.x = ::ad::map::point::ENUCoordinate(trigger_box_location.x);
769  trigger_box_position.y = ::ad::map::point::ENUCoordinate(-1 * trigger_box_location.y);
770  trigger_box_position.z = ::ad::map::point::ENUCoordinate(0.);
771 
772  _logger->trace("traffic light[{}] Position: {}", traffic_light->GetId(), trigger_box_position);
773  auto traffic_light_map_matched_positions = traffic_light_map_matching.getMapMatchedPositions(
774  trigger_box_position, ::ad::physics::Distance(0.25), ::ad::physics::Probability(0.1));
775 
776  _logger->trace("traffic light[{}] Map Matched Position: {}", traffic_light->GetId(),
777  traffic_light_map_matched_positions);
778 
779  for (auto matched_position : traffic_light_map_matched_positions) {
780  if (incoming_lanes.find(matched_position.lanePoint.paraPoint.laneId) != incoming_lanes.end()) {
781  if (found_relevant_traffic_light &&
782  (green_traffic_lights.empty() && (traffic_light_state == carla::rpc::TrafficLightState::Green))) {
783  _logger->warn("found another relevant traffic light on lane {}; {} state {}",
784  matched_position.lanePoint.paraPoint.laneId, traffic_light->GetId(),
785  (traffic_light_state == carla::rpc::TrafficLightState::Green) ? "green" : "not green");
786  } else {
787  _logger->debug("found relevant traffic light on lane {}; {} state {}",
788  matched_position.lanePoint.paraPoint.laneId, traffic_light->GetId(),
789  (traffic_light_state == carla::rpc::TrafficLightState::Green) ? "green" : "not green");
790  }
791 
792  found_relevant_traffic_light = true;
793 
794  // found matching traffic light
795  if (traffic_light_state == carla::rpc::TrafficLightState::Green) {
796  // @todo: currently there is only this workaround because of missign
797  // OpenDrive map support for actual traffic light ids
798  green_traffic_lights.insert(::ad::map::landmark::LandmarkId::getMax());
799  } else {
800  // if the light is not green, we don't have priority
801  green_traffic_lights.clear();
802  }
803  break;
804  }
805  }
806  }
807  }
808  return green_traffic_lights;
809 }
810 
812  ::ad::rss::map::RssSceneCreation &scene_creation,
813  carla::client::Vehicle const &carla_ego_vehicle,
814  CarlaRssState const &carla_rss_state,
815  ::ad::map::landmark::LandmarkIdSet const &green_traffic_lights)
816  : _rss_check(rss_check),
817  _scene_creation(scene_creation),
818  _carla_ego_vehicle(carla_ego_vehicle),
819  _carla_rss_state(carla_rss_state),
820  _green_traffic_lights(green_traffic_lights) {}
821 
823  const carla::SharedPtr<carla::client::Actor> other_traffic_participant) const {
824  try {
825  auto other_match_object = _rss_check.GetMatchObject(other_traffic_participant, ::ad::physics::Distance(2.0));
826 
827  _rss_check._logger->trace("OtherVehicleMapMatching: {} {}", other_traffic_participant->GetId(),
828  other_match_object.mapMatchedBoundingBox);
829 
832  other_match_object, other_traffic_participant}};
833  auto const actor_constellation_result = _rss_check._actor_constellation_callback(actor_constellation_data);
834 
835  auto other_speed = _rss_check.GetSpeed(*other_traffic_participant);
836  auto other_heading_change = _rss_check.GetHeadingChange(*other_traffic_participant);
837  auto other_steering_angle = ::ad::physics::Angle(0.);
838 
839  ::ad::rss::map::RssObjectData ego_object_data;
840  ego_object_data.id = _carla_ego_vehicle.GetId();
841  ego_object_data.type = ::ad::rss::world::ObjectType::EgoVehicle;
842  ego_object_data.matchObject = _carla_rss_state.ego_match_object;
843  ego_object_data.speed = _carla_rss_state.ego_dynamics_on_route.ego_speed;
845  ego_object_data.steeringAngle = _carla_rss_state.ego_dynamics_on_route.ego_steering_angle;
846  ego_object_data.rssDynamics = actor_constellation_result.ego_vehicle_dynamics;
847 
848  ::ad::rss::map::RssObjectData other_object_data;
849  other_object_data.id = ::ad::rss::world::ObjectId(other_traffic_participant->GetId());
850  other_object_data.type = actor_constellation_result.actor_object_type;
851  other_object_data.matchObject = other_match_object;
852  other_object_data.speed = other_speed;
853  other_object_data.yawRate = other_heading_change;
854  other_object_data.steeringAngle = other_steering_angle;
855  other_object_data.rssDynamics = actor_constellation_result.actor_dynamics;
856 
857  _scene_creation.appendScenes(ego_object_data, _carla_rss_state.ego_route, other_object_data,
858  actor_constellation_result.restrict_speed_limit_mode, _green_traffic_lights,
859  actor_constellation_result.rss_calculation_mode);
860 
861  } catch (...) {
862  _rss_check._logger->error("Exception processing other traffic participant {} -> Ignoring it",
863  other_traffic_participant->GetId());
864  }
865 }
866 
868  carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState &carla_rss_state) const {
869  // only loop once over the actors since always the respective objects are created
870  std::vector<SharedPtr<carla::client::TrafficLight>> traffic_lights;
871  std::vector<SharedPtr<carla::client::Actor>> other_traffic_participants;
872  for (const auto &actor : actors) {
873  const auto traffic_light = boost::dynamic_pointer_cast<carla::client::TrafficLight>(actor);
874  if (traffic_light != nullptr) {
875  traffic_lights.push_back(traffic_light);
876  continue;
877  }
878 
879  if ((boost::dynamic_pointer_cast<carla::client::Vehicle>(actor) != nullptr) ||
880  (boost::dynamic_pointer_cast<carla::client::Walker>(actor) != nullptr)) {
881  if (actor->GetId() == carla_ego_vehicle.GetId()) {
882  continue;
883  }
884  auto const relevant_distance =
885  std::max(static_cast<double>(carla_rss_state.ego_dynamics_on_route.min_stopping_distance), 100.);
886  if (actor->GetTransform().location.Distance(carla_ego_vehicle.GetTransform().location) < relevant_distance) {
887  other_traffic_participants.push_back(actor);
888  }
889  }
890  }
891 
892  ::ad::map::landmark::LandmarkIdSet green_traffic_lights =
893  GetGreenTrafficLightsOnRoute(traffic_lights, carla_rss_state.ego_route);
894 
895  ::ad::rss::map::RssSceneCreation scene_creation(timestamp.frame, carla_rss_state.default_ego_vehicle_dynamics);
896 
897 #ifdef RSS_USE_TBB
898  tbb::parallel_for_each(
899  other_traffic_participants.begin(), other_traffic_participants.end(),
900  RssObjectChecker(*this, scene_creation, carla_ego_vehicle, carla_rss_state, green_traffic_lights));
901 #else
902  for (auto const traffic_participant : other_traffic_participants) {
903  auto checker = RssObjectChecker(*this, scene_creation, carla_ego_vehicle, carla_rss_state, green_traffic_lights);
904  checker(traffic_participant);
905  }
906 #endif
907 
909  // add artifical objects on the road boundaries for "stay-on-road" feature
910  // use 'smart' dynamics
911  auto ego_vehicle_dynamics = carla_rss_state.default_ego_vehicle_dynamics;
912  ego_vehicle_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.);
913 
914  ::ad::rss::map::RssObjectData ego_object_data;
915  ego_object_data.id = carla_ego_vehicle.GetId();
916  ego_object_data.type = ::ad::rss::world::ObjectType::EgoVehicle;
917  ego_object_data.matchObject = _carla_rss_state.ego_match_object;
918  ego_object_data.speed = _carla_rss_state.ego_dynamics_on_route.ego_speed;
920  ego_object_data.steeringAngle = _carla_rss_state.ego_dynamics_on_route.ego_steering_angle;
921  ego_object_data.rssDynamics = ego_vehicle_dynamics;
922  scene_creation.appendRoadBoundaries(ego_object_data, carla_rss_state.ego_route,
923  // since the route always expanded, route isn't required to expand any
924  // more
925  ::ad::rss::map::RssSceneCreation::AppendRoadBoundariesMode::RouteOnly);
926  }
927 
928  carla_rss_state.world_model = scene_creation.getWorldModel();
929 }
930 
931 bool RssCheck::PerformCheck(CarlaRssState &carla_rss_state) const {
932  bool result = carla_rss_state.rss_check.calculateProperResponse(
933  carla_rss_state.world_model, carla_rss_state.situation_snapshot, carla_rss_state.rss_state_snapshot,
934  carla_rss_state.proper_response);
935 
936  if (!result) {
937  _logger->warn("calculateProperResponse failed!");
938  }
939  else if (!carla_rss_state.proper_response.isSafe) {
940  _logger->info("Unsafe route: {}", carla_rss_state.proper_response);
941  }
942  return result;
943 }
944 
945 void RssCheck::AnalyseCheckResults(CarlaRssState &carla_rss_state) const {
946  carla_rss_state.dangerous_state = false;
947  carla_rss_state.dangerous_vehicle = false;
948  carla_rss_state.dangerous_opposite_state = false;
949  bool left_border_is_dangerous = false;
950  bool right_border_is_dangerous = false;
951  bool vehicle_triggered_left_response = false;
952  bool vehicle_triggered_right_response = false;
953  bool vehicle_triggered_longitudinal_response = false;
954  for (auto const state : carla_rss_state.rss_state_snapshot.individualResponses) {
955  if (::ad::rss::state::isDangerous(state)) {
956  carla_rss_state.dangerous_state = true;
957  _logger->trace("DangerousState: {}", state);
958  auto dangerous_sitation_iter = std::find_if(carla_rss_state.situation_snapshot.situations.begin(),
959  carla_rss_state.situation_snapshot.situations.end(),
960  [&state](::ad::rss::situation::Situation const &situation) {
961  return situation.situationId == state.situationId;
962  });
963  if (dangerous_sitation_iter != carla_rss_state.situation_snapshot.situations.end()) {
964  _logger->trace("Situation: {}", *dangerous_sitation_iter);
965  if (dangerous_sitation_iter->objectId == ::ad::rss::map::RssSceneCreator::getRightBorderObjectId()) {
966  right_border_is_dangerous = true;
967  } else if (dangerous_sitation_iter->objectId == ::ad::rss::map::RssSceneCreator::getLeftBorderObjectId()) {
968  left_border_is_dangerous = true;
969  } else {
970  carla_rss_state.dangerous_vehicle = true;
971  if (state.longitudinalState.response != ::ad::rss::state::LongitudinalResponse::None) {
972  vehicle_triggered_longitudinal_response = true;
973  }
974  if (state.lateralStateLeft.response != ::ad::rss::state::LateralResponse::None) {
975  vehicle_triggered_left_response = true;
976  }
977  if (state.lateralStateRight.response != ::ad::rss::state::LateralResponse::None) {
978  vehicle_triggered_right_response = true;
979  }
980  }
981  if (dangerous_sitation_iter->situationType == ::ad::rss::situation::SituationType::OppositeDirection) {
982  carla_rss_state.dangerous_opposite_state = true;
983  }
984  }
985  }
986  }
987 
988  // border are restricting potentially too much, fix this
989  if (!vehicle_triggered_longitudinal_response &&
990  (carla_rss_state.proper_response.longitudinalResponse != ::ad::rss::state::LongitudinalResponse::None)) {
991  _logger->debug("!! longitudinalResponse only triggered by borders: ignore !!");
992  carla_rss_state.proper_response.longitudinalResponse = ::ad::rss::state::LongitudinalResponse::None;
993  carla_rss_state.proper_response.accelerationRestrictions.longitudinalRange.maximum =
994  carla_rss_state.default_ego_vehicle_dynamics.alphaLon.accelMax;
995  }
996  if (!vehicle_triggered_left_response && !left_border_is_dangerous &&
997  (carla_rss_state.proper_response.lateralResponseLeft != ::ad::rss::state::LateralResponse::None)) {
998  _logger->debug("!! lateralResponseLeft only triggered by right border: ignore !!");
999  carla_rss_state.proper_response.lateralResponseLeft = ::ad::rss::state::LateralResponse::None;
1000  carla_rss_state.proper_response.accelerationRestrictions.lateralLeftRange.maximum =
1001  carla_rss_state.default_ego_vehicle_dynamics.alphaLat.accelMax;
1002  carla_rss_state.ego_dynamics_on_route.crossing_border = true;
1003  }
1004  if (!vehicle_triggered_right_response && !right_border_is_dangerous &&
1005  (carla_rss_state.proper_response.lateralResponseRight != ::ad::rss::state::LateralResponse::None)) {
1006  _logger->debug("!! lateralResponseRight only triggered by left border: ignore !!");
1007  carla_rss_state.proper_response.lateralResponseRight = ::ad::rss::state::LateralResponse::None;
1008  carla_rss_state.proper_response.accelerationRestrictions.lateralRightRange.maximum =
1009  carla_rss_state.default_ego_vehicle_dynamics.alphaLat.accelMax;
1010  carla_rss_state.ego_dynamics_on_route.crossing_border = true;
1011  }
1012 
1013  _logger->trace("RouteResponse: {}", carla_rss_state.proper_response);
1014 }
1015 
1016 } // namespace rss
1017 } // namespace carla
const ::ad::rss::world::RssDynamics & GetDefaultActorConstellationCallbackPedestrianDynamics() const
Definition: RssCheck.cpp:250
double time_since_epoch_check_start_ms
the time since epoch in ms at start of the checkObjects call
Definition: RssCheck.h:47
void InverseRotateVector(Vector3D &in_point) const
Definition: Rotation.h:96
void SetMapLogLevel(const spdlog::level::level_enum &map_log_level)
sets the current log level
Definition: RssCheck.cpp:227
double time_since_epoch_check_end_ms
the time since epoch in ms at the end of the checkObjects call
Definition: RssCheck.h:49
::ad::rss::core::RssCheck rss_check
the actual RSS checker object
Definition: RssCheck.h:245
::ad::map::landmark::LandmarkIdSet const & _green_traffic_lights
Definition: RssCheck.h:290
struct defining the ego vehicles current dynamics in respect to the current route ...
Definition: RssCheck.h:40
void operator()(const carla::SharedPtr< carla::client::Actor > other_traffic_participant) const
Definition: RssCheck.cpp:822
::ad::map::point::ENUHeading route_heading
the considered heading of the route
Definition: RssCheck.h:70
std::vector<::ad::map::point::ENUPoint > _routing_targets
current used routing targets
Definition: RssCheck.h:238
void AnalyseCheckResults(CarlaRssState &carla_rss_state) const
Analyse the RSS check results.
Definition: RssCheck.cpp:945
std::vector<::ad::map::point::ENUPoint > _routing_targets_to_append
routing targets to be appended next run
Definition: RssCheck.h:240
::ad::physics::Speed route_speed_lon
the ego speed component lon in respect to a route
Definition: RssCheck.h:78
Location location
Center of the BoundingBox in local space.
::ad::rss::world::WorldModel world_model
check input: the RSS world model
Definition: RssCheck.h:260
::ad::physics::AngularVelocity GetHeadingChange(carla::client::Actor const &actor) const
calculate the heading change from the actor
Definition: RssCheck.cpp:466
RssObjectChecker(RssCheck const &rss_check, ::ad::rss::map::RssSceneCreation &scene_creation, carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState const &carla_rss_state, ::ad::map::landmark::LandmarkIdSet const &green_traffic_lights)
Definition: RssCheck.cpp:811
bool dangerous_state
flag indicating if the current state is overall dangerous
Definition: RssCheck.h:269
::ad::rss::world::RssDynamics _default_actor_constellation_callback_ego_vehicle_dynamics
current used vehicle dynamics for ego vehicle by the default actor constellation callback ...
Definition: RssCheck.h:226
std::shared_ptr< spdlog::logger > getLogger()
Definition: RssCheck.cpp:78
class implementing the actual RSS checks based on CARLA world description
Definition: RssCheck.h:132
carla::client::Timestamp timestamp
the carla timestamp of the last calculation
Definition: RssCheck.h:45
::ad::map::point::ENUHeading heading_diff
the considered heading diff towards the route
Definition: RssCheck.h:74
::ad::map::point::ENUHeading ego_heading
the considered heading of the ego vehicle
Definition: RssCheck.h:57
::ad::physics::Acceleration avg_route_accel_lon
the ego acceleration component lon in respect to a route smoothened by an average filter ...
Definition: RssCheck.h:88
bool dangerous_vehicle
flag indicating if the current state is dangerous because of a vehicle
Definition: RssCheck.h:272
bool crossing_border
flag indicating if the current state is already crossing one of the borders this is only evaluated if...
Definition: RssCheck.h:68
bool CheckObjects(carla::client::Timestamp const &timestamp, carla::SharedPtr< carla::client::ActorList > const &actors, carla::SharedPtr< carla::client::Actor > const &carla_ego_actor, ::ad::rss::state::ProperResponse &output_response, ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, ::ad::rss::situation::SituationSnapshot &output_situation_snapshot, ::ad::rss::world::WorldModel &output_world_model, EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route)
main function to trigger the RSS check at a certain point in time
Definition: RssCheck.cpp:296
void ResetRoutingTargets()
resets the current routing targets (
Definition: RssCheck.cpp:286
::ad::rss::world::RssDynamics default_ego_vehicle_dynamics
current used default vehicle dynamics for ego vehicle
Definition: RssCheck.h:257
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
void SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode)
sets the current mode for respecting the road boundaries (
Definition: RssCheck.cpp:263
::ad::map::point::ENUPoint ego_center
the considered enu position of the ego vehicle
Definition: RssCheck.h:55
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
std::size_t frame
Number of frames elapsed since the simulator was launched.
Definition: Timestamp.h:30
CarlaRssState _carla_rss_state
the current state of the ego vehicle
Definition: RssCheck.h:296
struct collecting the rss states required
Definition: RssCheck.h:243
::ad::rss::world::RssDynamics ego_vehicle_dynamics
The Rss dynamics to be applied for the ego vehicle.
Definition: RssCheck.h:105
::ad::map::route::FullRoute ego_route
the ego route
Definition: RssCheck.h:251
::ad::rss::world::RssDynamics _default_actor_constellation_callback_pedestrian_dynamics
current used vehicle dynamics for pedestrians by the default actor constellation callback ...
Definition: RssCheck.h:230
::ad::physics::Acceleration route_accel_lon
the ego acceleration component lon in respect to a route
Definition: RssCheck.h:82
std::function<::carla::rss::ActorConstellationResult(carla::SharedPtr< ActorConstellationData >)> ActorConstellationCallbackFunctionType
Definition: RssCheck.h:135
::ad::map::match::Object ego_match_object
the ego map matched information
Definition: RssCheck.h:248
::ad::physics::Acceleration route_accel_lat
the ego acceleration component lat in respect to a route
Definition: RssCheck.h:80
::ad::physics::Angle GetSteeringAngle(carla::client::Vehicle const &actor) const
calculate the steering angle from the actor
Definition: RssCheck.cpp:472
bool ego_center_within_route
check if the ego center is within route
Definition: RssCheck.h:63
::ad::physics::Speed ego_speed
the ego speed
Definition: RssCheck.h:51
static RoadBoundariesMode GetDefaultRoadBoundariesMode()
Definition: RssCheck.h:212
::ad::rss::state::RssStateSnapshot rss_state_snapshot
check result: the rss state snapshot
Definition: RssCheck.h:265
void SetDefaultActorConstellationCallbackOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics)
sets the vehicle dynamics to be used for other vehicles
Definition: RssCheck.cpp:245
Control GetControl() const
Return the control last applied to this vehicle.
Definition: Vehicle.cpp:62
::ad::rss::world::RssDynamics actor_dynamics
The Rss dynamics to be applied for the actor.
Definition: RssCheck.h:111
void AppendRoutingTarget(const ::carla::geom::Transform &routing_target)
appends a routing target to the current routing target list (
Definition: RssCheck.cpp:267
EgoDynamicsOnRoute ego_dynamics_on_route
the ego dynamics on the route
Definition: RssCheck.h:254
void UpdateRoute(CarlaRssState &carla_rss_state)
update the desired ego vehicle route
Definition: RssCheck.cpp:478
::ad::rss::world::RssDynamics GetDefaultPedestrianDynamics()
Definition: RssCheck.cpp:107
const geom::BoundingBox & GetBoundingBox() const
Definition: ActorState.h:60
::ad::map::point::ENUPoint route_nominal_center
the considered nominal center of the current route
Definition: RssCheck.h:72
void SetDefaultActorConstellationCallbackEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics)
sets the vehicle dynamics to be used for the ego vehicle
Definition: RssCheck.cpp:236
std::shared_ptr< spdlog::logger > _logger
standard logger
Definition: RssCheck.h:218
ActorConstellationCallbackFunctionType _actor_constellation_callback
the current actor constellation callback
Definition: RssCheck.h:233
carla::client::Vehicle const & _carla_ego_vehicle
Definition: RssCheck.h:288
bool dangerous_opposite_state
flag indicating if the current state is dangerous because of an opposite vehicle
Definition: RssCheck.h:275
RoadBoundariesMode
struct defining the different supported handling of road boundaries
Definition: RssCheck.h:28
void CreateWorldModel(carla::client::Timestamp const &timestamp, carla::client::ActorList const &actors, carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState &carla_rss_state) const
Create the RSS world model.
Definition: RssCheck.cpp:867
void UpdateDefaultRssDynamics(CarlaRssState &carla_rss_state)
Definition: RssCheck.cpp:723
void SetLogLevel(const spdlog::level::level_enum &log_level)
sets the current log level
Definition: RssCheck.cpp:222
std::shared_ptr< spdlog::logger > _timing_logger
logger for timing log messages
Definition: RssCheck.h:220
geom::Vector3D GetAngularVelocity() const
Return the current 3D angular velocity of the actor.
Definition: Actor.cpp:27
geom::Transform GetTransform() const
Return the current transform of the actor.
Definition: Actor.cpp:19
::ad::physics::AngularVelocity ego_heading_change
the considered heading change of the ego vehicle
Definition: RssCheck.h:59
::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode restrict_speed_limit_mode
The mode for restricting speed limit.
Definition: RssCheck.h:101
::carla::rss::RoadBoundariesMode _road_boundaries_mode
current used road boundaries mode
Definition: RssCheck.h:236
const std::vector<::carla::geom::Transform > GetRoutingTargets() const
Definition: RssCheck.cpp:272
const ::carla::rss::RoadBoundariesMode & GetRoadBoundariesMode() const
Definition: RssCheck.cpp:259
~RssCheck()
destructor
Definition: RssCheck.cpp:220
::ad::physics::Speed route_speed_lat
the ego speed component lat in respect to a route
Definition: RssCheck.h:76
void SetDefaultActorConstellationCallbackPedestrianDynamics(const ::ad::rss::world::RssDynamics &pedestrian_dynamics)
sets the dynamics to be used for pedestrians
Definition: RssCheck.cpp:254
void DropRoute()
drop the current route
Definition: RssCheck.cpp:291
CarlaRssState const & _carla_rss_state
Definition: RssCheck.h:289
float _maximum_steering_angle
maximum steering angle
Definition: RssCheck.h:223
EgoDynamicsOnRoute CalculateEgoDynamicsOnRoute(carla::client::Timestamp const &current_timestamp, double const &time_since_epoch_check_start_ms, carla::client::Vehicle const &carla_vehicle, ::ad::map::match::Object match_object, ::ad::map::route::FullRoute const &route, ::ad::rss::world::RssDynamics const &default_ego_vehicle_dynamics, EgoDynamicsOnRoute const &last_dynamics) const
calculate ego vehicle dynamics on the route
Definition: RssCheck.cpp:599
EgoDynamicsOnRoute()
constructor
Definition: RssCheck.cpp:55
::ad::rss::map::RssMode rss_calculation_mode
The calculation mode to be applied with the actor.
Definition: RssCheck.h:98
const ::ad::rss::world::RssDynamics & GetDefaultActorConstellationCallbackOtherVehicleDynamics() const
Definition: RssCheck.cpp:241
::ad::rss::world::ObjectType actor_object_type
The Rss object type to be used for the actor.
Definition: RssCheck.h:108
::ad::physics::Angle ego_steering_angle
the considered steering angle of the ego vehicle
Definition: RssCheck.h:61
Represents an actor in the simulation.
Definition: client/Actor.h:18
geom::Vector3D GetVelocity() const
Return the current 3D velocity of the actor.
Definition: Actor.cpp:23
::ad::map::landmark::LandmarkIdSet GetGreenTrafficLightsOnRoute(std::vector< SharedPtr< carla::client::TrafficLight >> const &traffic_lights, ::ad::map::route::FullRoute const &route) const
collect the green traffic lights on the current route
Definition: RssCheck.cpp:732
::ad::physics::Speed GetSpeed(carla::client::Actor const &actor) const
calculate the speed from the actor
Definition: RssCheck.cpp:453
::ad::rss::world::RssDynamics _default_actor_constellation_callback_other_vehicle_dynamics
current used vehicle dynamics for other vehicle by the default actor constellation callback ...
Definition: RssCheck.h:228
::ad::physics::Distance min_stopping_distance
the current minimum stopping distance
Definition: RssCheck.h:53
void printRoute(std::string const &route_descr, ::ad::map::route::FullRoute const &route)
Definition: RssCheck.cpp:40
constexpr float to_radians
Definition: RssCheck.cpp:53
::ad::rss::situation::SituationSnapshot situation_snapshot
check result: the situation snapshot
Definition: RssCheck.h:263
::ad::physics::Acceleration avg_route_accel_lat
the ego acceleration component lat in respect to a route smoothened by an average filter ...
Definition: RssCheck.h:85
Struct defining the configuration for RSS processing of a given actor.
Definition: RssCheck.h:96
::ad::rss::map::RssSceneCreation & _scene_creation
Definition: RssCheck.h:287
RssCheck(float max_steering_angle)
default constructor with default internal default actor constellation callback
Definition: RssCheck.cpp:127
const ::ad::rss::world::RssDynamics & GetDefaultActorConstellationCallbackEgoVehicleDynamics() const
Definition: RssCheck.cpp:232
::ad::rss::world::RssDynamics GetDefaultVehicleDynamics()
Definition: RssCheck.cpp:88
double elapsed_seconds
Simulated seconds elapsed since the beginning of the current episode.
Definition: Timestamp.h:33
::ad::map::match::Object GetMatchObject(carla::SharedPtr< carla::client::Actor > const &actor, ::ad::physics::Distance const &sampling_distance) const
calculate the map matched object from the actor
Definition: RssCheck.cpp:418
::ad::rss::state::ProperResponse proper_response
check result: the proper response
Definition: RssCheck.h:267
bool PerformCheck(CarlaRssState &carla_rss_state) const
Perform the actual RSS check.
Definition: RssCheck.cpp:931
std::shared_ptr< spdlog::logger > getTimingLogger()
Definition: RssCheck.cpp:83