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  for (auto &&successor : GetSuccessors(waypoint)) {
715  result.push_back({waypoint, successor});
716  }
717  });
718  }
719  return result;
720  }
721 
722  std::vector<std::pair<Waypoint, Waypoint>> Map::GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const {
723  std::vector<std::pair<Waypoint, Waypoint>> result;
724  const Junction * junction = GetJunction(id);
725  for(auto &connections : junction->GetConnections()) {
726  const Road &road = _data.GetRoad(connections.second.connecting_road);
727  ForEachLane(road, lane_type, [&](auto &&waypoint) {
728  const auto& lane = GetLane(waypoint);
729  const double final_s = GetDistanceAtEndOfLane(lane);
730  Waypoint lane_end(waypoint);
731  lane_end.s = final_s;
732  result.push_back({waypoint, lane_end});
733  });
734  }
735  return result;
736  }
737 
738  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
740 
741  const float epsilon = 0.0001f; // small delta in the road (set to 0.1
742  // millimeters to prevent numeric errors)
743  const Junction *junction = GetJunction(id);
744  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
745  conflicts;
746 
747  // 2d typedefs
748  typedef boost::geometry::model::point
749  <float, 2, boost::geometry::cs::cartesian> Point2d;
750  typedef boost::geometry::model::segment<Point2d> Segment2d;
751  typedef boost::geometry::model::box<Rtree::BPoint> Box;
752 
753  // box range
754  auto bbox_pos = junction->GetBoundingBox().location;
755  auto bbox_ext = junction->GetBoundingBox().extent;
756  auto min_corner = geom::Vector3D(
757  bbox_pos.x - bbox_ext.x,
758  bbox_pos.y - bbox_ext.y,
759  bbox_pos.z - bbox_ext.z - epsilon);
760  auto max_corner = geom::Vector3D(
761  bbox_pos.x + bbox_ext.x,
762  bbox_pos.y + bbox_ext.y,
763  bbox_pos.z + bbox_ext.z + epsilon);
764  Box box({min_corner.x, min_corner.y, min_corner.z},
765  {max_corner.x, max_corner.y, max_corner.z});
766  auto segments = _rtree.GetIntersections(box);
767 
768  for (size_t i = 0; i < segments.size(); ++i){
769  auto &segment1 = segments[i];
770  auto waypoint1 = segment1.second.first;
771  JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
772  // only segments in the junction
773  if(junc_id1 != id){
774  continue;
775  }
776  Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
777  {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
778  for (size_t j = i + 1; j < segments.size(); ++j){
779  auto &segment2 = segments[j];
780  auto waypoint2 = segment2.second.first;
781  JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
782  // only segments in the junction
783  if(junc_id2 != id){
784  continue;
785  }
786  // discard same road
787  if(waypoint1.road_id == waypoint2.road_id){
788  continue;
789  }
790  Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
791  {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
792 
793  double distance = boost::geometry::distance(seg1, seg2);
794  // better to set distance to lanewidth
795  if(distance > 2.0){
796  continue;
797  }
798  if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
799  conflicts[waypoint1.road_id].insert(waypoint2.road_id);
800  }
801  if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
802  conflicts[waypoint2.road_id].insert(waypoint1.road_id);
803  }
804  }
805  }
806  return conflicts;
807  }
808 
809  const Lane &Map::GetLane(Waypoint waypoint) const {
810  return _data.GetRoad(waypoint.road_id).GetLaneById(waypoint.section_id, waypoint.lane_id);
811  }
812 
813  // ===========================================================================
814  // -- Map: Private functions -------------------------------------------------
815  // ===========================================================================
816 
817  // Adds a new element to the rtree element list using the position of the
818  // waypoints both ends of the segment
820  std::vector<Rtree::TreeElement> &rtree_elements,
821  geom::Transform &current_transform,
822  geom::Transform &next_transform,
823  Waypoint &current_waypoint,
824  Waypoint &next_waypoint) {
825  Rtree::BPoint init =
827  current_transform.location.x,
828  current_transform.location.y,
829  current_transform.location.z);
830  Rtree::BPoint end =
832  next_transform.location.x,
833  next_transform.location.y,
834  next_transform.location.z);
835  rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init, end),
836  std::make_pair(current_waypoint, next_waypoint)));
837  }
838  // Adds a new element to the rtree element list using the position of the
839  // waypoints, both ends of the segment
841  std::vector<Rtree::TreeElement> &rtree_elements,
842  geom::Transform &current_transform,
843  Waypoint &current_waypoint,
844  Waypoint &next_waypoint) {
845  geom::Transform next_transform = ComputeTransform(next_waypoint);
846  AddElementToRtree(rtree_elements, current_transform, next_transform,
847  current_waypoint, next_waypoint);
848  current_waypoint = next_waypoint;
849  current_transform = next_transform;
850  }
851 
852  // returns the remaining length of the geometry depending on the lane
853  // direction
854  double GetRemainingLength(const Lane &lane, double current_s) {
855  if (lane.GetId() < 0) {
856  return (lane.GetDistance() + lane.GetLength() - current_s);
857  } else {
858  return (current_s - lane.GetDistance());
859  }
860  }
861 
863  const double epsilon = 0.000001; // small delta in the road (set to 1
864  // micrometer to prevent numeric errors)
865  const double min_delta_s = 1; // segments of minimum 1m through the road
866 
867  // 1.8 degrees, maximum angle in a curve to place a segment
868  constexpr double angle_threshold = geom::Math::Pi<double>() / 100.0;
869  // maximum distance of a segment
870  constexpr double max_segment_length = 100.0;
871 
872  // Generate waypoints at start of every lane
873  std::vector<Waypoint> topology;
874  for (const auto &pair : _data.GetRoads()) {
875  const auto &road = pair.second;
876  ForEachLane(road, Lane::LaneType::Any, [&](auto &&waypoint) {
877  if(waypoint.lane_id != 0) {
878  topology.push_back(waypoint);
879  }
880  });
881  }
882 
883  // Container of segments and waypoints
884  std::vector<Rtree::TreeElement> rtree_elements;
885  // Loop through all lanes
886  for (auto &waypoint : topology) {
887  auto &lane_start_waypoint = waypoint;
888 
889  auto current_waypoint = lane_start_waypoint;
890 
891  const Lane &lane = GetLane(current_waypoint);
892 
893  geom::Transform current_transform = ComputeTransform(current_waypoint);
894 
895  // Save computation time in straight lines
896  if (lane.IsStraight()) {
897  double delta_s = min_delta_s;
898  double remaining_length =
899  GetRemainingLength(lane, current_waypoint.s);
900  remaining_length -= epsilon;
901  delta_s = remaining_length;
902  if (delta_s < epsilon) {
903  continue;
904  }
905  auto next = GetNext(current_waypoint, delta_s);
906 
907  RELEASE_ASSERT(next.size() == 1);
908  RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
909  auto next_waypoint = next.front();
910 
911  AddElementToRtreeAndUpdateTransforms(
912  rtree_elements,
913  current_transform,
914  current_waypoint,
915  next_waypoint);
916  // end of lane
917  } else {
918  auto next_waypoint = current_waypoint;
919 
920  // Loop until the end of the lane
921  // Advance in small s-increments
922  while (true) {
923  double delta_s = min_delta_s;
924  double remaining_length =
925  GetRemainingLength(lane, next_waypoint.s);
926  remaining_length -= epsilon;
927  delta_s = std::min(delta_s, remaining_length);
928 
929  if (delta_s < epsilon) {
930  AddElementToRtreeAndUpdateTransforms(
931  rtree_elements,
932  current_transform,
933  current_waypoint,
934  next_waypoint);
935  break;
936  }
937 
938  auto next = GetNext(next_waypoint, delta_s);
939  if (next.size() != 1 ||
940  current_waypoint.section_id != next.front().section_id) {
941  AddElementToRtreeAndUpdateTransforms(
942  rtree_elements,
943  current_transform,
944  current_waypoint,
945  next_waypoint);
946  break;
947  }
948 
949  next_waypoint = next.front();
950  geom::Transform next_transform = ComputeTransform(next_waypoint);
951  double angle = geom::Math::GetVectorAngle(
952  current_transform.GetForwardVector(), next_transform.GetForwardVector());
953 
954  if (std::abs(angle) > angle_threshold ||
955  std::abs(current_waypoint.s - next_waypoint.s) > max_segment_length) {
956  AddElementToRtree(
957  rtree_elements,
958  current_transform,
959  next_transform,
960  current_waypoint,
961  next_waypoint);
962  current_waypoint = next_waypoint;
963  current_transform = next_transform;
964  }
965  }
966  }
967  }
968  // Add segments to Rtree
969  _rtree.InsertElements(rtree_elements);
970  }
971 
973  return _data.GetJunction(id);
974  }
975 
976  const Junction* Map::GetJunction(JuncId id) const {
977  return _data.GetJunction(id);
978  }
979 
981  const double distance,
982  const float extra_width,
983  const bool smooth_junctions) const {
984  RELEASE_ASSERT(distance > 0.0);
985  geom::MeshFactory mesh_factory;
986  geom::Mesh out_mesh;
987 
988  mesh_factory.road_param.resolution = static_cast<float>(distance);
989  mesh_factory.road_param.extra_lane_width = extra_width;
990 
991  // Generate roads outside junctions
992  for (auto &&pair : _data.GetRoads()) {
993  const auto &road = pair.second;
994  if (road.IsJunction()) {
995  continue;
996  }
997  out_mesh += *mesh_factory.Generate(road);
998  }
999 
1000  // Generate roads within junctions and smooth them
1001  for (const auto &junc_pair : _data.GetJunctions()) {
1002  const auto &junction = junc_pair.second;
1003  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1004  for(const auto &connection_pair : junction.GetConnections()) {
1005  const auto &connection = connection_pair.second;
1006  const auto &road = _data.GetRoads().at(connection.connecting_road);
1007  for (auto &&lane_section : road.GetLaneSections()) {
1008  for (auto &&lane_pair : lane_section.GetLanes()) {
1009  lane_meshes.push_back(mesh_factory.Generate(lane_pair.second));
1010  }
1011  }
1012  }
1013  if(smooth_junctions) {
1014  out_mesh += *mesh_factory.MergeAndSmooth(lane_meshes);
1015  } else {
1016  geom::Mesh junction_mesh;
1017  for(auto& lane : lane_meshes) {
1018  junction_mesh += *lane;
1019  }
1020  out_mesh += junction_mesh;
1021  }
1022  }
1023 
1024  return out_mesh;
1025  }
1026 
1027  std::vector<std::unique_ptr<geom::Mesh>> Map::GenerateChunkedMesh(
1028  const rpc::OpendriveGenerationParameters& params) const {
1029  geom::MeshFactory mesh_factory(params);
1030  std::vector<std::unique_ptr<geom::Mesh>> out_mesh_list;
1031 
1032  std::unordered_map<JuncId, geom::Mesh> junction_map;
1033  for (auto &&pair : _data.GetRoads()) {
1034  const auto &road = pair.second;
1035  if (!road.IsJunction()) {
1036  std::vector<std::unique_ptr<geom::Mesh>> road_mesh_list =
1037  mesh_factory.GenerateAllWithMaxLen(road);
1038 
1039  out_mesh_list.insert(
1040  out_mesh_list.end(),
1041  std::make_move_iterator(road_mesh_list.begin()),
1042  std::make_move_iterator(road_mesh_list.end()));
1043  }
1044  }
1045 
1046  // Generate roads within junctions and smooth them
1047  for (const auto &junc_pair : _data.GetJunctions()) {
1048  const auto &junction = junc_pair.second;
1049  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1050  std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1051  for(const auto &connection_pair : junction.GetConnections()) {
1052  const auto &connection = connection_pair.second;
1053  const auto &road = _data.GetRoads().at(connection.connecting_road);
1054  for (auto &&lane_section : road.GetLaneSections()) {
1055  for (auto &&lane_pair : lane_section.GetLanes()) {
1056  const auto &lane = lane_pair.second;
1057  if (lane.GetType() != road::Lane::LaneType::Sidewalk) {
1058  lane_meshes.push_back(mesh_factory.Generate(lane));
1059  } else {
1060  sidewalk_lane_meshes.push_back(mesh_factory.Generate(lane));
1061  }
1062  }
1063  }
1064  }
1065  if(params.smooth_junctions) {
1066  auto merged_mesh = mesh_factory.MergeAndSmooth(lane_meshes);
1067  for(auto& lane : sidewalk_lane_meshes) {
1068  *merged_mesh += *lane;
1069  }
1070  out_mesh_list.push_back(std::move(merged_mesh));
1071  } else {
1072  std::unique_ptr<geom::Mesh> junction_mesh = std::make_unique<geom::Mesh>();
1073  for(auto& lane : lane_meshes) {
1074  *junction_mesh += *lane;
1075  }
1076  for(auto& lane : sidewalk_lane_meshes) {
1077  *junction_mesh += *lane;
1078  }
1079  out_mesh_list.push_back(std::move(junction_mesh));
1080  }
1081  }
1082 
1083  auto min_pos = geom::Vector2D(
1084  out_mesh_list.front()->GetVertices().front().x,
1085  out_mesh_list.front()->GetVertices().front().y);
1086  auto max_pos = min_pos;
1087  for (auto & mesh : out_mesh_list) {
1088  auto vertex = mesh->GetVertices().front();
1089  min_pos.x = std::min(min_pos.x, vertex.x);
1090  min_pos.y = std::min(min_pos.y, vertex.y);
1091  max_pos.x = std::max(max_pos.x, vertex.x);
1092  max_pos.y = std::max(max_pos.y, vertex.y);
1093  }
1094  size_t mesh_amount_x = static_cast<size_t>((max_pos.x - min_pos.x)/params.max_road_length) + 1;
1095  size_t mesh_amount_y = static_cast<size_t>((max_pos.y - min_pos.y)/params.max_road_length) + 1;
1096  std::vector<std::unique_ptr<geom::Mesh>> result;
1097  result.reserve(mesh_amount_x*mesh_amount_y);
1098  for (size_t i = 0; i < mesh_amount_x*mesh_amount_y; ++i) {
1099  result.emplace_back(std::make_unique<geom::Mesh>());
1100  }
1101  for (auto & mesh : out_mesh_list) {
1102  auto vertex = mesh->GetVertices().front();
1103  size_t x_pos = static_cast<size_t>((vertex.x - min_pos.x) / params.max_road_length);
1104  size_t y_pos = static_cast<size_t>((vertex.y - min_pos.y) / params.max_road_length);
1105  *(result[x_pos + mesh_amount_x*y_pos]) += *mesh;
1106  }
1107 
1108  return result;
1109  }
1110 
1112  geom::Mesh out_mesh;
1113 
1114  // Get the crosswalk vertices for the current map
1115  const std::vector<geom::Location> crosswalk_vertex = GetAllCrosswalkZones();
1116  if (crosswalk_vertex.empty()) {
1117  return out_mesh;
1118  }
1119 
1120  // Create a a list of triangle fans with material "crosswalk"
1121  out_mesh.AddMaterial("crosswalk");
1122  size_t start_vertex_index = 0;
1123  size_t i = 0;
1124  std::vector<geom::Vector3D> vertices;
1125  // Iterate the vertices until a repeated one is found, this indicates
1126  // the triangle fan is done and another one must start
1127  do {
1128  // Except for the first iteration && triangle fan done
1129  if (i != 0 && crosswalk_vertex[start_vertex_index] == crosswalk_vertex[i]) {
1130  // Create the actual fan
1131  out_mesh.AddTriangleFan(vertices);
1132  vertices.clear();
1133  // End the loop if i reached the end of the vertex list
1134  if (i >= crosswalk_vertex.size() - 1) {
1135  break;
1136  }
1137  start_vertex_index = ++i;
1138  }
1139  // Append a new Vector3D that will be added to the triangle fan
1140  vertices.push_back(crosswalk_vertex[i++]);
1141  } while (i < crosswalk_vertex.size());
1142 
1143  out_mesh.EndMaterial();
1144  return out_mesh;
1145  }
1146 
1147 } // namespace road
1148 } // 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:980
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:739
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:1111
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:809
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:972
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:862
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:78
std::vector< std::unique_ptr< geom::Mesh > > GenerateChunkedMesh(const rpc::OpendriveGenerationParameters &params) const
Definition: road/Map.cpp:1027
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:854
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:819
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:840
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:722
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