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/geom/Vector3D.h"
11 #include "carla/road/MeshFactory.h"
12 #include "carla/road/Deformation.h"
22 
24 
25 #include <vector>
26 #include <unordered_map>
27 #include <stdexcept>
28 #include <chrono>
29 #include <thread>
30 #include <iomanip>
31 #include <cmath>
32 
33 namespace carla {
34 namespace road {
35 
36  using namespace carla::road::element;
37 
38  /// We use this epsilon to shift the waypoints away from the edges of the lane
39  /// sections to avoid floating point precision errors.
40  static constexpr double EPSILON = 10.0 * std::numeric_limits<double>::epsilon();
41 
42  // ===========================================================================
43  // -- Static local methods ---------------------------------------------------
44  // ===========================================================================
45 
46  template <typename T>
47  static std::vector<T> ConcatVectors(std::vector<T> dst, std::vector<T> src) {
48  if (src.size() > dst.size()) {
49  return ConcatVectors(src, dst);
50  }
51  dst.insert(
52  dst.end(),
53  std::make_move_iterator(src.begin()),
54  std::make_move_iterator(src.end()));
55  return dst;
56  }
57 
58  static double GetDistanceAtStartOfLane(const Lane &lane) {
59  if (lane.GetId() <= 0) {
60  return lane.GetDistance() + 10.0 * EPSILON;
61  } else {
62  return lane.GetDistance() + lane.GetLength() - 10.0 * EPSILON;
63  }
64  }
65 
66  static double GetDistanceAtEndOfLane(const Lane &lane) {
67  if (lane.GetId() > 0) {
68  return lane.GetDistance() + 10.0 * EPSILON;
69  } else {
70  return lane.GetDistance() + lane.GetLength() - 10.0 * EPSILON;
71  }
72  }
73 
74  /// Return a waypoint for each drivable lane on @a lane_section.
75  template <typename FuncT>
77  RoadId road_id,
78  const LaneSection &lane_section,
79  double distance,
80  FuncT &&func) {
81  for (const auto &pair : lane_section.GetLanes()) {
82  const auto &lane = pair.second;
83  if (lane.GetId() == 0) {
84  continue;
85  }
86  if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(Lane::LaneType::Driving)) > 0) {
87  std::forward<FuncT>(func)(Waypoint{
88  road_id,
89  lane_section.GetId(),
90  lane.GetId(),
91  distance < 0.0 ? GetDistanceAtStartOfLane(lane) : distance});
92  }
93  }
94  }
95 
96  template <typename FuncT>
97  static void ForEachLaneImpl(
98  RoadId road_id,
99  const LaneSection &lane_section,
100  double distance,
101  Lane::LaneType lane_type,
102  FuncT &&func) {
103  for (const auto &pair : lane_section.GetLanes()) {
104  const auto &lane = pair.second;
105  if (lane.GetId() == 0) {
106  continue;
107  }
108  if ((static_cast<int32_t>(lane.GetType()) & static_cast<int32_t>(lane_type)) > 0) {
109  std::forward<FuncT>(func)(Waypoint{
110  road_id,
111  lane_section.GetId(),
112  lane.GetId(),
113  distance < 0.0 ? GetDistanceAtStartOfLane(lane) : distance});
114  }
115  }
116  }
117 
118  /// Return a waypoint for each drivable lane on each lane section of @a road.
119  template <typename FuncT>
120  static void ForEachDrivableLane(const Road &road, FuncT &&func) {
121  for (const auto &lane_section : road.GetLaneSections()) {
123  road.GetId(),
124  lane_section,
125  -1.0, // At start of the lane
126  std::forward<FuncT>(func));
127  }
128  }
129 
130  /// Return a waypoint for each lane of the specified type on each lane section of @a road.
131  template <typename FuncT>
132  static void ForEachLane(const Road &road, Lane::LaneType lane_type, FuncT &&func) {
133  for (const auto &lane_section : road.GetLaneSections()) {
135  road.GetId(),
136  lane_section,
137  -1.0, // At start of the lane
138  lane_type,
139  std::forward<FuncT>(func));
140  }
141  }
142 
143  /// Return a waypoint for each drivable lane at @a distance on @a road.
144  template <typename FuncT>
145  static void ForEachDrivableLaneAt(const Road &road, double distance, FuncT &&func) {
146  for (const auto &lane_section : road.GetLaneSectionsAt(distance)) {
148  road.GetId(),
149  lane_section,
150  distance,
151  std::forward<FuncT>(func));
152  }
153  }
154 
155  /// Assumes road_id and section_id are valid.
156  static bool IsLanePresent(const MapData &data, Waypoint waypoint) {
157  const auto &section = data.GetRoad(waypoint.road_id).GetLaneSectionById(waypoint.section_id);
158  return section.ContainsLane(waypoint.lane_id);
159  }
160 
161  // ===========================================================================
162  // -- Map: Geometry ----------------------------------------------------------
163  // ===========================================================================
164 
165  boost::optional<Waypoint> Map::GetClosestWaypointOnRoad(
166  const geom::Location &pos,
167  int32_t lane_type) const {
168  std::vector<Rtree::TreeElement> query_result =
169  _rtree.GetNearestNeighboursWithFilter(Rtree::BPoint(pos.x, pos.y, pos.z),
170  [&](Rtree::TreeElement const &element) {
171  const Lane &lane = GetLane(element.second.first);
172  return (lane_type & static_cast<int32_t>(lane.GetType())) > 0;
173  });
174 
175  if (query_result.size() == 0) {
176  return boost::optional<Waypoint>{};
177  }
178 
179  Rtree::BSegment segment = query_result.front().first;
180  Rtree::BPoint s1 = segment.first;
181  Rtree::BPoint s2 = segment.second;
182  auto distance_to_segment = geom::Math::DistanceSegmentToPoint(pos,
183  geom::Vector3D(s1.get<0>(), s1.get<1>(), s1.get<2>()),
184  geom::Vector3D(s2.get<0>(), s2.get<1>(), s2.get<2>()));
185 
186  Waypoint result_start = query_result.front().second.first;
187  Waypoint result_end = query_result.front().second.second;
188 
189  if (result_start.lane_id < 0) {
190  double delta_s = distance_to_segment.first;
191  double final_s = result_start.s + delta_s;
192  if (final_s >= result_end.s) {
193  return result_end;
194  } else if (delta_s <= 0) {
195  return result_start;
196  } else {
197  return GetNext(result_start, delta_s).front();
198  }
199  } else {
200  double delta_s = distance_to_segment.first;
201  double final_s = result_start.s - delta_s;
202  if (final_s <= result_end.s) {
203  return result_end;
204  } else if (delta_s <= 0) {
205  return result_start;
206  } else {
207  return GetNext(result_start, delta_s).front();
208  }
209  }
210  }
211 
212  boost::optional<Waypoint> Map::GetWaypoint(
213  const geom::Location &pos,
214  int32_t lane_type) const {
215  boost::optional<Waypoint> w = GetClosestWaypointOnRoad(pos, lane_type);
216 
217  if (!w.has_value()) {
218  return w;
219  }
220 
221  const auto dist = geom::Math::Distance2D(ComputeTransform(*w).location, pos);
222  const auto lane_width_info = GetLane(*w).GetInfo<RoadInfoLaneWidth>(w->s);
223  const auto half_lane_width =
224  lane_width_info->GetPolynomial().Evaluate(w->s) * 0.5;
225 
226  if (dist < half_lane_width) {
227  return w;
228  }
229 
230  return boost::optional<Waypoint>{};
231  }
232 
233  boost::optional<Waypoint> Map::GetWaypoint(
234  RoadId road_id,
235  LaneId lane_id,
236  float s) const {
237 
238  // define the waypoint with the known parameters
239  Waypoint waypoint;
240  waypoint.road_id = road_id;
241  waypoint.lane_id = lane_id;
242  waypoint.s = s;
243 
244  // check the road
245  if (!_data.ContainsRoad(waypoint.road_id)) {
246  return boost::optional<Waypoint>{};
247  }
248  const Road &road = _data.GetRoad(waypoint.road_id);
249 
250  // check the 's' distance
251  if (s < 0.0f || s >= road.GetLength()) {
252  return boost::optional<Waypoint>{};
253  }
254 
255  // check the section
256  bool lane_found = false;
257  for (auto &section : road.GetLaneSectionsAt(s)) {
258  if (section.ContainsLane(lane_id)) {
259  waypoint.section_id = section.GetId();
260  lane_found = true;
261  break;
262  }
263  }
264 
265  // check the lane id
266  if (!lane_found) {
267  return boost::optional<Waypoint>{};
268  }
269 
270  return waypoint;
271  }
272 
274  return GetLane(waypoint).ComputeTransform(waypoint.s);
275  }
276 
277  // ===========================================================================
278  // -- Map: Road information --------------------------------------------------
279  // ===========================================================================
280 
281  Lane::LaneType Map::GetLaneType(const Waypoint waypoint) const {
282  return GetLane(waypoint).GetType();
283  }
284 
285  double Map::GetLaneWidth(const Waypoint waypoint) const {
286  const auto s = waypoint.s;
287 
288  const auto &lane = GetLane(waypoint);
289  RELEASE_ASSERT(lane.GetRoad() != nullptr);
290  RELEASE_ASSERT(s <= lane.GetRoad()->GetLength());
291 
292  const auto lane_width_info = lane.GetInfo<RoadInfoLaneWidth>(s);
293  RELEASE_ASSERT(lane_width_info != nullptr);
294 
295  return lane_width_info->GetPolynomial().Evaluate(s);
296  }
297 
299  return _data.GetRoad(road_id).GetJunctionId();
300  }
301 
302  bool Map::IsJunction(RoadId road_id) const {
303  return _data.GetRoad(road_id).IsJunction();
304  }
305 
306  std::pair<const RoadInfoMarkRecord *, const RoadInfoMarkRecord *>
307  Map::GetMarkRecord(const Waypoint waypoint) const {
308  // if lane Id is 0, just return a pair of nulls
309  if (waypoint.lane_id == 0)
310  return std::make_pair(nullptr, nullptr);
311 
312  const auto s = waypoint.s;
313 
314  const auto &current_lane = GetLane(waypoint);
315  RELEASE_ASSERT(current_lane.GetRoad() != nullptr);
316  RELEASE_ASSERT(s <= current_lane.GetRoad()->GetLength());
317 
318  const auto inner_lane_id = waypoint.lane_id < 0 ?
319  waypoint.lane_id + 1 :
320  waypoint.lane_id - 1;
321 
322  const auto &inner_lane = current_lane.GetRoad()->GetLaneById(waypoint.section_id, inner_lane_id);
323 
324  auto current_lane_info = current_lane.GetInfo<RoadInfoMarkRecord>(s);
325  auto inner_lane_info = inner_lane.GetInfo<RoadInfoMarkRecord>(s);
326 
327  return std::make_pair(current_lane_info, inner_lane_info);
328  }
329 
330  std::vector<Map::SignalSearchData> Map::GetSignalsInDistance(
331  Waypoint waypoint, double distance, bool stop_at_junction) const {
332 
333  const auto &lane = GetLane(waypoint);
334  const bool forward = (waypoint.lane_id <= 0);
335  const double signed_distance = forward ? distance : -distance;
336  const double relative_s = waypoint.s - lane.GetDistance();
337  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
338  DEBUG_ASSERT(remaining_lane_length >= 0.0);
339 
340  auto &road =_data.GetRoad(waypoint.road_id);
341  std::vector<SignalSearchData> result;
342 
343  // If after subtracting the distance we are still in the same lane, return
344  // same waypoint with the extra distance.
345  if (distance <= remaining_lane_length) {
346  auto signals = road.GetInfosInRange<RoadInfoSignal>(
347  waypoint.s, waypoint.s + signed_distance);
348  for(auto* signal : signals){
349  double distance_to_signal = 0;
350  if (waypoint.lane_id < 0){
351  distance_to_signal = signal->GetDistance() - waypoint.s;
352  } else {
353  distance_to_signal = waypoint.s - signal->GetDistance();
354  }
355  // check that the signal affects the waypoint
356  bool is_valid = false;
357  for (auto &validity : signal->GetValidities()) {
358  if (waypoint.lane_id >= validity._from_lane &&
359  waypoint.lane_id <= validity._to_lane) {
360  is_valid = true;
361  break;
362  }
363  }
364  if(!is_valid){
365  continue;
366  }
367  if (distance_to_signal == 0) {
368  result.emplace_back(SignalSearchData
369  {signal, waypoint,
370  distance_to_signal});
371  } else {
372  result.emplace_back(SignalSearchData
373  {signal, GetNext(waypoint, distance_to_signal).front(),
374  distance_to_signal});
375  }
376 
377  }
378  return result;
379  }
380  const double signed_remaining_length = forward ? remaining_lane_length : -remaining_lane_length;
381 
382  //result = road.GetInfosInRange<RoadInfoSignal>(waypoint.s, waypoint.s + signed_remaining_length);
383  auto signals = road.GetInfosInRange<RoadInfoSignal>(
384  waypoint.s, waypoint.s + signed_remaining_length);
385  for(auto* signal : signals){
386  double distance_to_signal = 0;
387  if (waypoint.lane_id < 0){
388  distance_to_signal = signal->GetDistance() - waypoint.s;
389  } else {
390  distance_to_signal = waypoint.s - signal->GetDistance();
391  }
392  // check that the signal affects the waypoint
393  bool is_valid = false;
394  for (auto &validity : signal->GetValidities()) {
395  if (waypoint.lane_id >= validity._from_lane &&
396  waypoint.lane_id <= validity._to_lane) {
397  is_valid = true;
398  break;
399  }
400  }
401  if(!is_valid){
402  continue;
403  }
404  if (distance_to_signal == 0) {
405  result.emplace_back(SignalSearchData
406  {signal, waypoint,
407  distance_to_signal});
408  } else {
409  result.emplace_back(SignalSearchData
410  {signal, GetNext(waypoint, distance_to_signal).front(),
411  distance_to_signal});
412  }
413  }
414  // If we run out of remaining_lane_length we have to go to the successors.
415  for (auto &successor : GetSuccessors(waypoint)) {
416  if(_data.GetRoad(successor.road_id).IsJunction() && stop_at_junction){
417  continue;
418  }
419  auto& sucessor_lane = _data.GetRoad(successor.road_id).
420  GetLaneByDistance(successor.s, successor.lane_id);
421  if (successor.lane_id < 0) {
422  successor.s = sucessor_lane.GetDistance();
423  } else {
424  successor.s = sucessor_lane.GetDistance() + sucessor_lane.GetLength();
425  }
426  auto sucessor_signals = GetSignalsInDistance(
427  successor, distance - remaining_lane_length, stop_at_junction);
428  for(auto& signal : sucessor_signals){
429  signal.accumulated_s += remaining_lane_length;
430  }
431  result = ConcatVectors(result, sucessor_signals);
432  }
433  return result;
434  }
435 
436  std::vector<const element::RoadInfoSignal*>
438  std::vector<const element::RoadInfoSignal*> result;
439  for (const auto& road_pair : _data.GetRoads()) {
440  const auto &road = road_pair.second;
441  auto road_infos = road.GetInfos<element::RoadInfoSignal>();
442  for(const auto* road_info : road_infos) {
443  result.push_back(road_info);
444  }
445  }
446  return result;
447  }
448 
449  std::vector<LaneMarking> Map::CalculateCrossedLanes(
450  const geom::Location &origin,
451  const geom::Location &destination) const {
452  return LaneCrossingCalculator::Calculate(*this, origin, destination);
453  }
454 
455  std::vector<geom::Location> Map::GetAllCrosswalkZones() const {
456  std::vector<geom::Location> result;
457 
458  for (const auto &pair : _data.GetRoads()) {
459  const auto &road = pair.second;
460  std::vector<const RoadInfoCrosswalk *> crosswalks = road.GetInfos<RoadInfoCrosswalk>();
461  if (crosswalks.size() > 0) {
462  for (auto crosswalk : crosswalks) {
463  // waypoint only at start position
464  std::vector<geom::Location> points;
465  Waypoint waypoint;
466  geom::Transform base;
467  for (const auto &section : road.GetLaneSectionsAt(crosswalk->GetS())) {
468  // get the section with the center lane
469  for (const auto &lane : section.GetLanes()) {
470  // is the center line
471  if (lane.first == 0) {
472  // get the center point
473  waypoint.road_id = pair.first;
474  waypoint.section_id = section.GetId();
475  waypoint.lane_id = 0;
476  waypoint.s = crosswalk->GetS();
477  base = ComputeTransform(waypoint);
478  }
479  }
480  }
481 
482  // move perpendicular ('t')
483  geom::Transform pivot = base;
484  pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
485  pivot.rotation.yaw -= 90; // move perpendicular to 's' for the lateral offset
486  geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f);
487  pivot.TransformPoint(v);
488  // restore pivot position and orientation
489  pivot = base;
490  pivot.location = v;
491  pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
492 
493  // calculate all the corners
494  for (auto corner : crosswalk->GetPoints()) {
495  geom::Vector3D v2(
496  static_cast<float>(corner.u),
497  static_cast<float>(corner.v),
498  static_cast<float>(corner.z));
499  // set the width larger to contact with the sidewalk (in case they have gutter area)
500  if (corner.u < 0) {
501  v2.x -= 1.0f;
502  } else {
503  v2.x += 1.0f;
504  }
505  pivot.TransformPoint(v2);
506  result.push_back(v2);
507  }
508  }
509  }
510  }
511  return result;
512  }
513 
514  // ===========================================================================
515  // -- Map: Waypoint generation -----------------------------------------------
516  // ===========================================================================
517 
518  std::vector<Waypoint> Map::GetSuccessors(const Waypoint waypoint) const {
519  const auto &next_lanes = GetLane(waypoint).GetNextLanes();
520  std::vector<Waypoint> result;
521  result.reserve(next_lanes.size());
522  for (auto *next_lane : next_lanes) {
523  RELEASE_ASSERT(next_lane != nullptr);
524  const auto lane_id = next_lane->GetId();
525  RELEASE_ASSERT(lane_id != 0);
526  const auto *section = next_lane->GetLaneSection();
527  RELEASE_ASSERT(section != nullptr);
528  const auto *road = next_lane->GetRoad();
529  RELEASE_ASSERT(road != nullptr);
530  const auto distance = GetDistanceAtStartOfLane(*next_lane);
531  result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
532  }
533  return result;
534  }
535 
536  std::vector<Waypoint> Map::GetPredecessors(const Waypoint waypoint) const {
537  const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes();
538  std::vector<Waypoint> result;
539  result.reserve(prev_lanes.size());
540  for (auto *next_lane : prev_lanes) {
541  RELEASE_ASSERT(next_lane != nullptr);
542  const auto lane_id = next_lane->GetId();
543  RELEASE_ASSERT(lane_id != 0);
544  const auto *section = next_lane->GetLaneSection();
545  RELEASE_ASSERT(section != nullptr);
546  const auto *road = next_lane->GetRoad();
547  RELEASE_ASSERT(road != nullptr);
548  const auto distance = GetDistanceAtEndOfLane(*next_lane);
549  result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
550  }
551  return result;
552  }
553 
554  std::vector<Waypoint> Map::GetNext(
555  const Waypoint waypoint,
556  const double distance) const {
557  RELEASE_ASSERT(distance > 0.0);
558  if (distance <= EPSILON) {
559  return {waypoint};
560  }
561  const auto &lane = GetLane(waypoint);
562  const bool forward = (waypoint.lane_id <= 0);
563  const double signed_distance = forward ? distance : -distance;
564  const double relative_s = waypoint.s - lane.GetDistance();
565  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
566  DEBUG_ASSERT(remaining_lane_length >= 0.0);
567 
568  // If after subtracting the distance we are still in the same lane, return
569  // same waypoint with the extra distance.
570  if (distance <= remaining_lane_length) {
571  Waypoint result = waypoint;
572  result.s += signed_distance;
573  result.s += forward ? -EPSILON : EPSILON;
574  RELEASE_ASSERT(result.s > 0.0);
575  return { result };
576  }
577 
578  // If we run out of remaining_lane_length we have to go to the successors.
579  std::vector<Waypoint> result;
580  for (const auto &successor : GetSuccessors(waypoint)) {
581  DEBUG_ASSERT(
582  successor.road_id != waypoint.road_id ||
583  successor.section_id != waypoint.section_id ||
584  successor.lane_id != waypoint.lane_id);
585  result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
586  }
587  return result;
588  }
589 
590  std::vector<Waypoint> Map::GetPrevious(
591  const Waypoint waypoint,
592  const double distance) const {
593  RELEASE_ASSERT(distance > 0.0);
594  if (distance <= EPSILON) {
595  return {waypoint};
596  }
597  const auto &lane = GetLane(waypoint);
598  const bool forward = !(waypoint.lane_id <= 0);
599  const double signed_distance = forward ? distance : -distance;
600  const double relative_s = waypoint.s - lane.GetDistance();
601  const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
602  DEBUG_ASSERT(remaining_lane_length >= 0.0);
603 
604  // If after subtracting the distance we are still in the same lane, return
605  // same waypoint with the extra distance.
606  if (distance <= remaining_lane_length) {
607  Waypoint result = waypoint;
608  result.s += signed_distance;
609  result.s += forward ? -EPSILON : EPSILON;
610  RELEASE_ASSERT(result.s > 0.0);
611  return { result };
612  }
613 
614  // If we run out of remaining_lane_length we have to go to the successors.
615  std::vector<Waypoint> result;
616  for (const auto &successor : GetPredecessors(waypoint)) {
617  DEBUG_ASSERT(
618  successor.road_id != waypoint.road_id ||
619  successor.section_id != waypoint.section_id ||
620  successor.lane_id != waypoint.lane_id);
621  result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
622  }
623  return result;
624  }
625 
626  boost::optional<Waypoint> Map::GetRight(Waypoint waypoint) const {
627  RELEASE_ASSERT(waypoint.lane_id != 0);
628  if (waypoint.lane_id > 0) {
629  ++waypoint.lane_id;
630  } else {
631  --waypoint.lane_id;
632  }
633  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
634  }
635 
636  boost::optional<Waypoint> Map::GetLeft(Waypoint waypoint) const {
637  RELEASE_ASSERT(waypoint.lane_id != 0);
638  if (std::abs(waypoint.lane_id) == 1) {
639  waypoint.lane_id *= -1;
640  } else if (waypoint.lane_id > 0) {
641  --waypoint.lane_id;
642  } else {
643  ++waypoint.lane_id;
644  }
645  return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
646  }
647 
648  std::vector<Waypoint> Map::GenerateWaypoints(const double distance) const {
649  RELEASE_ASSERT(distance > 0.0);
650  std::vector<Waypoint> result;
651  for (const auto &pair : _data.GetRoads()) {
652  const auto &road = pair.second;
653  for (double s = EPSILON; s < (road.GetLength() - EPSILON); s += distance) {
654  ForEachDrivableLaneAt(road, s, [&](auto &&waypoint) {
655  result.emplace_back(waypoint);
656  });
657  }
658  }
659  return result;
660  }
661 
662  std::vector<Waypoint> Map::GenerateWaypointsOnRoadEntries(Lane::LaneType lane_type) const {
663  std::vector<Waypoint> result;
664  for (const auto &pair : _data.GetRoads()) {
665  const auto &road = pair.second;
666  // right lanes start at s 0
667  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
668  for (const auto &lane : lane_section.GetLanes()) {
669  // add only the right (negative) lanes
670  if (lane.first < 0 &&
671  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
672  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
673  }
674  }
675  }
676  // left lanes start at s max
677  const auto road_len = road.GetLength();
678  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
679  for (const auto &lane : lane_section.GetLanes()) {
680  // add only the left (positive) lanes
681  if (lane.first > 0 &&
682  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
683  result.emplace_back(
684  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
685  }
686  }
687  }
688  }
689  return result;
690  }
691 
692  std::vector<Waypoint> Map::GenerateWaypointsInRoad(
693  RoadId road_id,
694  Lane::LaneType lane_type) const {
695  std::vector<Waypoint> result;
696  if(_data.GetRoads().count(road_id)) {
697  const auto &road = _data.GetRoads().at(road_id);
698  // right lanes start at s 0
699  for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
700  for (const auto &lane : lane_section.GetLanes()) {
701  // add only the right (negative) lanes
702  if (lane.first < 0 &&
703  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
704  result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
705  }
706  }
707  }
708  // left lanes start at s max
709  const auto road_len = road.GetLength();
710  for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
711  for (const auto &lane : lane_section.GetLanes()) {
712  // add only the left (positive) lanes
713  if (lane.first > 0 &&
714  static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
715  result.emplace_back(
716  Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
717  }
718  }
719  }
720  }
721  return result;
722  }
723 
724  std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology() const {
725  std::vector<std::pair<Waypoint, Waypoint>> result;
726  for (const auto &pair : _data.GetRoads()) {
727  const auto &road = pair.second;
728  ForEachDrivableLane(road, [&](auto &&waypoint) {
729  auto successors = GetSuccessors(waypoint);
730  if (successors.size() == 0){
731  auto distance = static_cast<float>(GetDistanceAtEndOfLane(GetLane(waypoint)));
732  auto last_waypoint = GetWaypoint(waypoint.road_id, waypoint.lane_id, distance);
733  if (last_waypoint.has_value()){
734  result.push_back({waypoint, *last_waypoint});
735  }
736  }
737  else{
738  for (auto &&successor : GetSuccessors(waypoint)) {
739  result.push_back({waypoint, successor});
740  }
741  }
742  });
743  }
744  return result;
745  }
746 
747  std::vector<std::pair<Waypoint, Waypoint>> Map::GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const {
748  std::vector<std::pair<Waypoint, Waypoint>> result;
749  const Junction * junction = GetJunction(id);
750  for(auto &connections : junction->GetConnections()) {
751  const Road &road = _data.GetRoad(connections.second.connecting_road);
752  ForEachLane(road, lane_type, [&](auto &&waypoint) {
753  const auto& lane = GetLane(waypoint);
754  const double final_s = GetDistanceAtEndOfLane(lane);
755  Waypoint lane_end(waypoint);
756  lane_end.s = final_s;
757  result.push_back({waypoint, lane_end});
758  });
759  }
760  return result;
761  }
762 
763  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
765 
766  const float epsilon = 0.0001f; // small delta in the road (set to 0.1
767  // millimeters to prevent numeric errors)
768  const Junction *junction = GetJunction(id);
769  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
770  conflicts;
771 
772  // 2d typedefs
773  typedef boost::geometry::model::point
774  <float, 2, boost::geometry::cs::cartesian> Point2d;
775  typedef boost::geometry::model::segment<Point2d> Segment2d;
776  typedef boost::geometry::model::box<Rtree::BPoint> Box;
777 
778  // box range
779  auto bbox_pos = junction->GetBoundingBox().location;
780  auto bbox_ext = junction->GetBoundingBox().extent;
781  auto min_corner = geom::Vector3D(
782  bbox_pos.x - bbox_ext.x,
783  bbox_pos.y - bbox_ext.y,
784  bbox_pos.z - bbox_ext.z - epsilon);
785  auto max_corner = geom::Vector3D(
786  bbox_pos.x + bbox_ext.x,
787  bbox_pos.y + bbox_ext.y,
788  bbox_pos.z + bbox_ext.z + epsilon);
789  Box box({min_corner.x, min_corner.y, min_corner.z},
790  {max_corner.x, max_corner.y, max_corner.z});
791  auto segments = _rtree.GetIntersections(box);
792 
793  for (size_t i = 0; i < segments.size(); ++i){
794  auto &segment1 = segments[i];
795  auto waypoint1 = segment1.second.first;
796  JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
797  // only segments in the junction
798  if(junc_id1 != id){
799  continue;
800  }
801  Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
802  {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
803  for (size_t j = i + 1; j < segments.size(); ++j){
804  auto &segment2 = segments[j];
805  auto waypoint2 = segment2.second.first;
806  JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
807  // only segments in the junction
808  if(junc_id2 != id){
809  continue;
810  }
811  // discard same road
812  if(waypoint1.road_id == waypoint2.road_id){
813  continue;
814  }
815  Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
816  {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
817 
818  double distance = boost::geometry::distance(seg1, seg2);
819  // better to set distance to lanewidth
820  if(distance > 2.0){
821  continue;
822  }
823  if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
824  conflicts[waypoint1.road_id].insert(waypoint2.road_id);
825  }
826  if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
827  conflicts[waypoint2.road_id].insert(waypoint1.road_id);
828  }
829  }
830  }
831  return conflicts;
832  }
833 
834  const Lane &Map::GetLane(Waypoint waypoint) const {
835  return _data.GetRoad(waypoint.road_id).GetLaneById(waypoint.section_id, waypoint.lane_id);
836  }
837 
838  // ===========================================================================
839  // -- Map: Private functions -------------------------------------------------
840  // ===========================================================================
841 
842  // Adds a new element to the rtree element list using the position of the
843  // waypoints both ends of the segment
845  std::vector<Rtree::TreeElement> &rtree_elements,
846  geom::Transform &current_transform,
847  geom::Transform &next_transform,
848  Waypoint &current_waypoint,
849  Waypoint &next_waypoint) {
850  Rtree::BPoint init =
852  current_transform.location.x,
853  current_transform.location.y,
854  current_transform.location.z);
855  Rtree::BPoint end =
857  next_transform.location.x,
858  next_transform.location.y,
859  next_transform.location.z);
860  rtree_elements.emplace_back(std::make_pair(Rtree::BSegment(init, end),
861  std::make_pair(current_waypoint, next_waypoint)));
862  }
863  // Adds a new element to the rtree element list using the position of the
864  // waypoints, both ends of the segment
866  std::vector<Rtree::TreeElement> &rtree_elements,
867  geom::Transform &current_transform,
868  Waypoint &current_waypoint,
869  Waypoint &next_waypoint) {
870  geom::Transform next_transform = ComputeTransform(next_waypoint);
871  AddElementToRtree(rtree_elements, current_transform, next_transform,
872  current_waypoint, next_waypoint);
873  current_waypoint = next_waypoint;
874  current_transform = next_transform;
875  }
876 
877  // returns the remaining length of the geometry depending on the lane
878  // direction
879  double GetRemainingLength(const Lane &lane, double current_s) {
880  if (lane.GetId() < 0) {
881  return (lane.GetDistance() + lane.GetLength() - current_s);
882  } else {
883  return (current_s - lane.GetDistance());
884  }
885  }
886 
888  const double epsilon = 0.000001; // small delta in the road (set to 1
889  // micrometer to prevent numeric errors)
890  const double min_delta_s = 1; // segments of minimum 1m through the road
891 
892  // 1.8 degrees, maximum angle in a curve to place a segment
893  constexpr double angle_threshold = geom::Math::Pi<double>() / 100.0;
894  // maximum distance of a segment
895  constexpr double max_segment_length = 100.0;
896 
897  // Generate waypoints at start of every lane
898  std::vector<Waypoint> topology;
899  for (const auto &pair : _data.GetRoads()) {
900  const auto &road = pair.second;
901  ForEachLane(road, Lane::LaneType::Any, [&](auto &&waypoint) {
902  if(waypoint.lane_id != 0) {
903  topology.push_back(waypoint);
904  }
905  });
906  }
907 
908  // Container of segments and waypoints
909  std::vector<Rtree::TreeElement> rtree_elements;
910  // Loop through all lanes
911  for (auto &waypoint : topology) {
912  auto &lane_start_waypoint = waypoint;
913 
914  auto current_waypoint = lane_start_waypoint;
915 
916  const Lane &lane = GetLane(current_waypoint);
917 
918  geom::Transform current_transform = ComputeTransform(current_waypoint);
919 
920  // Save computation time in straight lines
921  if (lane.IsStraight()) {
922  double delta_s = min_delta_s;
923  double remaining_length =
924  GetRemainingLength(lane, current_waypoint.s);
925  remaining_length -= epsilon;
926  delta_s = remaining_length;
927  if (delta_s < epsilon) {
928  continue;
929  }
930  auto next = GetNext(current_waypoint, delta_s);
931 
932  RELEASE_ASSERT(next.size() == 1);
933  RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
934  auto next_waypoint = next.front();
935 
936  AddElementToRtreeAndUpdateTransforms(
937  rtree_elements,
938  current_transform,
939  current_waypoint,
940  next_waypoint);
941  // end of lane
942  } else {
943  auto next_waypoint = current_waypoint;
944 
945  // Loop until the end of the lane
946  // Advance in small s-increments
947  while (true) {
948  double delta_s = min_delta_s;
949  double remaining_length =
950  GetRemainingLength(lane, next_waypoint.s);
951  remaining_length -= epsilon;
952  delta_s = std::min(delta_s, remaining_length);
953 
954  if (delta_s < epsilon) {
955  AddElementToRtreeAndUpdateTransforms(
956  rtree_elements,
957  current_transform,
958  current_waypoint,
959  next_waypoint);
960  break;
961  }
962 
963  auto next = GetNext(next_waypoint, delta_s);
964  if (next.size() != 1 ||
965  current_waypoint.section_id != next.front().section_id) {
966  AddElementToRtreeAndUpdateTransforms(
967  rtree_elements,
968  current_transform,
969  current_waypoint,
970  next_waypoint);
971  break;
972  }
973 
974  next_waypoint = next.front();
975  geom::Transform next_transform = ComputeTransform(next_waypoint);
976  double angle = geom::Math::GetVectorAngle(
977  current_transform.GetForwardVector(), next_transform.GetForwardVector());
978 
979  if (std::abs(angle) > angle_threshold ||
980  std::abs(current_waypoint.s - next_waypoint.s) > max_segment_length) {
981  AddElementToRtree(
982  rtree_elements,
983  current_transform,
984  next_transform,
985  current_waypoint,
986  next_waypoint);
987  current_waypoint = next_waypoint;
988  current_transform = next_transform;
989  }
990  }
991  }
992  }
993  // Add segments to Rtree
994  _rtree.InsertElements(rtree_elements);
995  }
996 
998  return _data.GetJunction(id);
999  }
1000 
1001  const Junction* Map::GetJunction(JuncId id) const {
1002  return _data.GetJunction(id);
1003  }
1004 
1006  const double distance,
1007  const float extra_width,
1008  const bool smooth_junctions) const {
1009  RELEASE_ASSERT(distance > 0.0);
1010  geom::MeshFactory mesh_factory;
1011  geom::Mesh out_mesh;
1012 
1013  mesh_factory.road_param.resolution = static_cast<float>(distance);
1014  mesh_factory.road_param.extra_lane_width = extra_width;
1015 
1016  // Generate roads outside junctions
1017  for (auto &&pair : _data.GetRoads()) {
1018  const auto &road = pair.second;
1019  if (road.IsJunction()) {
1020  continue;
1021  }
1022  out_mesh += *mesh_factory.Generate(road);
1023  }
1024 
1025  // Generate roads within junctions and smooth them
1026  for (const auto &junc_pair : _data.GetJunctions()) {
1027  const auto &junction = junc_pair.second;
1028  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1029  for(const auto &connection_pair : junction.GetConnections()) {
1030  const auto &connection = connection_pair.second;
1031  const auto &road = _data.GetRoads().at(connection.connecting_road);
1032  for (auto &&lane_section : road.GetLaneSections()) {
1033  for (auto &&lane_pair : lane_section.GetLanes()) {
1034  lane_meshes.push_back(mesh_factory.Generate(lane_pair.second));
1035  }
1036  }
1037  }
1038  if(smooth_junctions) {
1039  out_mesh += *mesh_factory.MergeAndSmooth(lane_meshes);
1040  } else {
1041  geom::Mesh junction_mesh;
1042  for(auto& lane : lane_meshes) {
1043  junction_mesh += *lane;
1044  }
1045  out_mesh += junction_mesh;
1046  }
1047  }
1048 
1049  return out_mesh;
1050  }
1051 
1052 
1053  std::vector<std::unique_ptr<geom::Mesh>> Map::GenerateChunkedMesh(
1054  const rpc::OpendriveGenerationParameters& params) const {
1055  geom::MeshFactory mesh_factory(params);
1056  std::vector<std::unique_ptr<geom::Mesh>> out_mesh_list;
1057 
1058  std::unordered_map<JuncId, geom::Mesh> junction_map;
1059  for (auto &&pair : _data.GetRoads()) {
1060  const auto &road = pair.second;
1061  if (!road.IsJunction()) {
1062  std::vector<std::unique_ptr<geom::Mesh>> road_mesh_list =
1063  mesh_factory.GenerateAllWithMaxLen(road);
1064 
1065  out_mesh_list.insert(
1066  out_mesh_list.end(),
1067  std::make_move_iterator(road_mesh_list.begin()),
1068  std::make_move_iterator(road_mesh_list.end()));
1069  }
1070  }
1071 
1072  // Generate roads within junctions and smooth them
1073  for (const auto &junc_pair : _data.GetJunctions()) {
1074  const auto &junction = junc_pair.second;
1075  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1076  std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1077  for(const auto &connection_pair : junction.GetConnections()) {
1078  const auto &connection = connection_pair.second;
1079  const auto &road = _data.GetRoads().at(connection.connecting_road);
1080  for (auto &&lane_section : road.GetLaneSections()) {
1081  for (auto &&lane_pair : lane_section.GetLanes()) {
1082  const auto &lane = lane_pair.second;
1083  if (lane.GetType() != road::Lane::LaneType::Sidewalk) {
1084  lane_meshes.push_back(mesh_factory.Generate(lane));
1085  } else {
1086  sidewalk_lane_meshes.push_back(mesh_factory.Generate(lane));
1087  }
1088  }
1089  }
1090  }
1091  if(params.smooth_junctions) {
1092  auto merged_mesh = mesh_factory.MergeAndSmooth(lane_meshes);
1093  for(auto& lane : sidewalk_lane_meshes) {
1094  *merged_mesh += *lane;
1095  }
1096  out_mesh_list.push_back(std::move(merged_mesh));
1097  } else {
1098  std::unique_ptr<geom::Mesh> junction_mesh = std::make_unique<geom::Mesh>();
1099  for(auto& lane : lane_meshes) {
1100  *junction_mesh += *lane;
1101  }
1102  for(auto& lane : sidewalk_lane_meshes) {
1103  *junction_mesh += *lane;
1104  }
1105  out_mesh_list.push_back(std::move(junction_mesh));
1106  }
1107  }
1108 
1109  auto min_pos = geom::Vector2D(
1110  out_mesh_list.front()->GetVertices().front().x,
1111  out_mesh_list.front()->GetVertices().front().y);
1112  auto max_pos = min_pos;
1113  for (auto & mesh : out_mesh_list) {
1114  auto vertex = mesh->GetVertices().front();
1115  min_pos.x = std::min(min_pos.x, vertex.x);
1116  min_pos.y = std::min(min_pos.y, vertex.y);
1117  max_pos.x = std::max(max_pos.x, vertex.x);
1118  max_pos.y = std::max(max_pos.y, vertex.y);
1119  }
1120  size_t mesh_amount_x = static_cast<size_t>((max_pos.x - min_pos.x)/params.max_road_length) + 1;
1121  size_t mesh_amount_y = static_cast<size_t>((max_pos.y - min_pos.y)/params.max_road_length) + 1;
1122  std::vector<std::unique_ptr<geom::Mesh>> result;
1123  result.reserve(mesh_amount_x*mesh_amount_y);
1124  for (size_t i = 0; i < mesh_amount_x*mesh_amount_y; ++i) {
1125  result.emplace_back(std::make_unique<geom::Mesh>());
1126  }
1127  for (auto & mesh : out_mesh_list) {
1128  auto vertex = mesh->GetVertices().front();
1129  size_t x_pos = static_cast<size_t>((vertex.x - min_pos.x) / params.max_road_length);
1130  size_t y_pos = static_cast<size_t>((vertex.y - min_pos.y) / params.max_road_length);
1131  *(result[x_pos + mesh_amount_x*y_pos]) += *mesh;
1132  }
1133 
1134  return result;
1135  }
1136 
1137  std::map<road::Lane::LaneType , std::vector<std::unique_ptr<geom::Mesh>>>
1139  const geom::Vector3D& minpos,
1140  const geom::Vector3D& maxpos) const
1141  {
1142 
1143  geom::MeshFactory mesh_factory(params);
1144  std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> road_out_mesh_list;
1145  std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> junction_out_mesh_list;
1146 
1147  std::thread juntction_thread( &Map::GenerateJunctions, this, mesh_factory, params,
1148  minpos, maxpos, &junction_out_mesh_list);
1149 
1150  const std::vector<RoadId> RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos);
1151 
1152  size_t num_roads = RoadsIDToGenerate.size();
1153  size_t num_roads_per_thread = 30;
1154  size_t num_threads = (num_roads / num_roads_per_thread) + 1;
1155  num_threads = num_threads > 1 ? num_threads : 1;
1156  std::vector<std::thread> workers;
1157  std::mutex write_mutex;
1158  std::cout << "Generating " << std::to_string(num_roads) << " roads" << std::endl;
1159 
1160  for ( size_t i = 0; i < num_threads; ++i ) {
1161  std::thread neworker(
1162  [this, &write_mutex, &mesh_factory, &RoadsIDToGenerate, &road_out_mesh_list, i, num_roads_per_thread]() {
1163  std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> Current =
1164  std::move(GenerateRoadsMultithreaded(mesh_factory, RoadsIDToGenerate,i, num_roads_per_thread ));
1165  std::lock_guard<std::mutex> guard(write_mutex);
1166  for ( auto&& pair : Current ) {
1167  if (road_out_mesh_list.find(pair.first) != road_out_mesh_list.end()) {
1168  road_out_mesh_list[pair.first].insert(road_out_mesh_list[pair.first].end(),
1169  std::make_move_iterator(pair.second.begin()),
1170  std::make_move_iterator(pair.second.end()));
1171  } else {
1172  road_out_mesh_list[pair.first] = std::move(pair.second);
1173  }
1174  }
1175  });
1176  workers.push_back(std::move(neworker));
1177  }
1178 
1179  for (size_t i = 0; i < workers.size(); ++i) {
1180  workers[i].join();
1181  }
1182  workers.clear();
1183  for (size_t i = 0; i < workers.size(); ++i) {
1184  if (workers[i].joinable()) {
1185  workers[i].join();
1186  }
1187  }
1188 
1189  juntction_thread.join();
1190  for (auto&& pair : junction_out_mesh_list) {
1191  if (road_out_mesh_list.find(pair.first) != road_out_mesh_list.end())
1192  {
1193  road_out_mesh_list[pair.first].insert(road_out_mesh_list[pair.first].end(),
1194  std::make_move_iterator(pair.second.begin()),
1195  std::make_move_iterator(pair.second.end()));
1196  }
1197  else
1198  {
1199  road_out_mesh_list[pair.first] = std::move(pair.second);
1200  }
1201  }
1202  std::cout << "Generated " << std::to_string(num_roads) << " roads" << std::endl;
1203 
1204  return road_out_mesh_list;
1205  }
1206 
1207  std::vector<std::pair<geom::Transform, std::string>> Map::GetTreesTransform(
1208  const geom::Vector3D& minpos,
1209  const geom::Vector3D& maxpos,
1210  float distancebetweentrees,
1211  float distancefromdrivinglineborder,
1212  float s_offset) const {
1213 
1214  std::vector<std::pair<geom::Transform, std::string>> transforms;
1215 
1216  const std::vector<RoadId> RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos);
1217  for ( RoadId id : RoadsIDToGenerate ) {
1218  const auto& road = _data.GetRoads().at(id);
1219  if (!road.IsJunction()) {
1220  for (auto &&lane_section : road.GetLaneSections()) {
1221  LaneId min_lane = 0;
1222  for (auto &pairlane : lane_section.GetLanes()) {
1223  if (min_lane > pairlane.first && pairlane.second.GetType() == Lane::LaneType::Driving) {
1224  min_lane = pairlane.first;
1225  }
1226  }
1227 
1228  const road::Lane* lane = lane_section.GetLane(min_lane);
1229  if( lane ) {
1230  double s_current = lane_section.GetDistance() + s_offset;
1231  const double s_end = lane_section.GetDistance() + lane_section.GetLength();
1232  while(s_current < s_end){
1233  if(lane->GetWidth(s_current) != 0.0f){
1234  const auto edges = lane->GetCornerPositions(s_current, 0);
1235  geom::Vector3D director = edges.second - edges.first;
1236  geom::Vector3D treeposition = edges.first - director.MakeUnitVector() * distancefromdrivinglineborder;
1237  geom::Transform lanetransform = lane->ComputeTransform(s_current);
1238  geom::Transform treeTransform(treeposition, lanetransform.rotation);
1240  transforms.push_back(std::make_pair(treeTransform,roadinfo->GetType()));
1241  }
1242  s_current += distancebetweentrees;
1243  }
1244 
1245  }
1246  }
1247  }
1248  }
1249  return transforms;
1250  }
1251 
1253  geom::Mesh out_mesh;
1254 
1255  // Get the crosswalk vertices for the current map
1256  const std::vector<geom::Location> crosswalk_vertex = GetAllCrosswalkZones();
1257  if (crosswalk_vertex.empty()) {
1258  return out_mesh;
1259  }
1260 
1261  // Create a a list of triangle fans with material "crosswalk"
1262  out_mesh.AddMaterial("crosswalk");
1263  size_t start_vertex_index = 0;
1264  size_t i = 0;
1265  std::vector<geom::Vector3D> vertices;
1266  // Iterate the vertices until a repeated one is found, this indicates
1267  // the triangle fan is done and another one must start
1268  do {
1269  // Except for the first iteration && triangle fan done
1270  if (i != 0 && crosswalk_vertex[start_vertex_index] == crosswalk_vertex[i]) {
1271  // Create the actual fan
1272  out_mesh.AddTriangleFan(vertices);
1273  vertices.clear();
1274  // End the loop if i reached the end of the vertex list
1275  if (i >= crosswalk_vertex.size() - 1) {
1276  break;
1277  }
1278  start_vertex_index = ++i;
1279  }
1280  // Append a new Vector3D that will be added to the triangle fan
1281  vertices.push_back(crosswalk_vertex[i++]);
1282  } while (i < crosswalk_vertex.size());
1283 
1284  out_mesh.EndMaterial();
1285  return out_mesh;
1286  }
1287 
1288  /// Buids a list of meshes related with LineMarkings
1289  std::vector<std::unique_ptr<geom::Mesh>> Map::GenerateLineMarkings(
1290  const rpc::OpendriveGenerationParameters& params,
1291  const geom::Vector3D& minpos,
1292  const geom::Vector3D& maxpos,
1293  std::vector<std::string>& outinfo ) const
1294  {
1295  std::vector<std::unique_ptr<geom::Mesh>> LineMarks;
1296  geom::MeshFactory mesh_factory(params);
1297 
1298  const std::vector<RoadId> RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos);
1299  for ( RoadId id : RoadsIDToGenerate ) {
1300  const auto& road = _data.GetRoads().at(id);
1301  if (!road.IsJunction()) {
1302  mesh_factory.GenerateLaneMarkForRoad(road, LineMarks, outinfo);
1303  }
1304  }
1305 
1306  return std::move(LineMarks);
1307  }
1308 
1309  std::vector<carla::geom::BoundingBox> Map::GetJunctionsBoundingBoxes() const {
1310  std::vector<carla::geom::BoundingBox> returning;
1311  for ( const auto& junc_pair : _data.GetJunctions() ) {
1312  const auto& junction = junc_pair.second;
1313  float box_extraextension_factor = 1.5f;
1314  carla::geom::BoundingBox bb = junction.GetBoundingBox();
1315  bb.extent *= box_extraextension_factor;
1316  returning.push_back(bb);
1317  }
1318  return returning;
1319  }
1320 
1321  inline float Map::GetZPosInDeformation(float posx, float posy) const {
1322  return geom::deformation::GetZPosInDeformation(posx, posy) +
1324  }
1325 
1326  std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>>
1328  const std::vector<RoadId>& RoadsId,
1329  const size_t index, const size_t number_of_roads_per_thread) const
1330  {
1331  std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> out;
1332 
1333  size_t start = index * number_of_roads_per_thread;
1334  size_t endoffset = (index+1) * number_of_roads_per_thread;
1335  size_t end = RoadsId.size();
1336 
1337  for (int i = start; i < endoffset && i < end; ++i) {
1338  const auto& road = _data.GetRoads().at(RoadsId[i]);
1339  if (!road.IsJunction()) {
1340  mesh_factory.GenerateAllOrderedWithMaxLen(road, out);
1341  }
1342  }
1343  std::cout << "Generated roads from " + std::to_string(index * number_of_roads_per_thread) + " to " + std::to_string((index+1) * number_of_roads_per_thread ) << std::endl;
1344  return out;
1345  }
1346 
1348  const rpc::OpendriveGenerationParameters& params,
1349  const geom::Vector3D& minpos,
1350  const geom::Vector3D& maxpos,
1351  std::map<road::Lane::LaneType,
1352  std::vector<std::unique_ptr<geom::Mesh>>>* junction_out_mesh_list) const {
1353 
1354  std::vector<JuncId> JunctionsToGenerate = FilterJunctionsByPosition(minpos, maxpos);
1355  size_t num_junctions = JunctionsToGenerate.size();
1356  std::cout << "Generating " << std::to_string(num_junctions) << " junctions" << std::endl;
1357  size_t junctionindex = 0;
1358  size_t num_junctions_per_thread = 5;
1359  size_t num_threads = (num_junctions / num_junctions_per_thread) + 1;
1360  num_threads = num_threads > 1 ? num_threads : 1;
1361  std::vector<std::thread> workers;
1362  std::mutex write_mutex;
1363 
1364  for ( size_t i = 0; i < num_threads; ++i ) {
1365  std::thread neworker(
1366  [this, &write_mutex, &mesh_factory, &junction_out_mesh_list, JunctionsToGenerate, i, num_junctions_per_thread, num_junctions]() {
1367  std::map<road::Lane::LaneType,
1368  std::vector<std::unique_ptr<geom::Mesh>>> junctionsofthisthread;
1369 
1370  size_t minimum = 0;
1371  if( (i + 1) * num_junctions_per_thread < num_junctions ){
1372  minimum = (i + 1) * num_junctions_per_thread;
1373  }else{
1374  minimum = num_junctions;
1375  }
1376  std::cout << "Generating Junctions between " << std::to_string(i * num_junctions_per_thread) << " and " << std::to_string(minimum) << std::endl;
1377 
1378  for ( size_t junctionindex = i * num_junctions_per_thread;
1379  junctionindex < minimum;
1380  ++junctionindex )
1381  {
1382  GenerateSingleJunction(mesh_factory, JunctionsToGenerate[junctionindex], &junctionsofthisthread);
1383  }
1384  std::cout << "Generated Junctions between " << std::to_string(i * num_junctions_per_thread) << " and " << std::to_string(minimum) << std::endl;
1385  std::lock_guard<std::mutex> guard(write_mutex);
1386  for ( auto&& pair : junctionsofthisthread ) {
1387  if ((*junction_out_mesh_list).find(pair.first) != (*junction_out_mesh_list).end()) {
1388  (*junction_out_mesh_list)[pair.first].insert((*junction_out_mesh_list)[pair.first].end(),
1389  std::make_move_iterator(pair.second.begin()),
1390  std::make_move_iterator(pair.second.end()));
1391  } else {
1392  (*junction_out_mesh_list)[pair.first] = std::move(pair.second);
1393  }
1394  }
1395  });
1396  workers.push_back(std::move(neworker));
1397  }
1398 
1399  for (size_t i = 0; i < workers.size(); ++i) {
1400  workers[i].join();
1401  }
1402  workers.clear();
1403  for (size_t i = 0; i < workers.size(); ++i) {
1404  if (workers[i].joinable()) {
1405  workers[i].join();
1406  }
1407  }
1408  }
1409 
1410  std::vector<JuncId> Map::FilterJunctionsByPosition( const geom::Vector3D& minpos,
1411  const geom::Vector3D& maxpos ) const {
1412 
1413  std::cout << "Filtered from " + std::to_string(_data.GetJunctions().size() ) + " junctions " << std::endl;
1414  std::vector<JuncId> ToReturn;
1415  for( auto& junction : _data.GetJunctions() ){
1416  geom::Location junctionLocation = junction.second.GetBoundingBox().location;
1417  if( minpos.x < junctionLocation.x && junctionLocation.x < maxpos.x &&
1418  minpos.y > junctionLocation.y && junctionLocation.y > maxpos.y ) {
1419  ToReturn.push_back(junction.first);
1420  }
1421  }
1422  std::cout << "To " + std::to_string(ToReturn.size() ) + " junctions " << std::endl;
1423 
1424  return ToReturn;
1425  }
1426 
1427  std::vector<RoadId> Map::FilterRoadsByPosition( const geom::Vector3D& minpos,
1428  const geom::Vector3D& maxpos ) const {
1429 
1430  std::vector<RoadId> ToReturn;
1431  std::cout << "Filtered from " + std::to_string(_data.GetRoads().size() ) + " roads " << std::endl;
1432  for( auto& road : _data.GetRoads() ){
1433  auto &&lane_section = (*road.second.GetLaneSections().begin());
1434  const road::Lane* lane = lane_section.GetLane(-1);
1435  if( lane ) {
1436  const double s_check = lane_section.GetDistance() + lane_section.GetLength() * 0.5;
1437  geom::Location roadLocation = lane->ComputeTransform(s_check).location;
1438  if( minpos.x < roadLocation.x && roadLocation.x < maxpos.x &&
1439  minpos.y > roadLocation.y && roadLocation.y > maxpos.y ) {
1440  ToReturn.push_back(road.first);
1441  }
1442  }
1443  }
1444  std::cout << "To " + std::to_string(ToReturn.size() ) + " roads " << std::endl;
1445  return ToReturn;
1446  }
1447 
1448  std::unique_ptr<geom::Mesh> Map::SDFToMesh(const road::Junction& jinput,
1449  const std::vector<geom::Vector3D>& sdfinput,
1450  int grid_cells_per_dim) const {
1451 
1452  int junctionid = jinput.GetId();
1453  float box_extraextension_factor = 1.2f;
1454  const double CubeSize = 0.5;
1456  carla::geom::Vector3D MinOffset = bb.location - geom::Location(bb.extent * box_extraextension_factor);
1457  carla::geom::Vector3D MaxOffset = bb.location + geom::Location(bb.extent * box_extraextension_factor);
1458  carla::geom::Vector3D OffsetPerCell = ( bb.extent * box_extraextension_factor * 2 ) / grid_cells_per_dim;
1459 
1460  auto junctionsdf = [this, OffsetPerCell, CubeSize, MinOffset, junctionid](MeshReconstruction::Vec3 const& pos)
1461  {
1462  geom::Vector3D worldloc(pos.x, pos.y, pos.z);
1463  boost::optional<element::Waypoint> CheckingWaypoint = GetWaypoint(geom::Location(worldloc), 0x1 << 1);
1464  if (CheckingWaypoint) {
1465  if ( pos.z < 0.2) {
1466  return 0.0;
1467  } else {
1468  return -abs(pos.z);
1469  }
1470  }
1471  boost::optional<element::Waypoint> InRoadWaypoint = GetClosestWaypointOnRoad(geom::Location(worldloc), 0x1 << 1);
1472  geom::Transform InRoadWPTransform = ComputeTransform(*InRoadWaypoint);
1473 
1474  geom::Vector3D director = geom::Location(worldloc) - (InRoadWPTransform.location);
1475  geom::Vector3D laneborder = InRoadWPTransform.location + geom::Location(director.MakeUnitVector() * GetLaneWidth(*InRoadWaypoint) * 0.5f);
1476 
1477  geom::Vector3D Distance = laneborder - worldloc;
1478  if (Distance.Length2D() < CubeSize * 1.1 && pos.z < 0.2) {
1479  return 0.0;
1480  }
1481  return Distance.Length() * -1.0;
1482  };
1483 
1484  double gridsizeindouble = grid_cells_per_dim;
1486  domain.min = { MinOffset.x, MinOffset.y, MinOffset.z };
1487  domain.size = { bb.extent.x * box_extraextension_factor * 2, bb.extent.y * box_extraextension_factor * 2, 0.4 };
1488 
1489  MeshReconstruction::Vec3 cubeSize{ CubeSize, CubeSize, 0.2 };
1490  auto mesh = MeshReconstruction::MarchCube(junctionsdf, domain, cubeSize );
1491  carla::geom::Rotation inverse = bb.rotation;
1492  carla::geom::Vector3D trasltation = bb.location;
1493  geom::Mesh out_mesh;
1494 
1495  for (auto& cv : mesh.vertices) {
1496  geom::Vector3D newvertex;
1497  newvertex.x = cv.x;
1498  newvertex.y = cv.y;
1499  newvertex.z = cv.z;
1500  out_mesh.AddVertex(newvertex);
1501  }
1502 
1503  auto finalvertices = out_mesh.GetVertices();
1504  for (auto ct : mesh.triangles) {
1505  out_mesh.AddIndex(ct[1] + 1);
1506  out_mesh.AddIndex(ct[0] + 1);
1507  out_mesh.AddIndex(ct[2] + 1);
1508  }
1509 
1510  for (auto& cv : out_mesh.GetVertices() ) {
1511  boost::optional<element::Waypoint> CheckingWaypoint = GetWaypoint(geom::Location(cv), 0x1 << 1);
1512  if (!CheckingWaypoint)
1513  {
1514  boost::optional<element::Waypoint> InRoadWaypoint = GetClosestWaypointOnRoad(geom::Location(cv), 0x1 << 1);
1515  geom::Transform InRoadWPTransform = ComputeTransform(*InRoadWaypoint);
1516 
1517  geom::Vector3D director = geom::Location(cv) - (InRoadWPTransform.location);
1518  geom::Vector3D laneborder = InRoadWPTransform.location + geom::Location(director.MakeUnitVector() * GetLaneWidth(*InRoadWaypoint) * 0.5f);
1519  cv = laneborder;
1520  }
1521  }
1522  return std::make_unique<geom::Mesh>(out_mesh);
1523  }
1524 
1526  const JuncId Id,
1527  std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>>*
1528  junction_out_mesh_list) const {
1529 
1530  const auto& junction = _data.GetJunctions().at(Id);
1531  if (junction.GetConnections().size() > 2) {
1532  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1533  std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1534  std::vector<carla::geom::Vector3D> perimeterpoints;
1535 
1536  auto pmesh = SDFToMesh(junction, perimeterpoints, 75);
1537  (*junction_out_mesh_list)[road::Lane::LaneType::Driving].push_back(std::move(pmesh));
1538 
1539  for (const auto& connection_pair : junction.GetConnections()) {
1540  const auto& connection = connection_pair.second;
1541  const auto& road = _data.GetRoads().at(connection.connecting_road);
1542  for (auto&& lane_section : road.GetLaneSections()) {
1543  for (auto&& lane_pair : lane_section.GetLanes()) {
1544  const auto& lane = lane_pair.second;
1545  if ( lane.GetType() == road::Lane::LaneType::Sidewalk ) {
1546  boost::optional<element::Waypoint> sw =
1547  GetWaypoint(road.GetId(), lane_pair.first, lane.GetDistance() + (lane.GetLength() * 0.5f));
1548  if( GetWaypoint(ComputeTransform(*sw).location).get_ptr () == nullptr ){
1549  sidewalk_lane_meshes.push_back(mesh_factory.GenerateSidewalk(lane));
1550  }
1551  }
1552  }
1553  }
1554  }
1555  std::unique_ptr<geom::Mesh> sidewalk_mesh = std::make_unique<geom::Mesh>();
1556  for (auto& lane : sidewalk_lane_meshes) {
1557  *sidewalk_mesh += *lane;
1558  }
1559  (*junction_out_mesh_list)[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh));
1560  } else {
1561  std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1562  std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1563  for (const auto& connection_pair : junction.GetConnections()) {
1564  const auto& connection = connection_pair.second;
1565  const auto& road = _data.GetRoads().at(connection.connecting_road);
1566  for (auto&& lane_section : road.GetLaneSections()) {
1567  for (auto&& lane_pair : lane_section.GetLanes()) {
1568  const auto& lane = lane_pair.second;
1569  if (lane.GetType() != road::Lane::LaneType::Sidewalk) {
1570  lane_meshes.push_back(mesh_factory.GenerateTesselated(lane));
1571  }
1572  else {
1573  sidewalk_lane_meshes.push_back(mesh_factory.GenerateSidewalk(lane));
1574  }
1575  }
1576  }
1577  }
1578  std::unique_ptr<geom::Mesh> merged_mesh = std::make_unique<geom::Mesh>();
1579  for (auto& lane : lane_meshes) {
1580  *merged_mesh += *lane;
1581  }
1582  std::unique_ptr<geom::Mesh> sidewalk_mesh = std::make_unique<geom::Mesh>();
1583  for (auto& lane : sidewalk_lane_meshes) {
1584  *sidewalk_mesh += *lane;
1585  }
1586 
1587  (*junction_out_mesh_list)[road::Lane::LaneType::Driving].push_back(std::move(merged_mesh));
1588  (*junction_out_mesh_list)[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh));
1589  }
1590  }
1591 
1592 } // namespace road
1593 } // namespace carla
std::vector< carla::geom::BoundingBox > GetJunctionsBoundingBoxes() const
Definition: road/Map.cpp:1309
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:76
std::vector< element::LaneMarking > CalculateCrossedLanes(const geom::Location &origin, const geom::Location &destination) const
Definition: road/Map.cpp:449
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:1005
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:764
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:590
void AddVertex(vertex_type vertex)
Appends a vertex to the vertices list.
Definition: Mesh.cpp:89
uint32_t RoadId
Definition: RoadTypes.h:15
static double GetDistanceAtStartOfLane(const Lane &lane)
Definition: road/Map.cpp:58
void GenerateSingleJunction(const carla::geom::MeshFactory &mesh_factory, const JuncId Id, std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh >>> *junction_out_mesh_list) const
Definition: road/Map.cpp:1525
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:724
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:212
void AddTriangleFan(const std::vector< vertex_type > &vertices)
Adds a triangle fan to the mesh, vertex order is counterclockwise.
Definition: Mesh.cpp:76
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:34
void GenerateJunctions(const carla::geom::MeshFactory &mesh_factory, const rpc::OpendriveGenerationParameters &params, const geom::Vector3D &minpos, const geom::Vector3D &maxpos, std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh >>> *juntion_out_mesh_list) const
Definition: road/Map.cpp:1347
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:145
std::unique_ptr< Mesh > GenerateSidewalk(const road::LaneSection &lane_section) const
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:1252
void AddIndex(index_type index)
Appends a index to the indexes list.
Definition: Mesh.cpp:101
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:455
Road * GetRoad() const
Definition: Lane.cpp:29
static double GetDistanceAtEndOfLane(const Lane &lane)
Definition: road/Map.cpp:66
const Lane & GetLane(Waypoint waypoint) const
======================================================================== – Road information --------...
Definition: road/Map.cpp:834
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:662
double GetDistance() const
Definition: Lane.cpp:46
Junction * GetJunction(JuncId id)
Definition: road/Map.cpp:997
double GetLength() const
Definition: Lane.cpp:51
void GenerateAllOrderedWithMaxLen(const road::Road &road, std::map< road::Lane::LaneType, std::vector< std::unique_ptr< Mesh >>> &roads) const
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
JuncId GetId() const
Definition: road/Junction.h:53
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:437
void CreateRtree()
Definition: road/Map.cpp:887
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
double GetWidth(const double s) const
Returns the total lane width given a s.
Definition: Lane.cpp:58
int32_t JuncId
Definition: RoadTypes.h:17
std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh > > > GenerateOrderedChunkedMeshInLocations(const rpc::OpendriveGenerationParameters &params, const geom::Vector3D &minpos, const geom::Vector3D &maxpos) const
Definition: road/Map.cpp:1138
static std::vector< T > ConcatVectors(std::vector< T > dst, std::vector< T > src)
Definition: road/Map.cpp:47
Vector3D GetForwardVector() const
float Length() const
Definition: geom/Vector3D.h:49
bg::model::box< Point3D > Box
Definition: InMemoryMap.h:52
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:165
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
std::vector< std::unique_ptr< geom::Mesh > > GenerateLineMarkings(const rpc::OpendriveGenerationParameters &params, const geom::Vector3D &minpos, const geom::Vector3D &maxpos, std::vector< std::string > &outinfo) const
Buids a list of meshes related with LineMarkings.
Definition: road/Map.cpp:1289
void AddMaterial(const std::string &material_name)
Starts applying a new material to the new added triangles.
Definition: Mesh.cpp:113
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:120
void EndMaterial()
Stops applying the material to the new added triangles.
Definition: Mesh.cpp:129
static bool IsLanePresent(const MapData &data, Waypoint waypoint)
Assumes road_id and section_id are valid.
Definition: road/Map.cpp:156
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
geom::Location Location
Definition: rpc/Location.h:14
float GetZPosInDeformation(float posx, float posy) const
Definition: road/Map.cpp:1321
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:554
boost::geometry::model::point< float, 3, boost::geometry::cs::cartesian > BPoint
Definition: Rtree.h:89
RoadParameters road_param
Definition: MeshFactory.h:149
std::vector< RoadId > FilterRoadsByPosition(const geom::Vector3D &minpos, const geom::Vector3D &maxpos) const
Definition: road/Map.cpp:1427
Data structure for the signal search.
Definition: road/Map.h:95
SectionId GetId() const
Definition: LaneSection.cpp:27
#define RELEASE_ASSERT(pred)
Definition: Debug.h:84
double min(double v1, double v2)
Definition: Simplify.h:294
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:273
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:307
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:1053
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:636
Lane::LaneType GetLaneType(Waypoint waypoint) const
Definition: road/Map.cpp:281
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:132
bool IsJunction(RoadId road_id) const
Definition: road/Map.cpp:302
std::vector< Waypoint > GetPredecessors(Waypoint waypoint) const
Definition: road/Map.cpp:536
std::vector< JuncId > FilterJunctionsByPosition(const geom::Vector3D &minpos, const geom::Vector3D &maxpos) const
Definition: road/Map.cpp:1410
const T * GetInfo(const double s) const
Definition: Lane.h:79
double GetRemainingLength(const Lane &lane, double current_s)
Definition: road/Map.cpp:879
void TransformPoint(Vector3D &in_point) const
Applies this transformation to in_point (first translation then rotation).
std::unique_ptr< geom::Mesh > SDFToMesh(const road::Junction &jinput, const std::vector< geom::Vector3D > &sdfinput, int grid_cells_per_dim) const
Definition: road/Map.cpp:1448
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:330
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
Rotation rotation
Rotation of the BoundingBox in local space.
bool IsStraight() const
Checks whether the geometry is straight or not.
Definition: Lane.cpp:67
std::vector< std::pair< geom::Transform, std::string > > GetTreesTransform(const geom::Vector3D &minpos, const geom::Vector3D &maxpos, float distancebetweentrees, float distancefromdrivinglineborder, float s_offset=0) const
Definition: road/Map.cpp:1207
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:692
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:844
geom::Transform ComputeTransform(const double s) const
Definition: Lane.cpp:131
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:518
void AddElementToRtreeAndUpdateTransforms(std::vector< Rtree::TreeElement > &rtree_elements, geom::Transform &current_transform, Waypoint &current_waypoint, Waypoint &next_waypoint)
Definition: road/Map.cpp:865
std::vector< Waypoint > GenerateWaypoints(double approx_distance) const
Generate all the waypoints in map separated by approx_distance.
Definition: road/Map.cpp:648
boost::geometry::model::segment< BPoint > BSegment
Definition: Rtree.h:90
void GenerateLaneMarkForRoad(const road::Road &road, std::vector< std::unique_ptr< Mesh >> &inout, std::vector< std::string > &outinfo) const
std::unordered_map< ConId, Connection > & GetConnections()
Definition: road/Junction.h:65
JuncId GetJunctionId(RoadId road_id) const
Definition: road/Map.cpp:298
Vector3D MakeUnitVector() const
Definition: geom/Vector3D.h:65
geom::Vector2D Vector2D
Definition: rpc/Vector2D.h:14
double GetLength() const
Definition: Road.cpp:38
Mesh MarchCube(Fun3s const &sdf, Rect3 const &domain)
Reconstructs a triangle mesh from a given signed distance function using Marching Cubes...
std::unique_ptr< Mesh > GenerateTesselated(const road::Lane &lane, const double s_start, const double s_end) const
Generates a mesh that defines a lane from a given s start and end with bigger tesselation.
float GetBumpDeformation(float posx, float posy)
Definition: Deformation.h:39
std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh > > > GenerateRoadsMultithreaded(const carla::geom::MeshFactory &mesh_factory, const std::vector< RoadId > &RoadsID, const size_t index, const size_t number_of_roads_per_thread) const
Definition: road/Map.cpp:1327
boost::optional< Waypoint > GetRight(Waypoint waypoint) const
Return a waypoint at the lane of waypoint&#39;s right lane.
Definition: road/Map.cpp:626
const std::vector< vertex_type > & GetVertices() const
Definition: Mesh.cpp:255
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
Definition: road/Map.cpp:747
std::pair< geom::Vector3D, geom::Vector3D > GetCornerPositions(const double s, const float extra_width=0.f) const
Computes the location of the edges given a s.
Definition: Lane.cpp:206
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:40
LaneId GetId() const
Definition: Lane.cpp:34
double GetLaneWidth(Waypoint waypoint) const
Definition: road/Map.cpp:285
static void ForEachLaneImpl(RoadId road_id, const LaneSection &lane_section, double distance, Lane::LaneType lane_type, FuncT &&func)
Definition: road/Map.cpp:97
float GetZPosInDeformation(float posx, float posy)
Definition: Deformation.h:21