From 728e1155fa47f0a262b47add66bd04e86cb1e20c Mon Sep 17 00:00:00 2001 From: Blyron Date: Wed, 2 Oct 2024 12:15:32 +0200 Subject: [PATCH] Added functions to get tesselatted crosswalks --- LibCarla/source/carla/road/Map.cpp | 126 +++++++++++++++++++++++++++++ LibCarla/source/carla/road/Map.h | 8 +- 2 files changed, 133 insertions(+), 1 deletion(-) diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index 6910196eb..31249844f 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -511,6 +511,67 @@ namespace road { return result; } + std::vector> Map::GetAllCrosswalkZonesListed() const { + std::vector> result; + + for (const auto &pair : _data.GetRoads()) { + const auto &road = pair.second; + std::vector crosswalks = road.GetInfos(); + if (crosswalks.size() > 0) { + for (auto crosswalk : crosswalks) { + // waypoint only at start position + std::vector points; + Waypoint waypoint; + geom::Transform base; + for (const auto §ion : road.GetLaneSectionsAt(crosswalk->GetS())) { + // get the section with the center lane + for (const auto &lane : section.GetLanes()) { + // is the center line + if (lane.first == 0) { + // get the center point + waypoint.road_id = pair.first; + waypoint.section_id = section.GetId(); + waypoint.lane_id = 0; + waypoint.s = crosswalk->GetS(); + base = ComputeTransform(waypoint); + } + } + } + + // move perpendicular ('t') + geom::Transform pivot = base; + pivot.rotation.yaw -= geom::Math::ToDegrees(static_cast(crosswalk->GetHeading())); + pivot.rotation.yaw -= 90; // move perpendicular to 's' for the lateral offset + geom::Vector3D v(static_cast(crosswalk->GetT()), 0.0f, 0.0f); + pivot.TransformPoint(v); + // restore pivot position and orientation + pivot = base; + pivot.location = v; + pivot.rotation.yaw -= geom::Math::ToDegrees(static_cast(crosswalk->GetHeading())); + + // calculate all the corners + std::vector quad; + for (auto corner : crosswalk->GetPoints()) { + geom::Vector3D v2( + static_cast(corner.u), + static_cast(corner.v), + static_cast(corner.z)); + // set the width larger to contact with the sidewalk (in case they have gutter area) + if (corner.u < 0) { + v2.x -= 1.0f; + } else { + v2.x += 1.0f; + } + pivot.TransformPoint(v2); + quad.push_back(v2); + } + result.push_back(quad); + } + } + } + return result; + } + // =========================================================================== // -- Map: Waypoint generation ----------------------------------------------- // =========================================================================== @@ -1285,6 +1346,71 @@ namespace road { return out_mesh; } + std::vector Map::GetAllCrosswalkMeshesTesselated() const { + std::vector out_meshes; + + const size_t NumVertexWidthTessCrosswalk = 30; + const size_t NumVertexLenghtTessCrosswalk = 200; + + // Get the crosswalk vertices for the current map + const std::vector> crosswalks_vertices = GetAllCrosswalkZonesListed(); + if (crosswalks_vertices.empty()) { + return out_meshes; + } + + // Create a a list of triangle fans with material "crosswalk" + size_t start_vertex_index = 0; + size_t i = 0; + std::vector vertices; + for(const std::vector& current_crosswalk : crosswalks_vertices) + { + geom::Mesh out_mesh; + out_mesh.AddMaterial("crosswalk"); + geom::Location leftvector = current_crosswalk[1] - current_crosswalk[0]; + geom::Location forwardvector = current_crosswalk[2] - current_crosswalk[0]; + + geom::Vector3D first_left_edge = current_crosswalk[0]; + geom::Vector3D first_right_edge = current_crosswalk[0]; + size_t current_vertex_length = 0; + + std::vector vertices; + // Ensure minimum vertices in width are two + const int vertices_in_width = NumVertexWidthTessCrosswalk; + const int segments_number = vertices_in_width - 1; + + do { + const geom::Vector3D left_edge = first_left_edge + (forwardvector * current_vertex_length); + const geom::Vector3D right_edge = first_right_edge + (forwardvector * current_vertex_length); + + const geom::Vector3D segments_size = ( right_edge - left_edge ) / segments_number; + geom::Vector3D current_vertex = left_edge; + for (int i = 0; i < vertices_in_width; ++i) { + vertices.push_back(current_vertex); + current_vertex = current_vertex + segments_size; + } + // Update the current waypoint's "s" + current_vertex_length++; + } while (current_vertex_length < NumVertexLenghtTessCrosswalk); + + for (size_t i = 0; i < (NumVertexLenghtTessCrosswalk - 1); ++i) { + for (size_t j = 0; j < NumVertexWidthTessCrosswalk - 1; ++j) { + out_mesh.AddIndex( j + ( i * NumVertexWidthTessCrosswalk ) + 1); + out_mesh.AddIndex( ( j + 1 ) + ( i * NumVertexWidthTessCrosswalk ) + 1); + out_mesh.AddIndex( j + ( ( i + 1 ) * NumVertexWidthTessCrosswalk ) + 1); + + out_mesh.AddIndex( ( j + 1 ) + ( i * NumVertexWidthTessCrosswalk ) + 1); + out_mesh.AddIndex( ( j + 1 ) + ( ( i + 1 ) * NumVertexWidthTessCrosswalk ) + 1); + out_mesh.AddIndex( j + ( ( i + 1 ) * NumVertexWidthTessCrosswalk ) + 1); + } + } + out_mesh.EndMaterial(); + out_meshes.push_back(std::move(out_mesh)); + } + + return out_meshes; + } + + /// Buids a list of meshes related with LineMarkings std::vector> Map::GenerateLineMarkings( const rpc::OpendriveGenerationParameters& params, diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index 2e5d0932a..995fcf2e0 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -28,7 +28,6 @@ namespace road { class Map : private MovableNonCopyable { public: - using Waypoint = element::Waypoint; /// ======================================================================== @@ -91,6 +90,10 @@ namespace road { /// when a location is repeated an area is finished std::vector GetAllCrosswalkZones() const; + /// Returns a list of quads of locations defining 2d areas, + /// when a location is repeated an area is finished + std::vector> GetAllCrosswalkZonesListed() const; + /// Data structure for the signal search struct SignalSearchData { const element::RoadInfoSignal *signal; @@ -169,6 +172,9 @@ namespace road { /// Buids a mesh of all crosswalks based on the OpenDRIVE geom::Mesh GetAllCrosswalkMesh() const; + /// Buids a mesh of all crosswalks based on the OpenDRIVE + std::vector GetAllCrosswalkMeshesTesselated() const; + std::vector> GetTreesTransform( const geom::Vector3D& minpos, const geom::Vector3D& maxpos,