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  if (distance <= EPSILON) {
550  return {waypoint};
551  }
552  const auto &lane = GetLane(waypoint);
553  const bool forward = (waypoint.lane_id <= 0);
554  const double signed_distance = forward ? distance : -distance;
555  const double relative_s = waypoint.s - lane.GetDistance();
556  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
557  DEBUG_ASSERT(remaining_lane_length >= 0.0);
558 
559  // If after subtracting the distance we are still in the same lane, return
560  // same waypoint with the extra distance.
561  if (distance <= remaining_lane_length) {
562  Waypoint result = waypoint;
563  result.s += signed_distance;
564  result.s += forward ? -EPSILON : EPSILON;
565  RELEASE_ASSERT(result.s > 0.0);
566  return { result };
567  }
568 
569  // If we run out of remaining_lane_length we have to go to the successors.
570  std::vector<Waypoint> result;
571  for (const auto &successor : GetSuccessors(waypoint)) {
572  DEBUG_ASSERT(
573  successor.road_id != waypoint.road_id ||
574  successor.section_id != waypoint.section_id ||
575  successor.lane_id != waypoint.lane_id);
576  result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
577  }
578  return result;
579  }
580 
581  std::vector<Waypoint> Map::GetPrevious(
582  const Waypoint waypoint,
583  const double distance) const {
584  RELEASE_ASSERT(distance > 0.0);
585  if (distance <= EPSILON) {
586  return {waypoint};
587  }
588  const auto &lane = GetLane(waypoint);
589  const bool forward = !(waypoint.lane_id <= 0);
590  const double signed_distance = forward ? distance : -distance;
591  const double relative_s = waypoint.s - lane.GetDistance();
592  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
593  DEBUG_ASSERT(remaining_lane_length >= 0.0);
594 
595  // If after subtracting the distance we are still in the same lane, return
596  // same waypoint with the extra distance.
597  if (distance <= remaining_lane_length) {
598  Waypoint result = waypoint;
599  result.s += signed_distance;
600  result.s += forward ? -EPSILON : EPSILON;
601  RELEASE_ASSERT(result.s > 0.0);
602  return { result };
603  }
604 
605  // If we run out of remaining_lane_length we have to go to the successors.
606  std::vector<Waypoint> result;
607  for (const auto &successor : GetPredecessors(waypoint)) {
608  DEBUG_ASSERT(
609  successor.road_id != waypoint.road_id ||
610  successor.section_id != waypoint.section_id ||
611  successor.lane_id != waypoint.lane_id);
612  result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
613  }
614  return result;
615  }
616 
617  boost::optional<Waypoint> Map::GetRight(Waypoint waypoint) const {
618  RELEASE_ASSERT(waypoint.lane_id != 0);
619  if (waypoint.lane_id > 0) {
620  ++waypoint.lane_id;
621  } else {
622  --waypoint.lane_id;
623  }
624  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
625  }
626 
627  boost::optional<Waypoint> Map::GetLeft(Waypoint waypoint) const {
628  RELEASE_ASSERT(waypoint.lane_id != 0);
629  if (std::abs(waypoint.lane_id) == 1) {
630  waypoint.lane_id *= -1;
631  } else if (waypoint.lane_id > 0) {
632  --waypoint.lane_id;
633  } else {
634  ++waypoint.lane_id;
635  }
636  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
637  }
638 
639  std::vector<Waypoint> Map::GenerateWaypoints(const double distance) const {
640  RELEASE_ASSERT(distance > 0.0);
641  std::vector<Waypoint> result;
642  for (const auto &pair : _data.GetRoads()) {
643  const auto &road = pair.second;
644  for (double s = EPSILON; s < (road.GetLength() - EPSILON); s += distance) {
645  ForEachDrivableLaneAt(road, s, [&](auto &&waypoint) {
646  result.emplace_back(waypoint);
647  });
648  }
649  }
650  return result;
651  }
652 
653  std::vector<Waypoint> Map::GenerateWaypointsOnRoadEntries(Lane::LaneType lane_type) const {
654  std::vector<Waypoint> result;
655  for (const auto &pair : _data.GetRoads()) {
656  const auto &road = pair.second;
657  // right lanes start at s 0
658  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
659  for (const auto &lane : lane_section.GetLanes()) {
660  // add only the right (negative) lanes
661  if (lane.first < 0 &&
662  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
663  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
664  }
665  }
666  }
667  // left lanes start at s max
668  const auto road_len = road.GetLength();
669  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
670  for (const auto &lane : lane_section.GetLanes()) {
671  // add only the left (positive) lanes
672  if (lane.first > 0 &&
673  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
674  result.emplace_back(
675  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
676  }
677  }
678  }
679  }
680  return result;
681  }
682 
683  std::vector<Waypoint> Map::GenerateWaypointsInRoad(
684  RoadId road_id,
685  Lane::LaneType lane_type) const {
686  std::vector<Waypoint> result;
687  if(_data.GetRoads().count(road_id)) {
688  const auto &road = _data.GetRoads().at(road_id);
689  // right lanes start at s 0
690  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
691  for (const auto &lane : lane_section.GetLanes()) {
692  // add only the right (negative) lanes
693  if (lane.first < 0 &&
694  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
695  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
696  }
697  }
698  }
699  // left lanes start at s max
700  const auto road_len = road.GetLength();
701  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
702  for (const auto &lane : lane_section.GetLanes()) {
703  // add only the left (positive) lanes
704  if (lane.first > 0 &&
705  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
706  result.emplace_back(
707  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
708  }
709  }
710  }
711  }
712  return result;
713  }
714 
715  std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology() const {
716  std::vector<std::pair<Waypoint, Waypoint>> result;
717  for (const auto &pair : _data.GetRoads()) {
718  const auto &road = pair.second;
719  ForEachDrivableLane(road, [&](auto &&waypoint) {
720  auto successors = GetSuccessors(waypoint);
721  if (successors.size() == 0){
722  auto distance = static_cast<float>(GetDistanceAtEndOfLane(GetLane(waypoint)));
723  auto last_waypoint = GetWaypoint(waypoint.road_id, waypoint.lane_id, distance);
724  if (last_waypoint.has_value()){
725  result.push_back({waypoint, *last_waypoint});
726  }
727  }
728  else{
729  for (auto &&successor : GetSuccessors(waypoint)) {
730  result.push_back({waypoint, successor});
731  }
732  }
733  });
734  }
735  return result;
736  }
737 
738  std::vector<std::pair<Waypoint, Waypoint>> Map::GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const {
739  std::vector<std::pair<Waypoint, Waypoint>> result;
740  const Junction * junction = GetJunction(id);
741  for(auto &connections : junction->GetConnections()) {
742  const Road &road = _data.GetRoad(connections.second.connecting_road);
743  ForEachLane(road, lane_type, [&](auto &&waypoint) {
744  const auto& lane = GetLane(waypoint);
745  const double final_s = GetDistanceAtEndOfLane(lane);
746  Waypoint lane_end(waypoint);
747  lane_end.s = final_s;
748  result.push_back({waypoint, lane_end});
749  });
750  }
751  return result;
752  }
753 
754  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
756 
757  const float epsilon = 0.0001f; // small delta in the road (set to 0.1
758  // millimeters to prevent numeric errors)
759  const Junction *junction = GetJunction(id);
760  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
761  conflicts;
762 
763  // 2d typedefs
764  typedef boost::geometry::model::point
765  <float, 2, boost::geometry::cs::cartesian> Point2d;
766  typedef boost::geometry::model::segment<Point2d> Segment2d;
767  typedef boost::geometry::model::box<Rtree::BPoint> Box;
768 
769  // box range
770  auto bbox_pos = junction->GetBoundingBox().location;
771  auto bbox_ext = junction->GetBoundingBox().extent;
772  auto min_corner = geom::Vector3D(
773  bbox_pos.x - bbox_ext.x,
774  bbox_pos.y - bbox_ext.y,
775  bbox_pos.z - bbox_ext.z - epsilon);
776  auto max_corner = geom::Vector3D(
777  bbox_pos.x + bbox_ext.x,
778  bbox_pos.y + bbox_ext.y,
779  bbox_pos.z + bbox_ext.z + epsilon);
780  Box box({min_corner.x, min_corner.y, min_corner.z},
781  {max_corner.x, max_corner.y, max_corner.z});
782  auto segments = _rtree.GetIntersections(box);
783 
784  for (size_t i = 0; i < segments.size(); ++i){
785  auto &segment1 = segments[i];
786  auto waypoint1 = segment1.second.first;
787  JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
788  // only segments in the junction
789  if(junc_id1 != id){
790  continue;
791  }
792  Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
793  {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
794  for (size_t j = i + 1; j < segments.size(); ++j){
795  auto &segment2 = segments[j];
796  auto waypoint2 = segment2.second.first;
797  JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
798  // only segments in the junction
799  if(junc_id2 != id){
800  continue;
801  }
802  // discard same road
803  if(waypoint1.road_id == waypoint2.road_id){
804  continue;
805  }
806  Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
807  {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
808 
809  double distance = boost::geometry::distance(seg1, seg2);
810  // better to set distance to lanewidth
811  if(distance > 2.0){
812  continue;
813  }
814  if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
815  conflicts[waypoint1.road_id].insert(waypoint2.road_id);
816  }
817  if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
818  conflicts[waypoint2.road_id].insert(waypoint1.road_id);
819  }
820  }
821  }
822  return conflicts;
823  }
824 
825  const Lane &Map::GetLane(Waypoint waypoint) const {
826  return _data.GetRoad(waypoint.road_id).GetLaneById(waypoint.section_id, waypoint.lane_id);
827  }
828 
829  // ===========================================================================
830  // -- Map: Private functions -------------------------------------------------
831  // ===========================================================================
832 
833  // Adds a new element to the rtree element list using the position of the
834  // waypoints both ends of the segment
836  std::vector<Rtree::TreeElement> &rtree_elements,
837  geom::Transform &current_transform,
838  geom::Transform &next_transform,
839  Waypoint &current_waypoint,
840  Waypoint &next_waypoint) {
841  Rtree::BPoint init =
843  current_transform.location.x,
844  current_transform.location.y,
845  current_transform.location.z);
846  Rtree::BPoint end =
848  next_transform.location.x,
849  next_transform.location.y,
850  next_transform.location.z);
851  rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init, end),
852  std::make_pair(current_waypoint, next_waypoint)));
853  }
854  // Adds a new element to the rtree element list using the position of the
855  // waypoints, both ends of the segment
857  std::vector<Rtree::TreeElement> &rtree_elements,
858  geom::Transform &current_transform,
859  Waypoint &current_waypoint,
860  Waypoint &next_waypoint) {
861  geom::Transform next_transform = ComputeTransform(next_waypoint);
862  AddElementToRtree(rtree_elements, current_transform, next_transform,
863  current_waypoint, next_waypoint);
864  current_waypoint = next_waypoint;
865  current_transform = next_transform;
866  }
867 
868  // returns the remaining length of the geometry depending on the lane
869  // direction
870  double GetRemainingLength(const Lane &lane, double current_s) {
871  if (lane.GetId() < 0) {
872  return (lane.GetDistance() + lane.GetLength() - current_s);
873  } else {
874  return (current_s - lane.GetDistance());
875  }
876  }
877 
879  const double epsilon = 0.000001; // small delta in the road (set to 1
880  // micrometer to prevent numeric errors)
881  const double min_delta_s = 1; // segments of minimum 1m through the road
882 
883  // 1.8 degrees, maximum angle in a curve to place a segment
884  constexpr double angle_threshold = geom::Math::Pi<double>() / 100.0;
885  // maximum distance of a segment
886  constexpr double max_segment_length = 100.0;
887 
888  // Generate waypoints at start of every lane
889  std::vector<Waypoint> topology;
890  for (const auto &pair : _data.GetRoads()) {
891  const auto &road = pair.second;
892  ForEachLane(road, Lane::LaneType::Any, [&](auto &&waypoint) {
893  if(waypoint.lane_id != 0) {
894  topology.push_back(waypoint);
895  }
896  });
897  }
898 
899  // Container of segments and waypoints
900  std::vector<Rtree::TreeElement> rtree_elements;
901  // Loop through all lanes
902  for (auto &waypoint : topology) {
903  auto &lane_start_waypoint = waypoint;
904 
905  auto current_waypoint = lane_start_waypoint;
906 
907  const Lane &lane = GetLane(current_waypoint);
908 
909  geom::Transform current_transform = ComputeTransform(current_waypoint);
910 
911  // Save computation time in straight lines
912  if (lane.IsStraight()) {
913  double delta_s = min_delta_s;
914  double remaining_length =
915  GetRemainingLength(lane, current_waypoint.s);
916  remaining_length -= epsilon;
917  delta_s = remaining_length;
918  if (delta_s < epsilon) {
919  continue;
920  }
921  auto next = GetNext(current_waypoint, delta_s);
922 
923  RELEASE_ASSERT(next.size() == 1);
924  RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
925  auto next_waypoint = next.front();
926 
927  AddElementToRtreeAndUpdateTransforms(
928  rtree_elements,
929  current_transform,
930  current_waypoint,
931  next_waypoint);
932  // end of lane
933  } else {
934  auto next_waypoint = current_waypoint;
935 
936  // Loop until the end of the lane
937  // Advance in small s-increments
938  while (true) {
939  double delta_s = min_delta_s;
940  double remaining_length =
941  GetRemainingLength(lane, next_waypoint.s);
942  remaining_length -= epsilon;
943  delta_s = std::min(delta_s, remaining_length);
944 
945  if (delta_s < epsilon) {
946  AddElementToRtreeAndUpdateTransforms(
947  rtree_elements,
948  current_transform,
949  current_waypoint,
950  next_waypoint);
951  break;
952  }
953 
954  auto next = GetNext(next_waypoint, delta_s);
955  if (next.size() != 1 ||
956  current_waypoint.section_id != next.front().section_id) {
957  AddElementToRtreeAndUpdateTransforms(
958  rtree_elements,
959  current_transform,
960  current_waypoint,
961  next_waypoint);
962  break;
963  }
964 
965  next_waypoint = next.front();
966  geom::Transform next_transform = ComputeTransform(next_waypoint);
967  double angle = geom::Math::GetVectorAngle(
968  current_transform.GetForwardVector(), next_transform.GetForwardVector());
969 
970  if (std::abs(angle) > angle_threshold ||
971  std::abs(current_waypoint.s - next_waypoint.s) > max_segment_length) {
972  AddElementToRtree(
973  rtree_elements,
974  current_transform,
975  next_transform,
976  current_waypoint,
977  next_waypoint);
978  current_waypoint = next_waypoint;
979  current_transform = next_transform;
980  }
981  }
982  }
983  }
984  // Add segments to Rtree
985  _rtree.InsertElements(rtree_elements);
986  }
987 
989  return _data.GetJunction(id);
990  }
991 
992  const Junction* Map::GetJunction(JuncId id) const {
993  return _data.GetJunction(id);
994  }
995 
997  const double distance,
998  const float extra_width,
999  const bool smooth_junctions) const {
1000  RELEASE_ASSERT(distance > 0.0);
1001  geom::MeshFactory mesh_factory;
1002  geom::Mesh out_mesh;
1003 
1004  mesh_factory.road_param.resolution = static_cast<float>(distance);
1005  mesh_factory.road_param.extra_lane_width = extra_width;
1006 
1007  // Generate roads outside junctions
1008  for (auto &&pair : _data.GetRoads()) {
1009  const auto &road = pair.second;
1010  if (road.IsJunction()) {
1011  continue;
1012  }
1013  out_mesh += *mesh_factory.Generate(road);
1014  }
1015 
1016  // Generate roads within junctions and smooth them
1017  for (const auto &junc_pair : _data.GetJunctions()) {
1018  const auto &junction = junc_pair.second;
1019  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1020  for(const auto &connection_pair : junction.GetConnections()) {
1021  const auto &connection = connection_pair.second;
1022  const auto &road = _data.GetRoads().at(connection.connecting_road);
1023  for (auto &&lane_section : road.GetLaneSections()) {
1024  for (auto &&lane_pair : lane_section.GetLanes()) {
1025  lane_meshes.push_back(mesh_factory.Generate(lane_pair.second));
1026  }
1027  }
1028  }
1029  if(smooth_junctions) {
1030  out_mesh += *mesh_factory.MergeAndSmooth(lane_meshes);
1031  } else {
1032  geom::Mesh junction_mesh;
1033  for(auto& lane : lane_meshes) {
1034  junction_mesh += *lane;
1035  }
1036  out_mesh += junction_mesh;
1037  }
1038  }
1039 
1040  return out_mesh;
1041  }
1042 
1043  std::vector<std::unique_ptr<geom::Mesh>> Map::GenerateChunkedMesh(
1044  const rpc::OpendriveGenerationParameters& params) const {
1045  geom::MeshFactory mesh_factory(params);
1046  std::vector<std::unique_ptr<geom::Mesh>> out_mesh_list;
1047 
1048  std::unordered_map<JuncId, geom::Mesh> junction_map;
1049  for (auto &&pair : _data.GetRoads()) {
1050  const auto &road = pair.second;
1051  if (!road.IsJunction()) {
1052  std::vector<std::unique_ptr<geom::Mesh>> road_mesh_list =
1053  mesh_factory.GenerateAllWithMaxLen(road);
1054 
1055  out_mesh_list.insert(
1056  out_mesh_list.end(),
1057  std::make_move_iterator(road_mesh_list.begin()),
1058  std::make_move_iterator(road_mesh_list.end()));
1059  }
1060  }
1061 
1062  // Generate roads within junctions and smooth them
1063  for (const auto &junc_pair : _data.GetJunctions()) {
1064  const auto &junction = junc_pair.second;
1065  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1066  std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1067  for(const auto &connection_pair : junction.GetConnections()) {
1068  const auto &connection = connection_pair.second;
1069  const auto &road = _data.GetRoads().at(connection.connecting_road);
1070  for (auto &&lane_section : road.GetLaneSections()) {
1071  for (auto &&lane_pair : lane_section.GetLanes()) {
1072  const auto &lane = lane_pair.second;
1073  if (lane.GetType() != road::Lane::LaneType::Sidewalk) {
1074  lane_meshes.push_back(mesh_factory.Generate(lane));
1075  } else {
1076  sidewalk_lane_meshes.push_back(mesh_factory.Generate(lane));
1077  }
1078  }
1079  }
1080  }
1081  if(params.smooth_junctions) {
1082  auto merged_mesh = mesh_factory.MergeAndSmooth(lane_meshes);
1083  for(auto& lane : sidewalk_lane_meshes) {
1084  *merged_mesh += *lane;
1085  }
1086  out_mesh_list.push_back(std::move(merged_mesh));
1087  } else {
1088  std::unique_ptr<geom::Mesh> junction_mesh = std::make_unique<geom::Mesh>();
1089  for(auto& lane : lane_meshes) {
1090  *junction_mesh += *lane;
1091  }
1092  for(auto& lane : sidewalk_lane_meshes) {
1093  *junction_mesh += *lane;
1094  }
1095  out_mesh_list.push_back(std::move(junction_mesh));
1096  }
1097  }
1098 
1099  auto min_pos = geom::Vector2D(
1100  out_mesh_list.front()->GetVertices().front().x,
1101  out_mesh_list.front()->GetVertices().front().y);
1102  auto max_pos = min_pos;
1103  for (auto & mesh : out_mesh_list) {
1104  auto vertex = mesh->GetVertices().front();
1105  min_pos.x = std::min(min_pos.x, vertex.x);
1106  min_pos.y = std::min(min_pos.y, vertex.y);
1107  max_pos.x = std::max(max_pos.x, vertex.x);
1108  max_pos.y = std::max(max_pos.y, vertex.y);
1109  }
1110  size_t mesh_amount_x = static_cast<size_t>((max_pos.x - min_pos.x)/params.max_road_length) + 1;
1111  size_t mesh_amount_y = static_cast<size_t>((max_pos.y - min_pos.y)/params.max_road_length) + 1;
1112  std::vector<std::unique_ptr<geom::Mesh>> result;
1113  result.reserve(mesh_amount_x*mesh_amount_y);
1114  for (size_t i = 0; i < mesh_amount_x*mesh_amount_y; ++i) {
1115  result.emplace_back(std::make_unique<geom::Mesh>());
1116  }
1117  for (auto & mesh : out_mesh_list) {
1118  auto vertex = mesh->GetVertices().front();
1119  size_t x_pos = static_cast<size_t>((vertex.x - min_pos.x) / params.max_road_length);
1120  size_t y_pos = static_cast<size_t>((vertex.y - min_pos.y) / params.max_road_length);
1121  *(result[x_pos + mesh_amount_x*y_pos]) += *mesh;
1122  }
1123 
1124  return result;
1125  }
1126 
1128  geom::Mesh out_mesh;
1129 
1130  // Get the crosswalk vertices for the current map
1131  const std::vector<geom::Location> crosswalk_vertex = GetAllCrosswalkZones();
1132  if (crosswalk_vertex.empty()) {
1133  return out_mesh;
1134  }
1135 
1136  // Create a a list of triangle fans with material "crosswalk"
1137  out_mesh.AddMaterial("crosswalk");
1138  size_t start_vertex_index = 0;
1139  size_t i = 0;
1140  std::vector<geom::Vector3D> vertices;
1141  // Iterate the vertices until a repeated one is found, this indicates
1142  // the triangle fan is done and another one must start
1143  do {
1144  // Except for the first iteration && triangle fan done
1145  if (i != 0 && crosswalk_vertex[start_vertex_index] == crosswalk_vertex[i]) {
1146  // Create the actual fan
1147  out_mesh.AddTriangleFan(vertices);
1148  vertices.clear();
1149  // End the loop if i reached the end of the vertex list
1150  if (i >= crosswalk_vertex.size() - 1) {
1151  break;
1152  }
1153  start_vertex_index = ++i;
1154  }
1155  // Append a new Vector3D that will be added to the triangle fan
1156  vertices.push_back(crosswalk_vertex[i++]);
1157  } while (i < crosswalk_vertex.size());
1158 
1159  out_mesh.EndMaterial();
1160  return out_mesh;
1161  }
1162 
1163 } // namespace road
1164 } // 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:996
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:755
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:581
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:715
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:1127
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:825
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:653
double GetDistance() const
Definition: Lane.cpp:46
Junction * GetJunction(JuncId id)
Definition: road/Map.cpp:988
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:878
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:1043
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:627
Lane::LaneType GetLaneType(Waypoint waypoint) const
Definition: road/Map.cpp:272
Mesh data container, validator and exporter.
Definition: Mesh.h:44
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:870
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:683
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:835
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:856
std::vector< Waypoint > GenerateWaypoints(double approx_distance) const
Generate all the waypoints in map separated by approx_distance.
Definition: road/Map.cpp:639
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:617
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
Definition: road/Map.cpp:738
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