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  if (distance_to_signal == 0) {
347  result.emplace_back(SignalSearchData
348  {signal, waypoint,
349  distance_to_signal});
350  } else {
351  result.emplace_back(SignalSearchData
352  {signal, GetNext(waypoint, distance_to_signal).front(),
353  distance_to_signal});
354  }
355 
356  }
357  return result;
358  }
359  const double signed_remaining_length = forward ? remaining_lane_length : -remaining_lane_length;
360 
361  //result = road.GetInfosInRange<RoadInfoSignal>(waypoint.s, waypoint.s + signed_remaining_length);
362  auto signals = road.GetInfosInRange<RoadInfoSignal>(
363  waypoint.s, waypoint.s + signed_remaining_length);
364  for(auto* signal : signals){
365  double distance_to_signal = 0;
366  if (waypoint.lane_id < 0){
367  distance_to_signal = signal->GetDistance() - waypoint.s;
368  } else {
369  distance_to_signal = waypoint.s - signal->GetDistance();
370  }
371  if (distance_to_signal == 0) {
372  result.emplace_back(SignalSearchData
373  {signal, waypoint,
374  distance_to_signal});
375  } else {
376  result.emplace_back(SignalSearchData
377  {signal, GetNext(waypoint, distance_to_signal).front(),
378  distance_to_signal});
379  }
380  }
381  // If we run out of remaining_lane_length we have to go to the successors.
382  for (auto &successor : GetSuccessors(waypoint)) {
383  if(_data.GetRoad(successor.road_id).IsJunction() && stop_at_junction){
384  continue;
385  }
386  auto& sucessor_lane = _data.GetRoad(successor.road_id).
387  GetLaneByDistance(successor.s, successor.lane_id);
388  if (successor.lane_id < 0) {
389  successor.s = sucessor_lane.GetDistance();
390  } else {
391  successor.s = sucessor_lane.GetDistance() + sucessor_lane.GetLength();
392  }
393  auto sucessor_signals = GetSignalsInDistance(
394  successor, distance - remaining_lane_length, stop_at_junction);
395  for(auto& signal : sucessor_signals){
396  signal.accumulated_s += remaining_lane_length;
397  }
398  result = ConcatVectors(result, sucessor_signals);
399  }
400  return result;
401  }
402 
403  std::vector<const element::RoadInfoSignal*>
405  std::vector<const element::RoadInfoSignal*> result;
406  for (const auto& road_pair : _data.GetRoads()) {
407  const auto &road = road_pair.second;
408  auto road_infos = road.GetInfos<element::RoadInfoSignal>();
409  for(const auto* road_info : road_infos) {
410  result.push_back(road_info);
411  }
412  }
413  return result;
414  }
415 
416  std::vector<LaneMarking> Map::CalculateCrossedLanes(
417  const geom::Location &origin,
418  const geom::Location &destination) const {
419  return LaneCrossingCalculator::Calculate(*this, origin, destination);
420  }
421 
422  std::vector<geom::Location> Map::GetAllCrosswalkZones() const {
423  std::vector<geom::Location> result;
424 
425  for (const auto &pair : _data.GetRoads()) {
426  const auto &road = pair.second;
427  std::vector<const RoadInfoCrosswalk *> crosswalks = road.GetInfos<RoadInfoCrosswalk>();
428  if (crosswalks.size() > 0) {
429  for (auto crosswalk : crosswalks) {
430  // waypoint only at start position
431  std::vector<geom::Location> points;
432  Waypoint waypoint;
433  geom::Transform base;
434  for (const auto &section : road.GetLaneSectionsAt(crosswalk->GetS())) {
435  // get the section with the center lane
436  for (const auto &lane : section.GetLanes()) {
437  // is the center line
438  if (lane.first == 0) {
439  // get the center point
440  waypoint.road_id = pair.first;
441  waypoint.section_id = section.GetId();
442  waypoint.lane_id = 0;
443  waypoint.s = crosswalk->GetS();
444  base = ComputeTransform(waypoint);
445  }
446  }
447  }
448 
449  // move perpendicular ('t')
450  geom::Transform pivot = base;
451  pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
452  pivot.rotation.yaw -= 90; // move perpendicular to 's' for the lateral offset
453  geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f);
454  pivot.TransformPoint(v);
455  // restore pivot position and orientation
456  pivot = base;
457  pivot.location = v;
458  pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
459 
460  // calculate all the corners
461  for (auto corner : crosswalk->GetPoints()) {
462  geom::Vector3D v2(
463  static_cast<float>(corner.u),
464  static_cast<float>(corner.v),
465  static_cast<float>(corner.z));
466  // set the width larger to contact with the sidewalk (in case they have gutter area)
467  if (corner.u < 0) {
468  v2.x -= 1.0f;
469  } else {
470  v2.x += 1.0f;
471  }
472  pivot.TransformPoint(v2);
473  result.push_back(v2);
474  }
475  }
476  }
477  }
478  return result;
479  }
480 
481  // ===========================================================================
482  // -- Map: Waypoint generation -----------------------------------------------
483  // ===========================================================================
484 
485  std::vector<Waypoint> Map::GetSuccessors(const Waypoint waypoint) const {
486  const auto &next_lanes = GetLane(waypoint).GetNextLanes();
487  std::vector<Waypoint> result;
488  result.reserve(next_lanes.size());
489  for (auto *next_lane : next_lanes) {
490  RELEASE_ASSERT(next_lane != nullptr);
491  const auto lane_id = next_lane->GetId();
492  RELEASE_ASSERT(lane_id != 0);
493  const auto *section = next_lane->GetLaneSection();
494  RELEASE_ASSERT(section != nullptr);
495  const auto *road = next_lane->GetRoad();
496  RELEASE_ASSERT(road != nullptr);
497  const auto distance = GetDistanceAtStartOfLane(*next_lane);
498  result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
499  }
500  return result;
501  }
502 
503  std::vector<Waypoint> Map::GetPredecessors(const Waypoint waypoint) const {
504  const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes();
505  std::vector<Waypoint> result;
506  result.reserve(prev_lanes.size());
507  for (auto *next_lane : prev_lanes) {
508  RELEASE_ASSERT(next_lane != nullptr);
509  const auto lane_id = next_lane->GetId();
510  RELEASE_ASSERT(lane_id != 0);
511  const auto *section = next_lane->GetLaneSection();
512  RELEASE_ASSERT(section != nullptr);
513  const auto *road = next_lane->GetRoad();
514  RELEASE_ASSERT(road != nullptr);
515  const auto distance = GetDistanceAtEndOfLane(*next_lane);
516  result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
517  }
518  return result;
519  }
520 
521  std::vector<Waypoint> Map::GetNext(
522  const Waypoint waypoint,
523  const double distance) const {
524  RELEASE_ASSERT(distance > 0.0);
525  const auto &lane = GetLane(waypoint);
526  const bool forward = (waypoint.lane_id <= 0);
527  const double signed_distance = forward ? distance : -distance;
528  const double relative_s = waypoint.s - lane.GetDistance();
529  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
530  DEBUG_ASSERT(remaining_lane_length >= 0.0);
531 
532  // If after subtracting the distance we are still in the same lane, return
533  // same waypoint with the extra distance.
534  if (distance <= remaining_lane_length) {
535  Waypoint result = waypoint;
536  result.s += signed_distance;
537  result.s += forward ? -EPSILON : EPSILON;
538  RELEASE_ASSERT(result.s > 0.0);
539  return { result };
540  }
541 
542  // If we run out of remaining_lane_length we have to go to the successors.
543  std::vector<Waypoint> result;
544  for (const auto &successor : GetSuccessors(waypoint)) {
545  DEBUG_ASSERT(
546  successor.road_id != waypoint.road_id ||
547  successor.section_id != waypoint.section_id ||
548  successor.lane_id != waypoint.lane_id);
549  result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
550  }
551  return result;
552  }
553 
554  std::vector<Waypoint> Map::GetPrevious(
555  const Waypoint waypoint,
556  const double distance) const {
557  RELEASE_ASSERT(distance > 0.0);
558  const auto &lane = GetLane(waypoint);
559  const bool forward = !(waypoint.lane_id <= 0);
560  const double signed_distance = forward ? distance : -distance;
561  const double relative_s = waypoint.s - lane.GetDistance();
562  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
563  DEBUG_ASSERT(remaining_lane_length >= 0.0);
564 
565  // If after subtracting the distance we are still in the same lane, return
566  // same waypoint with the extra distance.
567  if (distance <= remaining_lane_length) {
568  Waypoint result = waypoint;
569  result.s += signed_distance;
570  result.s += forward ? -EPSILON : EPSILON;
571  RELEASE_ASSERT(result.s > 0.0);
572  return { result };
573  }
574 
575  // If we run out of remaining_lane_length we have to go to the successors.
576  std::vector<Waypoint> result;
577  for (const auto &successor : GetPredecessors(waypoint)) {
578  DEBUG_ASSERT(
579  successor.road_id != waypoint.road_id ||
580  successor.section_id != waypoint.section_id ||
581  successor.lane_id != waypoint.lane_id);
582  result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
583  }
584  return result;
585  }
586 
587  boost::optional<Waypoint> Map::GetRight(Waypoint waypoint) const {
588  RELEASE_ASSERT(waypoint.lane_id != 0);
589  if (waypoint.lane_id > 0) {
590  ++waypoint.lane_id;
591  } else {
592  --waypoint.lane_id;
593  }
594  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
595  }
596 
597  boost::optional<Waypoint> Map::GetLeft(Waypoint waypoint) const {
598  RELEASE_ASSERT(waypoint.lane_id != 0);
599  if (std::abs(waypoint.lane_id) == 1) {
600  waypoint.lane_id *= -1;
601  } else if (waypoint.lane_id > 0) {
602  --waypoint.lane_id;
603  } else {
604  ++waypoint.lane_id;
605  }
606  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
607  }
608 
609  std::vector<Waypoint> Map::GenerateWaypoints(const double distance) const {
610  RELEASE_ASSERT(distance > 0.0);
611  std::vector<Waypoint> result;
612  for (const auto &pair : _data.GetRoads()) {
613  const auto &road = pair.second;
614  for (double s = EPSILON; s < (road.GetLength() - EPSILON); s += distance) {
615  ForEachDrivableLaneAt(road, s, [&](auto &&waypoint) {
616  result.emplace_back(waypoint);
617  });
618  }
619  }
620  return result;
621  }
622 
623  std::vector<Waypoint> Map::GenerateWaypointsOnRoadEntries(Lane::LaneType lane_type) const {
624  std::vector<Waypoint> result;
625  for (const auto &pair : _data.GetRoads()) {
626  const auto &road = pair.second;
627  // right lanes start at s 0
628  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
629  for (const auto &lane : lane_section.GetLanes()) {
630  // add only the right (negative) lanes
631  if (lane.first < 0 &&
632  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
633  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
634  }
635  }
636  }
637  // left lanes start at s max
638  const auto road_len = road.GetLength();
639  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
640  for (const auto &lane : lane_section.GetLanes()) {
641  // add only the left (positive) lanes
642  if (lane.first > 0 &&
643  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
644  result.emplace_back(
645  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
646  }
647  }
648  }
649  }
650  return result;
651  }
652 
653  std::vector<Waypoint> Map::GenerateWaypointsInRoad(
654  RoadId road_id,
655  Lane::LaneType lane_type) const {
656  std::vector<Waypoint> result;
657  if(_data.GetRoads().count(road_id)) {
658  const auto &road = _data.GetRoads().at(road_id);
659  // right lanes start at s 0
660  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
661  for (const auto &lane : lane_section.GetLanes()) {
662  // add only the right (negative) lanes
663  if (lane.first < 0 &&
664  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
665  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
666  }
667  }
668  }
669  // left lanes start at s max
670  const auto road_len = road.GetLength();
671  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
672  for (const auto &lane : lane_section.GetLanes()) {
673  // add only the left (positive) lanes
674  if (lane.first > 0 &&
675  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
676  result.emplace_back(
677  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
678  }
679  }
680  }
681  }
682  return result;
683  }
684 
685  std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology() const {
686  std::vector<std::pair<Waypoint, Waypoint>> result;
687  for (const auto &pair : _data.GetRoads()) {
688  const auto &road = pair.second;
689  ForEachDrivableLane(road, [&](auto &&waypoint) {
690  for (auto &&successor : GetSuccessors(waypoint)) {
691  result.push_back({waypoint, successor});
692  }
693  });
694  }
695  return result;
696  }
697 
698  std::vector<std::pair<Waypoint, Waypoint>> Map::GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const {
699  std::vector<std::pair<Waypoint, Waypoint>> result;
700  const Junction * junction = GetJunction(id);
701  for(auto &connections : junction->GetConnections()) {
702  const Road &road = _data.GetRoad(connections.second.connecting_road);
703  ForEachLane(road, lane_type, [&](auto &&waypoint) {
704  const auto& lane = GetLane(waypoint);
705  const double final_s = GetDistanceAtEndOfLane(lane);
706  Waypoint lane_end(waypoint);
707  lane_end.s = final_s;
708  result.push_back({waypoint, lane_end});
709  });
710  }
711  return result;
712  }
713 
714  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
716 
717  const float epsilon = 0.0001f; // small delta in the road (set to 0.1
718  // millimeters to prevent numeric errors)
719  const Junction *junction = GetJunction(id);
720  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
721  conflicts;
722 
723  // 2d typedefs
724  typedef boost::geometry::model::point
725  <float, 2, boost::geometry::cs::cartesian> Point2d;
726  typedef boost::geometry::model::segment<Point2d> Segment2d;
727  typedef boost::geometry::model::box<Rtree::BPoint> Box;
728 
729  // box range
730  auto bbox_pos = junction->GetBoundingBox().location;
731  auto bbox_ext = junction->GetBoundingBox().extent;
732  auto min_corner = geom::Vector3D(
733  bbox_pos.x - bbox_ext.x,
734  bbox_pos.y - bbox_ext.y,
735  bbox_pos.z - bbox_ext.z - epsilon);
736  auto max_corner = geom::Vector3D(
737  bbox_pos.x + bbox_ext.x,
738  bbox_pos.y + bbox_ext.y,
739  bbox_pos.z + bbox_ext.z + epsilon);
740  Box box({min_corner.x, min_corner.y, min_corner.z},
741  {max_corner.x, max_corner.y, max_corner.z});
742  auto segments = _rtree.GetIntersections(box);
743 
744  for (size_t i = 0; i < segments.size(); ++i){
745  auto &segment1 = segments[i];
746  auto waypoint1 = segment1.second.first;
747  JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
748  // only segments in the junction
749  if(junc_id1 != id){
750  continue;
751  }
752  Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
753  {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
754  for (size_t j = i + 1; j < segments.size(); ++j){
755  auto &segment2 = segments[j];
756  auto waypoint2 = segment2.second.first;
757  JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
758  // only segments in the junction
759  if(junc_id2 != id){
760  continue;
761  }
762  // discard same road
763  if(waypoint1.road_id == waypoint2.road_id){
764  continue;
765  }
766  Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
767  {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
768 
769  double distance = boost::geometry::distance(seg1, seg2);
770  // better to set distance to lanewidth
771  if(distance > 2.0){
772  continue;
773  }
774  if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
775  conflicts[waypoint1.road_id].insert(waypoint2.road_id);
776  }
777  if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
778  conflicts[waypoint2.road_id].insert(waypoint1.road_id);
779  }
780  }
781  }
782  return conflicts;
783  }
784 
785  const Lane &Map::GetLane(Waypoint waypoint) const {
786  return _data.GetRoad(waypoint.road_id).GetLaneById(waypoint.section_id, waypoint.lane_id);
787  }
788 
789  // ===========================================================================
790  // -- Map: Private functions -------------------------------------------------
791  // ===========================================================================
792 
793  // Adds a new element to the rtree element list using the position of the
794  // waypoints both ends of the segment
796  std::vector<Rtree::TreeElement> &rtree_elements,
797  geom::Transform &current_transform,
798  geom::Transform &next_transform,
799  Waypoint &current_waypoint,
800  Waypoint &next_waypoint) {
801  Rtree::BPoint init =
803  current_transform.location.x,
804  current_transform.location.y,
805  current_transform.location.z);
806  Rtree::BPoint end =
808  next_transform.location.x,
809  next_transform.location.y,
810  next_transform.location.z);
811  rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init, end),
812  std::make_pair(current_waypoint, next_waypoint)));
813  }
814  // Adds a new element to the rtree element list using the position of the
815  // waypoints, both ends of the segment
817  std::vector<Rtree::TreeElement> &rtree_elements,
818  geom::Transform &current_transform,
819  Waypoint &current_waypoint,
820  Waypoint &next_waypoint) {
821  geom::Transform next_transform = ComputeTransform(next_waypoint);
822  AddElementToRtree(rtree_elements, current_transform, next_transform,
823  current_waypoint, next_waypoint);
824  current_waypoint = next_waypoint;
825  current_transform = next_transform;
826  }
827 
828  // returns the remaining length of the geometry depending on the lane
829  // direction
830  double GetRemainingLength(const Lane &lane, double current_s) {
831  if (lane.GetId() < 0) {
832  return (lane.GetDistance() + lane.GetLength() - current_s);
833  } else {
834  return (current_s - lane.GetDistance());
835  }
836  }
837 
839  const double epsilon = 0.000001; // small delta in the road (set to 1
840  // micrometer to prevent numeric errors)
841  const double min_delta_s = 1; // segments of minimum 1m through the road
842 
843  // 1.8 degrees, maximum angle in a curve to place a segment
844  constexpr double angle_threshold = geom::Math::Pi<double>() / 100.0;
845  // maximum distance of a segment
846  constexpr double max_segment_length = 100.0;
847 
848  // Generate waypoints at start of every lane
849  std::vector<Waypoint> topology;
850  for (const auto &pair : _data.GetRoads()) {
851  const auto &road = pair.second;
852  ForEachLane(road, Lane::LaneType::Any, [&](auto &&waypoint) {
853  if(waypoint.lane_id != 0) {
854  topology.push_back(waypoint);
855  }
856  });
857  }
858 
859  // Container of segments and waypoints
860  std::vector<Rtree::TreeElement> rtree_elements;
861  // Loop through all lanes
862  for (auto &waypoint : topology) {
863  auto &lane_start_waypoint = waypoint;
864 
865  auto current_waypoint = lane_start_waypoint;
866 
867  const Lane &lane = GetLane(current_waypoint);
868 
869  geom::Transform current_transform = ComputeTransform(current_waypoint);
870 
871  // Save computation time in straight lines
872  if (lane.IsStraight()) {
873  double delta_s = min_delta_s;
874  double remaining_length =
875  GetRemainingLength(lane, current_waypoint.s);
876  remaining_length -= epsilon;
877  delta_s = remaining_length;
878  if (delta_s < epsilon) {
879  continue;
880  }
881  auto next = GetNext(current_waypoint, delta_s);
882 
883  RELEASE_ASSERT(next.size() == 1);
884  RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
885  auto next_waypoint = next.front();
886 
887  AddElementToRtreeAndUpdateTransforms(
888  rtree_elements,
889  current_transform,
890  current_waypoint,
891  next_waypoint);
892  // end of lane
893  } else {
894  auto next_waypoint = current_waypoint;
895 
896  // Loop until the end of the lane
897  // Advance in small s-increments
898  while (true) {
899  double delta_s = min_delta_s;
900  double remaining_length =
901  GetRemainingLength(lane, next_waypoint.s);
902  remaining_length -= epsilon;
903  delta_s = std::min(delta_s, remaining_length);
904 
905  if (delta_s < epsilon) {
906  AddElementToRtreeAndUpdateTransforms(
907  rtree_elements,
908  current_transform,
909  current_waypoint,
910  next_waypoint);
911  break;
912  }
913 
914  auto next = GetNext(next_waypoint, delta_s);
915  if (next.size() != 1 ||
916  current_waypoint.section_id != next.front().section_id) {
917  AddElementToRtreeAndUpdateTransforms(
918  rtree_elements,
919  current_transform,
920  current_waypoint,
921  next_waypoint);
922  break;
923  }
924 
925  next_waypoint = next.front();
926  geom::Transform next_transform = ComputeTransform(next_waypoint);
927  double angle = geom::Math::GetVectorAngle(
928  current_transform.GetForwardVector(), next_transform.GetForwardVector());
929 
930  if (std::abs(angle) > angle_threshold ||
931  std::abs(current_waypoint.s - next_waypoint.s) > max_segment_length) {
932  AddElementToRtree(
933  rtree_elements,
934  current_transform,
935  next_transform,
936  current_waypoint,
937  next_waypoint);
938  current_waypoint = next_waypoint;
939  current_transform = next_transform;
940  }
941  }
942  }
943  }
944  // Add segments to Rtree
945  _rtree.InsertElements(rtree_elements);
946  }
947 
949  return _data.GetJunction(id);
950  }
951 
952  const Junction* Map::GetJunction(JuncId id) const {
953  return _data.GetJunction(id);
954  }
955 
957  const double distance,
958  const float extra_width,
959  const bool smooth_junctions) const {
960  RELEASE_ASSERT(distance > 0.0);
961  geom::MeshFactory mesh_factory;
962  geom::Mesh out_mesh;
963 
964  mesh_factory.road_param.resolution = static_cast<float>(distance);
965  mesh_factory.road_param.extra_lane_width = extra_width;
966 
967  // Generate roads outside junctions
968  for (auto &&pair : _data.GetRoads()) {
969  const auto &road = pair.second;
970  if (road.IsJunction()) {
971  continue;
972  }
973  out_mesh += *mesh_factory.Generate(road);
974  }
975 
976  // Generate roads within junctions and smooth them
977  for (const auto &junc_pair : _data.GetJunctions()) {
978  const auto &junction = junc_pair.second;
979  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
980  for(const auto &connection_pair : junction.GetConnections()) {
981  const auto &connection = connection_pair.second;
982  const auto &road = _data.GetRoads().at(connection.connecting_road);
983  for (auto &&lane_section : road.GetLaneSections()) {
984  for (auto &&lane_pair : lane_section.GetLanes()) {
985  lane_meshes.push_back(mesh_factory.Generate(lane_pair.second));
986  }
987  }
988  }
989  if(smooth_junctions) {
990  out_mesh += *mesh_factory.MergeAndSmooth(lane_meshes);
991  } else {
992  geom::Mesh junction_mesh;
993  for(auto& lane : lane_meshes) {
994  junction_mesh += *lane;
995  }
996  out_mesh += junction_mesh;
997  }
998  }
999 
1000  return out_mesh;
1001  }
1002 
1003  std::vector<std::unique_ptr<geom::Mesh>> Map::GenerateChunkedMesh(
1004  const rpc::OpendriveGenerationParameters& params) const {
1005  geom::MeshFactory mesh_factory(params);
1006  std::vector<std::unique_ptr<geom::Mesh>> out_mesh_list;
1007 
1008  std::unordered_map<JuncId, geom::Mesh> junction_map;
1009  for (auto &&pair : _data.GetRoads()) {
1010  const auto &road = pair.second;
1011  if (!road.IsJunction()) {
1012  std::vector<std::unique_ptr<geom::Mesh>> road_mesh_list =
1013  mesh_factory.GenerateAllWithMaxLen(road);
1014 
1015  out_mesh_list.insert(
1016  out_mesh_list.end(),
1017  std::make_move_iterator(road_mesh_list.begin()),
1018  std::make_move_iterator(road_mesh_list.end()));
1019  }
1020  }
1021 
1022  // Generate roads within junctions and smooth them
1023  for (const auto &junc_pair : _data.GetJunctions()) {
1024  const auto &junction = junc_pair.second;
1025  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1026  std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1027  for(const auto &connection_pair : junction.GetConnections()) {
1028  const auto &connection = connection_pair.second;
1029  const auto &road = _data.GetRoads().at(connection.connecting_road);
1030  for (auto &&lane_section : road.GetLaneSections()) {
1031  for (auto &&lane_pair : lane_section.GetLanes()) {
1032  const auto &lane = lane_pair.second;
1033  if (lane.GetType() != road::Lane::LaneType::Sidewalk) {
1034  lane_meshes.push_back(mesh_factory.Generate(lane));
1035  } else {
1036  sidewalk_lane_meshes.push_back(mesh_factory.Generate(lane));
1037  }
1038  }
1039  }
1040  }
1041  if(params.smooth_junctions) {
1042  auto merged_mesh = mesh_factory.MergeAndSmooth(lane_meshes);
1043  for(auto& lane : sidewalk_lane_meshes) {
1044  *merged_mesh += *lane;
1045  }
1046  out_mesh_list.push_back(std::move(merged_mesh));
1047  } else {
1048  std::unique_ptr<geom::Mesh> junction_mesh = std::make_unique<geom::Mesh>();
1049  for(auto& lane : lane_meshes) {
1050  *junction_mesh += *lane;
1051  }
1052  for(auto& lane : sidewalk_lane_meshes) {
1053  *junction_mesh += *lane;
1054  }
1055  out_mesh_list.push_back(std::move(junction_mesh));
1056  }
1057  }
1058 
1059  auto min_pos = geom::Vector2D(
1060  out_mesh_list.front()->GetVertices().front().x,
1061  out_mesh_list.front()->GetVertices().front().y);
1062  auto max_pos = min_pos;
1063  for (auto & mesh : out_mesh_list) {
1064  auto vertex = mesh->GetVertices().front();
1065  min_pos.x = std::min(min_pos.x, vertex.x);
1066  min_pos.y = std::min(min_pos.y, vertex.y);
1067  max_pos.x = std::max(max_pos.x, vertex.x);
1068  max_pos.y = std::max(max_pos.y, vertex.y);
1069  }
1070  size_t mesh_amount_x = static_cast<size_t>((max_pos.x - min_pos.x)/params.max_road_length) + 1;
1071  size_t mesh_amount_y = static_cast<size_t>((max_pos.y - min_pos.y)/params.max_road_length) + 1;
1072  std::vector<std::unique_ptr<geom::Mesh>> result;
1073  result.reserve(mesh_amount_x*mesh_amount_y);
1074  for (size_t i = 0; i < mesh_amount_x*mesh_amount_y; ++i) {
1075  result.emplace_back(std::make_unique<geom::Mesh>());
1076  }
1077  for (auto & mesh : out_mesh_list) {
1078  auto vertex = mesh->GetVertices().front();
1079  size_t x_pos = static_cast<size_t>((vertex.x - min_pos.x) / params.max_road_length);
1080  size_t y_pos = static_cast<size_t>((vertex.y - min_pos.y) / params.max_road_length);
1081  *(result[x_pos + mesh_amount_x*y_pos]) += *mesh;
1082  }
1083 
1084  return result;
1085  }
1086 
1088  geom::Mesh out_mesh;
1089 
1090  // Get the crosswalk vertices for the current map
1091  const std::vector<geom::Location> crosswalk_vertex = GetAllCrosswalkZones();
1092  if (crosswalk_vertex.empty()) {
1093  return out_mesh;
1094  }
1095 
1096  // Create a a list of triangle fans with material "crosswalk"
1097  out_mesh.AddMaterial("crosswalk");
1098  size_t start_vertex_index = 0;
1099  size_t i = 0;
1100  std::vector<geom::Vector3D> vertices;
1101  // Iterate the vertices until a repeated one is found, this indicates
1102  // the triangle fan is done and another one must start
1103  do {
1104  // Except for the first iteration && triangle fan done
1105  if (i != 0 && crosswalk_vertex[start_vertex_index] == crosswalk_vertex[i]) {
1106  // Create the actual fan
1107  out_mesh.AddTriangleFan(vertices);
1108  vertices.clear();
1109  // End the loop if i reached the end of the vertex list
1110  if (i >= crosswalk_vertex.size() - 1) {
1111  break;
1112  }
1113  start_vertex_index = ++i;
1114  }
1115  // Append a new Vector3D that will be added to the triangle fan
1116  vertices.push_back(crosswalk_vertex[i++]);
1117  } while (i < crosswalk_vertex.size());
1118 
1119  out_mesh.EndMaterial();
1120  return out_mesh;
1121  }
1122 
1123 } // namespace road
1124 } // 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:416
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:956
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:715
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:554
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:685
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:1087
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:422
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:785
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:623
double GetDistance() const
Definition: Lane.cpp:46
Junction * GetJunction(JuncId id)
Definition: road/Map.cpp:948
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:99
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:404
void CreateRtree()
Definition: road/Map.cpp:838
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
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:521
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:1003
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:597
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:503
double GetRemainingLength(const Lane &lane, double current_s)
Definition: road/Map.cpp:830
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:653
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:795
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:485
void AddElementToRtreeAndUpdateTransforms(std::vector< Rtree::TreeElement > &rtree_elements, geom::Transform &current_transform, Waypoint &current_waypoint, Waypoint &next_waypoint)
Definition: road/Map.cpp:816
std::vector< Waypoint > GenerateWaypoints(double approx_distance) const
Generate all the waypoints in map separated by approx_distance.
Definition: road/Map.cpp:609
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:587
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
Definition: road/Map.cpp:698
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