CARLA
road/Map.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020 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 
7 #include "carla/road/Map.h"
8 #include "carla/Exception.h"
9 #include "carla/geom/Math.h"
10 #include "carla/road/MeshFactory.h"
19 
20 #include <vector>
21 #include <unordered_map>
22 #include <stdexcept>
23 
24 namespace carla {
25 namespace road {
26 
27  using namespace carla::road::element;
28 
29  /// We use this epsilon to shift the waypoints away from the edges of the lane
30  /// sections to avoid floating point precision errors.
31  static constexpr double EPSILON = 10.0 * std::numeric_limits<double>::epsilon();
32 
33  // ===========================================================================
34  // -- Static local methods ---------------------------------------------------
35  // ===========================================================================
36 
37  template <typename T>
38  static std::vector<T> ConcatVectors(std::vector<T> dst, std::vector<T> src) {
39  if (src.size() > dst.size()) {
40  return ConcatVectors(src, dst);
41  }
42  dst.insert(
43  dst.end(),
44  std::make_move_iterator(src.begin()),
45  std::make_move_iterator(src.end()));
46  return dst;
47  }
48 
49  static double GetDistanceAtStartOfLane(const Lane &lane) {
50  if (lane.GetId() <= 0) {
51  return lane.GetDistance() + 10.0 * EPSILON;
52  } else {
53  return lane.GetDistance() + lane.GetLength() - 10.0 * EPSILON;
54  }
55  }
56 
57  static double GetDistanceAtEndOfLane(const Lane &lane) {
58  if (lane.GetId() > 0) {
59  return lane.GetDistance() + 10.0 * EPSILON;
60  } else {
61  return lane.GetDistance() + lane.GetLength() - 10.0 * EPSILON;
62  }
63  }
64 
65  /// Return a waypoint for each drivable lane on @a lane_section.
66  template <typename FuncT>
68  RoadId road_id,
69  const LaneSection &lane_section,
70  double distance,
71  FuncT &&func) {
72  for (const auto &pair : lane_section.GetLanes()) {
73  const auto &lane = pair.second;
74  if (lane.GetId() == 0) {
75  continue;
76  }
77  if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(Lane::LaneType::Driving)) > 0) {
78  std::forward<FuncT>(func)(Waypoint{
79  road_id,
80  lane_section.GetId(),
81  lane.GetId(),
82  distance < 0.0 ? GetDistanceAtStartOfLane(lane) : distance});
83  }
84  }
85  }
86 
87  template <typename FuncT>
88  static void ForEachLaneImpl(
89  RoadId road_id,
90  const LaneSection &lane_section,
91  double distance,
92  Lane::LaneType lane_type,
93  FuncT &&func) {
94  for (const auto &pair : lane_section.GetLanes()) {
95  const auto &lane = pair.second;
96  if (lane.GetId() == 0) {
97  continue;
98  }
99  if ((static_cast<int32_t>(lane.GetType()) & static_cast<int32_t>(lane_type)) > 0) {
100  std::forward<FuncT>(func)(Waypoint{
101  road_id,
102  lane_section.GetId(),
103  lane.GetId(),
104  distance < 0.0 ? GetDistanceAtStartOfLane(lane) : distance});
105  }
106  }
107  }
108 
109  /// Return a waypoint for each drivable lane on each lane section of @a road.
110  template <typename FuncT>
111  static void ForEachDrivableLane(const Road &road, FuncT &&func) {
112  for (const auto &lane_section : road.GetLaneSections()) {
114  road.GetId(),
115  lane_section,
116  -1.0, // At start of the lane
117  std::forward<FuncT>(func));
118  }
119  }
120 
121  /// Return a waypoint for each lane of the specified type on each lane section of @a road.
122  template <typename FuncT>
123  static void ForEachLane(const Road &road, Lane::LaneType lane_type, FuncT &&func) {
124  for (const auto &lane_section : road.GetLaneSections()) {
126  road.GetId(),
127  lane_section,
128  -1.0, // At start of the lane
129  lane_type,
130  std::forward<FuncT>(func));
131  }
132  }
133 
134  /// Return a waypoint for each drivable lane at @a distance on @a road.
135  template <typename FuncT>
136  static void ForEachDrivableLaneAt(const Road &road, double distance, FuncT &&func) {
137  for (const auto &lane_section : road.GetLaneSectionsAt(distance)) {
139  road.GetId(),
140  lane_section,
141  distance,
142  std::forward<FuncT>(func));
143  }
144  }
145 
146  /// Assumes road_id and section_id are valid.
147  static bool IsLanePresent(const MapData &data, Waypoint waypoint) {
148  const auto &section = data.GetRoad(waypoint.road_id).GetLaneSectionById(waypoint.section_id);
149  return section.ContainsLane(waypoint.lane_id);
150  }
151 
152  // ===========================================================================
153  // -- Map: Geometry ----------------------------------------------------------
154  // ===========================================================================
155 
156  boost::optional<Waypoint> Map::GetClosestWaypointOnRoad(
157  const geom::Location &pos,
158  int32_t lane_type) const {
159  std::vector<Rtree::TreeElement> query_result =
160  _rtree.GetNearestNeighboursWithFilter(Rtree::BPoint(pos.x, pos.y, pos.z),
161  [&](Rtree::TreeElement const &element) {
162  const Lane &lane = GetLane(element.second.first);
163  return (lane_type & static_cast<int32_t>(lane.GetType())) > 0;
164  });
165 
166  if (query_result.size() == 0) {
167  return boost::optional<Waypoint>{};
168  }
169 
170  Rtree::BSegment segment = query_result.front().first;
171  Rtree::BPoint s1 = segment.first;
172  Rtree::BPoint s2 = segment.second;
173  auto distance_to_segment = geom::Math::DistanceSegmentToPoint(pos,
174  geom::Vector3D(s1.get<0>(), s1.get<1>(), s1.get<2>()),
175  geom::Vector3D(s2.get<0>(), s2.get<1>(), s2.get<2>()));
176 
177  Waypoint result_start = query_result.front().second.first;
178  Waypoint result_end = query_result.front().second.second;
179 
180  if (result_start.lane_id < 0) {
181  double delta_s = distance_to_segment.first;
182  double final_s = result_start.s + delta_s;
183  if (final_s >= result_end.s) {
184  return result_end;
185  } else if (delta_s <= 0) {
186  return result_start;
187  } else {
188  return GetNext(result_start, delta_s).front();
189  }
190  } else {
191  double delta_s = distance_to_segment.first;
192  double final_s = result_start.s - delta_s;
193  if (final_s <= result_end.s) {
194  return result_end;
195  } else if (delta_s <= 0) {
196  return result_start;
197  } else {
198  return GetNext(result_start, delta_s).front();
199  }
200  }
201  }
202 
203  boost::optional<Waypoint> Map::GetWaypoint(
204  const geom::Location &pos,
205  int32_t lane_type) const {
206  boost::optional<Waypoint> w = GetClosestWaypointOnRoad(pos, lane_type);
207 
208  if (!w.has_value()) {
209  return w;
210  }
211 
212  const auto dist = geom::Math::Distance2D(ComputeTransform(*w).location, pos);
213  const auto lane_width_info = GetLane(*w).GetInfo<RoadInfoLaneWidth>(w->s);
214  const auto half_lane_width =
215  lane_width_info->GetPolynomial().Evaluate(w->s) * 0.5;
216 
217  if (dist < half_lane_width) {
218  return w;
219  }
220 
221  return boost::optional<Waypoint>{};
222  }
223 
224  boost::optional<Waypoint> Map::GetWaypoint(
225  RoadId road_id,
226  LaneId lane_id,
227  float s) const {
228 
229  // define the waypoint with the known parameters
230  Waypoint waypoint;
231  waypoint.road_id = road_id;
232  waypoint.lane_id = lane_id;
233  waypoint.s = s;
234 
235  // check the road
236  if (!_data.ContainsRoad(waypoint.road_id)) {
237  return boost::optional<Waypoint>{};
238  }
239  const Road &road = _data.GetRoad(waypoint.road_id);
240 
241  // check the 's' distance
242  if (s < 0.0f || s >= road.GetLength()) {
243  return boost::optional<Waypoint>{};
244  }
245 
246  // check the section
247  bool lane_found = false;
248  for (auto &section : road.GetLaneSectionsAt(s)) {
249  if (section.ContainsLane(lane_id)) {
250  waypoint.section_id = section.GetId();
251  lane_found = true;
252  break;
253  }
254  }
255 
256  // check the lane id
257  if (!lane_found) {
258  return boost::optional<Waypoint>{};
259  }
260 
261  return waypoint;
262  }
263 
265  return GetLane(waypoint).ComputeTransform(waypoint.s);
266  }
267 
268  // ===========================================================================
269  // -- Map: Road information --------------------------------------------------
270  // ===========================================================================
271 
272  Lane::LaneType Map::GetLaneType(const Waypoint waypoint) const {
273  return GetLane(waypoint).GetType();
274  }
275 
276  double Map::GetLaneWidth(const Waypoint waypoint) const {
277  const auto s = waypoint.s;
278 
279  const auto &lane = GetLane(waypoint);
280  RELEASE_ASSERT(lane.GetRoad() != nullptr);
281  RELEASE_ASSERT(s <= lane.GetRoad()->GetLength());
282 
283  const auto lane_width_info = lane.GetInfo<RoadInfoLaneWidth>(s);
284  RELEASE_ASSERT(lane_width_info != nullptr);
285 
286  return lane_width_info->GetPolynomial().Evaluate(s);
287  }
288 
290  return _data.GetRoad(road_id).GetJunctionId();
291  }
292 
293  bool Map::IsJunction(RoadId road_id) const {
294  return _data.GetRoad(road_id).IsJunction();
295  }
296 
297  std::pair<const RoadInfoMarkRecord *, const RoadInfoMarkRecord *>
298  Map::GetMarkRecord(const Waypoint waypoint) const {
299  // if lane Id is 0, just return a pair of nulls
300  if (waypoint.lane_id == 0)
301  return std::make_pair(nullptr, nullptr);
302 
303  const auto s = waypoint.s;
304 
305  const auto &current_lane = GetLane(waypoint);
306  RELEASE_ASSERT(current_lane.GetRoad() != nullptr);
307  RELEASE_ASSERT(s <= current_lane.GetRoad()->GetLength());
308 
309  const auto inner_lane_id = waypoint.lane_id < 0 ?
310  waypoint.lane_id + 1 :
311  waypoint.lane_id - 1;
312 
313  const auto &inner_lane = current_lane.GetRoad()->GetLaneById(waypoint.section_id, inner_lane_id);
314 
315  auto current_lane_info = current_lane.GetInfo<RoadInfoMarkRecord>(s);
316  auto inner_lane_info = inner_lane.GetInfo<RoadInfoMarkRecord>(s);
317 
318  return std::make_pair(current_lane_info, inner_lane_info);
319  }
320 
321  std::vector<Map::SignalSearchData> Map::GetSignalsInDistance(
322  Waypoint waypoint, double distance, bool stop_at_junction) const {
323 
324  const auto &lane = GetLane(waypoint);
325  const bool forward = (waypoint.lane_id <= 0);
326  const double signed_distance = forward ? distance : -distance;
327  const double relative_s = waypoint.s - lane.GetDistance();
328  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
329  DEBUG_ASSERT(remaining_lane_length >= 0.0);
330 
331  auto &road =_data.GetRoad(waypoint.road_id);
332  std::vector<SignalSearchData> result;
333 
334  // If after subtracting the distance we are still in the same lane, return
335  // same waypoint with the extra distance.
336  if (distance <= remaining_lane_length) {
337  auto signals = road.GetInfosInRange<RoadInfoSignal>(
338  waypoint.s, waypoint.s + signed_distance);
339  for(auto* signal : signals){
340  double distance_to_signal = 0;
341  if (waypoint.lane_id < 0){
342  distance_to_signal = signal->GetDistance() - waypoint.s;
343  } else {
344  distance_to_signal = waypoint.s - signal->GetDistance();
345  }
346  // check that the signal affects the waypoint
347  bool is_valid = false;
348  for (auto &validity : signal->GetValidities()) {
349  if (waypoint.lane_id >= validity._from_lane &&
350  waypoint.lane_id <= validity._to_lane) {
351  is_valid = true;
352  break;
353  }
354  }
355  if(!is_valid){
356  continue;
357  }
358  if (distance_to_signal == 0) {
359  result.emplace_back(SignalSearchData
360  {signal, waypoint,
361  distance_to_signal});
362  } else {
363  result.emplace_back(SignalSearchData
364  {signal, GetNext(waypoint, distance_to_signal).front(),
365  distance_to_signal});
366  }
367 
368  }
369  return result;
370  }
371  const double signed_remaining_length = forward ? remaining_lane_length : -remaining_lane_length;
372 
373  //result = road.GetInfosInRange<RoadInfoSignal>(waypoint.s, waypoint.s + signed_remaining_length);
374  auto signals = road.GetInfosInRange<RoadInfoSignal>(
375  waypoint.s, waypoint.s + signed_remaining_length);
376  for(auto* signal : signals){
377  double distance_to_signal = 0;
378  if (waypoint.lane_id < 0){
379  distance_to_signal = signal->GetDistance() - waypoint.s;
380  } else {
381  distance_to_signal = waypoint.s - signal->GetDistance();
382  }
383  // check that the signal affects the waypoint
384  bool is_valid = false;
385  for (auto &validity : signal->GetValidities()) {
386  if (waypoint.lane_id >= validity._from_lane &&
387  waypoint.lane_id <= validity._to_lane) {
388  is_valid = true;
389  break;
390  }
391  }
392  if(!is_valid){
393  continue;
394  }
395  if (distance_to_signal == 0) {
396  result.emplace_back(SignalSearchData
397  {signal, waypoint,
398  distance_to_signal});
399  } else {
400  result.emplace_back(SignalSearchData
401  {signal, GetNext(waypoint, distance_to_signal).front(),
402  distance_to_signal});
403  }
404  }
405  // If we run out of remaining_lane_length we have to go to the successors.
406  for (auto &successor : GetSuccessors(waypoint)) {
407  if(_data.GetRoad(successor.road_id).IsJunction() && stop_at_junction){
408  continue;
409  }
410  auto& sucessor_lane = _data.GetRoad(successor.road_id).
411  GetLaneByDistance(successor.s, successor.lane_id);
412  if (successor.lane_id < 0) {
413  successor.s = sucessor_lane.GetDistance();
414  } else {
415  successor.s = sucessor_lane.GetDistance() + sucessor_lane.GetLength();
416  }
417  auto sucessor_signals = GetSignalsInDistance(
418  successor, distance - remaining_lane_length, stop_at_junction);
419  for(auto& signal : sucessor_signals){
420  signal.accumulated_s += remaining_lane_length;
421  }
422  result = ConcatVectors(result, sucessor_signals);
423  }
424  return result;
425  }
426 
427  std::vector<const element::RoadInfoSignal*>
429  std::vector<const element::RoadInfoSignal*> result;
430  for (const auto& road_pair : _data.GetRoads()) {
431  const auto &road = road_pair.second;
432  auto road_infos = road.GetInfos<element::RoadInfoSignal>();
433  for(const auto* road_info : road_infos) {
434  result.push_back(road_info);
435  }
436  }
437  return result;
438  }
439 
440  std::vector<LaneMarking> Map::CalculateCrossedLanes(
441  const geom::Location &origin,
442  const geom::Location &destination) const {
443  return LaneCrossingCalculator::Calculate(*this, origin, destination);
444  }
445 
446  std::vector<geom::Location> Map::GetAllCrosswalkZones() const {
447  std::vector<geom::Location> result;
448 
449  for (const auto &pair : _data.GetRoads()) {
450  const auto &road = pair.second;
451  std::vector<const RoadInfoCrosswalk *> crosswalks = road.GetInfos<RoadInfoCrosswalk>();
452  if (crosswalks.size() > 0) {
453  for (auto crosswalk : crosswalks) {
454  // waypoint only at start position
455  std::vector<geom::Location> points;
456  Waypoint waypoint;
457  geom::Transform base;
458  for (const auto &section : road.GetLaneSectionsAt(crosswalk->GetS())) {
459  // get the section with the center lane
460  for (const auto &lane : section.GetLanes()) {
461  // is the center line
462  if (lane.first == 0) {
463  // get the center point
464  waypoint.road_id = pair.first;
465  waypoint.section_id = section.GetId();
466  waypoint.lane_id = 0;
467  waypoint.s = crosswalk->GetS();
468  base = ComputeTransform(waypoint);
469  }
470  }
471  }
472 
473  // move perpendicular ('t')
474  geom::Transform pivot = base;
475  pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
476  pivot.rotation.yaw -= 90; // move perpendicular to 's' for the lateral offset
477  geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f);
478  pivot.TransformPoint(v);
479  // restore pivot position and orientation
480  pivot = base;
481  pivot.location = v;
482  pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
483 
484  // calculate all the corners
485  for (auto corner : crosswalk->GetPoints()) {
486  geom::Vector3D v2(
487  static_cast<float>(corner.u),
488  static_cast<float>(corner.v),
489  static_cast<float>(corner.z));
490  // set the width larger to contact with the sidewalk (in case they have gutter area)
491  if (corner.u < 0) {
492  v2.x -= 1.0f;
493  } else {
494  v2.x += 1.0f;
495  }
496  pivot.TransformPoint(v2);
497  result.push_back(v2);
498  }
499  }
500  }
501  }
502  return result;
503  }
504 
505  // ===========================================================================
506  // -- Map: Waypoint generation -----------------------------------------------
507  // ===========================================================================
508 
509  std::vector<Waypoint> Map::GetSuccessors(const Waypoint waypoint) const {
510  const auto &next_lanes = GetLane(waypoint).GetNextLanes();
511  std::vector<Waypoint> result;
512  result.reserve(next_lanes.size());
513  for (auto *next_lane : next_lanes) {
514  RELEASE_ASSERT(next_lane != nullptr);
515  const auto lane_id = next_lane->GetId();
516  RELEASE_ASSERT(lane_id != 0);
517  const auto *section = next_lane->GetLaneSection();
518  RELEASE_ASSERT(section != nullptr);
519  const auto *road = next_lane->GetRoad();
520  RELEASE_ASSERT(road != nullptr);
521  const auto distance = GetDistanceAtStartOfLane(*next_lane);
522  result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
523  }
524  return result;
525  }
526 
527  std::vector<Waypoint> Map::GetPredecessors(const Waypoint waypoint) const {
528  const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes();
529  std::vector<Waypoint> result;
530  result.reserve(prev_lanes.size());
531  for (auto *next_lane : prev_lanes) {
532  RELEASE_ASSERT(next_lane != nullptr);
533  const auto lane_id = next_lane->GetId();
534  RELEASE_ASSERT(lane_id != 0);
535  const auto *section = next_lane->GetLaneSection();
536  RELEASE_ASSERT(section != nullptr);
537  const auto *road = next_lane->GetRoad();
538  RELEASE_ASSERT(road != nullptr);
539  const auto distance = GetDistanceAtEndOfLane(*next_lane);
540  result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
541  }
542  return result;
543  }
544 
545  std::vector<Waypoint> Map::GetNext(
546  const Waypoint waypoint,
547  const double distance) const {
548  RELEASE_ASSERT(distance > 0.0);
549  const auto &lane = GetLane(waypoint);
550  const bool forward = (waypoint.lane_id <= 0);
551  const double signed_distance = forward ? distance : -distance;
552  const double relative_s = waypoint.s - lane.GetDistance();
553  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
554  DEBUG_ASSERT(remaining_lane_length >= 0.0);
555 
556  // If after subtracting the distance we are still in the same lane, return
557  // same waypoint with the extra distance.
558  if (distance <= remaining_lane_length) {
559  Waypoint result = waypoint;
560  result.s += signed_distance;
561  result.s += forward ? -EPSILON : EPSILON;
562  RELEASE_ASSERT(result.s > 0.0);
563  return { result };
564  }
565 
566  // If we run out of remaining_lane_length we have to go to the successors.
567  std::vector<Waypoint> result;
568  for (const auto &successor : GetSuccessors(waypoint)) {
569  DEBUG_ASSERT(
570  successor.road_id != waypoint.road_id ||
571  successor.section_id != waypoint.section_id ||
572  successor.lane_id != waypoint.lane_id);
573  result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
574  }
575  return result;
576  }
577 
578  std::vector<Waypoint> Map::GetPrevious(
579  const Waypoint waypoint,
580  const double distance) const {
581  RELEASE_ASSERT(distance > 0.0);
582  const auto &lane = GetLane(waypoint);
583  const bool forward = !(waypoint.lane_id <= 0);
584  const double signed_distance = forward ? distance : -distance;
585  const double relative_s = waypoint.s - lane.GetDistance();
586  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
587  DEBUG_ASSERT(remaining_lane_length >= 0.0);
588 
589  // If after subtracting the distance we are still in the same lane, return
590  // same waypoint with the extra distance.
591  if (distance <= remaining_lane_length) {
592  Waypoint result = waypoint;
593  result.s += signed_distance;
594  result.s += forward ? -EPSILON : EPSILON;
595  RELEASE_ASSERT(result.s > 0.0);
596  return { result };
597  }
598 
599  // If we run out of remaining_lane_length we have to go to the successors.
600  std::vector<Waypoint> result;
601  for (const auto &successor : GetPredecessors(waypoint)) {
602  DEBUG_ASSERT(
603  successor.road_id != waypoint.road_id ||
604  successor.section_id != waypoint.section_id ||
605  successor.lane_id != waypoint.lane_id);
606  result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
607  }
608  return result;
609  }
610 
611  boost::optional<Waypoint> Map::GetRight(Waypoint waypoint) const {
612  RELEASE_ASSERT(waypoint.lane_id != 0);
613  if (waypoint.lane_id > 0) {
614  ++waypoint.lane_id;
615  } else {
616  --waypoint.lane_id;
617  }
618  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
619  }
620 
621  boost::optional<Waypoint> Map::GetLeft(Waypoint waypoint) const {
622  RELEASE_ASSERT(waypoint.lane_id != 0);
623  if (std::abs(waypoint.lane_id) == 1) {
624  waypoint.lane_id *= -1;
625  } else if (waypoint.lane_id > 0) {
626  --waypoint.lane_id;
627  } else {
628  ++waypoint.lane_id;
629  }
630  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
631  }
632 
633  std::vector<Waypoint> Map::GenerateWaypoints(const double distance) const {
634  RELEASE_ASSERT(distance > 0.0);
635  std::vector<Waypoint> result;
636  for (const auto &pair : _data.GetRoads()) {
637  const auto &road = pair.second;
638  for (double s = EPSILON; s < (road.GetLength() - EPSILON); s += distance) {
639  ForEachDrivableLaneAt(road, s, [&](auto &&waypoint) {
640  result.emplace_back(waypoint);
641  });
642  }
643  }
644  return result;
645  }
646 
647  std::vector<Waypoint> Map::GenerateWaypointsOnRoadEntries(Lane::LaneType lane_type) const {
648  std::vector<Waypoint> result;
649  for (const auto &pair : _data.GetRoads()) {
650  const auto &road = pair.second;
651  // right lanes start at s 0
652  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
653  for (const auto &lane : lane_section.GetLanes()) {
654  // add only the right (negative) lanes
655  if (lane.first < 0 &&
656  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
657  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
658  }
659  }
660  }
661  // left lanes start at s max
662  const auto road_len = road.GetLength();
663  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
664  for (const auto &lane : lane_section.GetLanes()) {
665  // add only the left (positive) lanes
666  if (lane.first > 0 &&
667  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
668  result.emplace_back(
669  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
670  }
671  }
672  }
673  }
674  return result;
675  }
676 
677  std::vector<Waypoint> Map::GenerateWaypointsInRoad(
678  RoadId road_id,
679  Lane::LaneType lane_type) const {
680  std::vector<Waypoint> result;
681  if(_data.GetRoads().count(road_id)) {
682  const auto &road = _data.GetRoads().at(road_id);
683  // right lanes start at s 0
684  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
685  for (const auto &lane : lane_section.GetLanes()) {
686  // add only the right (negative) lanes
687  if (lane.first < 0 &&
688  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
689  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
690  }
691  }
692  }
693  // left lanes start at s max
694  const auto road_len = road.GetLength();
695  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
696  for (const auto &lane : lane_section.GetLanes()) {
697  // add only the left (positive) lanes
698  if (lane.first > 0 &&
699  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
700  result.emplace_back(
701  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
702  }
703  }
704  }
705  }
706  return result;
707  }
708 
709  std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology() const {
710  std::vector<std::pair<Waypoint, Waypoint>> result;
711  for (const auto &pair : _data.GetRoads()) {
712  const auto &road = pair.second;
713  ForEachDrivableLane(road, [&](auto &&waypoint) {
714  auto successors = GetSuccessors(waypoint);
715  if (successors.size() == 0){
716  auto distance = static_cast<float>(GetDistanceAtEndOfLane(GetLane(waypoint)));
717  auto last_waypoint = GetWaypoint(waypoint.road_id, waypoint.lane_id, distance);
718  if (last_waypoint.has_value()){
719  result.push_back({waypoint, *last_waypoint});
720  }
721  }
722  else{
723  for (auto &&successor : GetSuccessors(waypoint)) {
724  result.push_back({waypoint, successor});
725  }
726  }
727  });
728  }
729  return result;
730  }
731 
732  std::vector<std::pair<Waypoint, Waypoint>> Map::GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const {
733  std::vector<std::pair<Waypoint, Waypoint>> result;
734  const Junction * junction = GetJunction(id);
735  for(auto &connections : junction->GetConnections()) {
736  const Road &road = _data.GetRoad(connections.second.connecting_road);
737  ForEachLane(road, lane_type, [&](auto &&waypoint) {
738  const auto& lane = GetLane(waypoint);
739  const double final_s = GetDistanceAtEndOfLane(lane);
740  Waypoint lane_end(waypoint);
741  lane_end.s = final_s;
742  result.push_back({waypoint, lane_end});
743  });
744  }
745  return result;
746  }
747 
748  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
750 
751  const float epsilon = 0.0001f; // small delta in the road (set to 0.1
752  // millimeters to prevent numeric errors)
753  const Junction *junction = GetJunction(id);
754  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
755  conflicts;
756 
757  // 2d typedefs
758  typedef boost::geometry::model::point
759  <float, 2, boost::geometry::cs::cartesian> Point2d;
760  typedef boost::geometry::model::segment<Point2d> Segment2d;
761  typedef boost::geometry::model::box<Rtree::BPoint> Box;
762 
763  // box range
764  auto bbox_pos = junction->GetBoundingBox().location;
765  auto bbox_ext = junction->GetBoundingBox().extent;
766  auto min_corner = geom::Vector3D(
767  bbox_pos.x - bbox_ext.x,
768  bbox_pos.y - bbox_ext.y,
769  bbox_pos.z - bbox_ext.z - epsilon);
770  auto max_corner = geom::Vector3D(
771  bbox_pos.x + bbox_ext.x,
772  bbox_pos.y + bbox_ext.y,
773  bbox_pos.z + bbox_ext.z + epsilon);
774  Box box({min_corner.x, min_corner.y, min_corner.z},
775  {max_corner.x, max_corner.y, max_corner.z});
776  auto segments = _rtree.GetIntersections(box);
777 
778  for (size_t i = 0; i < segments.size(); ++i){
779  auto &segment1 = segments[i];
780  auto waypoint1 = segment1.second.first;
781  JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
782  // only segments in the junction
783  if(junc_id1 != id){
784  continue;
785  }
786  Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
787  {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
788  for (size_t j = i + 1; j < segments.size(); ++j){
789  auto &segment2 = segments[j];
790  auto waypoint2 = segment2.second.first;
791  JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
792  // only segments in the junction
793  if(junc_id2 != id){
794  continue;
795  }
796  // discard same road
797  if(waypoint1.road_id == waypoint2.road_id){
798  continue;
799  }
800  Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
801  {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
802 
803  double distance = boost::geometry::distance(seg1, seg2);
804  // better to set distance to lanewidth
805  if(distance > 2.0){
806  continue;
807  }
808  if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
809  conflicts[waypoint1.road_id].insert(waypoint2.road_id);
810  }
811  if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
812  conflicts[waypoint2.road_id].insert(waypoint1.road_id);
813  }
814  }
815  }
816  return conflicts;
817  }
818 
819  const Lane &Map::GetLane(Waypoint waypoint) const {
820  return _data.GetRoad(waypoint.road_id).GetLaneById(waypoint.section_id, waypoint.lane_id);
821  }
822 
823  // ===========================================================================
824  // -- Map: Private functions -------------------------------------------------
825  // ===========================================================================
826 
827  // Adds a new element to the rtree element list using the position of the
828  // waypoints both ends of the segment
830  std::vector<Rtree::TreeElement> &rtree_elements,
831  geom::Transform &current_transform,
832  geom::Transform &next_transform,
833  Waypoint &current_waypoint,
834  Waypoint &next_waypoint) {
835  Rtree::BPoint init =
837  current_transform.location.x,
838  current_transform.location.y,
839  current_transform.location.z);
840  Rtree::BPoint end =
842  next_transform.location.x,
843  next_transform.location.y,
844  next_transform.location.z);
845  rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init, end),
846  std::make_pair(current_waypoint, next_waypoint)));
847  }
848  // Adds a new element to the rtree element list using the position of the
849  // waypoints, both ends of the segment
851  std::vector<Rtree::TreeElement> &rtree_elements,
852  geom::Transform &current_transform,
853  Waypoint &current_waypoint,
854  Waypoint &next_waypoint) {
855  geom::Transform next_transform = ComputeTransform(next_waypoint);
856  AddElementToRtree(rtree_elements, current_transform, next_transform,
857  current_waypoint, next_waypoint);
858  current_waypoint = next_waypoint;
859  current_transform = next_transform;
860  }
861 
862  // returns the remaining length of the geometry depending on the lane
863  // direction
864  double GetRemainingLength(const Lane &lane, double current_s) {
865  if (lane.GetId() < 0) {
866  return (lane.GetDistance() + lane.GetLength() - current_s);
867  } else {
868  return (current_s - lane.GetDistance());
869  }
870  }
871 
873  const double epsilon = 0.000001; // small delta in the road (set to 1
874  // micrometer to prevent numeric errors)
875  const double min_delta_s = 1; // segments of minimum 1m through the road
876 
877  // 1.8 degrees, maximum angle in a curve to place a segment
878  constexpr double angle_threshold = geom::Math::Pi<double>() / 100.0;
879  // maximum distance of a segment
880  constexpr double max_segment_length = 100.0;
881 
882  // Generate waypoints at start of every lane
883  std::vector<Waypoint> topology;
884  for (const auto &pair : _data.GetRoads()) {
885  const auto &road = pair.second;
886  ForEachLane(road, Lane::LaneType::Any, [&](auto &&waypoint) {
887  if(waypoint.lane_id != 0) {
888  topology.push_back(waypoint);
889  }
890  });
891  }
892 
893  // Container of segments and waypoints
894  std::vector<Rtree::TreeElement> rtree_elements;
895  // Loop through all lanes
896  for (auto &waypoint : topology) {
897  auto &lane_start_waypoint = waypoint;
898 
899  auto current_waypoint = lane_start_waypoint;
900 
901  const Lane &lane = GetLane(current_waypoint);
902 
903  geom::Transform current_transform = ComputeTransform(current_waypoint);
904 
905  // Save computation time in straight lines
906  if (lane.IsStraight()) {
907  double delta_s = min_delta_s;
908  double remaining_length =
909  GetRemainingLength(lane, current_waypoint.s);
910  remaining_length -= epsilon;
911  delta_s = remaining_length;
912  if (delta_s < epsilon) {
913  continue;
914  }
915  auto next = GetNext(current_waypoint, delta_s);
916 
917  RELEASE_ASSERT(next.size() == 1);
918  RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
919  auto next_waypoint = next.front();
920 
921  AddElementToRtreeAndUpdateTransforms(
922  rtree_elements,
923  current_transform,
924  current_waypoint,
925  next_waypoint);
926  // end of lane
927  } else {
928  auto next_waypoint = current_waypoint;
929 
930  // Loop until the end of the lane
931  // Advance in small s-increments
932  while (true) {
933  double delta_s = min_delta_s;
934  double remaining_length =
935  GetRemainingLength(lane, next_waypoint.s);
936  remaining_length -= epsilon;
937  delta_s = std::min(delta_s, remaining_length);
938 
939  if (delta_s < epsilon) {
940  AddElementToRtreeAndUpdateTransforms(
941  rtree_elements,
942  current_transform,
943  current_waypoint,
944  next_waypoint);
945  break;
946  }
947 
948  auto next = GetNext(next_waypoint, delta_s);
949  if (next.size() != 1 ||
950  current_waypoint.section_id != next.front().section_id) {
951  AddElementToRtreeAndUpdateTransforms(
952  rtree_elements,
953  current_transform,
954  current_waypoint,
955  next_waypoint);
956  break;
957  }
958 
959  next_waypoint = next.front();
960  geom::Transform next_transform = ComputeTransform(next_waypoint);
961  double angle = geom::Math::GetVectorAngle(
962  current_transform.GetForwardVector(), next_transform.GetForwardVector());
963 
964  if (std::abs(angle) > angle_threshold ||
965  std::abs(current_waypoint.s - next_waypoint.s) > max_segment_length) {
966  AddElementToRtree(
967  rtree_elements,
968  current_transform,
969  next_transform,
970  current_waypoint,
971  next_waypoint);
972  current_waypoint = next_waypoint;
973  current_transform = next_transform;
974  }
975  }
976  }
977  }
978  // Add segments to Rtree
979  _rtree.InsertElements(rtree_elements);
980  }
981 
983  return _data.GetJunction(id);
984  }
985 
986  const Junction* Map::GetJunction(JuncId id) const {
987  return _data.GetJunction(id);
988  }
989 
991  const double distance,
992  const float extra_width,
993  const bool smooth_junctions) const {
994  RELEASE_ASSERT(distance > 0.0);
995  geom::MeshFactory mesh_factory;
996  geom::Mesh out_mesh;
997 
998  mesh_factory.road_param.resolution = static_cast<float>(distance);
999  mesh_factory.road_param.extra_lane_width = extra_width;
1000 
1001  // Generate roads outside junctions
1002  for (auto &&pair : _data.GetRoads()) {
1003  const auto &road = pair.second;
1004  if (road.IsJunction()) {
1005  continue;
1006  }
1007  out_mesh += *mesh_factory.Generate(road);
1008  }
1009 
1010  // Generate roads within junctions and smooth them
1011  for (const auto &junc_pair : _data.GetJunctions()) {
1012  const auto &junction = junc_pair.second;
1013  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1014  for(const auto &connection_pair : junction.GetConnections()) {
1015  const auto &connection = connection_pair.second;
1016  const auto &road = _data.GetRoads().at(connection.connecting_road);
1017  for (auto &&lane_section : road.GetLaneSections()) {
1018  for (auto &&lane_pair : lane_section.GetLanes()) {
1019  lane_meshes.push_back(mesh_factory.Generate(lane_pair.second));
1020  }
1021  }
1022  }
1023  if(smooth_junctions) {
1024  out_mesh += *mesh_factory.MergeAndSmooth(lane_meshes);
1025  } else {
1026  geom::Mesh junction_mesh;
1027  for(auto& lane : lane_meshes) {
1028  junction_mesh += *lane;
1029  }
1030  out_mesh += junction_mesh;
1031  }
1032  }
1033 
1034  return out_mesh;
1035  }
1036 
1037  std::vector<std::unique_ptr<geom::Mesh>> Map::GenerateChunkedMesh(
1038  const rpc::OpendriveGenerationParameters& params) const {
1039  geom::MeshFactory mesh_factory(params);
1040  std::vector<std::unique_ptr<geom::Mesh>> out_mesh_list;
1041 
1042  std::unordered_map<JuncId, geom::Mesh> junction_map;
1043  for (auto &&pair : _data.GetRoads()) {
1044  const auto &road = pair.second;
1045  if (!road.IsJunction()) {
1046  std::vector<std::unique_ptr<geom::Mesh>> road_mesh_list =
1047  mesh_factory.GenerateAllWithMaxLen(road);
1048 
1049  out_mesh_list.insert(
1050  out_mesh_list.end(),
1051  std::make_move_iterator(road_mesh_list.begin()),
1052  std::make_move_iterator(road_mesh_list.end()));
1053  }
1054  }
1055 
1056  // Generate roads within junctions and smooth them
1057  for (const auto &junc_pair : _data.GetJunctions()) {
1058  const auto &junction = junc_pair.second;
1059  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1060  std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1061  for(const auto &connection_pair : junction.GetConnections()) {
1062  const auto &connection = connection_pair.second;
1063  const auto &road = _data.GetRoads().at(connection.connecting_road);
1064  for (auto &&lane_section : road.GetLaneSections()) {
1065  for (auto &&lane_pair : lane_section.GetLanes()) {
1066  const auto &lane = lane_pair.second;
1067  if (lane.GetType() != road::Lane::LaneType::Sidewalk) {
1068  lane_meshes.push_back(mesh_factory.Generate(lane));
1069  } else {
1070  sidewalk_lane_meshes.push_back(mesh_factory.Generate(lane));
1071  }
1072  }
1073  }
1074  }
1075  if(params.smooth_junctions) {
1076  auto merged_mesh = mesh_factory.MergeAndSmooth(lane_meshes);
1077  for(auto& lane : sidewalk_lane_meshes) {
1078  *merged_mesh += *lane;
1079  }
1080  out_mesh_list.push_back(std::move(merged_mesh));
1081  } else {
1082  std::unique_ptr<geom::Mesh> junction_mesh = std::make_unique<geom::Mesh>();
1083  for(auto& lane : lane_meshes) {
1084  *junction_mesh += *lane;
1085  }
1086  for(auto& lane : sidewalk_lane_meshes) {
1087  *junction_mesh += *lane;
1088  }
1089  out_mesh_list.push_back(std::move(junction_mesh));
1090  }
1091  }
1092 
1093  auto min_pos = geom::Vector2D(
1094  out_mesh_list.front()->GetVertices().front().x,
1095  out_mesh_list.front()->GetVertices().front().y);
1096  auto max_pos = min_pos;
1097  for (auto & mesh : out_mesh_list) {
1098  auto vertex = mesh->GetVertices().front();
1099  min_pos.x = std::min(min_pos.x, vertex.x);
1100  min_pos.y = std::min(min_pos.y, vertex.y);
1101  max_pos.x = std::max(max_pos.x, vertex.x);
1102  max_pos.y = std::max(max_pos.y, vertex.y);
1103  }
1104  size_t mesh_amount_x = static_cast<size_t>((max_pos.x - min_pos.x)/params.max_road_length) + 1;
1105  size_t mesh_amount_y = static_cast<size_t>((max_pos.y - min_pos.y)/params.max_road_length) + 1;
1106  std::vector<std::unique_ptr<geom::Mesh>> result;
1107  result.reserve(mesh_amount_x*mesh_amount_y);
1108  for (size_t i = 0; i < mesh_amount_x*mesh_amount_y; ++i) {
1109  result.emplace_back(std::make_unique<geom::Mesh>());
1110  }
1111  for (auto & mesh : out_mesh_list) {
1112  auto vertex = mesh->GetVertices().front();
1113  size_t x_pos = static_cast<size_t>((vertex.x - min_pos.x) / params.max_road_length);
1114  size_t y_pos = static_cast<size_t>((vertex.y - min_pos.y) / params.max_road_length);
1115  *(result[x_pos + mesh_amount_x*y_pos]) += *mesh;
1116  }
1117 
1118  return result;
1119  }
1120 
1122  geom::Mesh out_mesh;
1123 
1124  // Get the crosswalk vertices for the current map
1125  const std::vector<geom::Location> crosswalk_vertex = GetAllCrosswalkZones();
1126  if (crosswalk_vertex.empty()) {
1127  return out_mesh;
1128  }
1129 
1130  // Create a a list of triangle fans with material "crosswalk"
1131  out_mesh.AddMaterial("crosswalk");
1132  size_t start_vertex_index = 0;
1133  size_t i = 0;
1134  std::vector<geom::Vector3D> vertices;
1135  // Iterate the vertices until a repeated one is found, this indicates
1136  // the triangle fan is done and another one must start
1137  do {
1138  // Except for the first iteration && triangle fan done
1139  if (i != 0 && crosswalk_vertex[start_vertex_index] == crosswalk_vertex[i]) {
1140  // Create the actual fan
1141  out_mesh.AddTriangleFan(vertices);
1142  vertices.clear();
1143  // End the loop if i reached the end of the vertex list
1144  if (i >= crosswalk_vertex.size() - 1) {
1145  break;
1146  }
1147  start_vertex_index = ++i;
1148  }
1149  // Append a new Vector3D that will be added to the triangle fan
1150  vertices.push_back(crosswalk_vertex[i++]);
1151  } while (i < crosswalk_vertex.size());
1152 
1153  out_mesh.EndMaterial();
1154  return out_mesh;
1155  }
1156 
1157 } // namespace road
1158 } // namespace carla
static void ForEachDrivableLaneImpl(RoadId road_id, const LaneSection &lane_section, double distance, FuncT &&func)
Return a waypoint for each drivable lane on lane_section.
Definition: road/Map.cpp:67
std::vector< element::LaneMarking > CalculateCrossedLanes(const geom::Location &origin, const geom::Location &destination) const
Definition: road/Map.cpp:440
geom::Mesh GenerateMesh(const double distance, const float extra_width=0.6f, const bool smooth_junctions=true) const
Buids a mesh based on the OpenDRIVE.
Definition: road/Map.cpp:990
Seting for map generation from opendrive without additional geometry.
std::unordered_map< road::RoadId, std::unordered_set< road::RoadId > > ComputeJunctionConflicts(JuncId id) const
Definition: road/Map.cpp:749
Mesh helper generator.
Definition: MeshFactory.h:22
std::vector< Waypoint > GetPrevious(Waypoint waypoint, double distance) const
Return the list of waypoints at distance in the reversed direction that a vehicle at waypoint could d...
Definition: road/Map.cpp:578
uint32_t RoadId
Definition: RoadTypes.h:15
static double GetDistanceAtStartOfLane(const Lane &lane)
Definition: road/Map.cpp:49
static std::pair< float, float > DistanceSegmentToPoint(const Vector3D &p, const Vector3D &v, const Vector3D &w)
Returns a pair containing:
Definition: Math.cpp:18
carla::geom::BoundingBox GetBoundingBox() const
Definition: road/Junction.h:73
std::vector< std::pair< Waypoint, Waypoint > > GenerateTopology() const
Generate the minimum set of waypoints that define the topology of map.
Definition: road/Map.cpp:709
Location location
Center of the BoundingBox in local space.
boost::optional< element::Waypoint > GetWaypoint(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
Definition: road/Map.cpp:203
void AddTriangleFan(const std::vector< vertex_type > &vertices)
Adds a triangle fan to the mesh, vertex order is counterclockwise.
Definition: Mesh.cpp:74
double GetDistance() const
Distance from road&#39;s start location.
Definition: RoadInfo.h:28
std::unique_ptr< Mesh > Generate(const road::Road &road) const
Generates a mesh that defines a road.
Definition: MeshFactory.cpp:29
const geom::CubicPolynomial & GetPolynomial() const
static void ForEachDrivableLaneAt(const Road &road, double distance, FuncT &&func)
Return a waypoint for each drivable lane at distance on road.
Definition: road/Map.cpp:136
Vector3D extent
Half the size of the BoundingBox in local space.
Each lane within a road cross section can be provided with several road markentries.
geom::Mesh GetAllCrosswalkMesh() const
Buids a mesh of all crosswalks based on the OpenDRIVE.
Definition: road/Map.cpp:1121
std::unique_ptr< Mesh > MergeAndSmooth(std::vector< std::unique_ptr< Mesh >> &lane_meshes) const
std::vector< geom::Location > GetAllCrosswalkZones() const
Returns a list of locations defining 2d areas, when a location is repeated an area is finished...
Definition: road/Map.cpp:446
Road * GetRoad() const
Definition: Lane.cpp:29
static double GetDistanceAtEndOfLane(const Lane &lane)
Definition: road/Map.cpp:57
const Lane & GetLane(Waypoint waypoint) const
======================================================================== – Road information --------...
Definition: road/Map.cpp:819
std::vector< Waypoint > GenerateWaypointsOnRoadEntries(Lane::LaneType lane_type=Lane::LaneType::Driving) const
Generate waypoints on each lane at the start of each road.
Definition: road/Map.cpp:647
double GetDistance() const
Definition: Lane.cpp:46
Junction * GetJunction(JuncId id)
Definition: road/Map.cpp:982
double GetLength() const
Definition: Lane.cpp:51
Road & GetRoad(const RoadId id)
Definition: MapData.cpp:21
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
Lane Width RecordEach lane within a road’scross section can be provided with severalwidth entries...
std::vector< const element::RoadInfoSignal * > GetAllSignalReferences() const
Return all RoadInfoSignal in the map.
Definition: road/Map.cpp:428
void CreateRtree()
Definition: road/Map.cpp:872
value_type Evaluate(const value_type &x) const
Evaluates f(x) = a + bx + cx^2 + dx^3.
auto GetLaneSectionsAt(const double s)
Definition: Road.h:149
int32_t JuncId
Definition: RoadTypes.h:17
static std::vector< T > ConcatVectors(std::vector< T > dst, std::vector< T > src)
Definition: road/Map.cpp:38
Vector3D GetForwardVector() const
bg::model::box< Point3D > Box
Definition: InMemoryMap.h:45
boost::optional< element::Waypoint > GetClosestWaypointOnRoad(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
======================================================================== – Geometry ----------------...
Definition: road/Map.cpp:156
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
void AddMaterial(const std::string &material_name)
Starts applying a new material to the new added triangles.
Definition: Mesh.cpp:107
static void ForEachDrivableLane(const Road &road, FuncT &&func)
Return a waypoint for each drivable lane on each lane section of road.
Definition: road/Map.cpp:111
void EndMaterial()
Stops applying the material to the new added triangles.
Definition: Mesh.cpp:123
static bool IsLanePresent(const MapData &data, Waypoint waypoint)
Assumes road_id and section_id are valid.
Definition: road/Map.cpp:147
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
auto GetLaneSections() const
Definition: Road.h:127
std::vector< Waypoint > GetNext(Waypoint waypoint, double distance) const
Return the list of waypoints at distance such that a vehicle at waypoint could drive to...
Definition: road/Map.cpp:545
boost::geometry::model::point< float, 3, boost::geometry::cs::cartesian > BPoint
Definition: Rtree.h:81
RoadParameters road_param
Definition: MeshFactory.h:103
Data structure for the signal search.
Definition: road/Map.h:93
SectionId GetId() const
Definition: LaneSection.cpp:27
#define RELEASE_ASSERT(pred)
Definition: Debug.h:84
RoadId GetId() const
Definition: Road.cpp:30
LaneType
Can be used as flags.
Definition: Lane.h:29
int32_t LaneId
Definition: RoadTypes.h:19
geom::Transform ComputeTransform(Waypoint waypoint) const
Definition: road/Map.cpp:264
Lane & GetLaneById(SectionId section_id, LaneId lane_id)
Definition: Road.cpp:110
std::pair< const element::RoadInfoMarkRecord *, const element::RoadInfoMarkRecord * > GetMarkRecord(Waypoint waypoint) const
Definition: road/Map.cpp:298
static auto Distance2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:82
std::vector< std::unique_ptr< geom::Mesh > > GenerateChunkedMesh(const rpc::OpendriveGenerationParameters &params) const
Definition: road/Map.cpp:1037
LaneSection & GetLaneSectionById(SectionId id)
Definition: Road.h:163
boost::optional< Waypoint > GetLeft(Waypoint waypoint) const
Return a waypoint at the lane of waypoint&#39;s left lane.
Definition: road/Map.cpp:621
Lane::LaneType GetLaneType(Waypoint waypoint) const
Definition: road/Map.cpp:272
Mesh data container, validator and exporter.
Definition: Mesh.h:42
static void ForEachLane(const Road &road, Lane::LaneType lane_type, FuncT &&func)
Return a waypoint for each lane of the specified type on each lane section of road.
Definition: road/Map.cpp:123
bool IsJunction(RoadId road_id) const
Definition: road/Map.cpp:293
std::vector< Waypoint > GetPredecessors(Waypoint waypoint) const
Definition: road/Map.cpp:527
double GetRemainingLength(const Lane &lane, double current_s)
Definition: road/Map.cpp:864
void TransformPoint(Vector3D &in_point) const
Applies this transformation to in_point (first translation then rotation).
std::vector< SignalSearchData > GetSignalsInDistance(Waypoint waypoint, double distance, bool stop_at_junction=false) const
Searches signals from an initial waypoint until the defined distance.
Definition: road/Map.cpp:321
static std::vector< LaneMarking > Calculate(const Map &map, const geom::Location &origin, const geom::Location &destination)
std::map< LaneId, Lane > & GetLanes()
Definition: LaneSection.cpp:47
bool IsStraight() const
Checks whether the geometry is straight or not.
Definition: Lane.cpp:65
static double GetVectorAngle(const Vector3D &a, const Vector3D &b)
Returns the angle between 2 vectors in radians.
Definition: Math.cpp:14
std::vector< Waypoint > GenerateWaypointsInRoad(RoadId road_id, Lane::LaneType lane_type=Lane::LaneType::Driving) const
Generate waypoints at the entry of each lane of the specified road.
Definition: road/Map.cpp:677
void AddElementToRtree(std::vector< Rtree::TreeElement > &rtree_elements, geom::Transform &current_transform, geom::Transform &next_transform, Waypoint &current_waypoint, Waypoint &next_waypoint)
Helper Functions for constructing the rtree element list.
Definition: road/Map.cpp:829
std::vector< std::unique_ptr< Mesh > > GenerateAllWithMaxLen(const road::Road &road) const
Generates a chunked road with all the features needed for simulation.
bool ContainsLane(LaneId id) const
Definition: LaneSection.h:39
std::vector< Waypoint > GetSuccessors(Waypoint waypoint) const
======================================================================== – Waypoint generation -----...
Definition: road/Map.cpp:509
void AddElementToRtreeAndUpdateTransforms(std::vector< Rtree::TreeElement > &rtree_elements, geom::Transform &current_transform, Waypoint &current_waypoint, Waypoint &next_waypoint)
Definition: road/Map.cpp:850
std::vector< Waypoint > GenerateWaypoints(double approx_distance) const
Generate all the waypoints in map separated by approx_distance.
Definition: road/Map.cpp:633
boost::geometry::model::segment< BPoint > BSegment
Definition: Rtree.h:82
std::unordered_map< ConId, Connection > & GetConnections()
Definition: road/Junction.h:65
JuncId GetJunctionId(RoadId road_id) const
Definition: road/Map.cpp:289
geom::Vector2D Vector2D
Definition: rpc/Vector2D.h:14
double GetLength() const
Definition: Road.cpp:38
boost::optional< Waypoint > GetRight(Waypoint waypoint) const
Return a waypoint at the lane of waypoint&#39;s right lane.
Definition: road/Map.cpp:611
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
Definition: road/Map.cpp:732
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
Definition: road/Map.cpp:31
LaneId GetId() const
Definition: Lane.cpp:34
std::pair< BSegment, std::pair< Waypoint, Waypoint > > TreeElement
Definition: Rtree.h:83
double GetLaneWidth(Waypoint waypoint) const
Definition: road/Map.cpp:276
static void ForEachLaneImpl(RoadId road_id, const LaneSection &lane_section, double distance, Lane::LaneType lane_type, FuncT &&func)
Definition: road/Map.cpp:88