From e2c0349ef6396d381d191a244768260baff70259 Mon Sep 17 00:00:00 2001 From: Marc Garcia Puig Date: Thu, 9 Apr 2020 15:42:18 +0200 Subject: [PATCH] Exposed max chunk size to the PythonAPI --- LibCarla/source/carla/geom/MeshFactory.cpp | 30 +++++++++---------- LibCarla/source/carla/geom/MeshFactory.h | 11 +++---- LibCarla/source/carla/road/Map.cpp | 3 +- LibCarla/source/carla/road/Map.h | 4 +-- .../carla/rpc/OpendriveGenerationParameters.h | 18 +++++++---- PythonAPI/carla/source/libcarla/Client.cpp | 3 +- PythonAPI/util/config.py | 16 +++++----- .../Carla/Source/Carla/Game/CarlaEpisode.cpp | 13 ++------ .../Carla/Source/Carla/Game/CarlaEpisode.h | 11 ------- .../Carla/OpenDrive/OpenDriveGenerator.cpp | 12 ++++---- 10 files changed, 58 insertions(+), 63 deletions(-) diff --git a/LibCarla/source/carla/geom/MeshFactory.cpp b/LibCarla/source/carla/geom/MeshFactory.cpp index afe994839..9cf061b54 100644 --- a/LibCarla/source/carla/geom/MeshFactory.cpp +++ b/LibCarla/source/carla/geom/MeshFactory.cpp @@ -214,10 +214,10 @@ namespace geom { } std::vector> MeshFactory::GenerateWithMaxLen( - const road::Road &road, const double max_len) const { + const road::Road &road) const { std::vector> mesh_uptr_list; for (auto &&lane_section : road.GetLaneSections()) { - auto section_uptr_list = GenerateWithMaxLen(lane_section, max_len); + auto section_uptr_list = GenerateWithMaxLen(lane_section); mesh_uptr_list.insert( mesh_uptr_list.end(), std::make_move_iterator(section_uptr_list.begin()), @@ -227,15 +227,15 @@ namespace geom { } std::vector> MeshFactory::GenerateWithMaxLen( - const road::LaneSection &lane_section, const double max_len) const { + const road::LaneSection &lane_section) const { std::vector> mesh_uptr_list; - if (lane_section.GetLength() < max_len) { + if (lane_section.GetLength() < road_param.max_road_len) { mesh_uptr_list.emplace_back(Generate(lane_section)); } else { double s_current = lane_section.GetDistance() + EPSILON; const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON; - while(s_current + max_len < s_end) { - const auto s_until = s_current + max_len; + while(s_current + road_param.max_road_len < s_end) { + const auto s_until = s_current + road_param.max_road_len; Mesh lane_section_mesh; for (auto &&lane_pair : lane_section.GetLanes()) { lane_section_mesh += *Generate(lane_pair.second, s_current, s_until); @@ -255,10 +255,10 @@ namespace geom { } std::vector> MeshFactory::GenerateWallsWithMaxLen( - const road::Road &road, const double max_len) const { + const road::Road &road) const { std::vector> mesh_uptr_list; for (auto &&lane_section : road.GetLaneSections()) { - auto section_uptr_list = GenerateWallsWithMaxLen(lane_section, max_len); + auto section_uptr_list = GenerateWallsWithMaxLen(lane_section); mesh_uptr_list.insert( mesh_uptr_list.end(), std::make_move_iterator(section_uptr_list.begin()), @@ -268,7 +268,7 @@ namespace geom { } std::vector> MeshFactory::GenerateWallsWithMaxLen( - const road::LaneSection &lane_section, const double max_len) const { + const road::LaneSection &lane_section) const { std::vector> mesh_uptr_list; const auto min_lane = lane_section.GetLanes().begin()->first == 0 ? @@ -276,13 +276,13 @@ namespace geom { const auto max_lane = lane_section.GetLanes().rbegin()->first == 0 ? -1 : lane_section.GetLanes().rbegin()->first; - if (lane_section.GetLength() < max_len) { + if (lane_section.GetLength() < road_param.max_road_len) { mesh_uptr_list.emplace_back(GenerateWalls(lane_section)); } else { double s_current = lane_section.GetDistance() + EPSILON; const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON; - while(s_current + max_len < s_end) { - const auto s_until = s_current + max_len; + while(s_current + road_param.max_road_len < s_end) { + const auto s_until = s_current + road_param.max_road_len; Mesh lane_section_mesh; for (auto &&lane_pair : lane_section.GetLanes()) { const auto &lane = lane_pair.second; @@ -314,11 +314,11 @@ namespace geom { } std::vector> MeshFactory::GenerateAllWithMaxLen( - const road::Road &road, const double max_len) const { + const road::Road &road) const { std::vector> mesh_uptr_list; // Get road meshes - auto roads = GenerateWithMaxLen(road, max_len); + auto roads = GenerateWithMaxLen(road); mesh_uptr_list.insert( mesh_uptr_list.end(), std::make_move_iterator(roads.begin()), @@ -326,7 +326,7 @@ namespace geom { // Get wall meshes only if is not a junction if (!road.IsJunction()) { - auto walls = GenerateWallsWithMaxLen(road, max_len); + auto walls = GenerateWallsWithMaxLen(road); if (roads.size() == walls.size()) { for (size_t i = 0; i < walls.size(); ++i) { diff --git a/LibCarla/source/carla/geom/MeshFactory.h b/LibCarla/source/carla/geom/MeshFactory.h index dfa13037d..6b2fc98ff 100644 --- a/LibCarla/source/carla/geom/MeshFactory.h +++ b/LibCarla/source/carla/geom/MeshFactory.h @@ -60,23 +60,23 @@ namespace geom { /// Generates list of meshes that defines a single road with a maximum length std::vector> GenerateWithMaxLen( - const road::Road &road, const double max_len) const; + const road::Road &road) const; /// Generates list of meshes that defines a single lane_section with a maximum length std::vector> GenerateWithMaxLen( - const road::LaneSection &lane_section, const double max_len) const; + const road::LaneSection &lane_section) const; std::vector> GenerateWallsWithMaxLen( - const road::Road &road, const double max_len) const; + const road::Road &road) const; std::vector> GenerateWallsWithMaxLen( - const road::LaneSection &lane_section, const double max_len) const; + const road::LaneSection &lane_section) const; // -- Util -- /// Generates a chunked road with all the features needed for simulation std::vector> GenerateAllWithMaxLen( - const road::Road &road, const double max_len) const; + const road::Road &road) const; // ========================================================================= // -- Generation parameters ------------------------------------------------ @@ -85,6 +85,7 @@ namespace geom { /// Parameters for the road generation struct RoadParameters { float resolution = 2.0f; + float max_road_len = 50.0f; float extra_lane_width = 1.0f; float wall_height = 0.6f; }; diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index c6f14ec85..0f5dc8579 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -955,6 +955,7 @@ namespace road { RELEASE_ASSERT(max_road_len > 0.0); geom::MeshFactory mesh_factory; mesh_factory.road_param.resolution = static_cast(distance); + mesh_factory.road_param.max_road_len = max_road_len; mesh_factory.road_param.extra_lane_width = extra_width; std::vector> out_mesh_list; @@ -962,7 +963,7 @@ namespace road { for (auto &&pair : _data.GetRoads()) { const auto &road = pair.second; std::vector> road_mesh_list = - mesh_factory.GenerateAllWithMaxLen(road, max_road_len); + mesh_factory.GenerateAllWithMaxLen(road); // If the road in in a junction, add the road to the junction mesh instead of // doing it separately, this is needed for the road mesh smooth algorithm diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index bc27b821a..27f294109 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -150,10 +150,10 @@ namespace road { ComputeJunctionConflicts(JuncId id) const; /// Buids a mesh based on the OpenDRIVE - geom::Mesh GenerateMesh(const double distance, const float extra_width = 0.f) const; + geom::Mesh GenerateMesh(const double distance, const float extra_width = 0.6f) const; std::vector> GenerateChunkedMesh( - const double distance, const float max_road_len, const float extra_width = 0.f) const; + const double distance, const float max_road_len = 50.0f, const float extra_width = 0.6f) const; /// Buids a mesh of all crosswalks based on the OpenDRIVE geom::Mesh GetAllCrosswalkMesh() const; diff --git a/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h b/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h index c448b177c..911fa895c 100644 --- a/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h +++ b/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h @@ -14,22 +14,30 @@ namespace rpc { OpendriveGenerationParameters(){} OpendriveGenerationParameters( double v_distance, + double max_road_len, double w_height, double a_width, bool e_visibility) : vertex_distance(v_distance), + max_road_length(max_road_len), wall_height(w_height), additional_width(a_width), enable_mesh_visibility(e_visibility) {} - double vertex_distance = 2.f; - double wall_height = 1.f; - double additional_width = 0.6f; + double vertex_distance = 2.0; + double max_road_length = 50.0; + double wall_height = 1.0; + double additional_width = 0.6; bool enable_mesh_visibility = true; - MSGPACK_DEFINE_ARRAY(vertex_distance, wall_height, additional_width, enable_mesh_visibility); + MSGPACK_DEFINE_ARRAY( + vertex_distance, + max_road_length, + wall_height, + additional_width, + enable_mesh_visibility); }; } -} \ No newline at end of file +} diff --git a/PythonAPI/carla/source/libcarla/Client.cpp b/PythonAPI/carla/source/libcarla/Client.cpp index fea9c445a..cde35ebbb 100644 --- a/PythonAPI/carla/source/libcarla/Client.cpp +++ b/PythonAPI/carla/source/libcarla/Client.cpp @@ -166,8 +166,9 @@ void export_client() { namespace rpc = carla::rpc; class_("OpendriveGenerationParameters", - init((arg("vertex_distance")=2.0, arg("wall_height")=1.0, arg("additional_width")=0.6, arg("enable_mesh_visibility")=true))) + init((arg("vertex_distance")=2.0, arg("max_road_length")=50.0, arg("wall_height")=1.0, arg("additional_width")=0.6, arg("enable_mesh_visibility")=true))) .def_readwrite("vertex_distance", &rpc::OpendriveGenerationParameters::vertex_distance) + .def_readwrite("max_road_length", &rpc::OpendriveGenerationParameters::max_road_length) .def_readwrite("wall_height", &rpc::OpendriveGenerationParameters::wall_height) .def_readwrite("additional_width", &rpc::OpendriveGenerationParameters::additional_width) .def_readwrite("enable_mesh_visibility", &rpc::OpendriveGenerationParameters::enable_mesh_visibility) diff --git a/PythonAPI/util/config.py b/PythonAPI/util/config.py index 5f395d5fe..af4f2cea0 100755 --- a/PythonAPI/util/config.py +++ b/PythonAPI/util/config.py @@ -210,15 +210,17 @@ def main(): print('file could not be readed.') sys.exit() print('load opendrive map %r.' % os.path.basename(args.xodr_path)) - vertex_distance = 2.0 # in meters - wall_height = 1.0 # in meters - extra_width = 0.6 # in meters + vertex_distance = 2.0 # in meters + max_road_length = 50.0 # in meters + wall_height = 1.0 # in meters + extra_width = 0.6 # in meters world = client.generate_opendrive_world( data, carla.OpendriveGenerationParameters( - vertex_distance, - wall_height, - extra_width, - True)) + vertex_distance=vertex_distance, + max_road_length=max_road_length, + wall_height=wall_height, + additional_width=extra_width, + enable_mesh_visibility=True)) else: print('file not found.') diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp index 17c812a56..68e6c31d1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp @@ -128,16 +128,6 @@ static FString BuildRecastBuilderFile() return AbsoluteRecastBuilderPath; } -bool UCarlaEpisode::LoadNewOpendriveEpisode( - const FString &OpenDriveString, - float VertexDistance, - float WallHeight, - float AdditionalWidth) -{ - return LoadNewOpendriveEpisode(OpenDriveString, - carla::rpc::OpendriveGenerationParameters(VertexDistance, WallHeight, AdditionalWidth, true)); -} - bool UCarlaEpisode::LoadNewOpendriveEpisode( const FString &OpenDriveString, const carla::rpc::OpendriveGenerationParameters &Params) @@ -204,8 +194,9 @@ bool UCarlaEpisode::LoadNewOpendriveEpisode( } // Build the mesh generation config file const FString ConfigData = FString::Printf( - TEXT("resolution=%s\nwall_height=%s\nadditional_width=%s\nmesh_visibility=%s"), + TEXT("resolution=%s\nmax_road_length=%s\nwall_height=%s\nadditional_width=%s\nmesh_visibility=%s\n"), *FString::SanitizeFloat(Params.vertex_distance), + *FString::SanitizeFloat(Params.max_road_length), *FString::SanitizeFloat(Params.wall_height), *FString::SanitizeFloat(Params.additional_width), *MeshVisibilityStr); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h index 49fe721ae..f75a7bf73 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h @@ -55,17 +55,6 @@ public: UFUNCTION(BlueprintCallable) bool LoadNewEpisode(const FString &MapString); - /// Load a new map generating the mesh from OpenDRIVE data and - /// start a new episode. - /// - /// If @a MapString is empty, it fails. - UFUNCTION(BlueprintCallable) - bool LoadNewOpendriveEpisode( - const FString &OpenDriveString, - float VertexDistance, - float WallHeight, - float AdditionalWidth); - /// Load a new map generating the mesh from OpenDRIVE data and /// start a new episode. /// diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp index 2d2d8b803..ff090ae25 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp @@ -68,6 +68,7 @@ void AOpenDriveGenerator::GenerateRoadMesh() static const FString ConfigFilePath = FPaths::ProjectContentDir() + "Carla/Maps/OpenDrive/OpenDriveMap.conf"; float Resolution = 2.f; + float MaxRoadLength = 50.0f; float WallHeight = 1.f; float AdditionalWidth = .6f; bool MeshVisibility = true; @@ -87,6 +88,10 @@ void AOpenDriveGenerator::GenerateRoadMesh() { WallHeight = FCString::Atof(*Value); } + else if (Key == "max_road_length") + { + MaxRoadLength = FCString::Atof(*Value); + } else if (Key == "additional_width") { AdditionalWidth = FCString::Atof(*Value); @@ -98,11 +103,8 @@ void AOpenDriveGenerator::GenerateRoadMesh() } } - // const FProceduralCustomMesh MeshData = - // CarlaMap->GenerateMesh(Resolution, AdditionalWidth) + - // CarlaMap->GenerateWalls(Resolution, WallHeight); - - const auto Meshes = CarlaMap->GenerateChunkedMesh(Resolution, 50.0f, AdditionalWidth); + const auto Meshes = CarlaMap->GenerateChunkedMesh( + Resolution, MaxRoadLength, AdditionalWidth); for (const auto &Mesh : Meshes) { AActor *TempActor = GetWorld()->SpawnActor(); UProceduralMeshComponent *TempPMC = NewObject(TempActor);