CARLA
LibCarla/source/carla/rss/RssSensor.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/RssSensor.h"
7 
8 #include <ad/map/access/Operation.hpp>
9 #include <ad/rss/state/ProperResponse.hpp>
10 #include <ad/rss/world/Velocity.hpp>
11 #include <exception>
12 #include <fstream>
13 
14 #include "carla/Logging.h"
15 #include "carla/client/Map.h"
16 #include "carla/client/Sensor.h"
17 #include "carla/client/Vehicle.h"
19 #include "carla/rss/RssCheck.h"
21 
22 namespace carla {
23 namespace client {
24 
25 RssSensor::RssSensor(ActorInitializer init) : Sensor(std::move(init)), _on_tick_register_id(0u), _drop_route(false) {}
26 
28  // ensure there is no processing anymore when deleting rss_check object
29  const std::lock_guard<std::mutex> lock(_processing_lock);
30  _rss_check.reset();
31 }
32 
34  if (IsListening()) {
36  ": registering of the actor constellation callback has to be done before start listening. Register has "
37  "no effect.");
38  return;
39  }
41 }
42 
44  if (IsListening()) {
45  log_error(GetDisplayId(), ": already listening");
46  return;
47  }
48 
49  if (GetParent() == nullptr) {
50  throw_exception(std::runtime_error(GetDisplayId() + ": not attached to actor"));
51  return;
52  }
53 
54  auto vehicle = boost::dynamic_pointer_cast<carla::client::Vehicle>(GetParent());
55  if (vehicle == nullptr) {
56  throw_exception(std::runtime_error(GetDisplayId() + ": parent is not a vehicle"));
57  return;
58  }
59 
60  // get maximum steering angle
61  float max_steer_angle_deg = 0.f;
62  for (auto const &wheel : vehicle->GetPhysicsControl().GetWheels()) {
63  max_steer_angle_deg = std::max(max_steer_angle_deg, wheel.max_steer_angle);
64  }
65  auto max_steering_angle = max_steer_angle_deg * static_cast<float>(M_PI) / 180.0f;
66 
67  auto map = GetWorld().GetMap();
68  DEBUG_ASSERT(map != nullptr);
69  std::string const open_drive_content = map->GetOpenDrive();
70 
71  _rss_check = nullptr;
72  ::ad::map::access::cleanup();
73 
74  ::ad::map::access::initFromOpenDriveContent(open_drive_content, 0.2,
76  ::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN);
77 
78  if (_rss_actor_constellation_callback == nullptr) {
79  _rss_check = std::make_shared<::carla::rss::RssCheck>(max_steering_angle);
80  } else {
81  _rss_check =
82  std::make_shared<::carla::rss::RssCheck>(max_steering_angle, _rss_actor_constellation_callback, GetParent());
83  }
84 
85  auto self = boost::static_pointer_cast<RssSensor>(shared_from_this());
86 
87  log_debug(GetDisplayId(), ": subscribing to tick event");
88  _on_tick_register_id = GetEpisode().Lock()->RegisterOnTickEvent(
89  [ cb = std::move(callback), weak_self = WeakPtr<RssSensor>(self) ](const auto &snapshot) {
90  auto self = weak_self.lock();
91  if (self != nullptr) {
92  auto data = self->TickRssSensor(snapshot.GetTimestamp());
93  if (data != nullptr) {
94  cb(std::move(data));
95  }
96  }
97  });
98 }
99 
101  if (!IsListening()) {
102  log_error(GetDisplayId(), ": not listening at all");
103  return;
104  }
105 
106  log_debug(GetDisplayId(), ": unsubscribing from tick event");
107  GetEpisode().Lock()->RemoveOnTickEvent(_on_tick_register_id);
109 }
110 
111 void RssSensor::SetLogLevel(const uint8_t &log_level) {
112  if (!bool(_rss_check)) {
113  log_error(GetDisplayId(), ": not yet listening. SetLogLevel has no effect.");
114  return;
115  }
116 
117  if (log_level < spdlog::level::n_levels) {
118  _rss_check->SetLogLevel(spdlog::level::level_enum(log_level));
119  }
120 }
121 
122 void RssSensor::SetMapLogLevel(const uint8_t &map_log_level) {
123  if (!bool(_rss_check)) {
124  log_error(GetDisplayId(), ": not yet listening. SetMapLogLevel has no effect.");
125  return;
126  }
127 
128  if (map_log_level < spdlog::level::n_levels) {
129  _rss_check->SetMapLogLevel(spdlog::level::level_enum(map_log_level));
130  }
131 }
132 
133 const ::ad::rss::world::RssDynamics &RssSensor::GetEgoVehicleDynamics() const {
134  static auto default_vehicle_dynamics = rss::RssCheck::GetDefaultVehicleDynamics();
135  if (!bool(_rss_check)) {
136  log_error(GetDisplayId(), ": not yet listening. GetEgoVehicleDynamics has no effect.");
137  return default_vehicle_dynamics;
138  }
139 
141  log_error(GetDisplayId(), ": Actor constellation callback registered. GetEgoVehicleDynamics has no effect.");
142  return default_vehicle_dynamics;
143  }
144 
145  return _rss_check->GetDefaultActorConstellationCallbackEgoVehicleDynamics();
146 }
147 
148 void RssSensor::SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_dynamics) {
149  if (!bool(_rss_check)) {
150  log_error(GetDisplayId(), ": not yet listening. SetEgoVehicleDynamics has no effect.");
151  return;
152  }
153 
155  log_error(GetDisplayId(), ": Actor constellation callback registered. SetEgoVehicleDynamics has no effect.");
156  return;
157  }
158 
159  _rss_check->SetDefaultActorConstellationCallbackEgoVehicleDynamics(ego_dynamics);
160 }
161 
162 const ::ad::rss::world::RssDynamics &RssSensor::GetOtherVehicleDynamics() const {
163  static auto default_vehicle_dynamics = rss::RssCheck::GetDefaultVehicleDynamics();
164  if (!bool(_rss_check)) {
165  log_error(GetDisplayId(), ": not yet listening. GetOtherVehicleDynamics has no effect.");
166  return default_vehicle_dynamics;
167  }
168 
170  log_error(GetDisplayId(), ": Actor constellation callback registered. GetOtherVehicleDynamics has no effect.");
171  return default_vehicle_dynamics;
172  }
173 
174  return _rss_check->GetDefaultActorConstellationCallbackOtherVehicleDynamics();
175 }
176 
177 void RssSensor::SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) {
178  if (!bool(_rss_check)) {
179  log_error(GetDisplayId(), ": not yet listening. SetOtherVehicleDynamics has no effect.");
180  return;
181  }
182 
184  log_error(GetDisplayId(), ": Actor constellation callback registered. SetOtherVehicleDynamics has no effect.");
185  return;
186  }
187 
188  _rss_check->SetDefaultActorConstellationCallbackOtherVehicleDynamics(other_vehicle_dynamics);
189 }
190 
191 const ::ad::rss::world::RssDynamics &RssSensor::GetPedestrianDynamics() const {
192  static auto default_pedestrian_dynamics = rss::RssCheck::GetDefaultPedestrianDynamics();
193  if (!bool(_rss_check)) {
194  log_error(GetDisplayId(), ": not yet listening. GetPedestrianDynamics has no effect.");
195  return default_pedestrian_dynamics;
196  }
197 
199  log_error(GetDisplayId(), ": Actor constellation callback registered. GetPedestrianDynamics has no effect.");
200  return default_pedestrian_dynamics;
201  }
202 
203  return _rss_check->GetDefaultActorConstellationCallbackPedestrianDynamics();
204 }
205 
206 void RssSensor::SetPedestrianDynamics(const ::ad::rss::world::RssDynamics &pedestrian_dynamics) {
207  if (!bool(_rss_check)) {
208  log_error(GetDisplayId(), ": not yet listening. SetPedestrianDynamics has no effect.");
209  return;
210  }
211 
213  log_error(GetDisplayId(), ": Actor constellation callback registered. SetPedestrianDynamics has no effect.");
214  return;
215  }
216 
217  _rss_check->SetDefaultActorConstellationCallbackPedestrianDynamics(pedestrian_dynamics);
218 }
219 
221  if (!bool(_rss_check)) {
222  log_error(GetDisplayId(), ": not yet listening. GetRoadBoundariesMode has no effect.");
223  static auto default_road_boundaries_mode = rss::RssCheck::GetDefaultRoadBoundariesMode();
224  return default_road_boundaries_mode;
225  }
226 
227  return _rss_check->GetRoadBoundariesMode();
228 }
229 
231  if (!bool(_rss_check)) {
232  log_error(GetDisplayId(), ": not yet listening. SetRoadBoundariesMode has no effect.");
233  return;
234  }
235 
236  _rss_check->SetRoadBoundariesMode(road_boundaries_mode);
237 }
238 
240  if (!bool(_rss_check)) {
241  log_error(GetDisplayId(), ": not yet listening. AppendRoutingTarget has no effect.");
242  return;
243  }
244 
245  _rss_check->AppendRoutingTarget(routing_target);
246 }
247 
248 const std::vector<::carla::geom::Transform> RssSensor::GetRoutingTargets() const {
249  if (!bool(_rss_check)) {
250  log_error(GetDisplayId(), ": not yet listening. GetRoutingTargets has no effect.");
251  return std::vector<::carla::geom::Transform>();
252  }
253 
254  return _rss_check->GetRoutingTargets();
255 }
256 
258  if (!bool(_rss_check)) {
259  log_error(GetDisplayId(), ": not yet listening. ResetRoutingTargets has no effect.");
260  return;
261  }
262 
263  _rss_check->ResetRoutingTargets();
264 }
265 
267  // don't execute this immediately as it might break calculations completely
268  // postpone to next sensor tick
269  _drop_route = true;
270 }
271 
273  try {
274  bool result = false;
275  ::ad::rss::state::ProperResponse response;
276  ::ad::rss::state::RssStateSnapshot rss_state_snapshot;
277  ::ad::rss::situation::SituationSnapshot situation_snapshot;
278  ::ad::rss::world::WorldModel world_model;
279  carla::rss::EgoDynamicsOnRoute ego_dynamics_on_route;
280  if (_processing_lock.try_lock()) {
281  spdlog::debug("RssSensor tick: T={}", timestamp.frame);
282 
283  if ((timestamp.frame < _last_processed_frame) && ((_last_processed_frame - timestamp.frame) < 0xffffffffu))
284  {
285  _processing_lock.unlock();
286  spdlog::warn("RssSensor tick dropped: T={}", timestamp.frame);
287  return nullptr;
288  }
289  _last_processed_frame = timestamp.frame;
290 
291  carla::client::World world = GetWorld();
293 
294  if (_drop_route) {
295  _drop_route = false;
296  _rss_check->DropRoute();
297  }
298 
299  // check all object<->ego pairs with RSS and calculate proper response
300  result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, rss_state_snapshot,
301  situation_snapshot, world_model, ego_dynamics_on_route);
302  _processing_lock.unlock();
303 
304  spdlog::debug(
305  "RssSensor response: T={} S:{}->E:{} DeltaT:{}", timestamp.frame,
306  ego_dynamics_on_route.time_since_epoch_check_start_ms, ego_dynamics_on_route.time_since_epoch_check_end_ms,
307  ego_dynamics_on_route.time_since_epoch_check_end_ms - ego_dynamics_on_route.time_since_epoch_check_start_ms);
308  return MakeShared<sensor::data::RssResponse>(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), result,
309  response, rss_state_snapshot, situation_snapshot, world_model,
310  ego_dynamics_on_route);
311  } else {
312  spdlog::debug("RssSensor tick dropped: T={}", timestamp.frame);
313  return nullptr;
314  }
315  } catch (const std::exception &e) {
316  /// @todo do we need to unsubscribe the sensor here?
317  std::cout << e.what() << std::endl;
318  _processing_lock.unlock();
319  spdlog::error("RssSensor tick exception");
320  return nullptr;
321  } catch (...) {
322  _processing_lock.unlock();
323  spdlog::error("RssSensor tick exception");
324  return nullptr;
325  }
326 }
327 
328 } // namespace client
329 } // namespace carla
double time_since_epoch_check_start_ms
the time since epoch in ms at start of the checkObjects call
Definition: RssCheck.h:47
double time_since_epoch_check_end_ms
the time since epoch in ms at the end of the checkObjects call
Definition: RssCheck.h:49
std::function<::carla::rss::ActorConstellationResult(carla::SharedPtr<::carla::rss::ActorConstellationData >)> ActorConstellationCallbackFunctionType
std::shared_ptr<::carla::rss::RssCheck > _rss_check
struct defining the ego vehicles current dynamics in respect to the current route ...
Definition: RssCheck.h:40
void SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_dynamics)
sets the ego vehicle dynamics to be used by the ego vehicle (
void Listen(CallbackFunctionType callback) override
Register a callback to be executed each time a new measurement is received.
void SetMapLogLevel(const uint8_t &map_log_level)
sets the current map log level
SharedPtr< ActorList > GetActors() const
Return a list with all the actors currently present in the world.
Definition: World.cpp:99
void DropRoute()
drop the current route (
void throw_exception(const std::exception &e)
Definition: Carla.cpp:101
void SetLogLevel(const uint8_t &log_level)
sets the current log level
std::mutex _processing_lock
the mutex to protect the actual RSS processing and in case it takes too long to process ever frame ...
static void log_error(Args &&... args)
Definition: Logging.h:110
const ::ad::rss::world::RssDynamics & GetPedestrianDynamics() const
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
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
SharedPtr< Map > GetMap() const
Return the map that describes this world.
Definition: World.cpp:21
std::size_t _on_tick_register_id
the id got when registering for the on tick event
The RSS Sensor class implementing the carla::client::Sensor interface This class is a proxy to the Rs...
void SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode)
sets the current mode for respecting the road boundaries (
static void log_debug(Args &&...)
Definition: Logging.h:75
const std::string & GetDisplayId() const
Definition: ActorState.h:33
static RoadBoundariesMode GetDefaultRoadBoundariesMode()
Definition: RssCheck.h:212
Used to initialize Actor classes.
Definition: ActorState.h:93
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
::ad::rss::world::RssDynamics GetDefaultPedestrianDynamics()
Definition: RssCheck.cpp:107
void SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics)
sets the ego vehicle dynamics to be used by other vehicles (
void AppendRoutingTarget(const ::carla::geom::Transform &routing_target)
appends a routing target to the current routing target list (
void ResetRoutingTargets()
resets the current routing target (
RoadBoundariesMode
struct defining the different supported handling of road boundaries
Definition: RssCheck.h:28
const ::ad::rss::world::RssDynamics & GetEgoVehicleDynamics() const
geom::Transform GetTransform() const
Return the current transform of the actor.
Definition: Actor.cpp:19
const ::ad::rss::world::RssDynamics & GetOtherVehicleDynamics() const
void RegisterActorConstellationCallback(ActorConstellationCallbackFunctionType callback)
Register a callback to be executed for each actor within each measurement to be processed to decide o...
SharedPtr< Actor > GetParent() const
Definition: ActorState.cpp:31
std::function< void(SharedPtr< sensor::SensorData >)> CallbackFunctionType
std::size_t _last_processed_frame
last processed frame
bool IsListening() const override
Return whether this Sensor instance is currently listening to the associated sensor in the simulator...
bool _drop_route
reqired to store DropRoute() requests until next sensor tick
ActorConstellationCallbackFunctionType _rss_actor_constellation_callback
SharedPtrType Lock() const
Same as TryLock but never return nullptr.
boost::weak_ptr< T > WeakPtr
Definition: Memory.h:23
const ::carla::rss::RoadBoundariesMode & GetRoadBoundariesMode() const
void Stop() override
Stop listening for new measurements.
void SetPedestrianDynamics(const ::ad::rss::world::RssDynamics &pedestrian_dynamics)
sets the ego vehicle dynamics to be used by pedestrians (
SharedPtr< sensor::SensorData > TickRssSensor(const Timestamp &timestamp)
the acutal sensor tick callback function
::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
geom::Transform Transform
Definition: rpc/Transform.h:16
const std::vector<::carla::geom::Transform > GetRoutingTargets() const
RssSensor(ActorInitializer init)
constructor