CARLA
RssRestrictor.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 
9 #include "carla/rss/RssCheck.h"
10 
11 #include <spdlog/sinks/stdout_color_sinks.h>
12 #include <ad/rss/state/ProperResponse.hpp>
13 #include <ad/rss/unstructured/Geometry.hpp>
14 #include <ad/rss/world/Velocity.hpp>
15 
16 namespace carla {
17 namespace rss {
18 
20  std::string logger_name = "RssRestrictor";
21  _logger = spdlog::get(logger_name);
22  if (!_logger) {
23  _logger = spdlog::create<spdlog::sinks::stdout_color_sink_st>(logger_name);
24  }
25 
26  SetLogLevel(spdlog::level::warn);
27 }
28 
30 
31 void RssRestrictor::SetLogLevel(const uint8_t log_level) {
32  if (log_level < spdlog::level::n_levels) {
33  const auto log_level_value = static_cast<spdlog::level::level_enum>(log_level);
34  _logger->set_level(log_level_value);
35  }
36 }
37 
39  const carla::rpc::VehicleControl &vehicle_control, const ::ad::rss::state::ProperResponse &proper_response,
40  const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route,
41  const carla::rpc::VehiclePhysicsControl &vehicle_physics) {
42  carla::rpc::VehicleControl restricted_vehicle_control(vehicle_control);
43 
44  // Pretty basic implementation of a RSS restrictor modifying the
45  // VehicleControl according to the given
46  // restrictions. Basic braking and countersteering actions are applied.
47  // In case countersteering is not possible anymore (i.e. the lateral velocity
48  // reached zero),
49  // as a fallback longitudinal braking is applied instead (escalation
50  // strategy).
51  float mass = vehicle_physics.mass;
52  float max_steer_angle_deg = 0.f;
53  float sum_max_brake_torque = 0.f;
54  float radius = 1.f;
55  for (auto const &wheel : vehicle_physics.wheels) {
56  sum_max_brake_torque += wheel.max_brake_torque;
57  radius = wheel.radius;
58  max_steer_angle_deg = std::max(max_steer_angle_deg, wheel.max_steer_angle);
59  }
60 
61  // do not apply any restrictions when in reverse gear
62  if (!vehicle_control.reverse) {
63  _logger->debug("Lon {}, L {}, R {}; LatSpeed {}, Accel {}, Avg {}, Hdg {}, AllowedHeadingRanges {}",
64  proper_response.accelerationRestrictions.longitudinalRange,
65  proper_response.accelerationRestrictions.lateralLeftRange,
66  proper_response.accelerationRestrictions.lateralRightRange, ego_dynamics_on_route.route_speed_lat,
67  ego_dynamics_on_route.route_accel_lat, ego_dynamics_on_route.avg_route_accel_lat,
68  ego_dynamics_on_route.ego_heading, proper_response.headingRanges);
69  if (proper_response.accelerationRestrictions.lateralLeftRange.maximum <= ::ad::physics::Acceleration(0.0)) {
70  if (ego_dynamics_on_route.route_speed_lat < ::ad::physics::Speed(0.0)) {
71  // driving to the left
72  if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) {
73  double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon);
74  float desired_steer_ratio = -180.f * static_cast<float>(angle_rad / M_PI) / max_steer_angle_deg;
75  if (ego_dynamics_on_route.crossing_border) {
76  desired_steer_ratio += 0.1f;
77  }
78  float orig_steer = restricted_vehicle_control.steer;
79  restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, desired_steer_ratio);
80  restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, 1.0f);
81  _logger->debug("EgoVelocity: {}", ego_dynamics_on_route);
82  _logger->debug("Countersteer left to right: {} -> {}", orig_steer, restricted_vehicle_control.steer);
83  }
84  }
85  }
86 
87  if (proper_response.accelerationRestrictions.lateralRightRange.maximum <= ::ad::physics::Acceleration(0.0)) {
88  if (ego_dynamics_on_route.route_speed_lat > ::ad::physics::Speed(0.0)) {
89  // driving to the right
90  if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) {
91  double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon);
92  float desired_steer_ratio = -180.f * static_cast<float>(angle_rad / M_PI) / max_steer_angle_deg;
93  if (ego_dynamics_on_route.crossing_border) {
94  desired_steer_ratio -= 0.1f;
95  }
96  float orig_steer = restricted_vehicle_control.steer;
97  restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, desired_steer_ratio);
98  restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, -1.0f);
99  _logger->debug("EgoVelocity: {}", ego_dynamics_on_route);
100  _logger->debug("Countersteer right to left: {} -> {}", orig_steer, restricted_vehicle_control.steer);
101  }
102  }
103  }
104 
105  // restrict longitudinal acceleration
106  auto accel_lon = proper_response.accelerationRestrictions.longitudinalRange.maximum;
107  if (proper_response.unstructuredSceneResponse == ad::rss::state::UnstructuredSceneResponse::DriveAway) {
108  // drive away is only allowed in certain direction
109  auto heading_allowed = false;
110  if (!proper_response.headingRanges.empty()) {
111  auto max_steer_angle = max_steer_angle_deg * (ad::physics::cPI / ad::physics::Angle(180.0));
112  auto current_steering_angle = static_cast<double>(ego_dynamics_on_route.ego_heading) - vehicle_control.steer * max_steer_angle;
113  for (auto it = proper_response.headingRanges.cbegin(); (it != proper_response.headingRanges.cend() && !heading_allowed); ++it) {
114  heading_allowed = ad::rss::unstructured::isInsideHeadingRange(current_steering_angle, *it);
115  }
116  }
117 
118  if (!heading_allowed) {
119  accel_lon = proper_response.accelerationRestrictions.longitudinalRange.minimum;
120  }
121  }
122 
123  if (accel_lon > ::ad::physics::Acceleration(0.0)) {
124  // TODO: determine acceleration and limit throttle
125  }
126 
127  // decelerate
128  if (accel_lon < ::ad::physics::Acceleration(0.0)) {
129  restricted_vehicle_control.throttle = 0.0f;
130 
131  double brake_acceleration =
132  std::fabs(static_cast<double>(proper_response.accelerationRestrictions.longitudinalRange.minimum));
133  double sum_brake_torque = mass * brake_acceleration * radius / 100.0;
134  restricted_vehicle_control.brake = std::min(static_cast<float>(sum_brake_torque / sum_max_brake_torque), 1.0f);
135  }
136  }
137  if (restricted_vehicle_control != vehicle_control) {
138  _logger->debug(
139  "Restrictor active: throttle({} -> {}), brake ({} -> {}). steer ({} -> "
140  "{})",
141  vehicle_control.throttle, restricted_vehicle_control.throttle, vehicle_control.brake,
142  restricted_vehicle_control.brake, vehicle_control.steer, restricted_vehicle_control.steer);
143  }
144  return restricted_vehicle_control;
145 }
146 
147 } // namespace rss
148 } // namespace carla
struct defining the ego vehicles current dynamics in respect to the current route ...
Definition: RssCheck.h:40
void SetLogLevel(const uint8_t log_level)
::ad::physics::Speed route_speed_lon
the ego speed component lon in respect to a route
Definition: RssCheck.h:78
std::shared_ptr< spdlog::logger > _logger
the logger instance
Definition: RssRestrictor.h:57
carla::rpc::VehicleControl RestrictVehicleControl(const carla::rpc::VehicleControl &vehicle_control, const ::ad::rss::state::ProperResponse &proper_response, const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, const carla::rpc::VehiclePhysicsControl &vehicle_physics)
the actual function to restrict the given vehicle control input to mimick RSS conform behavior by bra...
::ad::map::point::ENUHeading ego_heading
the considered heading of the ego vehicle
Definition: RssCheck.h:57
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
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
::ad::physics::Acceleration route_accel_lat
the ego acceleration component lat in respect to a route
Definition: RssCheck.h:80
double min(double v1, double v2)
Definition: Simplify.h:294
::ad::physics::Speed route_speed_lat
the ego speed component lat in respect to a route
Definition: RssCheck.h:76
::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