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  const SignId signal_id,
284  const double x,
285  const double y,
286  const double z,
287  const double hdg,
288  const double pitch,
289  const double roll) {
290  std::unique_ptr<Signal> &signal = _temp_signal_container[signal_id];
291  signal->_using_inertial_position = true;
292  geom::Location location = geom::Location(x, -y, z);
293  signal->_transform = geom::Transform (location, geom::Rotation(
294  geom::Math::ToDegrees(static_cast<float>(pitch)),
295  geom::Math::ToDegrees(static_cast<float>(-hdg)),
296  geom::Math::ToDegrees(static_cast<float>(roll))));
297  }
298 
300  const SignId signal_id,
301  const RoadId road_id,
302  const double s,
303  const double t,
304  const double zOffset,
305  const double hOffset,
306  const double pitch,
307  const double roll) {
308  std::unique_ptr<Signal> &signal = _temp_signal_container[signal_id];
309  signal->_road_id = road_id;
310  signal->_s = s;
311  signal->_t = t;
312  signal->_zOffset = zOffset;
313  signal->_hOffset = hOffset;
314  signal->_pitch = pitch;
315  signal->_roll = roll;
316  }
317 
319  Road* road,
320  const SignId signal_id,
321  const double s_position,
322  const double t_position,
323  const std::string signal_reference_orientation) {
324 
325  const double epsilon = 0.00001;
326  RELEASE_ASSERT(s_position >= 0.0);
327  // Prevent s_position from being equal to the road length
328  double fixed_s = geom::Math::Clamp(s_position, 0.0, road->GetLength() - epsilon);
329  _temp_road_info_container[road].emplace_back(std::make_unique<element::RoadInfoSignal>(
330  signal_id, road->GetId(), fixed_s, t_position, signal_reference_orientation));
331  auto road_info_signal = static_cast<element::RoadInfoSignal*>(
332  _temp_road_info_container[road].back().get());
333  _temp_signal_reference_container.emplace_back(road_info_signal);
334  return road_info_signal;
335  }
336 
338  element::RoadInfoSignal* signal_reference,
339  const LaneId from_lane,
340  const LaneId to_lane) {
341  signal_reference->_validities.emplace_back(LaneValidity(from_lane, to_lane));
342  }
343 
345  const SignId signal_id,
346  const std::string dependency_id,
347  const std::string dependency_type) {
348  _temp_signal_container[signal_id]->_dependencies.emplace_back(
349  SignalDependency(dependency_id, dependency_type));
350  }
351 
352  // build road objects
354  const RoadId road_id,
355  const std::string name,
356  const double length,
357  const JuncId junction_id,
358  const RoadId predecessor,
359  const RoadId successor)
360  {
361 
362  // add it
363  auto road = &(_map_data._roads.emplace(road_id, Road()).first->second);
364 
365  // set road data
366  road->_map_data = &_map_data;
367  road->_id = road_id;
368  road->_name = name;
369  road->_length = length;
370  road->_junction_id = junction_id;
371  (junction_id != -1) ? road->_is_junction = true : road->_is_junction = false;
372  road->_successor = successor;
373  road->_predecessor = predecessor;
374 
375  return road;
376  }
377 
379  Road *road,
380  const SectionId id,
381  const double s) {
382  DEBUG_ASSERT(road != nullptr);
383  carla::road::LaneSection &sec = road->_lane_sections.Emplace(id, s);
384  sec._road = road;
385  return &sec;
386  }
387 
389  carla::road::LaneSection *section,
390  const int32_t lane_id,
391  const uint32_t lane_type,
392  const bool lane_level,
393  const int32_t predecessor,
394  const int32_t successor) {
395  DEBUG_ASSERT(section != nullptr);
396 
397  // add the lane
398  auto *lane = &((section->_lanes.emplace(lane_id, Lane()).first)->second);
399 
400  // set lane data
401  lane->_id = lane_id;
402  lane->_lane_section = section;
403  lane->_level = lane_level;
404  lane->_type = static_cast<carla::road::Lane::LaneType>(lane_type);
405  lane->_successor = successor;
406  lane->_predecessor = predecessor;
407 
408  return lane;
409  }
410 
412  Road *road,
413  const double s,
414  const double x,
415  const double y,
416  const double hdg,
417  const double length) {
418  DEBUG_ASSERT(road != nullptr);
419  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
420  auto line_geometry = std::make_unique<GeometryLine>(
421  s,
422  length,
423  hdg,
424  location);
425 
426  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
427  std::move(line_geometry))));
428  }
429 
431  Road *road,
432  const double s,
433  const std::string /*type*/,
434  const double max,
435  const std::string /*unit*/) {
436  DEBUG_ASSERT(road != nullptr);
437  _temp_road_info_container[road].emplace_back(std::make_unique<RoadInfoSpeed>(s, max));
438  }
439 
441  Road *road,
442  const double s,
443  const double a,
444  const double b,
445  const double c,
446  const double d) {
447  DEBUG_ASSERT(road != nullptr);
448  _temp_road_info_container[road].emplace_back(std::make_unique<RoadInfoLaneOffset>(s, a, b, c, d));
449  }
450 
452  Road *road,
453  const double s,
454  const double x,
455  const double y,
456  const double hdg,
457  const double length,
458  const double curvature) {
459  DEBUG_ASSERT(road != nullptr);
460  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
461  auto arc_geometry = std::make_unique<GeometryArc>(
462  s,
463  length,
464  hdg,
465  location,
466  curvature);
467 
468  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
469  std::move(arc_geometry))));
470  }
471 
473  Road * road,
474  const double s,
475  const double x,
476  const double y,
477  const double hdg,
478  const double length,
479  const double curvStart,
480  const double curvEnd) {
481  //throw_exception(std::runtime_error("geometry spiral not supported"));
482  DEBUG_ASSERT(road != nullptr);
483  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
484  auto spiral_geometry = std::make_unique<GeometrySpiral>(
485  s,
486  length,
487  hdg,
488  location,
489  curvStart,
490  curvEnd);
491 
492  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
493  std::move(spiral_geometry))));
494  }
495 
497  Road * road,
498  const double s,
499  const double x,
500  const double y,
501  const double hdg,
502  const double length,
503  const double a,
504  const double b,
505  const double c,
506  const double d) {
507  //throw_exception(std::runtime_error("geometry poly3 not supported"));
508  DEBUG_ASSERT(road != nullptr);
509  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
510  auto poly3_geometry = std::make_unique<GeometryPoly3>(
511  s,
512  length,
513  hdg,
514  location,
515  a,
516  b,
517  c,
518  d);
519  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
520  std::move(poly3_geometry))));
521  }
522 
524  Road * road,
525  const double s,
526  const double x,
527  const double y,
528  const double hdg,
529  const double length,
530  const double aU,
531  const double bU,
532  const double cU,
533  const double dU,
534  const double aV,
535  const double bV,
536  const double cV,
537  const double dV,
538  const std::string p_range) {
539  //throw_exception(std::runtime_error("geometry poly3 not supported"));
540  bool arcLength;
541  if(p_range == "arcLength"){
542  arcLength = true;
543  }else{
544  arcLength = false;
545  }
546  DEBUG_ASSERT(road != nullptr);
547  const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
548  auto parampoly3_geometry = std::make_unique<GeometryParamPoly3>(
549  s,
550  length,
551  hdg,
552  location,
553  aU,
554  bU,
555  cU,
556  dU,
557  aV,
558  bV,
559  cV,
560  dV,
561  arcLength);
562  _temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
563  std::move(parampoly3_geometry))));
564  }
565 
566  void MapBuilder::AddJunction(const int32_t id, const std::string name) {
567  _map_data.GetJunctions().emplace(id, Junction(id, name));
568  }
569 
571  const JuncId junction_id,
572  const ConId connection_id,
573  const RoadId incoming_road,
574  const RoadId connecting_road) {
575  DEBUG_ASSERT(_map_data.GetJunction(junction_id) != nullptr);
576  _map_data.GetJunction(junction_id)->GetConnections().emplace(connection_id,
577  Junction::Connection(connection_id, incoming_road, connecting_road));
578  }
579 
581  const JuncId junction_id,
582  const ConId connection_id,
583  const LaneId from,
584  const LaneId to) {
585  DEBUG_ASSERT(_map_data.GetJunction(junction_id) != nullptr);
586  _map_data.GetJunction(junction_id)->GetConnection(connection_id)->AddLaneLink(from, to);
587  }
588 
590  const JuncId junction_id,
591  std::set<road::ContId>&& controllers) {
592  DEBUG_ASSERT(_map_data.GetJunction(junction_id) != nullptr);
593  _map_data.GetJunction(junction_id)->_controllers = std::move(controllers);
594  }
595 
597  const RoadId road_id,
598  const LaneId lane_id,
599  const double s) {
600  return &_map_data.GetRoad(road_id).GetLaneByDistance(s, lane_id);
601  }
602 
604  const RoadId road_id) {
605  return &_map_data.GetRoad(road_id);
606  }
607 
608  // return the pointer to a lane object
609  Lane *MapBuilder::GetEdgeLanePointer(RoadId road_id, bool from_start, LaneId lane_id) {
610 
611  if (!_map_data.ContainsRoad(road_id)) {
612  return nullptr;
613  }
614  Road &road = _map_data.GetRoad(road_id);
615 
616  // get the lane section
617  LaneSection *section;
618  if (from_start) {
619  section = road.GetStartSection(lane_id);
620  } else {
621  section = road.GetEndSection(lane_id);
622  }
623 
624  // get the lane
625  DEBUG_ASSERT(section != nullptr);
626  return section->GetLane(lane_id);
627  }
628 
629  // return a list of pointers to all lanes from a lane (using road and junction
630  // info)
631  std::vector<Lane *> MapBuilder::GetLaneNext(
632  RoadId road_id,
633  SectionId section_id,
634  LaneId lane_id) {
635  std::vector<Lane *> result;
636 
637  if (!_map_data.ContainsRoad(road_id)) {
638  return result;
639  }
640  Road &road = _map_data.GetRoad(road_id);
641 
642  // get the section
643  LaneSection &section = road._lane_sections.GetById(section_id);
644 
645  // get the lane
646  Lane *lane = section.GetLane(lane_id);
647  DEBUG_ASSERT(lane != nullptr);
648 
649  // successor and predecessor (road and lane)
650  LaneId next;
651  RoadId next_road;
652  if (lane_id <= 0) {
653  next_road = road.GetSuccessor();
654  next = lane->GetSuccessor();
655  } else {
656  next_road = road.GetPredecessor();
657  next = lane->GetPredecessor();
658  }
659 
660  // check to see if next is a road or a junction
661  bool next_is_junction = !_map_data.ContainsRoad(next_road);
662  double s = section.GetDistance();
663 
664  // check if we are in a lane section in the middle
665  if ((lane_id > 0 && s > 0) ||
666  (lane_id <= 0 && road._lane_sections.upper_bound(s) != road._lane_sections.end())) {
667  // check if lane has a next link (if not, it deads in the middle section)
668  if (next != 0 || (lane_id == 0 && next == 0)) {
669  // change to next / prev section
670  if (lane_id <= 0) {
671  result.push_back(road.GetNextLane(s, next));
672  } else {
673  result.push_back(road.GetPrevLane(s, next));
674  }
675  }
676  } else if (!next_is_junction) {
677  // change to another road / junction
678  if (next != 0 || (lane_id == 0 && next == 0)) {
679  // single road
680  result.push_back(GetEdgeLanePointer(next_road, (next <= 0), next));
681  }
682  } else {
683  // several roads (junction)
684 
685  /// @todo Is it correct to use a road id as section id? (NS: I just added
686  /// this cast to avoid compiler warnings).
687  auto next_road_as_junction = static_cast<JuncId>(next_road);
688  auto options = GetJunctionLanes(next_road_as_junction, road_id, lane_id);
689  for (auto opt : options) {
690  result.push_back(GetEdgeLanePointer(opt.first, (opt.second <= 0), opt.second));
691  }
692  }
693 
694  return result;
695  }
696 
697  std::vector<std::pair<RoadId, LaneId>> MapBuilder::GetJunctionLanes(
698  JuncId junction_id,
699  RoadId road_id,
700  LaneId lane_id) {
701  std::vector<std::pair<RoadId, LaneId>> result;
702 
703  // get the junction
704  Junction *junction = _map_data.GetJunction(junction_id);
705  if (junction == nullptr) {
706  return result;
707  }
708 
709  // check all connections
710  for (auto con : junction->_connections) {
711  // only connections for our road
712  if (con.second.incoming_road == road_id) {
713  // for center lane it is always next lane id 0, we don't need to search
714  // because it is not in the junction
715  if (lane_id == 0) {
716  result.push_back(std::make_pair(con.second.connecting_road, 0));
717  } else {
718  // check all lane links
719  for (auto link : con.second.lane_links) {
720  // is our lane id ?
721  if (link.from == lane_id) {
722  // add as option
723  result.push_back(std::make_pair(con.second.connecting_road, link.to));
724  }
725  }
726  }
727  }
728  }
729 
730  return result;
731  }
732 
733  // assign pointers to the next lanes
735  // process each lane to define its nexts
736  for (auto &road : _map_data._roads) {
737  for (auto &section : road.second._lane_sections) {
738  for (auto &lane : section.second._lanes) {
739 
740  // assign the next lane pointers
741  lane.second._next_lanes = GetLaneNext(road.first, section.second._id, lane.first);
742 
743  // add to each lane found, this as its predecessor
744  for (auto next_lane : lane.second._next_lanes) {
745  // add as previous
746  DEBUG_ASSERT(next_lane != nullptr);
747  next_lane->_prev_lanes.push_back(&lane.second);
748  }
749 
750  }
751  }
752  }
753 
754  // process each lane to define its nexts
755  for (auto &road : _map_data._roads) {
756  for (auto &section : road.second._lane_sections) {
757  for (auto &lane : section.second._lanes) {
758 
759  // add next roads
760  for (auto next_lane : lane.second._next_lanes) {
761  DEBUG_ASSERT(next_lane != nullptr);
762  // avoid same road
763  if (next_lane->GetRoad() != &road.second) {
764  if (std::find(road.second._nexts.begin(), road.second._nexts.end(),
765  next_lane->GetRoad()) == road.second._nexts.end()) {
766  road.second._nexts.push_back(next_lane->GetRoad());
767  }
768  }
769  }
770 
771  // add prev roads
772  for (auto prev_lane : lane.second._prev_lanes) {
773  DEBUG_ASSERT(prev_lane != nullptr);
774  // avoid same road
775  if (prev_lane->GetRoad() != &road.second) {
776  if (std::find(road.second._prevs.begin(), road.second._prevs.end(),
777  prev_lane->GetRoad()) == road.second._prevs.end()) {
778  road.second._prevs.push_back(prev_lane->GetRoad());
779  }
780  }
781  }
782 
783  }
784  }
785  }
786  }
787 
788  geom::Transform MapBuilder::ComputeSignalTransform(std::unique_ptr<Signal> &signal, MapData &data) {
789  DirectedPoint point = data.GetRoad(signal->_road_id).GetDirectedPointInNoLaneOffset(signal->_s);
790  point.ApplyLateralOffset(static_cast<float>(-signal->_t));
791  point.location.y *= -1; // Unreal Y axis hack
792  point.location.z += static_cast<float>(signal->_zOffset);
793  geom::Transform transform(point.location, geom::Rotation(
794  geom::Math::ToDegrees(static_cast<float>(signal->_pitch)),
795  geom::Math::ToDegrees(static_cast<float>(-(point.tangent + signal->_hOffset))),
796  geom::Math::ToDegrees(static_cast<float>(signal->_roll))));
797  return transform;
798  }
799 
801  for(auto signal_reference : _temp_signal_reference_container){
802  signal_reference->_signal =
803  _temp_signal_container[signal_reference->_signal_id].get();
804  }
805 
806  for(auto& signal_pair : _temp_signal_container) {
807  auto& signal = signal_pair.second;
808  if (signal->_using_inertial_position) {
809  continue;
810  }
811  auto transform = ComputeSignalTransform(signal, _map_data);
812  if (SignalType::IsTrafficLight(signal->GetType())) {
813  transform.location = transform.location +
814  geom::Location(transform.GetForwardVector()*0.25);
815  }
816  signal->_transform = transform;
817  }
818 
819  _map_data._signals = std::move(_temp_signal_container);
820 
821  GenerateDefaultValiditiesForSignalReferences();
822  }
823 
825  for(const auto& junction : _map_data._junctions) {
826  for(const auto& controller : junction.second._controllers) {
827  auto it = _map_data._controllers.find(controller);
828  DEBUG_ASSERT(it != _map_data._controllers.end());
829  it->second->_junctions.insert(junction.first);
830  for(const auto & signal : it->second->_signals) {
831  auto signal_it = _map_data._signals.find(signal);
832  signal_it->second->_controllers.insert(controller);
833  }
834  }
835  }
836  }
837 
839  for (auto &junctionpair : map._data.GetJunctions()) {
840  auto* junction = map.GetJunction(junctionpair.first);
841  auto waypoints = map.GetJunctionWaypoints(junction->GetId(), Lane::LaneType::Any);
842  const int number_intervals = 10;
843 
844  float minx = std::numeric_limits<float>::max();
845  float miny = std::numeric_limits<float>::max();
846  float minz = std::numeric_limits<float>::max();
847  float maxx = -std::numeric_limits<float>::max();
848  float maxy = -std::numeric_limits<float>::max();
849  float maxz = -std::numeric_limits<float>::max();
850 
851  auto get_min_max = [&](geom::Location position) {
852  if (position.x < minx) {
853  minx = position.x;
854  }
855  if (position.y < miny) {
856  miny = position.y;
857  }
858  if (position.z < minz) {
859  minz = position.z;
860  }
861 
862  if (position.x > maxx) {
863  maxx = position.x;
864  }
865  if (position.y > maxy) {
866  maxy = position.y;
867  }
868  if (position.z > maxz) {
869  maxz = position.z;
870  }
871  };
872 
873  for (auto &waypoint_p : waypoints) {
874  auto &waypoint_start = waypoint_p.first;
875  auto &waypoint_end = waypoint_p.second;
876  double interval = (waypoint_end.s - waypoint_start.s) / static_cast<double>(number_intervals);
877  auto next_wp = waypoint_end;
878  auto location = map.ComputeTransform(next_wp).location;
879 
880  get_min_max(location);
881 
882  next_wp = waypoint_start;
883  location = map.ComputeTransform(next_wp).location;
884 
885  get_min_max(location);
886 
887  for (int i = 0; i < number_intervals; ++i) {
888  if (interval < std::numeric_limits<double>::epsilon())
889  break;
890  auto next = map.GetNext(next_wp, interval);
891  if(next.size()){
892  next_wp = next.back();
893  }
894 
895  location = map.ComputeTransform(next_wp).location;
896  get_min_max(location);
897  }
898  }
899  carla::geom::Location location(0.5f * (maxx + minx), 0.5f * (maxy + miny), 0.5f * (maxz + minz));
900  carla::geom::Vector3D extent(0.5f * (maxx - minx), 0.5f * (maxy - miny), 0.5f * (maxz - minz));
901 
902  junction->_bounding_box = carla::geom::BoundingBox(location, extent);
903  }
904  }
905 
907  const ContId controller_id,
908  const std::string controller_name,
909  const uint32_t controller_sequence,
910  const std::set<road::SignId>&& signals) {
911 
912  // Add the Controller to MapData
913  auto controller_pair = _map_data._controllers.emplace(
914  std::make_pair(
915  controller_id,
916  std::make_unique<Controller>(controller_id, controller_name, controller_sequence)));
917 
918  DEBUG_ASSERT(controller_pair.first != _map_data._controllers.end());
919  DEBUG_ASSERT(controller_pair.first->second);
920 
921  // Add the signals owned by the controller
922  controller_pair.first->second->_signals = std::move(signals);
923 
924  // Add ContId to the signal owned by this Controller
925  auto& signals_map = _map_data._signals;
926  for(auto signal: signals) {
927  auto it = signals_map.find(signal);
928  if(it != signals_map.end()) {
929  it->second->_controllers.insert(signal);
930  }
931  }
932 }
933 
935  for (auto &junctionpair : map._data.GetJunctions()) {
936  auto& junction = junctionpair.second;
937  junction._road_conflicts = (map.ComputeJunctionConflicts(junction.GetId()));
938  }
939  }
940 
942  for (auto * signal_reference : _temp_signal_reference_container) {
943  if (signal_reference->_validities.size() == 0) {
944  Road* road = GetRoad(signal_reference->GetRoadId());
945  auto lanes = road->GetLanesByDistance(signal_reference->GetS());
946  switch (signal_reference->GetOrientation()) {
948  LaneId min_lane = 1;
949  LaneId max_lane = 0;
950  for (const auto* lane : lanes) {
951  auto lane_id = lane->GetId();
952  if(lane_id > max_lane) {
953  max_lane = lane_id;
954  }
955  }
956  if(min_lane <= max_lane) {
957  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
958  }
959  break;
960  }
962  LaneId min_lane = 0;
963  LaneId max_lane = -1;
964  for (const auto* lane : lanes) {
965  auto lane_id = lane->GetId();
966  if(lane_id < min_lane) {
967  min_lane = lane_id;
968  }
969  }
970  if(min_lane <= max_lane) {
971  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
972  }
973  break;
974  }
976  // Get positive lanes
977  LaneId min_lane = 1;
978  LaneId max_lane = 0;
979  for (const auto* lane : lanes) {
980  auto lane_id = lane->GetId();
981  if(lane_id > max_lane) {
982  max_lane = lane_id;
983  }
984  }
985  if(min_lane <= max_lane) {
986  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
987  }
988 
989  // get negative lanes
990  min_lane = 0;
991  max_lane = -1;
992  for (const auto* lane : lanes) {
993  auto lane_id = lane->GetId();
994  if(lane_id < min_lane) {
995  min_lane = lane_id;
996  }
997  }
998  if(min_lane <= max_lane) {
999  AddValidityToSignalReference(signal_reference, min_lane, max_lane);
1000  }
1001  break;
1002  }
1003  }
1004  }
1005  }
1006  }
1007 
1009  std::vector<element::RoadInfoSignal*> elements_to_remove;
1010  for (auto * signal_reference : _temp_signal_reference_container) {
1011  bool should_remove = true;
1012  for (auto & lane_validity : signal_reference->_validities) {
1013  if ( (lane_validity._from_lane != 0) ||
1014  (lane_validity._to_lane != 0)) {
1015  should_remove = false;
1016  break;
1017  }
1018  }
1019  if (signal_reference->_validities.size() == 0) {
1020  should_remove = false;
1021  }
1022  if (should_remove) {
1023  elements_to_remove.push_back(signal_reference);
1024  }
1025  }
1026  for (auto* element : elements_to_remove) {
1027  auto road_id = element->GetRoadId();
1028  auto& road_info = _temp_road_info_container[GetRoad(road_id)];
1029  road_info.erase(std::remove_if(road_info.begin(), road_info.end(),
1030  [=] (auto& info_ptr) {
1031  return (info_ptr.get() == element);
1032  }), road_info.end());
1033  _temp_signal_reference_container.erase(std::remove(_temp_signal_reference_container.begin(),
1034  _temp_signal_reference_container.end(), element),
1035  _temp_signal_reference_container.end());
1036  }
1037  }
1038 
1040  for (auto& signal_pair : map._data._signals) {
1041  auto& signal = signal_pair.second;
1042  auto signal_position = signal->GetTransform().location;
1043  auto closest_waypoint_to_signal =
1044  map.GetClosestWaypointOnRoad(signal_position);
1045  // workarround to not move speed signals
1046  if (signal->GetName().substr(0, 6) == "Speed_" ||
1047  signal->GetName().substr(0, 6) == "speed_" ||
1048  signal->GetName().find("Stencil_STOP") != std::string::npos ||
1049  signal->_using_inertial_position) {
1050  continue;
1051  }
1052  if(closest_waypoint_to_signal) {
1053  auto distance_to_road =
1054  (map.ComputeTransform(closest_waypoint_to_signal.get()).location -
1055  signal_position).Length();
1056  double lane_width = map.GetLaneWidth(closest_waypoint_to_signal.get());
1057  int iter = 0;
1058  int MaxIter = 10;
1059  // Displaces signal until it finds a suitable spot
1060  while(distance_to_road < lane_width * 0.5 && iter < MaxIter) {
1061  if(iter == 0) {
1062  log_warning("Traffic sign",
1063  signal->GetSignalId(),
1064  "overlaps a driving lane. Moving out of the road...");
1065  }
1066  geom::Vector3D displacement = 1.f*(signal->GetTransform().GetRightVector()) *
1067  static_cast<float>(abs(lane_width))*0.5f;
1068  signal_position += displacement;
1069  closest_waypoint_to_signal =
1070  map.GetClosestWaypointOnRoad(signal_position);
1071  distance_to_road =
1072  (map.ComputeTransform(closest_waypoint_to_signal.get()).location -
1073  signal_position).Length();
1074  lane_width = map.GetLaneWidth(closest_waypoint_to_signal.get());
1075  iter++;
1076  }
1077  if(iter == MaxIter) {
1078  log_warning("Failed to find suitable place for signal.");
1079  } else {
1080  // Only perform the displacement if a good location has been found
1081  signal->_transform.location = signal_position;
1082  }
1083  }
1084  }
1085  }
1086 
1087 } // namespace road
1088 } // namespace carla
void AddSignalPositionInertial(const SignId signal_id, const double x, const double y, const double z, const double hdg, const double pitch, const double roll)
Definition: MapBuilder.cpp:282
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:523
std::unordered_map< road::RoadId, std::unordered_set< road::RoadId > > ComputeJunctionConflicts(JuncId id) const
Definition: road/Map.cpp:749
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:603
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:318
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:609
void AddJunctionController(const JuncId junction_id, std::set< ContId > &&controllers)
Definition: MapBuilder.cpp:589
void ComputeJunctionRoadConflicts(Map &map)
Compute the conflicts of the roads (intersecting roads)
Definition: MapBuilder.cpp:934
void CreateJunctionBoundingBoxes(Map &map)
Create the bounding boxes of each junction.
Definition: MapBuilder.cpp:838
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:906
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:941
void SolveSignalReferencesAndTransforms()
Solves the signal references in the road.
Definition: MapBuilder.cpp:800
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:344
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:982
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:133
void CreateSectionOffset(Road *road, const double s, const double a, const double b, const double c, const double d)
Definition: MapBuilder.cpp:440
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:596
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:472
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:631
void AddValidityToSignalReference(element::RoadInfoSignal *signal_reference, const LaneId from_lane, const LaneId to_lane)
Definition: MapBuilder.cpp:337
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:451
std::vector< Waypoint > GetNext(Waypoint waypoint, double distance) const
Return the list of waypoints at distance such that a vehicle at waypoint could drive to...
Definition: road/Map.cpp:545
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:378
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:388
LaneId GetSuccessor() const
Definition: Lane.h:98
void AddSignalPositionRoad(const SignId signal_id, const RoadId road_id, const double s, const double t, const double zOffset, const double hOffset, const double pitch, const double roll)
Definition: MapBuilder.cpp:299
#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:580
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...
RoadId GetPredecessor() const
Definition: Road.cpp:54
std::unordered_map< ConId, Connection > _connections
Definition: road/Junction.h:97
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:788
void CreatePointersBetweenRoadSegments()
Create the pointers between RoadSegments based on the ids.
Definition: MapBuilder.cpp:734
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:824
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:566
void AddConnection(const JuncId junction_id, const ConId connection_id, const RoadId incoming_road, const RoadId connecting_road)
Definition: MapBuilder.cpp:570
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:430
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:697
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:496
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
Definition: road/Map.cpp:732
void ApplyLateralOffset(float lateral_offset)
Definition: Geometry.cpp:28
geom::Transform Transform
Definition: rpc/Transform.h:16
void AddRoadGeometryLine(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length)
Definition: MapBuilder.cpp:411
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:353
double GetLaneWidth(Waypoint waypoint) const
Definition: road/Map.cpp:276
carla::SharedPtr< carla::client::Junction > Junction
MapData _data
Definition: road/Map.h:184