21 #include <unordered_map> 31 static constexpr
double EPSILON = 10.0 * std::numeric_limits<double>::epsilon();
38 static std::vector<T>
ConcatVectors(std::vector<T> dst, std::vector<T> src) {
39 if (src.size() > dst.size()) {
44 std::make_move_iterator(src.begin()),
45 std::make_move_iterator(src.end()));
50 if (lane.
GetId() <= 0) {
58 if (lane.
GetId() > 0) {
66 template <
typename FuncT>
72 for (
const auto &pair : lane_section.
GetLanes()) {
73 const auto &lane = pair.second;
74 if (lane.GetId() == 0) {
87 template <
typename FuncT>
94 for (
const auto &pair : lane_section.
GetLanes()) {
95 const auto &lane = pair.second;
96 if (lane.GetId() == 0) {
99 if ((static_cast<int32_t>(lane.GetType()) & static_cast<int32_t>(lane_type)) > 0) {
102 lane_section.
GetId(),
110 template <
typename FuncT>
117 std::forward<FuncT>(func));
122 template <
typename FuncT>
130 std::forward<FuncT>(func));
135 template <
typename FuncT>
142 std::forward<FuncT>(func));
158 int32_t lane_type)
const {
159 std::vector<Rtree::TreeElement> query_result =
162 const Lane &lane = GetLane(element.second.first);
163 return (lane_type & static_cast<int32_t>(lane.GetType())) > 0;
166 if (query_result.size() == 0) {
167 return boost::optional<Waypoint>{};
177 Waypoint result_start = query_result.front().second.first;
178 Waypoint result_end = query_result.front().second.second;
180 if (result_start.
lane_id < 0) {
181 double delta_s = distance_to_segment.first;
182 double final_s = result_start.
s + delta_s;
183 if (final_s >= result_end.
s) {
185 }
else if (delta_s <= 0) {
188 return GetNext(result_start, delta_s).front();
191 double delta_s = distance_to_segment.first;
192 double final_s = result_start.
s - delta_s;
193 if (final_s <= result_end.
s) {
195 }
else if (delta_s <= 0) {
198 return GetNext(result_start, delta_s).front();
205 int32_t lane_type)
const {
206 boost::optional<Waypoint> w = GetClosestWaypointOnRoad(pos, lane_type);
208 if (!w.has_value()) {
214 const auto half_lane_width =
217 if (dist < half_lane_width) {
221 return boost::optional<Waypoint>{};
236 if (!_data.ContainsRoad(waypoint.
road_id)) {
237 return boost::optional<Waypoint>{};
239 const Road &road = _data.GetRoad(waypoint.
road_id);
243 return boost::optional<Waypoint>{};
247 bool lane_found =
false;
249 if (section.ContainsLane(lane_id)) {
258 return boost::optional<Waypoint>{};
265 return GetLane(waypoint).ComputeTransform(waypoint.
s);
273 return GetLane(waypoint).GetType();
277 const auto s = waypoint.
s;
279 const auto &lane = GetLane(waypoint);
286 return lane_width_info->GetPolynomial().Evaluate(s);
290 return _data.GetRoad(road_id).GetJunctionId();
294 return _data.GetRoad(road_id).IsJunction();
297 std::pair<const RoadInfoMarkRecord *, const RoadInfoMarkRecord *>
301 return std::make_pair(
nullptr,
nullptr);
303 const auto s = waypoint.
s;
305 const auto ¤t_lane = GetLane(waypoint);
309 const auto inner_lane_id = waypoint.
lane_id < 0 ?
313 const auto &inner_lane = current_lane.GetRoad()->GetLaneById(waypoint.
section_id, inner_lane_id);
318 return std::make_pair(current_lane_info, inner_lane_info);
322 Waypoint waypoint,
double distance,
bool stop_at_junction)
const {
324 const auto &lane = GetLane(waypoint);
325 const bool forward = (waypoint.
lane_id <= 0);
326 const double signed_distance = forward ? distance : -distance;
327 const double relative_s = waypoint.
s - lane.GetDistance();
328 const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
331 auto &road =_data.GetRoad(waypoint.
road_id);
332 std::vector<SignalSearchData> result;
336 if (distance <= remaining_lane_length) {
338 waypoint.
s, waypoint.
s + signed_distance);
339 for(
auto* signal : signals){
340 double distance_to_signal = 0;
342 distance_to_signal = signal->
GetDistance() - waypoint.
s;
344 distance_to_signal = waypoint.
s - signal->GetDistance();
347 bool is_valid =
false;
348 for (
auto &validity : signal->GetValidities()) {
349 if (waypoint.
lane_id >= validity._from_lane &&
350 waypoint.
lane_id <= validity._to_lane) {
358 if (distance_to_signal == 0) {
361 distance_to_signal});
364 {signal, GetNext(waypoint, distance_to_signal).front(),
365 distance_to_signal});
371 const double signed_remaining_length = forward ? remaining_lane_length : -remaining_lane_length;
375 waypoint.
s, waypoint.
s + signed_remaining_length);
376 for(
auto* signal : signals){
377 double distance_to_signal = 0;
379 distance_to_signal = signal->
GetDistance() - waypoint.
s;
381 distance_to_signal = waypoint.
s - signal->GetDistance();
384 bool is_valid =
false;
385 for (
auto &validity : signal->GetValidities()) {
386 if (waypoint.
lane_id >= validity._from_lane &&
387 waypoint.
lane_id <= validity._to_lane) {
395 if (distance_to_signal == 0) {
398 distance_to_signal});
401 {signal, GetNext(waypoint, distance_to_signal).front(),
402 distance_to_signal});
406 for (
auto &successor : GetSuccessors(waypoint)) {
407 if(_data.GetRoad(successor.road_id).IsJunction() && stop_at_junction){
410 auto& sucessor_lane = _data.GetRoad(successor.road_id).
411 GetLaneByDistance(successor.s, successor.lane_id);
412 if (successor.lane_id < 0) {
413 successor.s = sucessor_lane.GetDistance();
415 successor.s = sucessor_lane.GetDistance() + sucessor_lane.GetLength();
417 auto sucessor_signals = GetSignalsInDistance(
418 successor, distance - remaining_lane_length, stop_at_junction);
419 for(
auto& signal : sucessor_signals){
420 signal.accumulated_s += remaining_lane_length;
427 std::vector<const element::RoadInfoSignal*>
429 std::vector<const element::RoadInfoSignal*> result;
430 for (
const auto& road_pair : _data.GetRoads()) {
431 const auto &road = road_pair.second;
433 for(
const auto* road_info : road_infos) {
434 result.push_back(road_info);
447 std::vector<geom::Location> result;
449 for (
const auto &pair : _data.GetRoads()) {
450 const auto &road = pair.second;
451 std::vector<const RoadInfoCrosswalk *> crosswalks = road.GetInfos<
RoadInfoCrosswalk>();
452 if (crosswalks.size() > 0) {
453 for (
auto crosswalk : crosswalks) {
455 std::vector<geom::Location> points;
458 for (
const auto §ion : road.GetLaneSectionsAt(crosswalk->GetS())) {
460 for (
const auto &lane : section.GetLanes()) {
462 if (lane.first == 0) {
467 waypoint.
s = crosswalk->GetS();
468 base = ComputeTransform(waypoint);
475 pivot.
rotation.
yaw -= geom::Math::ToDegrees<float>(
static_cast<float>(crosswalk->GetHeading()));
477 geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f);
482 pivot.
rotation.
yaw -= geom::Math::ToDegrees<float>(
static_cast<float>(crosswalk->GetHeading()));
485 for (
auto corner : crosswalk->GetPoints()) {
487 static_cast<float>(corner.u),
488 static_cast<float>(corner.v),
489 static_cast<float>(corner.z));
497 result.push_back(v2);
510 const auto &next_lanes = GetLane(waypoint).GetNextLanes();
511 std::vector<Waypoint> result;
512 result.reserve(next_lanes.size());
513 for (
auto *next_lane : next_lanes) {
515 const auto lane_id = next_lane->GetId();
517 const auto *section = next_lane->GetLaneSection();
519 const auto *road = next_lane->GetRoad();
522 result.emplace_back(
Waypoint{road->GetId(), section->GetId(), lane_id, distance});
528 const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes();
529 std::vector<Waypoint> result;
530 result.reserve(prev_lanes.size());
531 for (
auto *next_lane : prev_lanes) {
533 const auto lane_id = next_lane->GetId();
535 const auto *section = next_lane->GetLaneSection();
537 const auto *road = next_lane->GetRoad();
540 result.emplace_back(
Waypoint{road->GetId(), section->GetId(), lane_id, distance});
547 const double distance)
const {
549 if (distance <= EPSILON) {
552 const auto &lane = GetLane(waypoint);
553 const bool forward = (waypoint.
lane_id <= 0);
554 const double signed_distance = forward ? distance : -distance;
555 const double relative_s = waypoint.
s - lane.GetDistance();
556 const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
561 if (distance <= remaining_lane_length) {
563 result.
s += signed_distance;
570 std::vector<Waypoint> result;
571 for (
const auto &successor : GetSuccessors(waypoint)) {
573 successor.road_id != waypoint.
road_id ||
574 successor.section_id != waypoint.
section_id ||
575 successor.lane_id != waypoint.
lane_id);
576 result =
ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
583 const double distance)
const {
585 if (distance <= EPSILON) {
588 const auto &lane = GetLane(waypoint);
589 const bool forward = !(waypoint.
lane_id <= 0);
590 const double signed_distance = forward ? distance : -distance;
591 const double relative_s = waypoint.
s - lane.GetDistance();
592 const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
597 if (distance <= remaining_lane_length) {
599 result.
s += signed_distance;
606 std::vector<Waypoint> result;
607 for (
const auto &successor : GetPredecessors(waypoint)) {
609 successor.road_id != waypoint.
road_id ||
610 successor.section_id != waypoint.
section_id ||
611 successor.lane_id != waypoint.
lane_id);
612 result =
ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
624 return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
629 if (std::abs(waypoint.
lane_id) == 1) {
631 }
else if (waypoint.
lane_id > 0) {
636 return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
641 std::vector<Waypoint> result;
642 for (
const auto &pair : _data.GetRoads()) {
643 const auto &road = pair.second;
644 for (
double s = EPSILON; s < (road.GetLength() -
EPSILON); s += distance) {
646 result.emplace_back(waypoint);
654 std::vector<Waypoint> result;
655 for (
const auto &pair : _data.GetRoads()) {
656 const auto &road = pair.second;
658 for (
const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
659 for (
const auto &lane : lane_section.GetLanes()) {
661 if (lane.first < 0 &&
662 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
663 result.emplace_back(
Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
668 const auto road_len = road.GetLength();
669 for (
const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
670 for (
const auto &lane : lane_section.GetLanes()) {
672 if (lane.first > 0 &&
673 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
675 Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
686 std::vector<Waypoint> result;
687 if(_data.GetRoads().count(road_id)) {
688 const auto &road = _data.GetRoads().at(road_id);
690 for (
const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
691 for (
const auto &lane : lane_section.GetLanes()) {
693 if (lane.first < 0 &&
694 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
695 result.emplace_back(
Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
700 const auto road_len = road.GetLength();
701 for (
const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
702 for (
const auto &lane : lane_section.GetLanes()) {
704 if (lane.first > 0 &&
705 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
707 Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
716 std::vector<std::pair<Waypoint, Waypoint>> result;
717 for (
const auto &pair : _data.GetRoads()) {
718 const auto &road = pair.second;
720 auto successors = GetSuccessors(waypoint);
721 if (successors.size() == 0){
723 auto last_waypoint = GetWaypoint(waypoint.road_id, waypoint.lane_id, distance);
724 if (last_waypoint.has_value()){
725 result.push_back({waypoint, *last_waypoint});
729 for (
auto &&successor : GetSuccessors(waypoint)) {
730 result.push_back({waypoint, successor});
739 std::vector<std::pair<Waypoint, Waypoint>> result;
740 const Junction * junction = GetJunction(
id);
742 const Road &road = _data.GetRoad(connections.second.connecting_road);
743 ForEachLane(road, lane_type, [&](
auto &&waypoint) {
744 const auto& lane = GetLane(waypoint);
747 lane_end.
s = final_s;
748 result.push_back({waypoint, lane_end});
754 std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
757 const float epsilon = 0.0001f;
759 const Junction *junction = GetJunction(
id);
760 std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
764 typedef boost::geometry::model::point
765 <float, 2, boost::geometry::cs::cartesian> Point2d;
766 typedef boost::geometry::model::segment<Point2d> Segment2d;
767 typedef boost::geometry::model::box<Rtree::BPoint>
Box;
773 bbox_pos.x - bbox_ext.x,
774 bbox_pos.y - bbox_ext.y,
775 bbox_pos.z - bbox_ext.z - epsilon);
777 bbox_pos.x + bbox_ext.x,
778 bbox_pos.y + bbox_ext.y,
779 bbox_pos.z + bbox_ext.z + epsilon);
780 Box box({min_corner.x, min_corner.y, min_corner.z},
781 {max_corner.x, max_corner.y, max_corner.z});
782 auto segments = _rtree.GetIntersections(box);
784 for (
size_t i = 0; i < segments.size(); ++i){
785 auto &segment1 = segments[i];
786 auto waypoint1 = segment1.second.first;
787 JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
792 Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
793 {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
794 for (
size_t j = i + 1; j < segments.size(); ++j){
795 auto &segment2 = segments[j];
796 auto waypoint2 = segment2.second.first;
797 JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
803 if(waypoint1.road_id == waypoint2.road_id){
806 Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
807 {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
809 double distance = boost::geometry::distance(seg1, seg2);
814 if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
815 conflicts[waypoint1.road_id].insert(waypoint2.road_id);
817 if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
818 conflicts[waypoint2.road_id].insert(waypoint1.road_id);
836 std::vector<Rtree::TreeElement> &rtree_elements,
852 std::make_pair(current_waypoint, next_waypoint)));
857 std::vector<Rtree::TreeElement> &rtree_elements,
862 AddElementToRtree(rtree_elements, current_transform, next_transform,
863 current_waypoint, next_waypoint);
864 current_waypoint = next_waypoint;
865 current_transform = next_transform;
871 if (lane.
GetId() < 0) {
879 const double epsilon = 0.000001;
881 const double min_delta_s = 1;
884 constexpr
double angle_threshold = geom::Math::Pi<double>() / 100.0;
886 constexpr
double max_segment_length = 100.0;
889 std::vector<Waypoint> topology;
890 for (
const auto &pair : _data.GetRoads()) {
891 const auto &road = pair.second;
893 if(waypoint.lane_id != 0) {
894 topology.push_back(waypoint);
900 std::vector<Rtree::TreeElement> rtree_elements;
902 for (
auto &waypoint : topology) {
903 auto &lane_start_waypoint = waypoint;
905 auto current_waypoint = lane_start_waypoint;
907 const Lane &lane = GetLane(current_waypoint);
909 geom::Transform current_transform = ComputeTransform(current_waypoint);
913 double delta_s = min_delta_s;
914 double remaining_length =
916 remaining_length -= epsilon;
917 delta_s = remaining_length;
918 if (delta_s < epsilon) {
921 auto next = GetNext(current_waypoint, delta_s);
925 auto next_waypoint = next.front();
927 AddElementToRtreeAndUpdateTransforms(
934 auto next_waypoint = current_waypoint;
939 double delta_s = min_delta_s;
940 double remaining_length =
942 remaining_length -= epsilon;
943 delta_s = std::min(delta_s, remaining_length);
945 if (delta_s < epsilon) {
946 AddElementToRtreeAndUpdateTransforms(
954 auto next = GetNext(next_waypoint, delta_s);
955 if (next.size() != 1 ||
956 current_waypoint.section_id != next.front().section_id) {
957 AddElementToRtreeAndUpdateTransforms(
965 next_waypoint = next.front();
970 if (std::abs(angle) > angle_threshold ||
971 std::abs(current_waypoint.s - next_waypoint.s) > max_segment_length) {
978 current_waypoint = next_waypoint;
979 current_transform = next_transform;
985 _rtree.InsertElements(rtree_elements);
989 return _data.GetJunction(
id);
993 return _data.GetJunction(
id);
997 const double distance,
998 const float extra_width,
999 const bool smooth_junctions)
const {
1008 for (
auto &&pair : _data.GetRoads()) {
1009 const auto &road = pair.second;
1010 if (road.IsJunction()) {
1013 out_mesh += *mesh_factory.
Generate(road);
1017 for (
const auto &junc_pair : _data.GetJunctions()) {
1018 const auto &junction = junc_pair.second;
1019 std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1020 for(
const auto &connection_pair : junction.GetConnections()) {
1021 const auto &connection = connection_pair.second;
1022 const auto &road = _data.GetRoads().at(connection.connecting_road);
1023 for (
auto &&lane_section : road.GetLaneSections()) {
1024 for (
auto &&lane_pair : lane_section.GetLanes()) {
1025 lane_meshes.push_back(mesh_factory.
Generate(lane_pair.second));
1029 if(smooth_junctions) {
1033 for(
auto& lane : lane_meshes) {
1034 junction_mesh += *lane;
1036 out_mesh += junction_mesh;
1046 std::vector<std::unique_ptr<geom::Mesh>> out_mesh_list;
1048 std::unordered_map<JuncId, geom::Mesh> junction_map;
1049 for (
auto &&pair : _data.GetRoads()) {
1050 const auto &road = pair.second;
1051 if (!road.IsJunction()) {
1052 std::vector<std::unique_ptr<geom::Mesh>> road_mesh_list =
1055 out_mesh_list.insert(
1056 out_mesh_list.end(),
1057 std::make_move_iterator(road_mesh_list.begin()),
1058 std::make_move_iterator(road_mesh_list.end()));
1063 for (
const auto &junc_pair : _data.GetJunctions()) {
1064 const auto &junction = junc_pair.second;
1065 std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1066 std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1067 for(
const auto &connection_pair : junction.GetConnections()) {
1068 const auto &connection = connection_pair.second;
1069 const auto &road = _data.GetRoads().at(connection.connecting_road);
1070 for (
auto &&lane_section : road.GetLaneSections()) {
1071 for (
auto &&lane_pair : lane_section.GetLanes()) {
1072 const auto &lane = lane_pair.second;
1074 lane_meshes.push_back(mesh_factory.
Generate(lane));
1076 sidewalk_lane_meshes.push_back(mesh_factory.
Generate(lane));
1083 for(
auto& lane : sidewalk_lane_meshes) {
1084 *merged_mesh += *lane;
1086 out_mesh_list.push_back(std::move(merged_mesh));
1088 std::unique_ptr<geom::Mesh> junction_mesh = std::make_unique<geom::Mesh>();
1089 for(
auto& lane : lane_meshes) {
1090 *junction_mesh += *lane;
1092 for(
auto& lane : sidewalk_lane_meshes) {
1093 *junction_mesh += *lane;
1095 out_mesh_list.push_back(std::move(junction_mesh));
1100 out_mesh_list.front()->GetVertices().front().x,
1101 out_mesh_list.front()->GetVertices().front().y);
1102 auto max_pos = min_pos;
1103 for (
auto & mesh : out_mesh_list) {
1104 auto vertex = mesh->GetVertices().front();
1105 min_pos.
x = std::min(min_pos.x, vertex.x);
1106 min_pos.y = std::min(min_pos.y, vertex.y);
1107 max_pos.
x = std::max(max_pos.
x, vertex.x);
1108 max_pos.
y = std::max(max_pos.
y, vertex.y);
1110 size_t mesh_amount_x =
static_cast<size_t>((max_pos.
x - min_pos.x)/params.
max_road_length) + 1;
1111 size_t mesh_amount_y =
static_cast<size_t>((max_pos.
y - min_pos.y)/params.
max_road_length) + 1;
1112 std::vector<std::unique_ptr<geom::Mesh>> result;
1113 result.reserve(mesh_amount_x*mesh_amount_y);
1114 for (
size_t i = 0; i < mesh_amount_x*mesh_amount_y; ++i) {
1115 result.emplace_back(std::make_unique<geom::Mesh>());
1117 for (
auto & mesh : out_mesh_list) {
1118 auto vertex = mesh->GetVertices().front();
1119 size_t x_pos =
static_cast<size_t>((vertex.x - min_pos.x) / params.
max_road_length);
1120 size_t y_pos =
static_cast<size_t>((vertex.y - min_pos.y) / params.
max_road_length);
1121 *(result[x_pos + mesh_amount_x*y_pos]) += *mesh;
1131 const std::vector<geom::Location> crosswalk_vertex = GetAllCrosswalkZones();
1132 if (crosswalk_vertex.empty()) {
1138 size_t start_vertex_index = 0;
1140 std::vector<geom::Vector3D> vertices;
1145 if (i != 0 && crosswalk_vertex[start_vertex_index] == crosswalk_vertex[i]) {
1150 if (i >= crosswalk_vertex.size() - 1) {
1153 start_vertex_index = ++i;
1156 vertices.push_back(crosswalk_vertex[i++]);
1157 }
while (i < crosswalk_vertex.size());
static void ForEachDrivableLaneImpl(RoadId road_id, const LaneSection &lane_section, double distance, FuncT &&func)
Return a waypoint for each drivable lane on lane_section.
std::vector< element::LaneMarking > CalculateCrossedLanes(const geom::Location &origin, const geom::Location &destination) const
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.
Seting for map generation from opendrive without additional geometry.
std::unordered_map< road::RoadId, std::unordered_set< road::RoadId > > ComputeJunctionConflicts(JuncId id) const
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...
static double GetDistanceAtStartOfLane(const Lane &lane)
static std::pair< float, float > DistanceSegmentToPoint(const Vector3D &p, const Vector3D &v, const Vector3D &w)
Returns a pair containing:
carla::geom::BoundingBox GetBoundingBox() const
std::vector< std::pair< Waypoint, Waypoint > > GenerateTopology() const
Generate the minimum set of waypoints that define the topology of map.
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
void AddTriangleFan(const std::vector< vertex_type > &vertices)
Adds a triangle fan to the mesh, vertex order is counterclockwise.
double GetDistance() const
Distance from road's start location.
std::unique_ptr< Mesh > Generate(const road::Road &road) const
Generates a mesh that defines a road.
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.
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.
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...
static double GetDistanceAtEndOfLane(const Lane &lane)
const Lane & GetLane(Waypoint waypoint) const
======================================================================== – Road information --------...
std::vector< Waypoint > GenerateWaypointsOnRoadEntries(Lane::LaneType lane_type=Lane::LaneType::Driving) const
Generate waypoints on each lane at the start of each road.
double GetDistance() const
Junction * GetJunction(JuncId id)
Road & GetRoad(const RoadId id)
This file contains definitions of common data structures used in traffic manager. ...
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.
value_type Evaluate(const value_type &x) const
Evaluates f(x) = a + bx + cx^2 + dx^3.
auto GetLaneSectionsAt(const double s)
static std::vector< T > ConcatVectors(std::vector< T > dst, std::vector< T > src)
bg::model::box< Point3D > Box
boost::optional< element::Waypoint > GetClosestWaypointOnRoad(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
======================================================================== – Geometry ----------------...
void AddMaterial(const std::string &material_name)
Starts applying a new material to the new added triangles.
static void ForEachDrivableLane(const Road &road, FuncT &&func)
Return a waypoint for each drivable lane on each lane section of road.
void EndMaterial()
Stops applying the material to the new added triangles.
static bool IsLanePresent(const MapData &data, Waypoint waypoint)
Assumes road_id and section_id are valid.
#define DEBUG_ASSERT(predicate)
auto GetLaneSections() const
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...
boost::geometry::model::point< float, 3, boost::geometry::cs::cartesian > BPoint
RoadParameters road_param
Data structure for the signal search.
#define RELEASE_ASSERT(pred)
LaneType
Can be used as flags.
geom::Transform ComputeTransform(Waypoint waypoint) const
Lane & GetLaneById(SectionId section_id, LaneId lane_id)
std::pair< const element::RoadInfoMarkRecord *, const element::RoadInfoMarkRecord * > GetMarkRecord(Waypoint waypoint) const
static auto Distance2D(const Vector3D &a, const Vector3D &b)
std::vector< std::unique_ptr< geom::Mesh > > GenerateChunkedMesh(const rpc::OpendriveGenerationParameters ¶ms) const
LaneSection & GetLaneSectionById(SectionId id)
boost::optional< Waypoint > GetLeft(Waypoint waypoint) const
Return a waypoint at the lane of waypoint's left lane.
Lane::LaneType GetLaneType(Waypoint waypoint) const
Mesh data container, validator and exporter.
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.
bool IsJunction(RoadId road_id) const
std::vector< Waypoint > GetPredecessors(Waypoint waypoint) const
double GetRemainingLength(const Lane &lane, double current_s)
std::vector< SignalSearchData > GetSignalsInDistance(Waypoint waypoint, double distance, bool stop_at_junction=false) const
Searches signals from an initial waypoint until the defined distance.
static std::vector< LaneMarking > Calculate(const Map &map, const geom::Location &origin, const geom::Location &destination)
std::map< LaneId, Lane > & GetLanes()
bool IsStraight() const
Checks whether the geometry is straight or not.
static double GetVectorAngle(const Vector3D &a, const Vector3D &b)
Returns the angle between 2 vectors in radians.
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.
void AddElementToRtree(std::vector< Rtree::TreeElement > &rtree_elements, geom::Transform ¤t_transform, geom::Transform &next_transform, Waypoint ¤t_waypoint, Waypoint &next_waypoint)
Helper Functions for constructing the rtree element list.
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
std::vector< Waypoint > GetSuccessors(Waypoint waypoint) const
======================================================================== – Waypoint generation -----...
void AddElementToRtreeAndUpdateTransforms(std::vector< Rtree::TreeElement > &rtree_elements, geom::Transform ¤t_transform, Waypoint ¤t_waypoint, Waypoint &next_waypoint)
std::vector< Waypoint > GenerateWaypoints(double approx_distance) const
Generate all the waypoints in map separated by approx_distance.
boost::geometry::model::segment< BPoint > BSegment
std::unordered_map< ConId, Connection > & GetConnections()
JuncId GetJunctionId(RoadId road_id) const
boost::optional< Waypoint > GetRight(Waypoint waypoint) const
Return a waypoint at the lane of waypoint's right lane.
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
std::pair< BSegment, std::pair< Waypoint, Waypoint > > TreeElement
double GetLaneWidth(Waypoint waypoint) const
static void ForEachLaneImpl(RoadId road_id, const LaneSection &lane_section, double distance, Lane::LaneType lane_type, FuncT &&func)