26 #include <unordered_map> 40 static constexpr
double EPSILON = 10.0 * std::numeric_limits<double>::epsilon();
47 static std::vector<T>
ConcatVectors(std::vector<T> dst, std::vector<T> src) {
48 if (src.size() > dst.size()) {
53 std::make_move_iterator(src.begin()),
54 std::make_move_iterator(src.end()));
59 if (lane.
GetId() <= 0) {
67 if (lane.
GetId() > 0) {
75 template <
typename FuncT>
81 for (
const auto &pair : lane_section.
GetLanes()) {
82 const auto &lane = pair.second;
83 if (lane.GetId() == 0) {
96 template <
typename FuncT>
103 for (
const auto &pair : lane_section.
GetLanes()) {
104 const auto &lane = pair.second;
105 if (lane.GetId() == 0) {
108 if ((static_cast<int32_t>(lane.GetType()) & static_cast<int32_t>(lane_type)) > 0) {
111 lane_section.
GetId(),
119 template <
typename FuncT>
126 std::forward<FuncT>(func));
131 template <
typename FuncT>
139 std::forward<FuncT>(func));
144 template <
typename FuncT>
151 std::forward<FuncT>(func));
167 int32_t lane_type)
const {
168 std::vector<Rtree::TreeElement> query_result =
170 [&](Rtree::TreeElement
const &element) {
171 const Lane &lane = GetLane(element.second.first);
172 return (lane_type & static_cast<int32_t>(lane.GetType())) > 0;
175 if (query_result.size() == 0) {
176 return boost::optional<Waypoint>{};
186 Waypoint result_start = query_result.front().second.first;
187 Waypoint result_end = query_result.front().second.second;
189 if (result_start.
lane_id < 0) {
190 double delta_s = distance_to_segment.first;
191 double final_s = result_start.
s + delta_s;
192 if (final_s >= result_end.
s) {
194 }
else if (delta_s <= 0) {
197 return GetNext(result_start, delta_s).front();
200 double delta_s = distance_to_segment.first;
201 double final_s = result_start.
s - delta_s;
202 if (final_s <= result_end.
s) {
204 }
else if (delta_s <= 0) {
207 return GetNext(result_start, delta_s).front();
214 int32_t lane_type)
const {
215 boost::optional<Waypoint> w = GetClosestWaypointOnRoad(pos, lane_type);
217 if (!w.has_value()) {
223 const auto half_lane_width =
226 if (dist < half_lane_width) {
230 return boost::optional<Waypoint>{};
245 if (!_data.ContainsRoad(waypoint.
road_id)) {
246 return boost::optional<Waypoint>{};
248 const Road &road = _data.GetRoad(waypoint.
road_id);
252 return boost::optional<Waypoint>{};
256 bool lane_found =
false;
258 if (section.ContainsLane(lane_id)) {
267 return boost::optional<Waypoint>{};
274 return GetLane(waypoint).ComputeTransform(waypoint.
s);
282 return GetLane(waypoint).GetType();
286 const auto s = waypoint.
s;
288 const auto &lane = GetLane(waypoint);
295 return lane_width_info->GetPolynomial().Evaluate(s);
299 return _data.GetRoad(road_id).GetJunctionId();
303 return _data.GetRoad(road_id).IsJunction();
306 std::pair<const RoadInfoMarkRecord *, const RoadInfoMarkRecord *>
310 return std::make_pair(
nullptr,
nullptr);
312 const auto s = waypoint.
s;
314 const auto ¤t_lane = GetLane(waypoint);
318 const auto inner_lane_id = waypoint.
lane_id < 0 ?
322 const auto &inner_lane = current_lane.GetRoad()->GetLaneById(waypoint.
section_id, inner_lane_id);
327 return std::make_pair(current_lane_info, inner_lane_info);
331 Waypoint waypoint,
double distance,
bool stop_at_junction)
const {
333 const auto &lane = GetLane(waypoint);
334 const bool forward = (waypoint.
lane_id <= 0);
335 const double signed_distance = forward ? distance : -distance;
336 const double relative_s = waypoint.
s - lane.GetDistance();
337 const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
340 auto &road =_data.GetRoad(waypoint.
road_id);
341 std::vector<SignalSearchData> result;
345 if (distance <= remaining_lane_length) {
347 waypoint.
s, waypoint.
s + signed_distance);
348 for(
auto* signal : signals){
349 double distance_to_signal = 0;
351 distance_to_signal = signal->
GetDistance() - waypoint.
s;
353 distance_to_signal = waypoint.
s - signal->GetDistance();
356 bool is_valid =
false;
357 for (
auto &validity : signal->GetValidities()) {
358 if (waypoint.
lane_id >= validity._from_lane &&
359 waypoint.
lane_id <= validity._to_lane) {
367 if (distance_to_signal == 0) {
370 distance_to_signal});
373 {signal, GetNext(waypoint, distance_to_signal).front(),
374 distance_to_signal});
380 const double signed_remaining_length = forward ? remaining_lane_length : -remaining_lane_length;
384 waypoint.
s, waypoint.
s + signed_remaining_length);
385 for(
auto* signal : signals){
386 double distance_to_signal = 0;
388 distance_to_signal = signal->
GetDistance() - waypoint.
s;
390 distance_to_signal = waypoint.
s - signal->GetDistance();
393 bool is_valid =
false;
394 for (
auto &validity : signal->GetValidities()) {
395 if (waypoint.
lane_id >= validity._from_lane &&
396 waypoint.
lane_id <= validity._to_lane) {
404 if (distance_to_signal == 0) {
407 distance_to_signal});
410 {signal, GetNext(waypoint, distance_to_signal).front(),
411 distance_to_signal});
415 for (
auto &successor : GetSuccessors(waypoint)) {
416 if(_data.GetRoad(successor.road_id).IsJunction() && stop_at_junction){
419 auto& sucessor_lane = _data.GetRoad(successor.road_id).
420 GetLaneByDistance(successor.s, successor.lane_id);
421 if (successor.lane_id < 0) {
422 successor.s = sucessor_lane.GetDistance();
424 successor.s = sucessor_lane.GetDistance() + sucessor_lane.GetLength();
426 auto sucessor_signals = GetSignalsInDistance(
427 successor, distance - remaining_lane_length, stop_at_junction);
428 for(
auto& signal : sucessor_signals){
429 signal.accumulated_s += remaining_lane_length;
436 std::vector<const element::RoadInfoSignal*>
438 std::vector<const element::RoadInfoSignal*> result;
439 for (
const auto& road_pair : _data.GetRoads()) {
440 const auto &road = road_pair.second;
442 for(
const auto* road_info : road_infos) {
443 result.push_back(road_info);
456 std::vector<geom::Location> result;
458 for (
const auto &pair : _data.GetRoads()) {
459 const auto &road = pair.second;
460 std::vector<const RoadInfoCrosswalk *> crosswalks = road.GetInfos<
RoadInfoCrosswalk>();
461 if (crosswalks.size() > 0) {
462 for (
auto crosswalk : crosswalks) {
464 std::vector<geom::Location> points;
467 for (
const auto §ion : road.GetLaneSectionsAt(crosswalk->GetS())) {
469 for (
const auto &lane : section.GetLanes()) {
471 if (lane.first == 0) {
476 waypoint.
s = crosswalk->GetS();
477 base = ComputeTransform(waypoint);
484 pivot.
rotation.
yaw -= geom::Math::ToDegrees<float>(
static_cast<float>(crosswalk->GetHeading()));
486 geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f);
491 pivot.
rotation.
yaw -= geom::Math::ToDegrees<float>(
static_cast<float>(crosswalk->GetHeading()));
494 for (
auto corner : crosswalk->GetPoints()) {
496 static_cast<float>(corner.u),
497 static_cast<float>(corner.v),
498 static_cast<float>(corner.z));
506 result.push_back(v2);
519 const auto &next_lanes = GetLane(waypoint).GetNextLanes();
520 std::vector<Waypoint> result;
521 result.reserve(next_lanes.size());
522 for (
auto *next_lane : next_lanes) {
524 const auto lane_id = next_lane->GetId();
526 const auto *section = next_lane->GetLaneSection();
528 const auto *road = next_lane->GetRoad();
531 result.emplace_back(
Waypoint{road->GetId(), section->GetId(), lane_id, distance});
537 const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes();
538 std::vector<Waypoint> result;
539 result.reserve(prev_lanes.size());
540 for (
auto *next_lane : prev_lanes) {
542 const auto lane_id = next_lane->GetId();
544 const auto *section = next_lane->GetLaneSection();
546 const auto *road = next_lane->GetRoad();
549 result.emplace_back(
Waypoint{road->GetId(), section->GetId(), lane_id, distance});
556 const double distance)
const {
558 if (distance <= EPSILON) {
561 const auto &lane = GetLane(waypoint);
562 const bool forward = (waypoint.
lane_id <= 0);
563 const double signed_distance = forward ? distance : -distance;
564 const double relative_s = waypoint.
s - lane.GetDistance();
565 const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
570 if (distance <= remaining_lane_length) {
572 result.
s += signed_distance;
579 std::vector<Waypoint> result;
580 for (
const auto &successor : GetSuccessors(waypoint)) {
582 successor.road_id != waypoint.
road_id ||
583 successor.section_id != waypoint.
section_id ||
584 successor.lane_id != waypoint.
lane_id);
585 result =
ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
592 const double distance)
const {
594 if (distance <= EPSILON) {
597 const auto &lane = GetLane(waypoint);
598 const bool forward = !(waypoint.
lane_id <= 0);
599 const double signed_distance = forward ? distance : -distance;
600 const double relative_s = waypoint.
s - lane.GetDistance();
601 const double remaining_lane_length = forward ? lane.GetLength() - relative_s : relative_s;
606 if (distance <= remaining_lane_length) {
608 result.
s += signed_distance;
615 std::vector<Waypoint> result;
616 for (
const auto &successor : GetPredecessors(waypoint)) {
618 successor.road_id != waypoint.
road_id ||
619 successor.section_id != waypoint.
section_id ||
620 successor.lane_id != waypoint.
lane_id);
621 result =
ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
633 return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
638 if (std::abs(waypoint.
lane_id) == 1) {
640 }
else if (waypoint.
lane_id > 0) {
645 return IsLanePresent(_data, waypoint) ? waypoint : boost::optional<Waypoint>{};
650 std::vector<Waypoint> result;
651 for (
const auto &pair : _data.GetRoads()) {
652 const auto &road = pair.second;
653 for (
double s = EPSILON; s < (road.GetLength() -
EPSILON); s += distance) {
655 result.emplace_back(waypoint);
663 std::vector<Waypoint> result;
664 for (
const auto &pair : _data.GetRoads()) {
665 const auto &road = pair.second;
667 for (
const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
668 for (
const auto &lane : lane_section.GetLanes()) {
670 if (lane.first < 0 &&
671 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
672 result.emplace_back(
Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
677 const auto road_len = road.GetLength();
678 for (
const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
679 for (
const auto &lane : lane_section.GetLanes()) {
681 if (lane.first > 0 &&
682 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
684 Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
695 std::vector<Waypoint> result;
696 if(_data.GetRoads().count(road_id)) {
697 const auto &road = _data.GetRoads().at(road_id);
699 for (
const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
700 for (
const auto &lane : lane_section.GetLanes()) {
702 if (lane.first < 0 &&
703 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
704 result.emplace_back(
Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
709 const auto road_len = road.GetLength();
710 for (
const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
711 for (
const auto &lane : lane_section.GetLanes()) {
713 if (lane.first > 0 &&
714 static_cast<int32_t>(lane.second.GetType()) & static_cast<int32_t>(lane_type)) {
716 Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
725 std::vector<std::pair<Waypoint, Waypoint>> result;
726 for (
const auto &pair : _data.GetRoads()) {
727 const auto &road = pair.second;
729 auto successors = GetSuccessors(waypoint);
730 if (successors.size() == 0){
732 auto last_waypoint = GetWaypoint(waypoint.road_id, waypoint.lane_id, distance);
733 if (last_waypoint.has_value()){
734 result.push_back({waypoint, *last_waypoint});
738 for (
auto &&successor : GetSuccessors(waypoint)) {
739 result.push_back({waypoint, successor});
748 std::vector<std::pair<Waypoint, Waypoint>> result;
749 const Junction * junction = GetJunction(
id);
751 const Road &road = _data.GetRoad(connections.second.connecting_road);
752 ForEachLane(road, lane_type, [&](
auto &&waypoint) {
753 const auto& lane = GetLane(waypoint);
756 lane_end.
s = final_s;
757 result.push_back({waypoint, lane_end});
763 std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
766 const float epsilon = 0.0001f;
768 const Junction *junction = GetJunction(
id);
769 std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
773 typedef boost::geometry::model::point
774 <float, 2, boost::geometry::cs::cartesian> Point2d;
775 typedef boost::geometry::model::segment<Point2d> Segment2d;
776 typedef boost::geometry::model::box<Rtree::BPoint>
Box;
782 bbox_pos.x - bbox_ext.x,
783 bbox_pos.y - bbox_ext.y,
784 bbox_pos.z - bbox_ext.z - epsilon);
786 bbox_pos.x + bbox_ext.x,
787 bbox_pos.y + bbox_ext.y,
788 bbox_pos.z + bbox_ext.z + epsilon);
789 Box box({min_corner.x, min_corner.y, min_corner.z},
790 {max_corner.x, max_corner.y, max_corner.z});
791 auto segments = _rtree.GetIntersections(box);
793 for (
size_t i = 0; i < segments.size(); ++i){
794 auto &segment1 = segments[i];
795 auto waypoint1 = segment1.second.first;
796 JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
801 Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
802 {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
803 for (
size_t j = i + 1; j < segments.size(); ++j){
804 auto &segment2 = segments[j];
805 auto waypoint2 = segment2.second.first;
806 JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
812 if(waypoint1.road_id == waypoint2.road_id){
815 Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
816 {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
818 double distance = boost::geometry::distance(seg1, seg2);
823 if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
824 conflicts[waypoint1.road_id].insert(waypoint2.road_id);
826 if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
827 conflicts[waypoint2.road_id].insert(waypoint1.road_id);
845 std::vector<Rtree::TreeElement> &rtree_elements,
861 std::make_pair(current_waypoint, next_waypoint)));
866 std::vector<Rtree::TreeElement> &rtree_elements,
871 AddElementToRtree(rtree_elements, current_transform, next_transform,
872 current_waypoint, next_waypoint);
873 current_waypoint = next_waypoint;
874 current_transform = next_transform;
880 if (lane.
GetId() < 0) {
888 const double epsilon = 0.000001;
890 const double min_delta_s = 1;
893 constexpr
double angle_threshold = geom::Math::Pi<double>() / 100.0;
895 constexpr
double max_segment_length = 100.0;
898 std::vector<Waypoint> topology;
899 for (
const auto &pair : _data.GetRoads()) {
900 const auto &road = pair.second;
902 if(waypoint.lane_id != 0) {
903 topology.push_back(waypoint);
909 std::vector<Rtree::TreeElement> rtree_elements;
911 for (
auto &waypoint : topology) {
912 auto &lane_start_waypoint = waypoint;
914 auto current_waypoint = lane_start_waypoint;
916 const Lane &lane = GetLane(current_waypoint);
918 geom::Transform current_transform = ComputeTransform(current_waypoint);
922 double delta_s = min_delta_s;
923 double remaining_length =
925 remaining_length -= epsilon;
926 delta_s = remaining_length;
927 if (delta_s < epsilon) {
930 auto next = GetNext(current_waypoint, delta_s);
934 auto next_waypoint = next.front();
936 AddElementToRtreeAndUpdateTransforms(
943 auto next_waypoint = current_waypoint;
948 double delta_s = min_delta_s;
949 double remaining_length =
951 remaining_length -= epsilon;
952 delta_s =
std::min(delta_s, remaining_length);
954 if (delta_s < epsilon) {
955 AddElementToRtreeAndUpdateTransforms(
963 auto next = GetNext(next_waypoint, delta_s);
964 if (next.size() != 1 ||
965 current_waypoint.section_id != next.front().section_id) {
966 AddElementToRtreeAndUpdateTransforms(
974 next_waypoint = next.front();
979 if (std::abs(angle) > angle_threshold ||
980 std::abs(current_waypoint.s - next_waypoint.s) > max_segment_length) {
987 current_waypoint = next_waypoint;
988 current_transform = next_transform;
994 _rtree.InsertElements(rtree_elements);
998 return _data.GetJunction(
id);
1002 return _data.GetJunction(
id);
1006 const double distance,
1007 const float extra_width,
1008 const bool smooth_junctions)
const {
1017 for (
auto &&pair : _data.GetRoads()) {
1018 const auto &road = pair.second;
1019 if (road.IsJunction()) {
1022 out_mesh += *mesh_factory.
Generate(road);
1026 for (
const auto &junc_pair : _data.GetJunctions()) {
1027 const auto &junction = junc_pair.second;
1028 std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1029 for(
const auto &connection_pair : junction.GetConnections()) {
1030 const auto &connection = connection_pair.second;
1031 const auto &road = _data.GetRoads().at(connection.connecting_road);
1032 for (
auto &&lane_section : road.GetLaneSections()) {
1033 for (
auto &&lane_pair : lane_section.GetLanes()) {
1034 lane_meshes.push_back(mesh_factory.
Generate(lane_pair.second));
1038 if(smooth_junctions) {
1042 for(
auto& lane : lane_meshes) {
1043 junction_mesh += *lane;
1045 out_mesh += junction_mesh;
1056 std::vector<std::unique_ptr<geom::Mesh>> out_mesh_list;
1058 std::unordered_map<JuncId, geom::Mesh> junction_map;
1059 for (
auto &&pair : _data.GetRoads()) {
1060 const auto &road = pair.second;
1061 if (!road.IsJunction()) {
1062 std::vector<std::unique_ptr<geom::Mesh>> road_mesh_list =
1065 out_mesh_list.insert(
1066 out_mesh_list.end(),
1067 std::make_move_iterator(road_mesh_list.begin()),
1068 std::make_move_iterator(road_mesh_list.end()));
1073 for (
const auto &junc_pair : _data.GetJunctions()) {
1074 const auto &junction = junc_pair.second;
1075 std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1076 std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1077 for(
const auto &connection_pair : junction.GetConnections()) {
1078 const auto &connection = connection_pair.second;
1079 const auto &road = _data.GetRoads().at(connection.connecting_road);
1080 for (
auto &&lane_section : road.GetLaneSections()) {
1081 for (
auto &&lane_pair : lane_section.GetLanes()) {
1082 const auto &lane = lane_pair.second;
1084 lane_meshes.push_back(mesh_factory.
Generate(lane));
1086 sidewalk_lane_meshes.push_back(mesh_factory.
Generate(lane));
1093 for(
auto& lane : sidewalk_lane_meshes) {
1094 *merged_mesh += *lane;
1096 out_mesh_list.push_back(std::move(merged_mesh));
1098 std::unique_ptr<geom::Mesh> junction_mesh = std::make_unique<geom::Mesh>();
1099 for(
auto& lane : lane_meshes) {
1100 *junction_mesh += *lane;
1102 for(
auto& lane : sidewalk_lane_meshes) {
1103 *junction_mesh += *lane;
1105 out_mesh_list.push_back(std::move(junction_mesh));
1110 out_mesh_list.front()->GetVertices().front().x,
1111 out_mesh_list.front()->GetVertices().front().y);
1112 auto max_pos = min_pos;
1113 for (
auto & mesh : out_mesh_list) {
1114 auto vertex = mesh->GetVertices().front();
1115 min_pos.
x =
std::min(min_pos.x, vertex.x);
1116 min_pos.y =
std::min(min_pos.y, vertex.y);
1117 max_pos.
x = std::max(max_pos.
x, vertex.x);
1118 max_pos.
y = std::max(max_pos.
y, vertex.y);
1120 size_t mesh_amount_x =
static_cast<size_t>((max_pos.
x - min_pos.x)/params.
max_road_length) + 1;
1121 size_t mesh_amount_y =
static_cast<size_t>((max_pos.
y - min_pos.y)/params.
max_road_length) + 1;
1122 std::vector<std::unique_ptr<geom::Mesh>> result;
1123 result.reserve(mesh_amount_x*mesh_amount_y);
1124 for (
size_t i = 0; i < mesh_amount_x*mesh_amount_y; ++i) {
1125 result.emplace_back(std::make_unique<geom::Mesh>());
1127 for (
auto & mesh : out_mesh_list) {
1128 auto vertex = mesh->GetVertices().front();
1129 size_t x_pos =
static_cast<size_t>((vertex.x - min_pos.x) / params.
max_road_length);
1130 size_t y_pos =
static_cast<size_t>((vertex.y - min_pos.y) / params.
max_road_length);
1131 *(result[x_pos + mesh_amount_x*y_pos]) += *mesh;
1137 std::map<road::Lane::LaneType , std::vector<std::unique_ptr<geom::Mesh>>>
1144 std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> road_out_mesh_list;
1145 std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> junction_out_mesh_list;
1148 minpos, maxpos, &junction_out_mesh_list);
1150 const std::vector<RoadId> RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos);
1152 size_t num_roads = RoadsIDToGenerate.size();
1153 size_t num_roads_per_thread = 30;
1154 size_t num_threads = (num_roads / num_roads_per_thread) + 1;
1155 num_threads = num_threads > 1 ? num_threads : 1;
1156 std::vector<std::thread> workers;
1157 std::mutex write_mutex;
1158 std::cout <<
"Generating " << std::to_string(num_roads) <<
" roads" << std::endl;
1160 for (
size_t i = 0; i < num_threads; ++i ) {
1161 std::thread neworker(
1162 [
this, &write_mutex, &mesh_factory, &RoadsIDToGenerate, &road_out_mesh_list, i, num_roads_per_thread]() {
1163 std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> Current =
1164 std::move(GenerateRoadsMultithreaded(mesh_factory, RoadsIDToGenerate,i, num_roads_per_thread ));
1165 std::lock_guard<std::mutex> guard(write_mutex);
1166 for (
auto&& pair : Current ) {
1167 if (road_out_mesh_list.find(pair.first) != road_out_mesh_list.end()) {
1168 road_out_mesh_list[pair.first].insert(road_out_mesh_list[pair.first].end(),
1169 std::make_move_iterator(pair.second.begin()),
1170 std::make_move_iterator(pair.second.end()));
1172 road_out_mesh_list[pair.first] = std::move(pair.second);
1176 workers.push_back(std::move(neworker));
1179 for (
size_t i = 0; i < workers.size(); ++i) {
1183 for (
size_t i = 0; i < workers.size(); ++i) {
1184 if (workers[i].joinable()) {
1189 juntction_thread.join();
1190 for (
auto&& pair : junction_out_mesh_list) {
1191 if (road_out_mesh_list.find(pair.first) != road_out_mesh_list.end())
1193 road_out_mesh_list[pair.first].insert(road_out_mesh_list[pair.first].end(),
1194 std::make_move_iterator(pair.second.begin()),
1195 std::make_move_iterator(pair.second.end()));
1199 road_out_mesh_list[pair.first] = std::move(pair.second);
1202 std::cout <<
"Generated " << std::to_string(num_roads) <<
" roads" << std::endl;
1204 return road_out_mesh_list;
1210 float distancebetweentrees,
1211 float distancefromdrivinglineborder,
1212 float s_offset)
const {
1214 std::vector<std::pair<geom::Transform, std::string>> transforms;
1216 const std::vector<RoadId> RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos);
1217 for (
RoadId id : RoadsIDToGenerate ) {
1218 const auto& road = _data.GetRoads().at(
id);
1219 if (!road.IsJunction()) {
1220 for (
auto &&lane_section : road.GetLaneSections()) {
1222 for (
auto &pairlane : lane_section.GetLanes()) {
1224 min_lane = pairlane.first;
1228 const road::Lane* lane = lane_section.GetLane(min_lane);
1230 double s_current = lane_section.
GetDistance() + s_offset;
1231 const double s_end = lane_section.GetDistance() + lane_section.GetLength();
1232 while(s_current < s_end){
1233 if(lane->
GetWidth(s_current) != 0.0f){
1240 transforms.push_back(std::make_pair(treeTransform,roadinfo->
GetType()));
1242 s_current += distancebetweentrees;
1256 const std::vector<geom::Location> crosswalk_vertex = GetAllCrosswalkZones();
1257 if (crosswalk_vertex.empty()) {
1263 size_t start_vertex_index = 0;
1265 std::vector<geom::Vector3D> vertices;
1270 if (i != 0 && crosswalk_vertex[start_vertex_index] == crosswalk_vertex[i]) {
1275 if (i >= crosswalk_vertex.size() - 1) {
1278 start_vertex_index = ++i;
1281 vertices.push_back(crosswalk_vertex[i++]);
1282 }
while (i < crosswalk_vertex.size());
1293 std::vector<std::string>& outinfo )
const 1295 std::vector<std::unique_ptr<geom::Mesh>> LineMarks;
1298 const std::vector<RoadId> RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos);
1299 for (
RoadId id : RoadsIDToGenerate ) {
1300 const auto& road = _data.GetRoads().at(
id);
1301 if (!road.IsJunction()) {
1306 return std::move(LineMarks);
1310 std::vector<carla::geom::BoundingBox> returning;
1311 for (
const auto& junc_pair : _data.GetJunctions() ) {
1312 const auto& junction = junc_pair.second;
1313 float box_extraextension_factor = 1.5f;
1315 bb.
extent *= box_extraextension_factor;
1316 returning.push_back(bb);
1326 std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>>
1328 const std::vector<RoadId>& RoadsId,
1329 const size_t index,
const size_t number_of_roads_per_thread)
const 1331 std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>> out;
1333 size_t start = index * number_of_roads_per_thread;
1334 size_t endoffset = (index+1) * number_of_roads_per_thread;
1335 size_t end = RoadsId.size();
1337 for (
int i = start; i < endoffset && i < end; ++i) {
1338 const auto& road = _data.GetRoads().at(RoadsId[i]);
1339 if (!road.IsJunction()) {
1343 std::cout <<
"Generated roads from " + std::to_string(index * number_of_roads_per_thread) +
" to " + std::to_string((index+1) * number_of_roads_per_thread ) << std::endl;
1352 std::vector<std::unique_ptr<geom::Mesh>>>* junction_out_mesh_list)
const {
1354 std::vector<JuncId> JunctionsToGenerate = FilterJunctionsByPosition(minpos, maxpos);
1355 size_t num_junctions = JunctionsToGenerate.size();
1356 std::cout <<
"Generating " << std::to_string(num_junctions) <<
" junctions" << std::endl;
1357 size_t junctionindex = 0;
1358 size_t num_junctions_per_thread = 5;
1359 size_t num_threads = (num_junctions / num_junctions_per_thread) + 1;
1360 num_threads = num_threads > 1 ? num_threads : 1;
1361 std::vector<std::thread> workers;
1362 std::mutex write_mutex;
1364 for (
size_t i = 0; i < num_threads; ++i ) {
1365 std::thread neworker(
1366 [
this, &write_mutex, &mesh_factory, &junction_out_mesh_list, JunctionsToGenerate, i, num_junctions_per_thread, num_junctions]() {
1368 std::vector<std::unique_ptr<geom::Mesh>>> junctionsofthisthread;
1371 if( (i + 1) * num_junctions_per_thread < num_junctions ){
1372 minimum = (i + 1) * num_junctions_per_thread;
1374 minimum = num_junctions;
1376 std::cout <<
"Generating Junctions between " << std::to_string(i * num_junctions_per_thread) <<
" and " << std::to_string(minimum) << std::endl;
1378 for (
size_t junctionindex = i * num_junctions_per_thread;
1379 junctionindex < minimum;
1382 GenerateSingleJunction(mesh_factory, JunctionsToGenerate[junctionindex], &junctionsofthisthread);
1384 std::cout <<
"Generated Junctions between " << std::to_string(i * num_junctions_per_thread) <<
" and " << std::to_string(minimum) << std::endl;
1385 std::lock_guard<std::mutex> guard(write_mutex);
1386 for (
auto&& pair : junctionsofthisthread ) {
1387 if ((*junction_out_mesh_list).find(pair.first) != (*junction_out_mesh_list).end()) {
1388 (*junction_out_mesh_list)[pair.first].insert((*junction_out_mesh_list)[pair.first].end(),
1389 std::make_move_iterator(pair.second.begin()),
1390 std::make_move_iterator(pair.second.end()));
1392 (*junction_out_mesh_list)[pair.first] = std::move(pair.second);
1396 workers.push_back(std::move(neworker));
1399 for (
size_t i = 0; i < workers.size(); ++i) {
1403 for (
size_t i = 0; i < workers.size(); ++i) {
1404 if (workers[i].joinable()) {
1413 std::cout <<
"Filtered from " + std::to_string(_data.GetJunctions().size() ) +
" junctions " << std::endl;
1414 std::vector<JuncId> ToReturn;
1415 for(
auto& junction : _data.GetJunctions() ){
1416 geom::Location junctionLocation = junction.second.GetBoundingBox().location;
1417 if( minpos.
x < junctionLocation.
x && junctionLocation.
x < maxpos.
x &&
1418 minpos.
y > junctionLocation.
y && junctionLocation.
y > maxpos.
y ) {
1419 ToReturn.push_back(junction.first);
1422 std::cout <<
"To " + std::to_string(ToReturn.size() ) +
" junctions " << std::endl;
1430 std::vector<RoadId> ToReturn;
1431 std::cout <<
"Filtered from " + std::to_string(_data.GetRoads().size() ) +
" roads " << std::endl;
1432 for(
auto& road : _data.GetRoads() ){
1433 auto &&lane_section = (*road.second.GetLaneSections().begin());
1434 const road::Lane* lane = lane_section.GetLane(-1);
1436 const double s_check = lane_section.
GetDistance() + lane_section.GetLength() * 0.5;
1438 if( minpos.
x < roadLocation.
x && roadLocation.
x < maxpos.
x &&
1439 minpos.
y > roadLocation.
y && roadLocation.
y > maxpos.
y ) {
1440 ToReturn.push_back(road.first);
1444 std::cout <<
"To " + std::to_string(ToReturn.size() ) +
" roads " << std::endl;
1449 const std::vector<geom::Vector3D>& sdfinput,
1450 int grid_cells_per_dim)
const {
1452 int junctionid = jinput.
GetId();
1453 float box_extraextension_factor = 1.2f;
1454 const double CubeSize = 0.5;
1463 boost::optional<element::Waypoint> CheckingWaypoint = GetWaypoint(
geom::Location(worldloc), 0x1 << 1);
1464 if (CheckingWaypoint) {
1471 boost::optional<element::Waypoint> InRoadWaypoint = GetClosestWaypointOnRoad(
geom::Location(worldloc), 0x1 << 1);
1472 geom::Transform InRoadWPTransform = ComputeTransform(*InRoadWaypoint);
1478 if (Distance.Length2D() < CubeSize * 1.1 && pos.z < 0.2) {
1481 return Distance.
Length() * -1.0;
1484 double gridsizeindouble = grid_cells_per_dim;
1486 domain.
min = { MinOffset.
x, MinOffset.y, MinOffset.z };
1487 domain.
size = { bb.
extent.
x * box_extraextension_factor * 2, bb.
extent.
y * box_extraextension_factor * 2, 0.4 };
1495 for (
auto& cv : mesh.vertices) {
1504 for (
auto ct : mesh.triangles) {
1511 boost::optional<element::Waypoint> CheckingWaypoint = GetWaypoint(
geom::Location(cv), 0x1 << 1);
1512 if (!CheckingWaypoint)
1514 boost::optional<element::Waypoint> InRoadWaypoint = GetClosestWaypointOnRoad(
geom::Location(cv), 0x1 << 1);
1515 geom::Transform InRoadWPTransform = ComputeTransform(*InRoadWaypoint);
1522 return std::make_unique<geom::Mesh>(out_mesh);
1528 junction_out_mesh_list)
const {
1530 const auto& junction = _data.GetJunctions().at(Id);
1531 if (junction.GetConnections().size() > 2) {
1532 std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1533 std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1534 std::vector<carla::geom::Vector3D> perimeterpoints;
1536 auto pmesh = SDFToMesh(junction, perimeterpoints, 75);
1539 for (
const auto& connection_pair : junction.GetConnections()) {
1540 const auto& connection = connection_pair.second;
1541 const auto& road = _data.GetRoads().at(connection.connecting_road);
1542 for (
auto&& lane_section : road.GetLaneSections()) {
1543 for (
auto&& lane_pair : lane_section.GetLanes()) {
1544 const auto& lane = lane_pair.second;
1546 boost::optional<element::Waypoint> sw =
1547 GetWaypoint(road.GetId(), lane_pair.first, lane.GetDistance() + (lane.GetLength() * 0.5f));
1548 if( GetWaypoint(ComputeTransform(*sw).location).get_ptr () == nullptr ){
1555 std::unique_ptr<geom::Mesh> sidewalk_mesh = std::make_unique<geom::Mesh>();
1556 for (
auto& lane : sidewalk_lane_meshes) {
1557 *sidewalk_mesh += *lane;
1561 std::vector<std::unique_ptr<geom::Mesh>> lane_meshes;
1562 std::vector<std::unique_ptr<geom::Mesh>> sidewalk_lane_meshes;
1563 for (
const auto& connection_pair : junction.GetConnections()) {
1564 const auto& connection = connection_pair.second;
1565 const auto& road = _data.GetRoads().at(connection.connecting_road);
1566 for (
auto&& lane_section : road.GetLaneSections()) {
1567 for (
auto&& lane_pair : lane_section.GetLanes()) {
1568 const auto& lane = lane_pair.second;
1578 std::unique_ptr<geom::Mesh> merged_mesh = std::make_unique<geom::Mesh>();
1579 for (
auto& lane : lane_meshes) {
1580 *merged_mesh += *lane;
1582 std::unique_ptr<geom::Mesh> sidewalk_mesh = std::make_unique<geom::Mesh>();
1583 for (
auto& lane : sidewalk_lane_meshes) {
1584 *sidewalk_mesh += *lane;
std::vector< carla::geom::BoundingBox > GetJunctionsBoundingBoxes() const
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...
void AddVertex(vertex_type vertex)
Appends a vertex to the vertices list.
static double GetDistanceAtStartOfLane(const Lane &lane)
void GenerateSingleJunction(const carla::geom::MeshFactory &mesh_factory, const JuncId Id, std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh >>> *junction_out_mesh_list) const
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.
void GenerateJunctions(const carla::geom::MeshFactory &mesh_factory, const rpc::OpendriveGenerationParameters ¶ms, const geom::Vector3D &minpos, const geom::Vector3D &maxpos, std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh >>> *juntion_out_mesh_list) const
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.
std::unique_ptr< Mesh > GenerateSidewalk(const road::LaneSection &lane_section) const
Vector3D extent
Half the size of the BoundingBox in local space.
Each lane within a road cross section can be provided with several road markentries.
geom::Mesh GetAllCrosswalkMesh() const
Buids a mesh of all crosswalks based on the OpenDRIVE.
void AddIndex(index_type index)
Appends a index to the indexes list.
std::unique_ptr< Mesh > MergeAndSmooth(std::vector< std::unique_ptr< Mesh >> &lane_meshes) const
std::string GetType() 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)
void GenerateAllOrderedWithMaxLen(const road::Road &road, std::map< road::Lane::LaneType, std::vector< std::unique_ptr< Mesh >>> &roads) const
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)
double GetWidth(const double s) const
Returns the total lane width given a s.
std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh > > > GenerateOrderedChunkedMeshInLocations(const rpc::OpendriveGenerationParameters ¶ms, const geom::Vector3D &minpos, const geom::Vector3D &maxpos) const
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 ----------------...
std::vector< std::unique_ptr< geom::Mesh > > GenerateLineMarkings(const rpc::OpendriveGenerationParameters ¶ms, const geom::Vector3D &minpos, const geom::Vector3D &maxpos, std::vector< std::string > &outinfo) const
Buids a list of meshes related with LineMarkings.
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)
float GetZPosInDeformation(float posx, float posy) const
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
std::vector< RoadId > FilterRoadsByPosition(const geom::Vector3D &minpos, const geom::Vector3D &maxpos) const
Data structure for the signal search.
#define RELEASE_ASSERT(pred)
double min(double v1, double v2)
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
std::vector< JuncId > FilterJunctionsByPosition(const geom::Vector3D &minpos, const geom::Vector3D &maxpos) const
const T * GetInfo(const double s) const
double GetRemainingLength(const Lane &lane, double current_s)
std::unique_ptr< geom::Mesh > SDFToMesh(const road::Junction &jinput, const std::vector< geom::Vector3D > &sdfinput, int grid_cells_per_dim) const
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()
Rotation rotation
Rotation of the BoundingBox in local space.
bool IsStraight() const
Checks whether the geometry is straight or not.
std::vector< std::pair< geom::Transform, std::string > > GetTreesTransform(const geom::Vector3D &minpos, const geom::Vector3D &maxpos, float distancebetweentrees, float distancefromdrivinglineborder, float s_offset=0) const
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.
geom::Transform ComputeTransform(const double s) const
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
void GenerateLaneMarkForRoad(const road::Road &road, std::vector< std::unique_ptr< Mesh >> &inout, std::vector< std::string > &outinfo) const
std::unordered_map< ConId, Connection > & GetConnections()
JuncId GetJunctionId(RoadId road_id) const
Vector3D MakeUnitVector() const
Mesh MarchCube(Fun3s const &sdf, Rect3 const &domain)
Reconstructs a triangle mesh from a given signed distance function using Marching Cubes...
std::unique_ptr< Mesh > GenerateTesselated(const road::Lane &lane, const double s_start, const double s_end) const
Generates a mesh that defines a lane from a given s start and end with bigger tesselation.
std::map< road::Lane::LaneType, std::vector< std::unique_ptr< geom::Mesh > > > GenerateRoadsMultithreaded(const carla::geom::MeshFactory &mesh_factory, const std::vector< RoadId > &RoadsID, const size_t index, const size_t number_of_roads_per_thread) const
boost::optional< Waypoint > GetRight(Waypoint waypoint) const
Return a waypoint at the lane of waypoint's right lane.
const std::vector< vertex_type > & GetVertices() const
std::vector< std::pair< Waypoint, Waypoint > > GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const
Generate waypoints of the junction.
std::pair< geom::Vector3D, geom::Vector3D > GetCornerPositions(const double s, const float extra_width=0.f) const
Computes the location of the edges given a s.
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
double GetLaneWidth(Waypoint waypoint) const
static void ForEachLaneImpl(RoadId road_id, const LaneSection &lane_section, double distance, Lane::LaneType lane_type, FuncT &&func)