CARLA
LibCarla/source/carla/client/LaneInvasionSensor.cpp
Go to the documentation of this file.
1 // Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
8 
9 #include "carla/Logging.h"
10 #include "carla/client/Map.h"
11 #include "carla/client/Vehicle.h"
13 #include "carla/geom/Location.h"
14 #include "carla/geom/Math.h"
16 
17 #include <exception>
18 
19 namespace carla {
20 namespace client {
21 
22  // ===========================================================================
23  // -- Static local methods ---------------------------------------------------
24  // ===========================================================================
25 
26  static geom::Location Rotate(float yaw, const geom::Location &location) {
27  yaw *= geom::Math::Pi<float>() / 180.0f;
28  const float c = std::cos(yaw);
29  const float s = std::sin(yaw);
30  return {
31  c * location.x - s * location.y,
32  s * location.x + c * location.y,
33  location.z};
34  }
35 
36  // ===========================================================================
37  // -- LaneInvasionCallback ---------------------------------------------------
38  // ===========================================================================
39 
41  public:
42 
44  const Vehicle &vehicle,
45  SharedPtr<Map> &&map,
46  Sensor::CallbackFunctionType &&user_callback)
47  : _parent(vehicle.GetId()),
48  _parent_bounding_box(vehicle.GetBoundingBox()),
49  _map(std::move(map)),
50  _callback(std::move(user_callback)) {
51  DEBUG_ASSERT(_map != nullptr);
52  }
53 
54  void Tick(const WorldSnapshot &snapshot) const;
55 
56  private:
57 
58  struct Bounds {
59  size_t frame;
60  std::array<geom::Location, 4u> corners;
61  };
62 
63  std::shared_ptr<const Bounds> MakeBounds(
64  size_t frame,
65  const geom::Transform &vehicle_transform) const;
66 
68 
70 
72 
74 
76  };
77 
78  void LaneInvasionCallback::Tick(const WorldSnapshot &snapshot) const {
79  // Make sure the parent is alive.
80  auto parent = snapshot.Find(_parent);
81  if (!parent) {
82  return;
83  }
84 
85  auto next = MakeBounds(snapshot.GetFrame(), parent->transform);
86  auto prev = _bounds.load();
87 
88  // First frame it'll be null.
89  if ((prev == nullptr) && _bounds.compare_exchange(&prev, next)) {
90  return;
91  }
92 
93  // Make sure the distance is long enough.
94  constexpr float distance_threshold = 10.0f * std::numeric_limits<float>::epsilon();
95  for (auto i = 0u; i < 4u; ++i) {
96  if ((next->corners[i] - prev->corners[i]).Length() < distance_threshold) {
97  return;
98  }
99  }
100 
101  // Make sure the current frame is up-to-date.
102  do {
103  if (prev->frame >= next->frame) {
104  return;
105  }
106  } while (!_bounds.compare_exchange(&prev, next));
107 
108  // Finally it's safe to compute the crossed lanes.
109  std::vector<road::element::LaneMarking> crossed_lanes;
110  for (auto i = 0u; i < 4u; ++i) {
111  const auto lanes = _map->CalculateCrossedLanes(prev->corners[i], next->corners[i]);
112  crossed_lanes.insert(crossed_lanes.end(), lanes.begin(), lanes.end());
113  }
114 
115  if (!crossed_lanes.empty()) {
116  _callback(MakeShared<sensor::data::LaneInvasionEvent>(
117  snapshot.GetTimestamp().frame,
118  snapshot.GetTimestamp().elapsed_seconds,
119  parent->transform,
120  _parent,
121  std::move(crossed_lanes)));
122  }
123  }
124 
125  std::shared_ptr<const LaneInvasionCallback::Bounds> LaneInvasionCallback::MakeBounds(
126  const size_t frame,
127  const geom::Transform &transform) const {
128  const auto &box = _parent_bounding_box;
129  const auto location = transform.location + box.location;
130  const auto yaw = transform.rotation.yaw;
131  return std::make_shared<Bounds>(Bounds{frame, {
132  location + Rotate(yaw, geom::Location( box.extent.x, box.extent.y, 0.0f)),
133  location + Rotate(yaw, geom::Location(-box.extent.x, box.extent.y, 0.0f)),
134  location + Rotate(yaw, geom::Location( box.extent.x, -box.extent.y, 0.0f)),
135  location + Rotate(yaw, geom::Location(-box.extent.x, -box.extent.y, 0.0f))}});
136  }
137 
138  // ===========================================================================
139  // -- LaneInvasionSensor -----------------------------------------------------
140  // ===========================================================================
141 
143  Stop();
144  }
145 
147  auto vehicle = boost::dynamic_pointer_cast<Vehicle>(GetParent());
148  if (vehicle == nullptr) {
149  log_error(GetDisplayId(), ": not attached to a vehicle");
150  return;
151  }
152 
153  auto episode = GetEpisode().Lock();
154 
155  auto cb = std::make_shared<LaneInvasionCallback>(
156  *vehicle,
157  episode->GetCurrentMap(),
158  std::move(callback));
159 
160  const size_t callback_id = episode->RegisterOnTickEvent([cb=std::move(cb)](const auto &snapshot) {
161  try {
162  cb->Tick(snapshot);
163  } catch (const std::exception &e) {
164  log_error("LaneInvasionSensor:", e.what());
165  }
166  });
167 
168  const size_t previous = _callback_id.exchange(callback_id);
169  if (previous != 0u) {
170  episode->RemoveOnTickEvent(previous);
171  }
172  }
173 
175  const size_t previous = _callback_id.exchange(0u);
176  auto episode = GetEpisode().TryLock();
177  if ((previous != 0u) && (episode != nullptr)) {
178  episode->RemoveOnTickEvent(previous);
179  }
180  }
181 
182 } // namespace client
183 } // namespace carla
LaneInvasionCallback(const Vehicle &vehicle, SharedPtr< Map > &&map, Sensor::CallbackFunctionType &&user_callback)
boost::optional< ActorSnapshot > Find(ActorId actor_id) const
Find an ActorSnapshot by id.
Definition: WorldSnapshot.h:44
static void log_error(Args &&... args)
Definition: Logging.h:110
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
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
const Timestamp & GetTimestamp() const
Get timestamp of this snapshot.
Definition: WorldSnapshot.h:34
carla::ActorId ActorId
static geom::Location Rotate(float yaw, const geom::Location &location)
void Stop() override
Stop listening for new measurements.
std::shared_ptr< const Bounds > MakeBounds(size_t frame, const geom::Transform &vehicle_transform) const
std::function< void(SharedPtr< sensor::SensorData >)> CallbackFunctionType
A very simple atomic shared ptr with release-acquire memory order.
void Listen(CallbackFunctionType callback) override
Register a callback to be executed each time a new measurement is received.
double elapsed_seconds
Simulated seconds elapsed since the beginning of the current episode.
Definition: Timestamp.h:33