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  //_logger->set_level(spdlog::level::debug);
26 }
27 
29 
31  const carla::rpc::VehicleControl &vehicle_control, const ::ad::rss::state::ProperResponse &proper_response,
32  const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route,
33  const carla::rpc::VehiclePhysicsControl &vehicle_physics) {
34  carla::rpc::VehicleControl restricted_vehicle_control(vehicle_control);
35 
36  // Pretty basic implementation of a RSS restrictor modifying the
37  // VehicleControl according to the given
38  // restrictions. Basic braking and countersteering actions are applied.
39  // In case countersteering is not possible anymore (i.e. the lateral velocity
40  // reached zero),
41  // as a fallback longitudinal braking is applied instead (escalation
42  // strategy).
43  float mass = vehicle_physics.mass;
44  float max_steer_angle_deg = 0.f;
45  float sum_max_brake_torque = 0.f;
46  float radius = 1.f;
47  for (auto const &wheel : vehicle_physics.wheels) {
48  sum_max_brake_torque += wheel.max_brake_torque;
49  radius = wheel.radius;
50  max_steer_angle_deg = std::max(max_steer_angle_deg, wheel.max_steer_angle);
51  }
52 
53  // do not apply any restrictions when in reverse gear
54  if (!vehicle_control.reverse) {
55  _logger->debug("Lon {}, L {}, R {}; LatSpeed {}, Accel {}, Avg {}, Hdg {}, AllowedHeadingRanges {}",
56  proper_response.accelerationRestrictions.longitudinalRange,
57  proper_response.accelerationRestrictions.lateralLeftRange,
58  proper_response.accelerationRestrictions.lateralRightRange, ego_dynamics_on_route.route_speed_lat,
59  ego_dynamics_on_route.route_accel_lat, ego_dynamics_on_route.avg_route_accel_lat,
60  ego_dynamics_on_route.ego_heading, proper_response.headingRanges);
61  if (proper_response.accelerationRestrictions.lateralLeftRange.maximum <= ::ad::physics::Acceleration(0.0)) {
62  if (ego_dynamics_on_route.route_speed_lat < ::ad::physics::Speed(0.0)) {
63  // driving to the left
64  if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) {
65  double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon);
66  float desired_steer_ratio = -180.f * static_cast<float>(angle_rad / M_PI) / max_steer_angle_deg;
67  if (ego_dynamics_on_route.crossing_border) {
68  desired_steer_ratio += 0.1f;
69  }
70  float orig_steer = restricted_vehicle_control.steer;
71  restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, desired_steer_ratio);
72  restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, 1.0f);
73  _logger->debug("EgoVelocity: {}", ego_dynamics_on_route);
74  _logger->debug("Countersteer left to right: {} -> {}", orig_steer, restricted_vehicle_control.steer);
75  }
76  }
77  }
78 
79  if (proper_response.accelerationRestrictions.lateralRightRange.maximum <= ::ad::physics::Acceleration(0.0)) {
80  if (ego_dynamics_on_route.route_speed_lat > ::ad::physics::Speed(0.0)) {
81  // driving to the right
82  if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) {
83  double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon);
84  float desired_steer_ratio = -180.f * static_cast<float>(angle_rad / M_PI) / max_steer_angle_deg;
85  if (ego_dynamics_on_route.crossing_border) {
86  desired_steer_ratio -= 0.1f;
87  }
88  float orig_steer = restricted_vehicle_control.steer;
89  restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, desired_steer_ratio);
90  restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, -1.0f);
91  _logger->debug("EgoVelocity: {}", ego_dynamics_on_route);
92  _logger->debug("Countersteer right to left: {} -> {}", orig_steer, restricted_vehicle_control.steer);
93  }
94  }
95  }
96 
97  // restrict longitudinal acceleration
98  auto accel_lon = proper_response.accelerationRestrictions.longitudinalRange.maximum;
99  if (proper_response.unstructuredSceneResponse == ad::rss::state::UnstructuredSceneResponse::DriveAway) {
100  // drive away is only allowed in certain direction
101  auto heading_allowed = false;
102  if (!proper_response.headingRanges.empty()) {
103  auto max_steer_angle = max_steer_angle_deg * (ad::physics::cPI / ad::physics::Angle(180.0));
104  auto current_steering_angle = static_cast<double>(ego_dynamics_on_route.ego_heading) - vehicle_control.steer * max_steer_angle;
105  for (auto it = proper_response.headingRanges.cbegin(); (it != proper_response.headingRanges.cend() && !heading_allowed); ++it) {
106  heading_allowed = ad::rss::unstructured::isInsideHeadingRange(current_steering_angle, *it);
107  }
108  }
109 
110  if (!heading_allowed) {
111  accel_lon = proper_response.accelerationRestrictions.longitudinalRange.minimum;
112  }
113  }
114 
115  if (accel_lon > ::ad::physics::Acceleration(0.0)) {
116  // TODO: determine acceleration and limit throttle
117  }
118 
119  // decelerate
120  if (accel_lon < ::ad::physics::Acceleration(0.0)) {
121  restricted_vehicle_control.throttle = 0.0f;
122 
123  double brake_acceleration =
124  std::fabs(static_cast<double>(proper_response.accelerationRestrictions.longitudinalRange.minimum));
125  double sum_brake_torque = mass * brake_acceleration * radius / 100.0;
126  restricted_vehicle_control.brake = std::min(static_cast<float>(sum_brake_torque / sum_max_brake_torque), 1.0f);
127  }
128  }
129  if (restricted_vehicle_control != vehicle_control) {
130  _logger->debug(
131  "Restrictor active: throttle({} -> {}), brake ({} -> {}). steer ({} -> "
132  "{})",
133  vehicle_control.throttle, restricted_vehicle_control.throttle, vehicle_control.brake,
134  restricted_vehicle_control.brake, vehicle_control.steer, restricted_vehicle_control.steer);
135  }
136  return restricted_vehicle_control;
137 }
138 
139 } // namespace rss
140 } // namespace carla
struct defining the ego vehicles current dynamics in respect to the current route ...
Definition: RssCheck.h:40
::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:55
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:99
::ad::physics::Acceleration route_accel_lat
the ego acceleration component lat in respect to a route
Definition: RssCheck.h:80
::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