CARLA
MapBuilder.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 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/StringUtil.h"
26 #include "carla/road/Signal.h"
27 #include "carla/road/SignalType.h"
28 
29 #include <iterator>
30 #include <memory>
31 #include <algorithm>
32 
33 using namespace carla::road::element;
34 
35 namespace carla {
36 namespace road {
37 
38  boost::optional<Map> MapBuilder::Build() {
39 
40  CreatePointersBetweenRoadSegments();
41  RemoveZeroLaneValiditySignalReferences();
42 
43  for (auto &&info : _temp_road_info_container) {
44  DEBUG_ASSERT(info.first != nullptr);
45  info.first->_info = InformationSet(std::move(info.second));
46  }
47 
48  for (auto &&info : _temp_lane_info_container) {
49  DEBUG_ASSERT(info.first != nullptr);
50  info.first->_info = InformationSet(std::move(info.second));
51  }
52 
53  // compute transform requires the roads to have the RoadInfo
54  SolveSignalReferencesAndTransforms();
55 
56  SolveControllerAndJuntionReferences();
57 
58  // remove temporal already used information
59  _temp_road_info_container.clear();
60  _temp_lane_info_container.clear();
61 
62  // _map_data is a memeber of MapBuilder so you must especify if
63  // you want to keep it (will return copy -> Map(const Map &))
64  // or move it (will return move -> Map(Map &&))
65  Map map(std::move(_map_data));
66  CreateJunctionBoundingBoxes(map);
67  ComputeJunctionRoadConflicts(map);
68  CheckSignalsOnRoads(map);
69 
70  return map;
71  }
72 
73  // called from profiles parser
75  Road *road,
76  const double s,
77  const double a,
78  const double b,
79  const double c,
80  const double d) {
81  DEBUG_ASSERT(road != nullptr);
82  auto elevation = std::make_unique<RoadInfoElevation>(s, a, b, c, d);
83  _temp_road_info_container[road].emplace_back(std::move(elevation));
84  }
85 
87  Road *road,
88  const std::string name,
89  const double s,
90  const double t,
91  const double zOffset,
92  const double hdg,
93  const double pitch,
94  const double roll,
95  const std::string orientation,
96  const double width,
97  const double length,
98  const std::vector<road::element::CrosswalkPoint> points) {
99  DEBUG_ASSERT(road != nullptr);
100  auto cross = std::make_unique<RoadInfoCrosswalk>(s, name, t, zOffset, hdg, pitch, roll, std::move(orientation), width, length, std::move(points));
101  _temp_road_info_container[road].emplace_back(std::move(cross));
102  }
103 
104  // called from lane parser
106  Lane *lane,
107  const double s,
108  const std::string restriction) {
109  DEBUG_ASSERT(lane != nullptr);
110  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoLaneAccess>(s, restriction));
111  }
112 
114  Lane *lane,
115  const double s,
116  const double a,
117  const double b,
118  const double c,
119  const double d) {
120  DEBUG_ASSERT(lane != nullptr);
121  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoLaneBorder>(s, a, b, c, d));
122  }
123 
125  Lane *lane,
126  const double s,
127  const double inner,
128  const double outer) {
129  DEBUG_ASSERT(lane != nullptr);
130  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoLaneHeight>(s, inner, outer));
131  }
132 
134  Lane *lane,
135  const double s,
136  const std::string surface,
137  const double friction,
138  const double roughness) {
139  DEBUG_ASSERT(lane != nullptr);
140  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoLaneMaterial>(s, surface, friction,
141  roughness));
142  }
143 
145  Lane *lane,
146  const double s,
147  const std::string value) {
148  DEBUG_ASSERT(lane != nullptr);
149  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoLaneRule>(s, value));
150  }
151 
153  Lane *lane,
154  const double s,
155  const double forward,
156  const double back,
157  const double left,
158  const double right) {
159  DEBUG_ASSERT(lane != nullptr);
160  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoLaneVisibility>(s, forward, back,
161  left, right));
162  }
163 
165  Lane *lane,
166  const double s,
167  const double a,
168  const double b,
169  const double c,
170  const double d) {
171  DEBUG_ASSERT(lane != nullptr);
172  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoLaneWidth>(s, a, b, c, d));
173  }
174 
176  Lane *lane,
177  const int road_mark_id,
178  const double s,
179  const std::string type,
180  const std::string weight,
181  const std::string color,
182  const std::string material,
183  const double width,
184  std::string lane_change,
185  const double height,
186  const std::string type_name,
187  const double type_width) {
188  DEBUG_ASSERT(lane != nullptr);
190 
191  StringUtil::ToLower(lane_change);
192 
193  if (lane_change == "increase") {
195  } else if (lane_change == "decrease") {
197  } else if (lane_change == "none") {
199  } else {
201  }
202  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoMarkRecord>(s, road_mark_id, type,
203  weight, color,
204  material, width, lc, height, type_name, type_width));
205  }
206 
208  Lane *lane,
209  const int road_mark_id,
210  const double length,
211  const double space,
212  const double tOffset,
213  const double s,
214  const std::string rule,
215  const double width) {
216  DEBUG_ASSERT(lane != nullptr);
217  auto it = MakeRoadInfoIterator<RoadInfoMarkRecord>(_temp_lane_info_container[lane]);
218  for (; !it.IsAtEnd(); ++it) {
219  if (it->GetRoadMarkId() == road_mark_id) {
220  it->GetLines().emplace_back(std::make_unique<RoadInfoMarkTypeLine>(s, road_mark_id, length, space,
221  tOffset, rule, width));
222  break;
223  }
224  }
225 
226  }
227 
229  Lane *lane,
230  const double s,
231  const double max,
232  const std::string /*unit*/) {
233  DEBUG_ASSERT(lane != nullptr);
234  _temp_lane_info_container[lane].emplace_back(std::make_unique<RoadInfoSpeed>(s, max));
235  }
236 
237 
239  Road* road,
240  const SignId signal_id,
241  const double s,
242  const double t,
243  const std::string name,
244  const std::string dynamic,
245  const std::string orientation,
246  const double zOffset,
247  const std::string country,
248  const std::string type,
249  const std::string subtype,
250  const double value,
251  const std::string unit,
252  const double height,
253  const double width,
254  const std::string text,
255  const double hOffset,
256  const double pitch,
257  const double roll) {
258  _temp_signal_container[signal_id] = std::make_unique<Signal>(
259  road->GetId(),
260  signal_id,
261  s,
262  t,
263  name,
264  dynamic,
265  orientation,
266  zOffset,
267  country,
268  type,
269  subtype,
270  value,
271  unit,
272  height,
273  width,
274  text,
275  hOffset,
276  pitch,
277  roll);
278 
279  return AddSignalReference(road, signal_id, s, t, orientation);
280  }
281 
283  Road* road,
284  const SignId signal_id,
285  const double s_position,
286  const double t_position,
287  const std::string signal_reference_orientation) {
288 
289  const double epsilon = 0.00001;
290  RELEASE_ASSERT(s_position >= 0.0);
291  // Prevent s_position from being equal to the road length
292  double fixed_s = geom::Math::Clamp(s_position, 0.0, road->GetLength() - epsilon);
293  _temp_road_info_container[road].emplace_back(std::make_unique<element::RoadInfoSignal>(
294  signal_id, road->GetId(), fixed_s, t_position, signal_reference_orientation));
295  auto road_info_signal = static_cast<element::RoadInfoSignal*>(
296  _temp_road_info_container[road].back().get());
297  _temp_signal_reference_container.emplace_back(road_info_signal);
298  return road_info_signal;
299  }
300 
302  element::RoadInfoSignal* signal_reference,
303  const LaneId from_lane,
304  const LaneId to_lane) {
305  signal_reference->_validities.emplace_back(LaneValidity(from_lane, to_lane));
306  }
307 
309  const SignId signal_id,
310  const std::string dependency_id,
311  const std::string dependency_type) {
312  _temp_signal_container[signal_id]->_dependencies.emplace_back(
313  SignalDependency(dependency_id, dependency_type));
314  }
315 
316  // build road objects
318  const RoadId road_id,
319  const std::string name,
320  const double length,
321  const JuncId junction_id,
322  const RoadId predecessor,
323  const RoadId successor)
324  {
325 
326  // add it
327  auto road = &(_map_data._roads.emplace(road_id, Road()).first->second);
328 
329  // set road data
330  road->_map_data = &_map_data;
331  road->_id = road_id;
332  road->_name = name;
333  road->_length = length;
334  road->_junction_id = junction_id;
335  (junction_id != -1) ? road->_is_junction = true : road->_is_junction = false;
336  road->_successor = successor;
337  road->_predecessor = predecessor;
338 
339  return road;
340  }
341 
343  Road *road,
344  const SectionId id,
345  const double s) {
346  DEBUG_ASSERT(road != nullptr);
347  carla::road::LaneSection &sec = road->_lane_sections.Emplace(id, s);
348  sec._road = road;
349  return &sec;
350  }
351 
353  carla::road::LaneSection *section,
354  const int32_t lane_id,
355  const uint32_t lane_type,
356  const bool lane_level,
357  const int32_t predecessor,
358  const int32_t successor) {
359  DEBUG_ASSERT(section != nullptr);
360 
361  // add the lane
362  auto *lane = &((section->_lanes.emplace(lane_id, Lane()).first)->second);
363 
364  // set lane data
365  lane->_id = lane_id;
366  lane->_lane_section = section;
367  lane->_level = lane_level;
368  lane->_type = static_cast<carla::road::Lane::LaneType>(lane_type);
369  lane->_successor = successor;
370  lane->_predecessor = predecessor;
371 
372  return lane;
373  }
374 
376  Road *road,
377  const double s,
378  const double x,
379  const double y,
380  const double hdg,
381  const double length) {
382  DEBUG_ASSERT(road != nullptr);
383  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
384  auto line_geometry = std::make_unique<GeometryLine>(
385  s,
386  length,
387  hdg,
388  location);
389 
390  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
391  std::move(line_geometry))));
392  }
393 
395  Road *road,
396  const double s,
397  const std::string /*type*/,
398  const double max,
399  const std::string /*unit*/) {
400  DEBUG_ASSERT(road != nullptr);
401  _temp_road_info_container[road].emplace_back(std::make_unique<RoadInfoSpeed>(s, max));
402  }
403 
405  Road *road,
406  const double s,
407  const double a,
408  const double b,
409  const double c,
410  const double d) {
411  DEBUG_ASSERT(road != nullptr);
412  _temp_road_info_container[road].emplace_back(std::make_unique<RoadInfoLaneOffset>(s, a, b, c, d));
413  }
414 
416  Road *road,
417  const double s,
418  const double x,
419  const double y,
420  const double hdg,
421  const double length,
422  const double curvature) {
423  DEBUG_ASSERT(road != nullptr);
424  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
425  auto arc_geometry = std::make_unique<GeometryArc>(
426  s,
427  length,
428  hdg,
429  location,
430  curvature);
431 
432  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
433  std::move(arc_geometry))));
434  }
435 
437  Road * road,
438  const double s,
439  const double x,
440  const double y,
441  const double hdg,
442  const double length,
443  const double curvStart,
444  const double curvEnd) {
445  //throw_exception(std::runtime_error("geometry spiral not supported"));
446  DEBUG_ASSERT(road != nullptr);
447  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
448  auto spiral_geometry = std::make_unique<GeometrySpiral>(
449  s,
450  length,
451  hdg,
452  location,
453  curvStart,
454  curvEnd);
455 
456  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
457  std::move(spiral_geometry))));
458  }
459 
461  Road * road,
462  const double s,
463  const double x,
464  const double y,
465  const double hdg,
466  const double length,
467  const double a,
468  const double b,
469  const double c,
470  const double d) {
471  //throw_exception(std::runtime_error("geometry poly3 not supported"));
472  DEBUG_ASSERT(road != nullptr);
473  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
474  auto poly3_geometry = std::make_unique<GeometryPoly3>(
475  s,
476  length,
477  hdg,
478  location,
479  a,
480  b,
481  c,
482  d);
483  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
484  std::move(poly3_geometry))));
485  }
486 
488  Road * road,
489  const double s,
490  const double x,
491  const double y,
492  const double hdg,
493  const double length,
494  const double aU,
495  const double bU,
496  const double cU,
497  const double dU,
498  const double aV,
499  const double bV,
500  const double cV,
501  const double dV,
502  const std::string p_range) {
503  //throw_exception(std::runtime_error("geometry poly3 not supported"));
504  bool arcLength;
505  if(p_range == "arcLength"){
506  arcLength = true;
507  }else{
508  arcLength = false;
509  }
510  DEBUG_ASSERT(road != nullptr);
511  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
512  auto parampoly3_geometry = std::make_unique<GeometryParamPoly3>(
513  s,
514  length,
515  hdg,
516  location,
517  aU,
518  bU,
519  cU,
520  dU,
521  aV,
522  bV,
523  cV,
524  dV,
525  arcLength);
526  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
527  std::move(parampoly3_geometry))));
528  }
529 
530  void MapBuilder::AddJunction(const int32_t id, const std::string name) {
531  _map_data.GetJunctions().emplace(id, Junction(id, name));
532  }
533 
535  const JuncId junction_id,
536  const ConId connection_id,
537  const RoadId incoming_road,
538  const RoadId connecting_road) {
539  DEBUG_ASSERT(_map_data.GetJunction(junction_id) != nullptr);
540  _map_data.GetJunction(junction_id)->GetConnections().emplace(connection_id,
541  Junction::Connection(connection_id, incoming_road, connecting_road));
542  }
543 
545  const JuncId junction_id,
546  const ConId connection_id,
547  const LaneId from,
548  const LaneId to) {
549  DEBUG_ASSERT(_map_data.GetJunction(junction_id) != nullptr);
550  _map_data.GetJunction(junction_id)->GetConnection(connection_id)->AddLaneLink(from, to);
551  }
552 
554  const JuncId junction_id,
555  std::set<road::ContId>&& controllers) {
556  DEBUG_ASSERT(_map_data.GetJunction(junction_id) != nullptr);
557  _map_data.GetJunction(junction_id)->_controllers = std::move(controllers);
558  }
559 
561  const RoadId road_id,
562  const LaneId lane_id,
563  const double s) {
564  return &_map_data.GetRoad(road_id).GetLaneByDistance(s, lane_id);
565  }
566 
568  const RoadId road_id) {
569  return &_map_data.GetRoad(road_id);
570  }
571 
572  // return the pointer to a lane object
573  Lane *MapBuilder::GetEdgeLanePointer(RoadId road_id, bool from_start, LaneId lane_id) {
574 
575  if (!_map_data.ContainsRoad(road_id)) {
576  return nullptr;
577  }
578  Road &road = _map_data.GetRoad(road_id);
579 
580  // get the lane section
581  LaneSection *section;
582  if (from_start) {
583  section = road.GetStartSection(lane_id);
584  } else {
585  section = road.GetEndSection(lane_id);
586  }
587 
588  // get the lane
589  DEBUG_ASSERT(section != nullptr);
590  return section->GetLane(lane_id);
591  }
592 
593  // return a list of pointers to all lanes from a lane (using road and junction
594  // info)
595  std::vector<Lane *> MapBuilder::GetLaneNext(
596  RoadId road_id,
597  SectionId section_id,
598  LaneId lane_id) {
599  std::vector<Lane *> result;
600 
601  if (!_map_data.ContainsRoad(road_id)) {
602  return result;
603  }
604  Road &road = _map_data.GetRoad(road_id);
605 
606  // get the section
607  LaneSection &section = road._lane_sections.GetById(section_id);
608 
609  // get the lane
610  Lane *lane = section.GetLane(lane_id);
611  DEBUG_ASSERT(lane != nullptr);
612 
613  // successor and predecessor (road and lane)
614  LaneId next;
615  RoadId next_road;
616  if (lane_id <= 0) {
617  next_road = road.GetSuccessor();
618  next = lane->GetSuccessor();
619  } else {
620  next_road = road.GetPredecessor();
621  next = lane->GetPredecessor();
622  }
623 
624  // check to see if next is a road or a junction
625  bool next_is_junction = !_map_data.ContainsRoad(next_road);
626  double s = section.GetDistance();
627 
628  // check if we are in a lane section in the middle
629  if ((lane_id > 0 && s > 0) ||
630  (lane_id <= 0 && road._lane_sections.upper_bound(s) != road._lane_sections.end())) {
631  // check if lane has a next link (if not, it deads in the middle section)
632  if (next != 0 || (lane_id == 0 && next == 0)) {
633  // change to next / prev section
634  if (lane_id <= 0) {
635  result.push_back(road.GetNextLane(s, next));
636  } else {
637  result.push_back(road.GetPrevLane(s, next));
638  }
639  }
640  } else if (!next_is_junction) {
641  // change to another road / junction
642  if (next != 0 || (lane_id == 0 && next == 0)) {
643  // single road
644  result.push_back(GetEdgeLanePointer(next_road, (next <= 0), next));
645  }
646  } else {
647  // several roads (junction)
648 
649  /// @todo Is it correct to use a road id as section id? (NS: I just added
650  /// this cast to avoid compiler warnings).
651  auto next_road_as_junction = static_cast<JuncId>(next_road);
652  auto options = GetJunctionLanes(next_road_as_junction, road_id, lane_id);
653  for (auto opt : options) {
654  result.push_back(GetEdgeLanePointer(opt.first, (opt.second <= 0), opt.second));
655  }
656  }
657 
658  return result;
659  }
660 
661  std::vector<std::pair<RoadId, LaneId>> MapBuilder::GetJunctionLanes(
662  JuncId junction_id,
663  RoadId road_id,
664  LaneId lane_id) {
665  std::vector<std::pair<RoadId, LaneId>> result;
666 
667  // get the junction
668  Junction *junction = _map_data.GetJunction(junction_id);
669  if (junction == nullptr) {
670  return result;
671  }
672 
673  // check all connections
674  for (auto con : junction->_connections) {
675  // only connections for our road
676  if (con.second.incoming_road == road_id) {
677  // for center lane it is always next lane id 0, we don't need to search
678  // because it is not in the junction
679  if (lane_id == 0) {
680  result.push_back(std::make_pair(con.second.connecting_road, 0));
681  } else {
682  // check all lane links
683  for (auto link : con.second.lane_links) {
684  // is our lane id ?
685  if (link.from == lane_id) {
686  // add as option
687  result.push_back(std::make_pair(con.second.connecting_road, link.to));
688  }
689  }
690  }
691  }
692  }
693 
694  return result;
695  }
696 
697  // assign pointers to the next lanes
699  // process each lane to define its nexts
700  for (auto &road : _map_data._roads) {
701  for (auto &section : road.second._lane_sections) {
702  for (auto &lane : section.second._lanes) {
703 
704  // assign the next lane pointers
705  lane.second._next_lanes = GetLaneNext(road.first, section.second._id, lane.first);
706 
707  // add to each lane found, this as its predecessor
708  for (auto next_lane : lane.second._next_lanes) {
709  // add as previous
710  DEBUG_ASSERT(next_lane != nullptr);
711  next_lane->_prev_lanes.push_back(&lane.second);
712  }
713 
714  }
715  }
716  }
717 
718  // process each lane to define its nexts
719  for (auto &road : _map_data._roads) {
720  for (auto &section : road.second._lane_sections) {
721  for (auto &lane : section.second._lanes) {
722 
723  // add next roads
724  for (auto next_lane : lane.second._next_lanes) {
725  DEBUG_ASSERT(next_lane != nullptr);
726  // avoid same road
727  if (next_lane->GetRoad() != &road.second) {
728  if (std::find(road.second._nexts.begin(), road.second._nexts.end(),
729  next_lane->GetRoad()) == road.second._nexts.end()) {
730  road.second._nexts.push_back(next_lane->GetRoad());
731  }
732  }
733  }
734 
735  // add prev roads
736  for (auto prev_lane : lane.second._prev_lanes) {
737  DEBUG_ASSERT(prev_lane != nullptr);
738  // avoid same road
739  if (prev_lane->GetRoad() != &road.second) {
740  if (std::find(road.second._prevs.begin(), road.second._prevs.end(),
741  prev_lane->GetRoad()) == road.second._prevs.end()) {
742  road.second._prevs.push_back(prev_lane->GetRoad());
743  }
744  }
745  }
746 
747  }
748  }
749  }
750  }
751 
752  geom::Transform MapBuilder::ComputeSignalTransform(std::unique_ptr<Signal> &signal, MapData &data) {
753  DirectedPoint point = data.GetRoad(signal->_road_id).GetDirectedPointInNoLaneOffset(signal->_s);
754  point.ApplyLateralOffset(static_cast<float>(-signal->_t));
755  point.location.y *= -1; // Unreal Y axis hack
756  point.location.z += static_cast<float>(signal->_zOffset);
757  geom::Transform transform(point.location, geom::Rotation(
758  geom::Math::ToDegrees(static_cast<float>(signal->_pitch)),
759  geom::Math::ToDegrees(static_cast<float>(-(point.tangent + signal->_hOffset))),
760  geom::Math::ToDegrees(static_cast<float>(signal->_roll))));
761  return transform;
762  }
763 
765  for(auto signal_reference : _temp_signal_reference_container){
766  signal_reference->_signal =
767  _temp_signal_container[signal_reference->_signal_id].get();
768  }
769 
770  for(auto& signal_pair : _temp_signal_container) {
771  auto& signal = signal_pair.second;
772  auto transform = ComputeSignalTransform(signal, _map_data);
773  // Hack: compensate RoadRunner displacement (25cm) due to lightbox size
774  if (SignalType::IsTrafficLight(signal->GetType())) {
775  transform.location = transform.location +
776  geom::Location(transform.GetForwardVector()*0.25);
777  }
778  signal->_transform = transform;
779  }
780 
781  _map_data._signals = std::move(_temp_signal_container);
782 
783  GenerateDefaultValiditiesForSignalReferences();
784  }
785 
787  for(const auto& junction : _map_data._junctions) {
788  for(const auto& controller : junction.second._controllers) {
789  auto it = _map_data._controllers.find(controller);
790  DEBUG_ASSERT(it != _map_data._controllers.end());
791  it->second->_junctions.insert(junction.first);
792  for(const auto & signal : it->second->_signals) {
793  auto signal_it = _map_data._signals.find(signal);
794  signal_it->second->_controllers.insert(controller);
795  }
796  }
797  }
798  }
799 
801  for (auto &junctionpair : map._data.GetJunctions()) {
802  auto* junction = map.GetJunction(junctionpair.first);
803  auto waypoints = map.GetJunctionWaypoints(junction->GetId(), Lane::LaneType::Any);
804  const int number_intervals = 10;
805 
806  float minx = std::numeric_limits<float>::max();
807  float miny = std::numeric_limits<float>::max();
808  float minz = std::numeric_limits<float>::max();
809  float maxx = -std::numeric_limits<float>::max();
810  float maxy = -std::numeric_limits<float>::max();
811  float maxz = -std::numeric_limits<float>::max();
812 
813  auto get_min_max = [&](geom::Location position) {
814  if (position.x < minx) {
815  minx = position.x;
816  }
817  if (position.y < miny) {
818  miny = position.y;
819  }
820  if (position.z < minz) {
821  minz = position.z;
822  }
823 
824  if (position.x > maxx) {
825  maxx = position.x;
826  }
827  if (position.y > maxy) {
828  maxy = position.y;
829  }
830  if (position.z > maxz) {
831  maxz = position.z;
832  }
833  };
834 
835  for (auto &waypoint_p : waypoints) {
836  auto &waypoint_start = waypoint_p.first;
837  auto &waypoint_end = waypoint_p.second;
838  double interval = (waypoint_end.s - waypoint_start.s) / static_cast<double>(number_intervals);
839  auto next_wp = waypoint_end;
840  auto location = map.ComputeTransform(next_wp).location;
841 
842  get_min_max(location);
843 
844  next_wp = waypoint_start;
845  location = map.ComputeTransform(next_wp).location;
846 
847  get_min_max(location);
848 
849  for (int i = 0; i < number_intervals; ++i) {
850  if (interval < std::numeric_limits<double>::epsilon())
851  break;
852  auto next = map.GetNext(next_wp, interval);
853  if(next.size()){
854  next_wp = next.back();
855  }
856 
857  location = map.ComputeTransform(next_wp).location;
858  get_min_max(location);
859  }
860  }
861  carla::geom::Location location(0.5f * (maxx + minx), 0.5f * (maxy + miny), 0.5f * (maxz + minz));
862  carla::geom::Vector3D extent(0.5f * (maxx - minx), 0.5f * (maxy - miny), 0.5f * (maxz - minz));
863 
864  junction->_bounding_box = carla::geom::BoundingBox(location, extent);
865  }
866  }
867 
869  const ContId controller_id,
870  const std::string controller_name,
871  const uint32_t controller_sequence,
872  const std::set<road::SignId>&& signals) {
873 
874  // Add the Controller to MapData
875  auto controller_pair = _map_data._controllers.emplace(
876  std::make_pair(
877  controller_id,
878  std::make_unique<Controller>(controller_id, controller_name, controller_sequence)));
879 
880  DEBUG_ASSERT(controller_pair.first != _map_data._controllers.end());
881  DEBUG_ASSERT(controller_pair.first->second);
882 
883  // Add the signals owned by the controller
884  controller_pair.first->second->_signals = std::move(signals);
885 
886  // Add ContId to the signal owned by this Controller
887  auto& signals_map = _map_data._signals;
888  for(auto signal: signals) {
889  auto it = signals_map.find(signal);
890  if(it != signals_map.end()) {
891  it->second->_controllers.insert(signal);
892  }
893  }
894 }
895 
897  for (auto &junctionpair : map._data.GetJunctions()) {
898  auto& junction = junctionpair.second;
899  junction._road_conflicts = (map.ComputeJunctionConflicts(junction.GetId()));
900  }
901  }
902 
904  for (auto * signal_reference : _temp_signal_reference_container) {
905  if (signal_reference->_validities.size() == 0) {
906  Road* road = GetRoad(signal_reference->GetRoadId());
907  auto lanes = road->GetLanesByDistance(signal_reference->GetS());
908  switch (signal_reference->GetOrientation()) {
910  LaneId min_lane = 1;
911  LaneId max_lane = 0;
912  for (const auto* lane : lanes) {
913  auto lane_id = lane->GetId();
914  if(lane_id > max_lane) {
915  max_lane = lane_id;
916  }
917  }
918  if(min_lane <= max_lane) {
919  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
920  }
921  break;
922  }
924  LaneId min_lane = 0;
925  LaneId max_lane = -1;
926  for (const auto* lane : lanes) {
927  auto lane_id = lane->GetId();
928  if(lane_id < min_lane) {
929  min_lane = lane_id;
930  }
931  }
932  if(min_lane <= max_lane) {
933  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
934  }
935  break;
936  }
938  // Get positive lanes
939  LaneId min_lane = 1;
940  LaneId max_lane = 0;
941  for (const auto* lane : lanes) {
942  auto lane_id = lane->GetId();
943  if(lane_id > max_lane) {
944  max_lane = lane_id;
945  }
946  }
947  if(min_lane <= max_lane) {
948  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
949  }
950 
951  // get negative lanes
952  min_lane = 0;
953  max_lane = -1;
954  for (const auto* lane : lanes) {
955  auto lane_id = lane->GetId();
956  if(lane_id < min_lane) {
957  min_lane = lane_id;
958  }
959  }
960  if(min_lane <= max_lane) {
961  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
962  }
963  break;
964  }
965  }
966  }
967  }
968  }
969 
971  std::vector<element::RoadInfoSignal*> elements_to_remove;
972  for (auto * signal_reference : _temp_signal_reference_container) {
973  bool should_remove = true;
974  for (auto & lane_validity : signal_reference->_validities) {
975  if ( (lane_validity._from_lane != 0) ||
976  (lane_validity._to_lane != 0)) {
977  should_remove = false;
978  break;
979  }
980  }
981  if (signal_reference->_validities.size() == 0) {
982  should_remove = false;
983  }
984  if (should_remove) {
985  elements_to_remove.push_back(signal_reference);
986  }
987  }
988  for (auto* element : elements_to_remove) {
989  auto road_id = element->GetRoadId();
990  auto& road_info = _temp_road_info_container[GetRoad(road_id)];
991  road_info.erase(std::remove_if(road_info.begin(), road_info.end(),
992  [=] (auto& info_ptr) {
993  return (info_ptr.get() == element);
994  }), road_info.end());
995  _temp_signal_reference_container.erase(std::remove(_temp_signal_reference_container.begin(),
996  _temp_signal_reference_container.end(), element),
997  _temp_signal_reference_container.end());
998  }
999  }
1000 
1002  for (auto& signal_pair : map._data._signals) {
1003  auto& signal = signal_pair.second;
1004  auto signal_position = signal->GetTransform().location;
1005  auto closest_waypoint_to_signal =
1006  map.GetClosestWaypointOnRoad(signal_position);
1007  // workarround to not move speed signals
1008  if (signal->GetName().substr(0, 6) == "Speed_" ||
1009  signal->GetName().substr(0, 6) == "speed_" ||
1010  signal->GetName().find("Stencil_STOP") != std::string::npos) {
1011  continue;
1012  }
1013  if(closest_waypoint_to_signal) {
1014  auto distance_to_road =
1015  (map.ComputeTransform(closest_waypoint_to_signal.get()).location -
1016  signal_position).Length();
1017  double lane_width = map.GetLaneWidth(closest_waypoint_to_signal.get());
1018  int iter = 0;
1019  int MaxIter = 10;
1020  // Displaces signal until it finds a suitable spot
1021  while(distance_to_road < lane_width * 0.5 && iter < MaxIter) {
1022  if(iter == 0) {
1023  log_warning("Traffic sign",
1024  signal->GetSignalId(),
1025  "overlaps a driving lane. Moving out of the road...");
1026  }
1027  geom::Vector3D displacement = 1.f*(signal->GetTransform().GetRightVector()) *
1028  static_cast<float>(abs(lane_width))*0.5f;
1029  signal_position += displacement;
1030  closest_waypoint_to_signal =
1031  map.GetClosestWaypointOnRoad(signal_position);
1032  distance_to_road =
1033  (map.ComputeTransform(closest_waypoint_to_signal.get()).location -
1034  signal_position).Length();
1035  lane_width = map.GetLaneWidth(closest_waypoint_to_signal.get());
1036  iter++;
1037  }
1038  if(iter == MaxIter) {
1039  log_warning("Failed to find suitable place for signal.");
1040  } else {
1041  // Only perform the displacement if a good location has been found
1042  signal->_transform.location = signal_position;
1043  }
1044  }
1045  }
1046  }
1047 
1048 } // namespace road
1049 } // namespace carla
void CreateLaneWidth(Lane *lane, const double s, const double a, const double b, const double c, const double d)
Definition: MapBuilder.cpp:164
std::string SignId
Definition: RoadTypes.h:25
void AddRoadGeometryParamPoly3(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double aU, const double bU, const double cU, const double dU, const double aV, const double bV, const double cV, const double dV, const std::string p_range)
Definition: MapBuilder.cpp:487
std::unordered_map< road::RoadId, std::unordered_set< road::RoadId > > ComputeJunctionConflicts(JuncId id) const
Definition: road/Map.cpp:715
void AddRoadObjectCrosswalk(Road *road, const std::string name, const double s, const double t, const double zOffset, const double hdg, const double pitch, const double roll, const std::string orientation, const double width, const double length, const std::vector< road::element::CrosswalkPoint > points)
Definition: MapBuilder.cpp:86
void CheckSignalsOnRoads(Map &map)
Checks signals overlapping driving lanes and emits a warning.
Road * GetRoad(const RoadId road_id)
Definition: MapBuilder.cpp:567
double GetDistance() const
Definition: LaneSection.cpp:13
void AddRoadElevationProfile(Road *road, const double s, const double a, const double b, const double c, const double d)
Definition: MapBuilder.cpp:74
uint32_t RoadId
Definition: RoadTypes.h:15
element::RoadInfoSignal * AddSignalReference(Road *road, const SignId signal_id, const double s_position, const double t_position, const std::string signal_reference_orientation)
Definition: MapBuilder.cpp:282
std::unordered_map< JuncId, Junction > & GetJunctions()
Definition: MapData.cpp:17
Lane * GetEdgeLanePointer(RoadId road_id, bool from_start, LaneId lane_id)
Return the pointer to a lane object.
Definition: MapBuilder.cpp:573
void AddJunctionController(const JuncId junction_id, std::set< ContId > &&controllers)
Definition: MapBuilder.cpp:553
void ComputeJunctionRoadConflicts(Map &map)
Compute the conflicts of the roads (intersecting roads)
Definition: MapBuilder.cpp:896
void CreateJunctionBoundingBoxes(Map &map)
Create the bounding boxes of each junction.
Definition: MapBuilder.cpp:800
boost::optional< Map > Build()
Definition: MapBuilder.cpp:38
RoadId GetSuccessor() const
Definition: Road.cpp:50
void CreateController(const ContId controller_id, const std::string controller_name, const uint32_t controller_sequence, const std::set< road::SignId > &&signals)
Definition: MapBuilder.cpp:868
std::vector< Lane * > GetLanesByDistance(double s)
Get all lanes from all lane sections in a specific s.
Definition: Road.cpp:88
void GenerateDefaultValiditiesForSignalReferences()
Generates a default validity field for signal references with missing validity record in OpenDRIVE...
Definition: MapBuilder.cpp:903
void SolveSignalReferencesAndTransforms()
Solves the signal references in the road.
Definition: MapBuilder.cpp:764
Road * GetRoad() const
Definition: Lane.cpp:29
void AddDependencyToSignal(const SignId signal_id, const std::string dependency_id, const std::string dependency_type)
Definition: MapBuilder.cpp:308
std::unordered_map< SignId, std::unique_ptr< Signal > > _signals
Definition: MapData.h:98
static T Clamp(T a, T min=T(0), T max=T(1))
Definition: Math.h:49
Lane * GetNextLane(const double s, const LaneId lane_id)
Definition: Road.cpp:119
Junction * GetJunction(JuncId id)
Definition: road/Map.cpp:948
void CreateLaneRule(Lane *lane, const double s, const std::string value)
Definition: MapBuilder.cpp:144
Road & GetRoad(const RoadId id)
Definition: MapData.cpp:21
Lane * GetPrevLane(const double s, const LaneId lane_id)
Definition: Road.cpp:136
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
void CreateSectionOffset(Road *road, const double s, const double a, const double b, const double c, const double d)
Definition: MapBuilder.cpp:404
LaneSection * GetEndSection(LaneId id)
Get the end section (from road coordinates s) given a lane id.
Definition: Road.cpp:167
Lane * GetLane(const RoadId road_id, const LaneId lane_id, const double s)
Definition: MapBuilder.cpp:560
uint32_t SectionId
Definition: RoadTypes.h:21
void AddRoadGeometrySpiral(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double curvStart, const double curvEnd)
Definition: MapBuilder.cpp:436
std::map< LaneId, Lane > _lanes
Definition: LaneSection.h:61
int32_t JuncId
Definition: RoadTypes.h:17
void CreateLaneVisibility(Lane *lane, const double s, const double forward, const double back, const double left, const double right)
Definition: MapBuilder.cpp:152
std::vector< Lane * > GetLaneNext(RoadId road_id, SectionId section_id, LaneId lane_id)
Return a list of pointers to all lanes from a lane (using road and junction info).
Definition: MapBuilder.cpp:595
void AddValidityToSignalReference(element::RoadInfoSignal *signal_reference, const LaneId from_lane, const LaneId to_lane)
Definition: MapBuilder.cpp:301
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
LaneId GetPredecessor() const
Definition: Lane.h:102
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
geom::Location Location
Definition: rpc/Location.h:14
void AddRoadGeometryArc(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double curvature)
Definition: MapBuilder.cpp:415
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
void CreateLaneBorder(Lane *lane, const double s, const double a, const double b, const double c, const double d)
Definition: MapBuilder.cpp:113
carla::road::LaneSection * AddRoadSection(carla::road::Road *road, const SectionId id, const double s)
Definition: MapBuilder.cpp:342
carla::road::Lane * AddRoadSectionLane(carla::road::LaneSection *section, const LaneId lane_id, const uint32_t lane_type, const bool lane_level, const LaneId predecessor, const LaneId successor)
Definition: MapBuilder.cpp:352
LaneId GetSuccessor() const
Definition: Lane.h:98
#define RELEASE_ASSERT(pred)
Definition: Debug.h:84
RoadId GetId() const
Definition: Road.cpp:30
MapData * _map_data
Definition: Road.h:185
LaneType
Can be used as flags.
Definition: Lane.h:29
Lane & GetLaneByDistance(double s, LaneId lane_id)
Definition: Road.cpp:74
int32_t LaneId
Definition: RoadTypes.h:19
geom::Transform ComputeTransform(Waypoint waypoint) const
Definition: road/Map.cpp:264
void AddLaneLink(const JuncId junction_id, const ConId connection_id, const LaneId from, const LaneId to)
Definition: MapBuilder.cpp:544
LaneSectionMap _lane_sections
Definition: Road.h:197
void CreateRoadMarkTypeLine(Lane *lane, const int road_mark_id, const double length, const double space, const double tOffset, const double s, const std::string rule, const double width)
Definition: MapBuilder.cpp:207
static void log_warning(Args &&... args)
Definition: Logging.h:96
void RemoveZeroLaneValiditySignalReferences()
Removes signal references with lane validity equal to [0,0] as they have no effect on any road...
Definition: MapBuilder.cpp:970
RoadId GetPredecessor() const
Definition: Road.cpp:54
std::unordered_map< ConId, Connection > _connections
Definition: road/Junction.h:93
void CreateLaneMaterial(Lane *lane, const double s, const std::string surface, const double friction, const double roughness)
Definition: MapBuilder.cpp:133
static void ToLower(WritableRangeT &str)
Definition: StringUtil.h:36
void CreateLaneSpeed(Lane *lane, const double s, const double max, const std::string unit)
Definition: MapBuilder.cpp:228
LaneSection & Emplace(SectionId id, double s)
geom::Transform ComputeSignalTransform(std::unique_ptr< Signal > &signal, MapData &data)
Definition: MapBuilder.cpp:752
void CreatePointersBetweenRoadSegments()
Create the pointers between RoadSegments based on the ids.
Definition: MapBuilder.cpp:698
LaneSection * GetStartSection(LaneId id)
Get the start section (from road coordinates s) given a lane id.
Definition: Road.cpp:154
void CreateLaneHeight(Lane *lane, const double s, const double inner, const double outer)
Definition: MapBuilder.cpp:124
void SolveControllerAndJuntionReferences()
Solve the references between Controllers and Juntions.
Definition: MapBuilder.cpp:786
element::DirectedPoint GetDirectedPointInNoLaneOffset(const double s) const
Returns a directed point on the center of the road (lane 0), with the corresponding laneOffset and el...
Definition: Road.cpp:202
void AddJunction(const JuncId id, const std::string name)
Definition: MapBuilder.cpp:530
void AddConnection(const JuncId junction_id, const ConId connection_id, const RoadId incoming_road, const RoadId connecting_road)
Definition: MapBuilder.cpp:534
uint32_t ConId
Definition: RoadTypes.h:27
void CreateRoadSpeed(Road *road, const double s, const std::string type, const double max, const std::string unit)
Definition: MapBuilder.cpp:394
std::vector< LaneValidity > _validities
element::RoadInfoSignal * AddSignal(Road *road, const SignId signal_id, const double s, const double t, const std::string name, const std::string dynamic, const std::string orientation, const double zOffset, const std::string country, const std::string type, const std::string subtype, const double value, const std::string unit, const double height, const double width, const std::string text, const double hOffset, const double pitch, const double roll)
Definition: MapBuilder.cpp:238
std::vector< std::pair< RoadId, LaneId > > GetJunctionLanes(JuncId junction_id, RoadId road_id, LaneId lane_id)
Definition: MapBuilder.cpp:661
static bool IsTrafficLight(const std::string &type)
Definition: SignalType.cpp:121
static constexpr T ToDegrees(T rad)
Definition: Math.h:37
void CreateRoadMark(Lane *lane, const int road_mark_id, const double s, const std::string type, const std::string weight, const std::string color, const std::string material, const double width, const std::string lane_change, const double height, const std::string type_name, const double type_width)
Definition: MapBuilder.cpp:175
double GetLength() const
Definition: Road.cpp:38
std::string ContId
Definition: RoadTypes.h:29
LaneSection & GetById(SectionId id)
void AddRoadGeometryPoly3(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double a, const double b, const double c, const double d)
Definition: MapBuilder.cpp:460
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
Definition: road/Map.cpp:698
void ApplyLateralOffset(float lateral_offset)
Definition: Geometry.cpp:28
void AddRoadGeometryLine(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length)
Definition: MapBuilder.cpp:375
Lane * GetLane(const LaneId id)
Definition: LaneSection.cpp:31
void CreateLaneAccess(Lane *lane, const double s, const std::string restriction)
Definition: MapBuilder.cpp:105
carla::road::Road * AddRoad(const RoadId road_id, const std::string name, const double length, const JuncId junction_id, const RoadId predecessor, const RoadId successor)
Definition: MapBuilder.cpp:317
double GetLaneWidth(Waypoint waypoint) const
Definition: road/Map.cpp:276
MapData _data
Definition: road/Map.h:184