From 86fcfe5e807b22c174ed38e8002da89248095c8d Mon Sep 17 00:00:00 2001 From: synkrotron Date: Mon, 18 Dec 2023 17:02:33 +0800 Subject: [PATCH] Fix problems of lanemarking generation and tree generation (#7006) Co-authored-by: xiaofei <13319202082@163.com> --- LibCarla/source/carla/road/Map.cpp | 1 + LibCarla/source/carla/road/MeshFactory.cpp | 113 +++++++++------------ LibCarla/source/carla/road/MeshFactory.h | 9 ++ 3 files changed, 56 insertions(+), 67 deletions(-) diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index e746ad893..70bedbeee 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -1232,6 +1232,7 @@ namespace road { while(s_current < s_end){ if(lane->GetWidth(s_current) != 0.0f){ const auto edges = lane->GetCornerPositions(s_current, 0); + if (edges.first == edges.second) continue; geom::Vector3D director = edges.second - edges.first; geom::Vector3D treeposition = edges.first - director.MakeUnitVector() * distancefromdrivinglineborder; geom::Transform lanetransform = lane->ComputeTransform(s_current); diff --git a/LibCarla/source/carla/road/MeshFactory.cpp b/LibCarla/source/carla/road/MeshFactory.cpp index 83cb3f513..8d7019ebb 100644 --- a/LibCarla/source/carla/road/MeshFactory.cpp +++ b/LibCarla/source/carla/road/MeshFactory.cpp @@ -761,14 +761,11 @@ std::map>> MeshFactory: case carla::road::element::LaneMarking::Type::Solid: { size_t currentIndex = out_mesh.GetVertices().size() + 1; - std::pair edges = lane.GetCornerPositions(s_current, 0); - - geom::Vector3D director = edges.second - edges.first; - director /= director.Length(); - geom::Vector3D endmarking = edges.first + director * lane_mark_info.width; + std::pair edges = + ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width); out_mesh.AddVertex(edges.first); - out_mesh.AddVertex(endmarking); + out_mesh.AddVertex(edges.second); out_mesh.AddIndex(currentIndex); out_mesh.AddIndex(currentIndex + 1); @@ -784,30 +781,23 @@ std::map>> MeshFactory: case carla::road::element::LaneMarking::Type::Broken: { size_t currentIndex = out_mesh.GetVertices().size() + 1; - std::pair edges = - lane.GetCornerPositions(s_current, road_param.extra_lane_width); - - geom::Vector3D director = edges.second - edges.first; - director /= director.Length(); - geom::Vector3D endmarking = edges.first + director * lane_mark_info.width; + std::pair edges = + ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width); out_mesh.AddVertex(edges.first); - out_mesh.AddVertex(endmarking); + out_mesh.AddVertex(edges.second); s_current += road_param.resolution * 3; if (s_current > s_end) { s_current = s_end; } - edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); - director = edges.second - edges.first; - director /= director.Length(); - endmarking = edges.first + director * lane_mark_info.width; + edges = ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width); out_mesh.AddVertex(edges.first); - out_mesh.AddVertex(endmarking); - + out_mesh.AddVertex(edges.second); + out_mesh.AddIndex(currentIndex); out_mesh.AddIndex(currentIndex + 1); out_mesh.AddIndex(currentIndex + 2); @@ -864,13 +854,12 @@ std::map>> MeshFactory: const carla::road::element::RoadInfoMarkRecord* road_info_mark = lane.GetInfo(s_current); if (road_info_mark != nullptr) { carla::road::element::LaneMarking lane_mark_info(*road_info_mark); - std::pair edges = lane.GetCornerPositions(s_end, 0); - geom::Vector3D director = edges.second - edges.first; - director /= director.Length(); - geom::Vector3D endmarking = edges.first + director * lane_mark_info.width; + + std::pair edges = + ComputeEdgesForLanemark(lane_section, lane, s_end, lane_mark_info.width); out_mesh.AddVertex(edges.first); - out_mesh.AddVertex(endmarking); + out_mesh.AddVertex(edges.second); } inout.push_back(std::make_unique(out_mesh)); } @@ -927,58 +916,21 @@ std::map>> MeshFactory: case carla::road::element::LaneMarking::Type::Broken: { size_t currentIndex = out_mesh.GetVertices().size() + 1; - std::pair edges = - lane.GetCornerPositions(s_current, road_param.extra_lane_width); - - geom::Vector3D director; - if (lane.GetWidth(s_current) != 0) { - director = edges.second - edges.first; - director /= director.Length(); - } else { - const std::map & lanes = lane_section.GetLanes(); - for (const auto& lane_pair : lanes) { - if (lane_pair.second.GetWidth(s_current) != 0) { - std::pair another_edge = - lane_pair.second.GetCornerPositions(s_current, road_param.extra_lane_width); - director = another_edge.second - another_edge.first; - director /= director.Length(); - break; - } - } - } - - geom::Vector3D endmarking = edges.first + director * lane_mark_info.width; - + std::pair edges = + ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width); + out_mesh.AddVertex(edges.first); - out_mesh.AddVertex(endmarking); + out_mesh.AddVertex(edges.second); s_current += road_param.resolution * 3; if (s_current > s_end) { s_current = s_end; } - edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); - - if (lane.GetWidth(s_current) != 0) { - director = edges.second - edges.first; - director /= director.Length(); - } else { - const std::map & lanes = lane_section.GetLanes(); - for (const auto& lane_pair : lanes) { - if (lane_pair.second.GetWidth(s_current) != 0) { - std::pair another_edge = - lane_pair.second.GetCornerPositions(s_current, road_param.extra_lane_width); - director = another_edge.second - another_edge.first; - director /= director.Length(); - break; - } - } - } - - endmarking = edges.first + director * lane_mark_info.width; + edges = ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width); out_mesh.AddVertex(edges.first); - out_mesh.AddVertex(endmarking); + out_mesh.AddVertex(edges.second); out_mesh.AddIndex(currentIndex); out_mesh.AddIndex(currentIndex + 1); @@ -1179,6 +1131,33 @@ std::map>> MeshFactory: return std::make_unique(out_mesh); } + std::pair MeshFactory::ComputeEdgesForLanemark( + const road::LaneSection& lane_section, + const road::Lane& lane, + const double s_current, + const double lanemark_width) const { + std::pair edges = + lane.GetCornerPositions(s_current, road_param.extra_lane_width); + + geom::Vector3D director; + if (edges.first != edges.second) { + director = edges.second - edges.first; + director /= director.Length(); + } else { + const std::map & lanes = lane_section.GetLanes(); + for (const auto& lane_pair : lanes) { + std::pair another_edge = + lane_pair.second.GetCornerPositions(s_current, road_param.extra_lane_width); + if (another_edge.first != another_edge.second) { + director = another_edge.second - another_edge.first; + director /= director.Length(); + break; + } + } + } + geom::Vector3D endmarking = edges.first + director * lanemark_width; + return std::make_pair(edges.first, endmarking); + } } // namespace geom } // namespace carla diff --git a/LibCarla/source/carla/road/MeshFactory.h b/LibCarla/source/carla/road/MeshFactory.h index 0334d01d4..42bc60ea1 100644 --- a/LibCarla/source/carla/road/MeshFactory.h +++ b/LibCarla/source/carla/road/MeshFactory.h @@ -148,6 +148,15 @@ namespace geom { RoadParameters road_param; + private: + + // Calculate the points on both sides of the lane mark for the specified s_current + std::pair ComputeEdgesForLanemark( + const road::LaneSection& lane_section, + const road::Lane& lane, + const double s_current, + const double lanemark_width) const; + }; } // namespace geom