diff --git a/.gitignore b/.gitignore index 02e40f0eb..7ceb7149d 100644 --- a/.gitignore +++ b/.gitignore @@ -7,8 +7,10 @@ Util/Build Install Plugins !Unreal/CarlaUE4/Plugins +Unreal/CarlaUE4/Plugins/Streetmap Unreal/CarlaUE4/Plugins/HoudiniEngine + /ExportedMaps /Import/* !/Import/README.md diff --git a/LibCarla/source/carla/road/Lane.cpp b/LibCarla/source/carla/road/Lane.cpp index 98e202563..84719600a 100644 --- a/LibCarla/source/carla/road/Lane.cpp +++ b/LibCarla/source/carla/road/Lane.cpp @@ -58,8 +58,10 @@ namespace road { double Lane::GetWidth(const double s) const { RELEASE_ASSERT(s <= GetRoad()->GetLength()); const auto width_info = GetInfo(s); - RELEASE_ASSERT(width_info != nullptr); - return width_info->GetPolynomial().Evaluate(s); + if(width_info != nullptr){ + return width_info->GetPolynomial().Evaluate(s); + } + return 0.0f; } bool Lane::IsStraight() const { diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index 549f5dda2..e746ad893 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -1135,27 +1135,33 @@ namespace road { } std::map>> - Map::GenerateOrderedChunkedMesh( const rpc::OpendriveGenerationParameters& params) const + Map::GenerateOrderedChunkedMeshInLocations( const rpc::OpendriveGenerationParameters& params, + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos) const { geom::MeshFactory mesh_factory(params); std::map>> road_out_mesh_list; std::map>> junction_out_mesh_list; - std::thread juntction_thread( &Map::GenerateJunctions, this, mesh_factory, params, &junction_out_mesh_list); + std::thread juntction_thread( &Map::GenerateJunctions, this, mesh_factory, params, + minpos, maxpos, &junction_out_mesh_list); - size_t num_roads = _data.GetRoads().size(); - size_t num_roads_per_thread = 20; + const std::vector RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos); + + size_t num_roads = RoadsIDToGenerate.size(); + size_t num_roads_per_thread = 30; size_t num_threads = (num_roads / num_roads_per_thread) + 1; num_threads = num_threads > 1 ? num_threads : 1; std::vector workers; std::mutex write_mutex; + std::cout << "Generating " << std::to_string(num_roads) << " roads" << std::endl; for ( size_t i = 0; i < num_threads; ++i ) { std::thread neworker( - [this, &write_mutex, &mesh_factory, &road_out_mesh_list, i, num_roads_per_thread]() { + [this, &write_mutex, &mesh_factory, &RoadsIDToGenerate, &road_out_mesh_list, i, num_roads_per_thread]() { std::map>> Current = - std::move(GenerateRoadsMultithreaded(mesh_factory, i, num_roads_per_thread)); + std::move(GenerateRoadsMultithreaded(mesh_factory, RoadsIDToGenerate,i, num_roads_per_thread )); std::lock_guard guard(write_mutex); for ( auto&& pair : Current ) { if (road_out_mesh_list.find(pair.first) != road_out_mesh_list.end()) { @@ -1193,17 +1199,23 @@ namespace road { road_out_mesh_list[pair.first] = std::move(pair.second); } } + std::cout << "Generated " << std::to_string(num_roads) << " roads" << std::endl; return road_out_mesh_list; } + std::vector> Map::GetTreesTransform( + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos, float distancebetweentrees, float distancefromdrivinglineborder, float s_offset) const { std::vector> transforms; - for (auto &&pair : _data.GetRoads()) { - const auto &road = pair.second; + + const std::vector RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos); + for ( RoadId id : RoadsIDToGenerate ) { + const auto& road = _data.GetRoads().at(id); if (!road.IsJunction()) { for (auto &&lane_section : road.GetLaneSections()) { LaneId min_lane = 0; @@ -1212,20 +1224,21 @@ namespace road { min_lane = pairlane.first; } } - const auto max_lane = lane_section.GetLanes().rbegin()->first == 0 ? - -1 : lane_section.GetLanes().rbegin()->first; + const road::Lane* lane = lane_section.GetLane(min_lane); if( lane ) { double s_current = lane_section.GetDistance() + s_offset; const double s_end = lane_section.GetDistance() + lane_section.GetLength(); while(s_current < s_end){ - const auto edges = lane->GetCornerPositions(s_current, 0); - geom::Vector3D director = edges.second - edges.first; - geom::Vector3D treeposition = edges.first - director.MakeUnitVector() * distancefromdrivinglineborder; - geom::Transform lanetransform = lane->ComputeTransform(s_current); - geom::Transform treeTransform(treeposition, lanetransform.rotation); - const carla::road::element::RoadInfoSpeed* roadinfo = lane->GetInfo(s_current); - transforms.push_back(std::make_pair(treeTransform,roadinfo->GetType())); + if(lane->GetWidth(s_current) != 0.0f){ + const auto edges = lane->GetCornerPositions(s_current, 0); + geom::Vector3D director = edges.second - edges.first; + geom::Vector3D treeposition = edges.first - director.MakeUnitVector() * distancefromdrivinglineborder; + geom::Transform lanetransform = lane->ComputeTransform(s_current); + geom::Transform treeTransform(treeposition, lanetransform.rotation); + const carla::road::element::RoadInfoSpeed* roadinfo = lane->GetInfo(s_current); + transforms.push_back(std::make_pair(treeTransform,roadinfo->GetType())); + } s_current += distancebetweentrees; } @@ -1275,15 +1288,19 @@ namespace road { /// Buids a list of meshes related with LineMarkings std::vector> Map::GenerateLineMarkings( const rpc::OpendriveGenerationParameters& params, + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos, std::vector& outinfo ) const { std::vector> LineMarks; geom::MeshFactory mesh_factory(params); - for ( auto&& pair : _data.GetRoads() ) { - if ( pair.second.IsJunction() ) { - continue; + + const std::vector RoadsIDToGenerate = FilterRoadsByPosition(minpos, maxpos); + for ( RoadId id : RoadsIDToGenerate ) { + const auto& road = _data.GetRoads().at(id); + if (!road.IsJunction()) { + mesh_factory.GenerateLaneMarkForRoad(road, LineMarks, outinfo); } - mesh_factory.GenerateLaneMarkForRoad(pair.second, LineMarks, outinfo); } return std::move(LineMarks); @@ -1308,91 +1325,126 @@ namespace road { std::map>> Map::GenerateRoadsMultithreaded( const carla::geom::MeshFactory& mesh_factory, + const std::vector& RoadsId, const size_t index, const size_t number_of_roads_per_thread) const { std::map>> out; - auto start = std::next( _data.GetRoads().begin(), (index ) * number_of_roads_per_thread); + size_t start = index * number_of_roads_per_thread; size_t endoffset = (index+1) * number_of_roads_per_thread; - if( endoffset >= _data.GetRoads().size() ) { - endoffset = _data.GetRoads().size(); - } - auto end = std::next( _data.GetRoads().begin(), endoffset ); + size_t end = RoadsId.size(); - for (auto pair = start; pair != end && pair != _data.GetRoads().end(); ++pair) { - const auto& road = pair->second; + for (int i = start; i < endoffset && i < end; ++i) { + const auto& road = _data.GetRoads().at(RoadsId[i]); if (!road.IsJunction()) { mesh_factory.GenerateAllOrderedWithMaxLen(road, out); } } + 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; return out; } void Map::GenerateJunctions(const carla::geom::MeshFactory& mesh_factory, const rpc::OpendriveGenerationParameters& params, + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos, std::map>>* junction_out_mesh_list) const { - for (const auto& junc_pair : _data.GetJunctions()) { - const auto& junction = junc_pair.second; - if (junction.GetConnections().size() > 2) { - std::vector> lane_meshes; - std::vector> sidewalk_lane_meshes; - std::vector perimeterpoints; + std::vector JunctionsToGenerate = FilterJunctionsByPosition(minpos, maxpos); + size_t num_junctions = JunctionsToGenerate.size(); + std::cout << "Generating " << std::to_string(num_junctions) << " junctions" << std::endl; + size_t junctionindex = 0; + size_t num_junctions_per_thread = 5; + size_t num_threads = (num_junctions / num_junctions_per_thread) + 1; + num_threads = num_threads > 1 ? num_threads : 1; + std::vector workers; + std::mutex write_mutex; - auto pmesh = SDFToMesh(junction, perimeterpoints, 75); - (*junction_out_mesh_list)[road::Lane::LaneType::Driving].push_back(std::move(pmesh)); + for ( size_t i = 0; i < num_threads; ++i ) { + std::thread neworker( + [this, &write_mutex, &mesh_factory, &junction_out_mesh_list, JunctionsToGenerate, i, num_junctions_per_thread, num_junctions]() { + std::map>> junctionsofthisthread; - for (const auto& connection_pair : junction.GetConnections()) { - const auto& connection = connection_pair.second; - const auto& road = _data.GetRoads().at(connection.connecting_road); - for (auto&& lane_section : road.GetLaneSections()) { - for (auto&& lane_pair : lane_section.GetLanes()) { - const auto& lane = lane_pair.second; - if (lane.GetType() == road::Lane::LaneType::Sidewalk) { - sidewalk_lane_meshes.push_back(mesh_factory.GenerateSidewalk(lane)); - } - } + size_t minimum = 0; + if( (i + 1) * num_junctions_per_thread < num_junctions ){ + minimum = (i + 1) * num_junctions_per_thread; + }else{ + minimum = num_junctions; + } + std::cout << "Generating Junctions between " << std::to_string(i * num_junctions_per_thread) << " and " << std::to_string(minimum) << std::endl; + + for ( size_t junctionindex = i * num_junctions_per_thread; + junctionindex < minimum; + ++junctionindex ) + { + GenerateSingleJunction(mesh_factory, JunctionsToGenerate[junctionindex], &junctionsofthisthread); + } + std::cout << "Generated Junctions between " << std::to_string(i * num_junctions_per_thread) << " and " << std::to_string(minimum) << std::endl; + std::lock_guard guard(write_mutex); + for ( auto&& pair : junctionsofthisthread ) { + if ((*junction_out_mesh_list).find(pair.first) != (*junction_out_mesh_list).end()) { + (*junction_out_mesh_list)[pair.first].insert((*junction_out_mesh_list)[pair.first].end(), + std::make_move_iterator(pair.second.begin()), + std::make_move_iterator(pair.second.end())); + } else { + (*junction_out_mesh_list)[pair.first] = std::move(pair.second); } } - std::unique_ptr sidewalk_mesh = std::make_unique(); - for (auto& lane : sidewalk_lane_meshes) { - *sidewalk_mesh += *lane; - } - (*junction_out_mesh_list)[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh)); - } else { - std::vector> lane_meshes; - std::vector> sidewalk_lane_meshes; - for (const auto& connection_pair : junction.GetConnections()) { - const auto& connection = connection_pair.second; - const auto& road = _data.GetRoads().at(connection.connecting_road); - for (auto&& lane_section : road.GetLaneSections()) { - for (auto&& lane_pair : lane_section.GetLanes()) { - const auto& lane = lane_pair.second; - if (lane.GetType() != road::Lane::LaneType::Sidewalk) { - lane_meshes.push_back(mesh_factory.GenerateTesselated(lane)); - } - else { - sidewalk_lane_meshes.push_back(mesh_factory.GenerateSidewalk(lane)); - } - } - } - } - std::unique_ptr merged_mesh = std::make_unique(); - for (auto& lane : lane_meshes) { - *merged_mesh += *lane; - } - std::unique_ptr sidewalk_mesh = std::make_unique(); - for (auto& lane : sidewalk_lane_meshes) { - *sidewalk_mesh += *lane; - } + }); + workers.push_back(std::move(neworker)); + } - (*junction_out_mesh_list)[road::Lane::LaneType::Driving].push_back(std::move(merged_mesh)); - (*junction_out_mesh_list)[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh)); + for (size_t i = 0; i < workers.size(); ++i) { + workers[i].join(); + } + workers.clear(); + for (size_t i = 0; i < workers.size(); ++i) { + if (workers[i].joinable()) { + workers[i].join(); } } } + std::vector Map::FilterJunctionsByPosition( const geom::Vector3D& minpos, + const geom::Vector3D& maxpos ) const { + + std::cout << "Filtered from " + std::to_string(_data.GetJunctions().size() ) + " junctions " << std::endl; + std::vector ToReturn; + for( auto& junction : _data.GetJunctions() ){ + geom::Location junctionLocation = junction.second.GetBoundingBox().location; + if( minpos.x < junctionLocation.x && junctionLocation.x < maxpos.x && + minpos.y > junctionLocation.y && junctionLocation.y > maxpos.y ) { + ToReturn.push_back(junction.first); + } + } + std::cout << "To " + std::to_string(ToReturn.size() ) + " junctions " << std::endl; + + return ToReturn; + } + + std::vector Map::FilterRoadsByPosition( const geom::Vector3D& minpos, + const geom::Vector3D& maxpos ) const { + + std::vector ToReturn; + std::cout << "Filtered from " + std::to_string(_data.GetRoads().size() ) + " roads " << std::endl; + for( auto& road : _data.GetRoads() ){ + auto &&lane_section = (*road.second.GetLaneSections().begin()); + const road::Lane* lane = lane_section.GetLane(-1); + if( lane ) { + const double s_check = lane_section.GetDistance() + lane_section.GetLength() * 0.5; + geom::Location roadLocation = lane->ComputeTransform(s_check).location; + if( minpos.x < roadLocation.x && roadLocation.x < maxpos.x && + minpos.y > roadLocation.y && roadLocation.y > maxpos.y ) { + ToReturn.push_back(road.first); + } + } + } + std::cout << "To " + std::to_string(ToReturn.size() ) + " roads " << std::endl; + return ToReturn; + } + std::unique_ptr Map::SDFToMesh(const road::Junction& jinput, const std::vector& sdfinput, int grid_cells_per_dim) const { @@ -1441,42 +1493,18 @@ namespace road { geom::Mesh out_mesh; for (auto& cv : mesh.vertices) { - geom::Vector3D newvertex; newvertex.x = cv.x; newvertex.y = cv.y; newvertex.z = cv.z; - if ( std::find( out_mesh.GetVertices().begin(), out_mesh.GetVertices().end(), newvertex) == out_mesh.GetVertices().end() ) { - out_mesh.AddVertex(newvertex); - } + out_mesh.AddVertex(newvertex); } auto finalvertices = out_mesh.GetVertices(); for (auto ct : mesh.triangles) { - auto cv = mesh.vertices[ct[1]]; - geom::Vector3D newvertex; - newvertex.x = cv.x; - newvertex.y = cv.y; - newvertex.z = cv.z; - - auto it = std::find(finalvertices.begin(), finalvertices.end(), newvertex); - out_mesh.AddIndex(it - finalvertices.begin() + 1); - - cv = mesh.vertices[ct[0]]; - newvertex.x = cv.x; - newvertex.y = cv.y; - newvertex.z = cv.z; - - it = std::find(finalvertices.begin(), finalvertices.end(), newvertex); - out_mesh.AddIndex(it - finalvertices.begin() + 1); - - cv = mesh.vertices[ct[2]]; - newvertex.x = cv.x; - newvertex.y = cv.y; - newvertex.z = cv.z; - - it = std::find(finalvertices.begin(), finalvertices.end(), newvertex); - out_mesh.AddIndex(it - finalvertices.begin() + 1); + out_mesh.AddIndex(ct[1] + 1); + out_mesh.AddIndex(ct[0] + 1); + out_mesh.AddIndex(ct[2] + 1); } for (auto& cv : out_mesh.GetVertices() ) { @@ -1493,5 +1521,73 @@ namespace road { } return std::make_unique(out_mesh); } + + void Map::GenerateSingleJunction(const carla::geom::MeshFactory& mesh_factory, + const JuncId Id, + std::map>>* + junction_out_mesh_list) const { + + const auto& junction = _data.GetJunctions().at(Id); + if (junction.GetConnections().size() > 2) { + std::vector> lane_meshes; + std::vector> sidewalk_lane_meshes; + std::vector perimeterpoints; + + auto pmesh = SDFToMesh(junction, perimeterpoints, 75); + (*junction_out_mesh_list)[road::Lane::LaneType::Driving].push_back(std::move(pmesh)); + + for (const auto& connection_pair : junction.GetConnections()) { + const auto& connection = connection_pair.second; + const auto& road = _data.GetRoads().at(connection.connecting_road); + for (auto&& lane_section : road.GetLaneSections()) { + for (auto&& lane_pair : lane_section.GetLanes()) { + const auto& lane = lane_pair.second; + if ( lane.GetType() == road::Lane::LaneType::Sidewalk ) { + boost::optional sw = + GetWaypoint(road.GetId(), lane_pair.first, lane.GetDistance() + (lane.GetLength() * 0.5f)); + if( GetWaypoint(ComputeTransform(*sw).location).get_ptr () == nullptr ){ + sidewalk_lane_meshes.push_back(mesh_factory.GenerateSidewalk(lane)); + } + } + } + } + } + std::unique_ptr sidewalk_mesh = std::make_unique(); + for (auto& lane : sidewalk_lane_meshes) { + *sidewalk_mesh += *lane; + } + (*junction_out_mesh_list)[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh)); + } else { + std::vector> lane_meshes; + std::vector> sidewalk_lane_meshes; + for (const auto& connection_pair : junction.GetConnections()) { + const auto& connection = connection_pair.second; + const auto& road = _data.GetRoads().at(connection.connecting_road); + for (auto&& lane_section : road.GetLaneSections()) { + for (auto&& lane_pair : lane_section.GetLanes()) { + const auto& lane = lane_pair.second; + if (lane.GetType() != road::Lane::LaneType::Sidewalk) { + lane_meshes.push_back(mesh_factory.GenerateTesselated(lane)); + } + else { + sidewalk_lane_meshes.push_back(mesh_factory.GenerateSidewalk(lane)); + } + } + } + } + std::unique_ptr merged_mesh = std::make_unique(); + for (auto& lane : lane_meshes) { + *merged_mesh += *lane; + } + std::unique_ptr sidewalk_mesh = std::make_unique(); + for (auto& lane : sidewalk_lane_meshes) { + *sidewalk_mesh += *lane; + } + + (*junction_out_mesh_list)[road::Lane::LaneType::Driving].push_back(std::move(merged_mesh)); + (*junction_out_mesh_list)[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh)); + } + } + } // namespace road } // namespace carla diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index 681968c34..ab9a2afde 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -16,6 +16,7 @@ #include "carla/road/MapData.h" #include "carla/road/RoadTypes.h" #include "carla/road/MeshFactory.h" +#include "carla/geom/Vector3D.h" #include "carla/rpc/OpendriveGenerationParameters.h" #include @@ -161,12 +162,16 @@ namespace road { const rpc::OpendriveGenerationParameters& params) const; std::map>> - GenerateOrderedChunkedMesh( const rpc::OpendriveGenerationParameters& params) const; + GenerateOrderedChunkedMeshInLocations( const rpc::OpendriveGenerationParameters& params, + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos) const; /// Buids a mesh of all crosswalks based on the OpenDRIVE geom::Mesh GetAllCrosswalkMesh() const; std::vector> GetTreesTransform( + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos, float distancebetweentrees, float distancefromdrivinglineborder, float s_offset = 0) const; @@ -176,6 +181,8 @@ namespace road { /// Buids a list of meshes related with LineMarkings std::vector> GenerateLineMarkings( const rpc::OpendriveGenerationParameters& params, + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos, std::vector& outinfo ) const; const std::unordered_map>& GetSignals() const { @@ -223,15 +230,32 @@ public: std::map>> GenerateRoadsMultithreaded( const carla::geom::MeshFactory& mesh_factory, - const size_t index, const size_t number_of_roads_per_thread) const; + const std::vector& RoadsID, + const size_t index, + const size_t number_of_roads_per_thread) const; void GenerateJunctions(const carla::geom::MeshFactory& mesh_factory, const rpc::OpendriveGenerationParameters& params, + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos, std::map>>* juntion_out_mesh_list) const; - std::unique_ptr SDFToMesh(const road::Junction& jinput, const std::vector& sdfinput, int grid_cells_per_dim) const; + void GenerateSingleJunction(const carla::geom::MeshFactory& mesh_factory, + const JuncId Id, + std::map>>* + junction_out_mesh_list) const; + // Return list of junction ID which are between those positions + std::vector FilterJunctionsByPosition( + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos) const; + // Return list of roads ID which are between those positions + std::vector FilterRoadsByPosition( + const geom::Vector3D& minpos, + const geom::Vector3D& maxpos ) const; + + std::unique_ptr SDFToMesh(const road::Junction& jinput, const std::vector& sdfinput, int grid_cells_per_dim) const; }; } // namespace road diff --git a/LibCarla/source/carla/road/MapBuilder.cpp b/LibCarla/source/carla/road/MapBuilder.cpp index 90319bd12..1fcb1f7a5 100644 --- a/LibCarla/source/carla/road/MapBuilder.cpp +++ b/LibCarla/source/carla/road/MapBuilder.cpp @@ -825,11 +825,16 @@ namespace road { for(const auto& junction : _map_data._junctions) { for(const auto& controller : junction.second._controllers) { auto it = _map_data._controllers.find(controller); - DEBUG_ASSERT(it != _map_data._controllers.end()); - it->second->_junctions.insert(junction.first); - for(const auto & signal : it->second->_signals) { - auto signal_it = _map_data._signals.find(signal); - signal_it->second->_controllers.insert(controller); + if(it != _map_data._controllers.end()){ + if( it->second != nullptr ){ + it->second->_junctions.insert(junction.first); + for(const auto & signal : it->second->_signals) { + auto signal_it = _map_data._signals.find(signal); + if( signal_it->second != nullptr ){ + signal_it->second->_controllers.insert(controller); + } + } + } } } } @@ -1052,7 +1057,7 @@ void MapBuilder::CreateController( continue; } if(closest_waypoint_to_signal) { - auto road_transform = map.ComputeTransform(closest_waypoint_to_signal.get()); + auto road_transform = map.ComputeTransform(closest_waypoint_to_signal.get()); auto distance_to_road = (road_transform.location -signal_position).Length(); double lane_width = map.GetLaneWidth(closest_waypoint_to_signal.get()); int displacement_direction = 1; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/BlueprintLibary/MapGenFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/BlueprintLibary/MapGenFunctionLibrary.cpp new file mode 100644 index 000000000..b7b3ae61a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/BlueprintLibary/MapGenFunctionLibrary.cpp @@ -0,0 +1,228 @@ +// Copyright (c) 2023 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "MapGenFunctionLibrary.h" + +// Engine headers +#include "AssetRegistry/AssetRegistryModule.h" +#include "Materials/MaterialInstance.h" +#include "StaticMeshAttributes.h" +#include "RenderingThread.h" +// Carla C++ headers + +// Carla plugin headers + + +DEFINE_LOG_CATEGORY(LogCarlaMapGenFunctionLibrary); +static const float OSMToCentimetersScaleFactor = 100.0f; + +FMeshDescription UMapGenFunctionLibrary::BuildMeshDescriptionFromData( + const FProceduralCustomMesh& Data, + const TArray& ParamTangents, + UMaterialInstance* MaterialInstance ) +{ + + int32 VertexCount = Data.Vertices.Num(); + int32 VertexInstanceCount = Data.Triangles.Num(); + int32 PolygonCount = Data.Vertices.Num()/3; + + FMeshDescription MeshDescription; + FStaticMeshAttributes AttributeGetter(MeshDescription); + AttributeGetter.Register(); + + TPolygonGroupAttributesRef PolygonGroupNames = AttributeGetter.GetPolygonGroupMaterialSlotNames(); + TVertexAttributesRef VertexPositions = AttributeGetter.GetVertexPositions(); + TVertexInstanceAttributesRef Tangents = AttributeGetter.GetVertexInstanceTangents(); + TVertexInstanceAttributesRef BinormalSigns = AttributeGetter.GetVertexInstanceBinormalSigns(); + TVertexInstanceAttributesRef Normals = AttributeGetter.GetVertexInstanceNormals(); + TVertexInstanceAttributesRef Colors = AttributeGetter.GetVertexInstanceColors(); + TVertexInstanceAttributesRef UVs = AttributeGetter.GetVertexInstanceUVs(); + + // Calculate the totals for each ProcMesh element type + FPolygonGroupID PolygonGroupForSection; + MeshDescription.ReserveNewVertices(VertexCount); + MeshDescription.ReserveNewVertexInstances(VertexInstanceCount); + MeshDescription.ReserveNewPolygons(PolygonCount); + MeshDescription.ReserveNewEdges(PolygonCount * 2); + UVs.SetNumIndices(4); + + // Create Materials + TMap UniqueMaterials; + const int32 NumSections = 1; + UniqueMaterials.Reserve(1); + FPolygonGroupID NewPolygonGroup = MeshDescription.CreatePolygonGroup(); + + if( MaterialInstance != nullptr ){ + UMaterialInterface *Material = MaterialInstance; + UniqueMaterials.Add(Material, NewPolygonGroup); + PolygonGroupNames[NewPolygonGroup] = Material->GetFName(); + }else{ + UE_LOG(LogCarla, Error, TEXT("MaterialInstance is nullptr")); + } + PolygonGroupForSection = NewPolygonGroup; + + + + // Create the vertex + int32 NumVertex = Data.Vertices.Num(); + TMap VertexIndexToVertexID; + VertexIndexToVertexID.Reserve(NumVertex); + for (int32 VertexIndex = 0; VertexIndex < NumVertex; ++VertexIndex) + { + const FVector &Vert = Data.Vertices[VertexIndex]; + const FVertexID VertexID = MeshDescription.CreateVertex(); + VertexPositions[VertexID] = Vert; + VertexIndexToVertexID.Add(VertexIndex, VertexID); + } + + // Create the VertexInstance + int32 NumIndices = Data.Triangles.Num(); + int32 NumTri = NumIndices / 3; + TMap IndiceIndexToVertexInstanceID; + IndiceIndexToVertexInstanceID.Reserve(NumVertex); + for (int32 IndiceIndex = 0; IndiceIndex < NumIndices; IndiceIndex++) + { + const int32 VertexIndex = Data.Triangles[IndiceIndex]; + const FVertexID VertexID = VertexIndexToVertexID[VertexIndex]; + const FVertexInstanceID VertexInstanceID = + MeshDescription.CreateVertexInstance(VertexID); + IndiceIndexToVertexInstanceID.Add(IndiceIndex, VertexInstanceID); + Normals[VertexInstanceID] = Data.Normals[VertexIndex]; + + if(ParamTangents.Num() == Data.Vertices.Num()) + { + Tangents[VertexInstanceID] = ParamTangents[VertexIndex].TangentX; + BinormalSigns[VertexInstanceID] = + ParamTangents[VertexIndex].bFlipTangentY ? -1.f : 1.f; + }else{ + + } + Colors[VertexInstanceID] = FLinearColor(0,0,0); + if(Data.UV0.Num() == Data.Vertices.Num()) + { + UVs.Set(VertexInstanceID, 0, Data.UV0[VertexIndex]); + }else{ + UVs.Set(VertexInstanceID, 0, FVector2D(0,0)); + } + UVs.Set(VertexInstanceID, 1, FVector2D(0,0)); + UVs.Set(VertexInstanceID, 2, FVector2D(0,0)); + UVs.Set(VertexInstanceID, 3, FVector2D(0,0)); + } + + for (int32 TriIdx = 0; TriIdx < NumTri; TriIdx++) + { + FVertexID VertexIndexes[3]; + TArray VertexInstanceIDs; + VertexInstanceIDs.SetNum(3); + + for (int32 CornerIndex = 0; CornerIndex < 3; ++CornerIndex) + { + const int32 IndiceIndex = (TriIdx * 3) + CornerIndex; + const int32 VertexIndex = Data.Triangles[IndiceIndex]; + VertexIndexes[CornerIndex] = VertexIndexToVertexID[VertexIndex]; + VertexInstanceIDs[CornerIndex] = + IndiceIndexToVertexInstanceID[IndiceIndex]; + } + + // Insert a polygon into the mesh + MeshDescription.CreatePolygon(NewPolygonGroup, VertexInstanceIDs); + + } + + return MeshDescription; +} + +UStaticMesh* UMapGenFunctionLibrary::CreateMesh( + const FProceduralCustomMesh& Data, + const TArray& ParamTangents, + UMaterialInstance* MaterialInstance, + FString MapName, + FString FolderName, + FName MeshName) +{ + IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile(); + + UStaticMesh::FBuildMeshDescriptionsParams Params; + Params.bBuildSimpleCollision = true; + + FString PackageName = "/Game/CustomMaps/" + MapName + "/Static/" + FolderName + "/" + MeshName.ToString(); + + if (!PlatformFile.DirectoryExists(*PackageName)) + { + //PlatformFile.CreateDirectory(*PackageName); + } + + + FMeshDescription Description = BuildMeshDescriptionFromData(Data,ParamTangents, MaterialInstance); + + if (Description.Polygons().Num() > 0) + { + UPackage* Package = CreatePackage(*PackageName); + check(Package); + UStaticMesh* Mesh = NewObject( Package, MeshName, RF_Public | RF_Standalone); + + Mesh->InitResources(); + + Mesh->LightingGuid = FGuid::NewGuid(); + Mesh->StaticMaterials.Add(FStaticMaterial(MaterialInstance)); + Mesh->BuildFromMeshDescriptions({ &Description }, Params); + Mesh->CreateBodySetup(); + Mesh->BodySetup->CollisionTraceFlag = ECollisionTraceFlag::CTF_UseComplexAsSimple; + Mesh->BodySetup->CreatePhysicsMeshes(); + // Build mesh from source + Mesh->NeverStream = false; + TArray CreatedAssets; + CreatedAssets.Add(Mesh); + + // Notify asset registry of new asset + FAssetRegistryModule::AssetCreated(Mesh); + //UPackage::SavePackage(Package, Mesh, EObjectFlags::RF_Public | EObjectFlags::RF_Standalone, *(MeshName.ToString()), GError, nullptr, true, true, SAVE_NoError); + Package->MarkPackageDirty(); + return Mesh; + } + return nullptr; +} + +FVector2D UMapGenFunctionLibrary::GetTransversemercProjection(float lat, float lon, float lat0, float lon0) +{ + // earth radius in m + const float R = 6373000.0f; + float latt = FMath::DegreesToRadians(lat); + float lonn = FMath::DegreesToRadians(lon - lon0); + float latt0 = FMath::DegreesToRadians(lat0); + float eps = atan(tan(latt)/cos(lonn)); + float nab = asinh(sin(lonn)/sqrt(tan(latt)*tan(latt)+cos(lonn)*cos(lonn))); + float x = R*nab; + float y = R*eps; + float eps0 = atan(tan(latt0)/cos(0)); + float nab0 = asinh(sin(0)/sqrt(tan(latt0)*tan(latt0)+cos(0)*cos(0))); + float x0 = R*nab0; + float y0 = R*eps0; + FVector2D Result = FVector2D(x, -(y - y0)) * OSMToCentimetersScaleFactor; + + return Result; +} + +void UMapGenFunctionLibrary::SetThreadToSleep(float seconds){ + //FGenericPlatformProcess::Sleep(seconds); +} + +void UMapGenFunctionLibrary::FlushRenderingCommandsInBlueprint(){ + FlushRenderingCommands(true); + FlushPendingDeleteRHIResources_GameThread(); +} + +void UMapGenFunctionLibrary::CleanupGEngine(){ + GEngine->PerformGarbageCollectionAndCleanupActors(); +#if WITH_EDITOR + FText TransResetText(FText::FromString("Clean up after Move actors to sublevels")); + if ( GEditor->Trans ) + { + GEditor->Trans->Reset(TransResetText); + GEditor->Cleanse(true, true, TransResetText); + } +#endif +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/BlueprintLibary/MapGenFunctionLibrary.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/BlueprintLibary/MapGenFunctionLibrary.h new file mode 100644 index 000000000..345b5bd4f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/BlueprintLibary/MapGenFunctionLibrary.h @@ -0,0 +1,54 @@ +// Copyright (c) 2023 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +// Engine headers +#include "CoreMinimal.h" +#include "Kismet/BlueprintFunctionLibrary.h" +#include "MeshDescription.h" +#include "ProceduralMeshComponent.h" +// Carla C++ headers + +// Carla plugin headers +#include "Carla/Util/ProceduralCustomMesh.h" + +#include "MapGenFunctionLibrary.generated.h" + + +DECLARE_LOG_CATEGORY_EXTERN(LogCarlaMapGenFunctionLibrary, Log, All); + +UCLASS(BlueprintType) +class CARLA_API UMapGenFunctionLibrary : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() +public: + UFUNCTION(BlueprintCallable) + static UStaticMesh* CreateMesh( + const FProceduralCustomMesh& Data, + const TArray& ParamTangents, + UMaterialInstance* MaterialInstance, + FString MapName, + FString FolderName, + FName MeshName); + + static FMeshDescription BuildMeshDescriptionFromData( + const FProceduralCustomMesh& Data, + const TArray& ParamTangents, + UMaterialInstance* MaterialInstance ); + + UFUNCTION(BlueprintCallable) + static FVector2D GetTransversemercProjection(float lat, float lon, float lat0, float lon0); + + UFUNCTION(BlueprintCallable) + static void SetThreadToSleep(float seconds); + + UFUNCTION(BlueprintCallable) + static void FlushRenderingCommandsInBlueprint(); + + UFUNCTION(BlueprintCallable) + static void CleanupGEngine(); +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ProceduralCustomMesh.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ProceduralCustomMesh.h index 4f113801c..d67c99e39 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ProceduralCustomMesh.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ProceduralCustomMesh.h @@ -9,24 +9,24 @@ #include "ProceduralCustomMesh.generated.h" /// A definition of a Carla Mesh. -USTRUCT() +USTRUCT(Blueprintable) struct CARLA_API FProceduralCustomMesh { GENERATED_BODY() - UPROPERTY() + UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="VertexData") TArray Vertices; - UPROPERTY() + UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="VertexData") TArray Triangles; - UPROPERTY() + UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="VertexData") TArray Normals; - UPROPERTY() + UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="VertexData") TArray UV0; - UPROPERTY() + UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="VertexData") TArray VertexColor; // This is commented due to an strange bug including ProceduralMeshComponent.h diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin b/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin index 245d378f3..fc641e357 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin @@ -18,7 +18,7 @@ { "Name": "CarlaTools", "Type": "Editor", - "LoadingPhase": "PostEngineInit" + "LoadingPhase": "Default" } ], "Plugins": [ @@ -37,6 +37,10 @@ { "Name": "ProceduralMeshComponent", "Enabled": true + }, + { + "Name": "StreetMap", + "Enabled": true } ] } \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_OpenDriveToMap.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_OpenDriveToMap.uasset index b4fdfca20..b73e2caa5 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_OpenDriveToMap.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_OpenDriveToMap.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_BuildingGenerator.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_BuildingGenerator.uasset new file mode 100644 index 000000000..165501747 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_BuildingGenerator.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_RoofPropsGenerator.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_RoofPropsGenerator.uasset new file mode 100644 index 000000000..8e7b889bb Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_RoofPropsGenerator.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_Veg_Caller.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_Veg_Caller.uasset new file mode 100644 index 000000000..c0a5e09e3 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_Veg_Caller.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_Veg_Scatter.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_Veg_Scatter.uasset new file mode 100644 index 000000000..a70c84284 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BP_Veg_Scatter.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BuildingStyleHolder.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BuildingStyleHolder.uasset index 8a83e1967..51336edb1 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BuildingStyleHolder.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BuildingStyleHolder.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/DT_BuildingStyles.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/DT_BuildingStyles.uasset index fe643a78a..eb5c3d0e1 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/DT_BuildingStyles.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/DT_BuildingStyles.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/DT_TreesGeneration.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/DT_TreesGeneration.uasset index b4876f094..af26667ed 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/DT_TreesGeneration.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/DT_TreesGeneration.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/LevelCreator.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/LevelCreator.uasset index dd6086ea2..830e40ef3 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/LevelCreator.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/LevelCreator.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_HoudiniBuildingImporter.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_HoudiniBuildingImporter.uasset index ebdaf3219..fbf48f5db 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_HoudiniBuildingImporter.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_HoudiniBuildingImporter.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_OnRoadMainWidget.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_OnRoadMainWidget.uasset index 1db1b85fa..d1c30094b 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_OnRoadMainWidget.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/UW_OnRoadMainWidget.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/Python/generate_tile.py b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/Python/generate_tile.py new file mode 100644 index 000000000..e68a03b11 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/Python/generate_tile.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python +import unreal +import argparse +import json +import os +import subprocess + + +"""Generic function for running a commandlet with its arguments.""" +ue4_path = os.environ["UE4_ROOT"] +uproject_path = unreal.Paths.project_dir() + ("CarlaUE4.uproject") +run = "-run=%s" % ("GenerateTileCommandlet") + +print("Before any Commandlet:") + +argparser = argparse.ArgumentParser() + +argparser.add_argument( + '-s', '--paramstring', + metavar='S', + default='', + type=str, + help='String to put as arguments') +args = argparser.parse_args() + +arguments = args.paramstring + +if os.name == "nt": + sys_name = "Win64" + editor_path = "%s/Engine/Binaries/%s/UE4Editor" % (ue4_path, sys_name) + command = [editor_path, uproject_path, run] + command.extend(arguments) + print("Commandlet:", command) + print("Arguments:", arguments) + subprocess.check_call(command, shell=True) +elif os.name == "posix": + sys_name = "Linux" + editor_path = "%s/Engine/Binaries/%s/UE4Editor" % (ue4_path, sys_name) + full_command = editor_path + " " + uproject_path + " " + run + " " + arguments + print("Commandlet:", full_command) + print("Arguments:", arguments) + subprocess.call([full_command], shell=True) + diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs index 45006bde3..5f1dc68d2 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs @@ -44,15 +44,15 @@ public class CarlaTools : ModuleRules // ... add public include paths required here ... } ); - - + + PrivateIncludePaths.AddRange( new string[] { // ... add other private include paths required here ... } ); - - + + PublicDependencyModuleNames.AddRange( new string[] { @@ -64,8 +64,8 @@ public class CarlaTools : ModuleRules // ... add other public dependencies that you statically link with here ... } ); - - + + PrivateDependencyModuleNames.AddRange( new string[] { @@ -82,15 +82,19 @@ public class CarlaTools : ModuleRules "FoliageEdit", "MeshMergeUtilities", "Carla", + "StaticMeshDescription", "PhysXVehicles", "Json", "JsonUtilities", "Networking", "Sockets", + "HTTP", "RHI", "RenderCore", - "MeshMergeUtilities" - // ... add private dependencies that you statically link with here ... + "MeshMergeUtilities", + "StreetMapImporting", + "StreetMapRuntime" + // ... add private dependencies that you statically link with here ... } ); if(UsingHoudini) @@ -145,7 +149,7 @@ public class CarlaTools : ModuleRules foreach (string file in files) { PublicAdditionalLibraries.Add(file); - } + } } diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/Commandlet/GenerateTileCommandlet.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/Commandlet/GenerateTileCommandlet.cpp new file mode 100644 index 000000000..db235e023 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/Commandlet/GenerateTileCommandlet.cpp @@ -0,0 +1,84 @@ +// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "GenerateTileCommandlet.h" + +#include +#include + +#if WITH_EDITOR +#include "FileHelpers.h" +#endif +#include "UObject/ConstructorHelpers.h" + +DEFINE_LOG_CATEGORY(LogCarlaToolsMapGenerateTileCommandlet); + + +UGenerateTileCommandlet::UGenerateTileCommandlet() +{ +#if WITH_EDITOR + ConstructorHelpers::FClassFinder OpenDrivelassFinder(TEXT("/CarlaTools/OnroadMapGenerator/BP_OpenDriveToMap")); + OpenDriveClass = OpenDrivelassFinder.Class; +#endif +} + +UGenerateTileCommandlet::UGenerateTileCommandlet(const FObjectInitializer& Initializer) + : Super(Initializer) +{ +#if WITH_EDITOR + ConstructorHelpers::FClassFinder OpenDrivelassFinder(TEXT("/CarlaTools/OnroadMapGenerator/BP_OpenDriveToMap")); + OpenDriveClass = OpenDrivelassFinder.Class; +#endif +} + +#if WITH_EDITORONLY_DATA + +int32 UGenerateTileCommandlet::Main(const FString &Params) +{ + UE_LOG(LogCarlaToolsMapGenerateTileCommandlet, Log, TEXT("UGenerateTileCommandlet::Main Arguments %s"), *Params); + TArray Tokens; + TArray Switches; + TMap ParamsMap; + + ParseCommandLine(*Params, Tokens, Switches, ParamsMap ); + + std::string logstr = std::string(TCHAR_TO_UTF8(*Params)); + std::ofstream file("CommandletParameters.txt", std::ios::app); + file << logstr << std::endl; + + for( auto fstr : Tokens ) + { + logstr = std::string(TCHAR_TO_UTF8(*fstr)); + file << " Tokens " << logstr << std::endl; + } + + for( auto fstr : Switches ) + { + logstr = std::string(TCHAR_TO_UTF8(*fstr)); + file << " SWITCHES " << logstr << std::endl; + } + + for( auto PairMap : ParamsMap ) + { + std::string key = std::string(TCHAR_TO_UTF8(*PairMap.Key)); + std::string value = std::string(TCHAR_TO_UTF8(*PairMap.Value)); + + file << "Key: " << key << " Value: " << value << std::endl; + } + + + + OpenDriveMap = NewObject(this, OpenDriveClass); + OpenDriveMap->FilePath = ParamsMap["FilePath"]; + OpenDriveMap->BaseLevelName = ParamsMap["BaseLevelName"]; + OpenDriveMap->OriginGeoCoordinates = FVector2D(FCString::Atof(*ParamsMap["GeoCoordsX"]),FCString::Atof(*ParamsMap["GeoCoordsY"])); + OpenDriveMap->CurrentTilesInXY = FIntVector(FCString::Atof(*ParamsMap["CTileX"]),FCString::Atof(*ParamsMap["CTileY"]), 0); + // Parse Params + OpenDriveMap->GenerateTile(); + return 0; +} + +#endif diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/DigitalTwinsBaseWidget.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/DigitalTwinsBaseWidget.cpp new file mode 100644 index 000000000..d7706033f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/DigitalTwinsBaseWidget.cpp @@ -0,0 +1,32 @@ +// Copyright (c) 2023 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "DigitalTwinsBaseWidget.h" + +#include "OpenDriveToMap.h" + +static UOpenDriveToMap* OpenDriveToMapObject = nullptr; + +UOpenDriveToMap* UDigitalTwinsBaseWidget::InitializeOpenDriveToMap(TSubclassOf BaseClass){ + if( OpenDriveToMapObject == nullptr && BaseClass != nullptr ){ + UE_LOG(LogCarlaTools, Error, TEXT("Creating New Object") ); + OpenDriveToMapObject = NewObject(this, BaseClass); + } + return OpenDriveToMapObject; +} + +UOpenDriveToMap* UDigitalTwinsBaseWidget::GetOpenDriveToMap(){ + return OpenDriveToMapObject; +} + +void UDigitalTwinsBaseWidget::SetOpenDriveToMap(UOpenDriveToMap* ToSet){ + OpenDriveToMapObject = ToSet; +} + +void UDigitalTwinsBaseWidget::DestroyOpenDriveToMap(){ + OpenDriveToMapObject->ConditionalBeginDestroy(); + OpenDriveToMapObject = nullptr; +} diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/HoudiniImporterWidget.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/HoudiniImporterWidget.cpp index 155bf6305..c75887058 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/HoudiniImporterWidget.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/HoudiniImporterWidget.cpp @@ -10,6 +10,7 @@ #include "Kismet/GameplayStatics.h" #include "Kismet/KismetSystemLibrary.h" #include "FileHelpers.h" +#include "EditorLevelLibrary.h" #include "Components/PrimitiveComponent.h" void UHoudiniImporterWidget::CreateSubLevels(ALargeMapManager* LargeMapManager) @@ -46,8 +47,8 @@ void UHoudiniImporterWidget::MoveActorsToSubLevelWithLargeMap(TArray Ac FCarlaMapTile* Tile = LargeMapManager->GetCarlaMapTile(ActorLocation); if(!Tile) { - UE_LOG(LogCarlaTools, Error, TEXT("Error: actor in location %s is outside the map"), - *ActorLocation.ToString()); + UE_LOG(LogCarlaTools, Error, TEXT("Error: actor %s in location %s is outside the map"), + *Actor->GetName(),*ActorLocation.ToString()); continue; } @@ -70,14 +71,13 @@ void UHoudiniImporterWidget::MoveActorsToSubLevelWithLargeMap(TArray Ac { continue; } - UWorld* World = ActorList[0]->GetWorld(); + + UWorld* World = UEditorLevelLibrary::GetEditorWorld(); ULevelStreamingDynamic* StreamingLevel = Tile->StreamingLevel; - UE_LOG(LogCarlaTools, Log, TEXT("Got Tile %s in location %s"), - *StreamingLevel->PackageNameToLoad.ToString(), *Tile->Location.ToString()); StreamingLevel->bShouldBlockOnLoad = true; StreamingLevel->SetShouldBeVisible(true); StreamingLevel->SetShouldBeLoaded(true); - ULevelStreaming* Level = + ULevelStreaming* Level = UEditorLevelUtils::AddLevelToWorld( World, *Tile->Name, ULevelStreamingDynamic::StaticClass(), FTransform()); int MovedActors = UEditorLevelUtils::MoveActorsToLevel(ActorList, Level, false, false); @@ -86,15 +86,51 @@ void UHoudiniImporterWidget::MoveActorsToSubLevelWithLargeMap(TArray Ac FEditorFileUtils::SaveDirtyPackages(false, true, true, false, false, false, nullptr); UEditorLevelUtils::RemoveLevelFromWorld(Level->GetLoadedLevel()); } + + GEngine->PerformGarbageCollectionAndCleanupActors(); + FText TransResetText(FText::FromString("Clean up after Move actors to sublevels")); + if ( GEditor->Trans ) + { + GEditor->Trans->Reset(TransResetText); + GEditor->Cleanse(true, true, TransResetText); + } +} + +void UHoudiniImporterWidget::ForceStreamingLevelsToUnload( ALargeMapManager* LargeMapManager ) +{ + UWorld* World = UEditorLevelLibrary::GetGameWorld(); + + FIntVector NumTilesInXY = LargeMapManager->GetNumTilesInXY(); + + for(int x = 0; x < NumTilesInXY.X; ++x) + { + for(int y = 0; y < NumTilesInXY.Y; ++y) + { + FIntVector CurrentTileVector(x, y, 0); + FCarlaMapTile CarlaTile = LargeMapManager->GetCarlaMapTile(CurrentTileVector); + ULevelStreamingDynamic* StreamingLevel = CarlaTile.StreamingLevel; + ULevelStreaming* Level = + UEditorLevelUtils::AddLevelToWorld( + World, *CarlaTile.Name, ULevelStreamingDynamic::StaticClass(), FTransform()); + FEditorFileUtils::SaveDirtyPackages(false, true, true, false, false, false, nullptr); + UEditorLevelUtils::RemoveLevelFromWorld(Level->GetLoadedLevel()); + } + } + } void UHoudiniImporterWidget::MoveActorsToSubLevel(TArray Actors, ULevelStreaming* Level) { int MovedActors = UEditorLevelUtils::MoveActorsToLevel(Actors, Level, false, false); - // StreamingLevel->SetShouldBeLoaded(false); + UE_LOG(LogCarlaTools, Log, TEXT("Moved %d actors"), MovedActors); FEditorFileUtils::SaveDirtyPackages(false, true, true, false, false, false, nullptr); UEditorLevelUtils::RemoveLevelFromWorld(Level->GetLoadedLevel()); + FText TransResetText(FText::FromString("Clean up after Move actors to sublevels")); + if ( GEditor->Trans ) + { + GEditor->Trans->Reset(TransResetText); + } } void UHoudiniImporterWidget::UpdateGenericActorCoordinates( @@ -102,8 +138,8 @@ void UHoudiniImporterWidget::UpdateGenericActorCoordinates( { FVector LocalLocation = Actor->GetActorLocation() - TileOrigin; Actor->SetActorLocation(LocalLocation); - UE_LOG(LogCarlaTools, Log, TEXT("New location %s"), - *LocalLocation.ToString()); + UE_LOG(LogCarlaTools, Log, TEXT("%s New location %s"), + *Actor->GetName(), *LocalLocation.ToString()); } void UHoudiniImporterWidget::UpdateInstancedMeshCoordinates( @@ -133,13 +169,6 @@ void UHoudiniImporterWidget::UseCOMasActorLocation(TArray Actors) FBodyInstance* BodyInstance = Primitive->GetBodyInstance(); FVector CenterOfMass = BodyInstance->COMNudge; Actor->SetActorLocation(CenterOfMass); - UE_LOG(LogCarlaTools, Log, TEXT("Updating actor %s to %s"), - *Actor->GetName(), *CenterOfMass.ToString()); - } - else - { - UE_LOG(LogCarlaTools, Log, TEXT("Not updating actor %s"), - *Actor->GetName()); } } } @@ -147,7 +176,7 @@ void UHoudiniImporterWidget::UseCOMasActorLocation(TArray Actors) bool UHoudiniImporterWidget::GetNumberOfClusters( TArray ActorList, int& OutNumClusters) { - + for (AActor* Actor : ActorList) { FString ObjectName = UKismetSystemLibrary::GetObjectName(Actor); diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp index eb1d8304e..c757b8756 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp @@ -61,24 +61,29 @@ void UMapPreviewUserWidget::ConnectToSocket(FString DatabasePath, FString Styles // Send a message FString Message = "-C " + DatabasePath + " " + StylesheetPath + " " + FString::FromInt(Size); - SendStr(Message); + if( !SendStr(Message) ){ + return; + } UE_LOG(LogTemp, Log, TEXT("Configuration Completed")); } void UMapPreviewUserWidget::RenderMap(FString Latitude, FString Longitude, FString Zoom) { FString Message = "-R " + Latitude + " " + Longitude + " " + Zoom; - SendStr(Message); + if( !SendStr(Message) ){ + UE_LOG(LogTemp, Log, TEXT("Send Str failed")); + return; + } TArray ReceivedData; uint32 ReceivedDataSize = 0; - + { SocketPtr->wait(boost::asio::ip::tcp::socket::wait_read); while (SocketPtr->available()) { AsioStreamBuf Buffer; - std::size_t BytesReceived = + std::size_t BytesReceived = Asio::read(*SocketPtr, Buffer, Asio::transfer_at_least(2)); TArray ThisReceivedData; const char* DataPtr = Asio::buffer_cast(Buffer.data()); @@ -105,7 +110,7 @@ void UMapPreviewUserWidget::RenderMap(FString Latitude, FString Longitude, FStri Region.DestY = 0; Region.Width = Texture->GetSizeX(); Region.Height = Texture->GetSizeY(); - + FTexture2DResource* Resource = (FTexture2DResource*)Texture->Resource; RHIUpdateTexture2D(Resource->GetTexture2DRHI(), 0, Region, Region.Width * sizeof(uint8_t) * 4, &NewData[0]); } @@ -116,13 +121,16 @@ void UMapPreviewUserWidget::RenderMap(FString Latitude, FString Longitude, FStri FString UMapPreviewUserWidget::RecvCornersLatLonCoords() { - SendStr("-L"); + if( !SendStr("-L") ){ + UE_LOG(LogTemp, Error, TEXT("Error sending message: num bytes mismatch")); + return FString(); + } AsioStreamBuf Buffer; - std::size_t BytesReceived = + std::size_t BytesReceived = Asio::read(*SocketPtr, Buffer, Asio::transfer_at_least(2)); std::string BytesStr = Asio::buffer_cast(Buffer.data()); - + FString CoordStr = FString(BytesStr.size(), UTF8_TO_TCHAR(BytesStr.c_str())); UE_LOG(LogTemp, Log, TEXT("Received Coords %s"), *CoordStr); return CoordStr; @@ -141,7 +149,10 @@ void UMapPreviewUserWidget::OpenServer() void UMapPreviewUserWidget::CloseServer() { - SendStr("-X"); + if( !SendStr("-X") ){ + UE_LOG(LogTemp, Error, TEXT("Error sending message")); + return; + } } bool UMapPreviewUserWidget::SendStr(FString Msg) @@ -166,24 +177,29 @@ bool UMapPreviewUserWidget::SendStr(FString Msg) if (BytesSent != MessageStr.size()) { UE_LOG(LogTemp, Error, TEXT("Error sending message: num bytes mismatch")); - return true; + return false; } else { UE_LOG(LogTemp, Log, TEXT("Sent %d bytes"), BytesSent); - return false; + return true; } } void UMapPreviewUserWidget::UpdateLatLonCoordProperties() { FString CoordStr = RecvCornersLatLonCoords(); + if(CoordStr.Len() == 0) + { + UE_LOG(LogTemp, Error, TEXT("Error during update of lat lon coord properties. Check osm server connection or use OSMURL to generate map") ); + return; + } UE_LOG(LogTemp, Log, TEXT("Received laton [%s] with size %d"), *CoordStr, CoordStr.Len()); TArray CoordsArray; CoordStr.ParseIntoArray(CoordsArray, TEXT("&"), true); - check(CoordsArray.Num() >= 4); + ensure(CoordsArray.Num() == 4); TopRightLat = FCString::Atof(*CoordsArray[0]); TopRightLon = FCString::Atof(*CoordsArray[1]); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/Online/CustomFileDownloader.cpp similarity index 65% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.cpp rename to Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/Online/CustomFileDownloader.cpp index 4ad546047..79f306c6c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/Online/CustomFileDownloader.cpp @@ -3,6 +3,7 @@ #undef CreateDirectory #include "Online/CustomFileDownloader.h" +#include "OpenDriveToMap.h" #include "HttpModule.h" #include "Http.h" #include "Misc/FileHelper.h" @@ -20,16 +21,16 @@ void UCustomFileDownloader::ConvertOSMInOpenDrive(FString FilePath, float Lat_0, // We use the LoadFileToString to load the file into if (FFileHelper::LoadFileToString(FileContent, *FilePath, FFileHelper::EHashOptions::None)) { - UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Text From File: %s"), *FilePath); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FileManipulation: Text From File: %s"), *FilePath); } else { - UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Did not load text from file")); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FileManipulation: Did not load text from file")); } } else { - UE_LOG(LogCarla, Warning, TEXT("File: %s does not exist"), *FilePath); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("File: %s does not exist"), *FilePath); return; } std::string OsmFile = std::string(TCHAR_TO_UTF8(*FileContent)); @@ -45,17 +46,18 @@ void UCustomFileDownloader::ConvertOSMInOpenDrive(FString FilePath, float Lat_0, // We use the LoadFileToString to load the file into if (FFileHelper::SaveStringToFile(FString(OpenDriveFile.c_str()), *FilePath)) { - UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Sucsesfuly Written: \"%s\" to the text file"), *FilePath); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FileManipulation: Sucsesfuly Written: \"%s\" to the text file"), *FilePath); } else { - UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Failed to write FString to file.")); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FileManipulation: Failed to write FString to file.")); } } void UCustomFileDownloader::StartDownload() { - UE_LOG(LogCarla, Log, TEXT("FHttpDownloader CREATED")); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("FHttpDownloader CREATED")); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Map Name Is %s"), *ResultFileName ); FHttpDownloader *Download = new FHttpDownloader("GET", Url, ResultFileName, DownloadDelegate); Download->Run(); } @@ -65,10 +67,16 @@ FHttpDownloader::FHttpDownloader(const FString &InVerb, const FString &InUrl, co { } +FHttpDownloader::FHttpDownloader() +{ + +} + void FHttpDownloader::Run(void) { - UE_LOG(LogCarla, Log, TEXT("Starting download [%s] Url=[%s]"), *Verb, *Url); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Starting download [%s] Url=[%s]"), *Verb, *Url); TSharedPtr Request = FHttpModule::Get().CreateRequest(); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Map Name Is %s"), *Filename ); Request->OnProcessRequestComplete().BindRaw(this, &FHttpDownloader::RequestComplete); Request->SetURL(Url); Request->SetVerb(Verb); @@ -77,31 +85,29 @@ void FHttpDownloader::Run(void) void FHttpDownloader::RequestComplete(FHttpRequestPtr HttpRequest, FHttpResponsePtr HttpResponse, bool bSucceeded) { - if (!HttpResponse.IsValid()) + if (!HttpResponse.IsValid() ) { - UE_LOG(LogCarla, Log, TEXT("Download failed. NULL response")); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Download failed. NULL response")); } else { // If we do not get success responses codes we do not do anything if (HttpResponse->GetResponseCode() < 200 || 300 <= HttpResponse->GetResponseCode()) { - UE_LOG(LogCarla, Error, TEXT("Error during download [%s] Url=[%s] Response=[%d]"), + UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Error during download [%s] Url=[%s] Response=[%d]"), *HttpRequest->GetVerb(), *HttpRequest->GetURL(), HttpResponse->GetResponseCode()); - delete this; return; } - UE_LOG(LogCarla, Log, TEXT("Completed download [%s] Url=[%s] Response=[%d]"), + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Completed download [%s] Url=[%s] Response=[%d]"), *HttpRequest->GetVerb(), *HttpRequest->GetURL(), HttpResponse->GetResponseCode()); - HttpRequest->OnProcessRequestComplete().Unbind(); - - FString CurrentFile = FPaths::ProjectContentDir() + "CustomMaps/" + Filename + "/OpenDrive/"; + FString CurrentFile = FPaths::ProjectContentDir() + "CustomMaps/" + (Filename) + "/OpenDrive/"; + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FHttpDownloader::RequestComplete CurrentFile %s."), *CurrentFile); // We will use this FileManager to deal with the file. IPlatformFile &FileManager = FPlatformFileManager::Get().GetPlatformFile(); @@ -116,14 +122,14 @@ void FHttpDownloader::RequestComplete(FHttpRequestPtr HttpRequest, FHttpResponse // We use the LoadFileToString to load the file into if (FFileHelper::SaveStringToFile(StringToWrite, *CurrentFile, FFileHelper::EEncodingOptions::ForceUTF8WithoutBOM)) { - UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Sucsesfuly Written ")); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FileManipulation: Sucsesfuly Written ")); + DelegateToCall.ExecuteIfBound(); } else { - UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Failed to write FString to file.")); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FileManipulation: Failed to write FString to file.")); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("FileManipulation: CurrentFile %s."), *CurrentFile); } } - DelegateToCall.ExecuteIfBound(); - delete this; } diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp index 13e243913..8ee027793 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp @@ -1,7 +1,6 @@ // Copyright (c) 2023 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). This work is licensed under the terms of the MIT license. For a copy, see . #include "OpenDriveToMap.h" -#include "Components/Button.h" #include "DesktopPlatform/Public/IDesktopPlatform.h" #include "DesktopPlatform/Public/DesktopPlatformModule.h" #include "Misc/FileHelper.h" @@ -10,12 +9,13 @@ #include "Runtime/Core/Public/Async/ParallelFor.h" #include "Kismet/KismetRenderingLibrary.h" #include "KismetProceduralMeshLibrary.h" +#include "StaticMeshAttributes.h" -#include "Carla/Game/CarlaStatics.h" #include "Traffic/TrafficLightManager.h" #include "Online/CustomFileDownloader.h" #include "Util/ProceduralCustomMesh.h" - +#include "Carla/Game/CarlaStatics.h" +#include "Carla/BlueprintLibary/MapGenFunctionLibrary.h" #include "OpenDrive/OpenDriveGenerator.h" #include @@ -29,8 +29,10 @@ #include "Engine/Classes/Interfaces/Interface_CollisionDataProvider.h" #include "Engine/TriggerBox.h" +#include "Engine/AssetManager.h" #include "Factories/MaterialInstanceConstantFactoryNew.h" #include "PhysicsCore/Public/BodySetupEnums.h" +#include "PhysicsEngine/BodySetup.h" #include "RawMesh.h" #include "AssetRegistryModule.h" #include "Engine/StaticMesh.h" @@ -38,6 +40,7 @@ #include "MeshDescription.h" #include "EditorLevelLibrary.h" #include "ProceduralMeshConversion.h" +#include "EditorLevelLibrary.h" #include "ContentBrowserModule.h" #include "Materials/MaterialInstanceConstant.h" @@ -46,6 +49,20 @@ #include "DrawDebugHelpers.h" +#if WITH_EDITOR +UOpenDriveToMap::UOpenDriveToMap() +{ + AddToRoot(); + bRoadsFinished = false; + bHasStarted = false; + bMapLoaded = false; +} + +UOpenDriveToMap::~UOpenDriveToMap() +{ + +} + FString LaneTypeToFString(carla::road::Lane::LaneType LaneType) { switch (LaneType) @@ -125,6 +142,8 @@ void UOpenDriveToMap::ConvertOSMInOpenDrive() FilePath.RemoveFromEnd(".osm", ESearchCase::Type::IgnoreCase); FilePath += ".xodr"; + DownloadFinished(); + UEditorLoadingAndSavingUtils::SaveDirtyPackages(true, true); LoadMap(); } @@ -139,66 +158,45 @@ void UOpenDriveToMap::CreateMap() { FileDownloader = NewObject(); } + FileDownloader->ResultFileName = MapName; FileDownloader->Url = Url; FileDownloader->DownloadDelegate.BindUObject( this, &UOpenDriveToMap::ConvertOSMInOpenDrive ); FileDownloader->StartDownload(); - - RoadType.Empty(); - RoadMesh.Empty(); - MeshesToSpawn.Empty(); - ActorMeshList.Empty(); } -void UOpenDriveToMap::CreateTerrain( const int MeshGridSize, const float MeshGridSectionSize, const class UTexture2D* HeightmapTexture) +void UOpenDriveToMap::CreateTerrain( const int MeshGridSize, const float MeshGridSectionSize) { TArray FoundActors; - UGameplayStatics::GetAllActorsOfClass(GetWorld(), AProceduralMeshActor::StaticClass(), FoundActors); - FVector BoxOrigin; - FVector BoxExtent; - UGameplayStatics::GetActorArrayBounds(FoundActors, false, BoxOrigin, BoxExtent); - FVector MinBox = BoxOrigin - BoxExtent; + UGameplayStatics::GetAllActorsOfClass(UEditorLevelLibrary::GetEditorWorld(), AStaticMeshActor::StaticClass(), FoundActors); + FVector BoxExtent = FVector(TileSize, TileSize,0); + FVector MinBox = FVector(MinPosition.X, MaxPosition.Y,0); - int NumI = ( BoxExtent.X * 2.0f ) / MeshGridSize; - int NumJ = ( BoxExtent.Y * 2.0f ) / MeshGridSize; - ASceneCapture2D* SceneCapture = Cast(GetWorld()->SpawnActor(ASceneCapture2D::StaticClass())); - SceneCapture->SetActorRotation(FRotator(-90,90,0)); - SceneCapture->GetCaptureComponent2D()->ProjectionType = ECameraProjectionMode::Type::Orthographic; - SceneCapture->GetCaptureComponent2D()->OrthoWidth = MeshGridSize; - SceneCapture->GetCaptureComponent2D()->CaptureSource = ESceneCaptureSource::SCS_FinalColorLDR; - SceneCapture->GetCaptureComponent2D()->CompositeMode = ESceneCaptureCompositeMode::SCCM_Overwrite; - SceneCapture->GetCaptureComponent2D()->bCaptureEveryFrame = false; - SceneCapture->GetCaptureComponent2D()->bCaptureOnMovement = false; - //UTextureRenderTarget2D* RenderTarget = UKismetRenderingLibrary::CreateRenderTarget2D(GetWorld(), 256, 256, - // ETextureRenderTargetFormat::RTF_RGBA8, FLinearColor(0,0,0), false ); - //SceneCapture->GetCaptureComponent2D()->TextureTarget = RenderTarget; + int NumI = BoxExtent.X / MeshGridSize; + int NumJ = BoxExtent.Y / MeshGridSize; - /* Blueprint darfted code should be here */ - for( int i = 0; i < NumI; i++ ) + for( int i = 0; i <= NumI; i++ ) { - for( int j = 0; j < NumJ; j++ ) + for( int j = 0; j <= NumJ; j++ ) { // Offset that each procedural mesh is displaced to accomodate all the tiles FVector2D Offset( MinBox.X + i * MeshGridSize, MinBox.Y + j * MeshGridSize); - SceneCapture->SetActorLocation(FVector(Offset.X + MeshGridSize/2, Offset.Y + MeshGridSize/2, 500)); - //SceneCapture->GetCaptureComponent2D()->CaptureScene(); - CreateTerrainMesh(i * NumJ + j, Offset, MeshGridSize, MeshGridSectionSize, HeightmapTexture, nullptr ); + CreateTerrainMesh(i * NumJ + j, Offset, MeshGridSize, MeshGridSectionSize ); } } } -void UOpenDriveToMap::CreateTerrainMesh(const int MeshIndex, const FVector2D Offset, const int GridSize, const float GridSectionSize, const UTexture2D* HeightmapTexture, UTextureRenderTarget2D* RoadMask) +void UOpenDriveToMap::CreateTerrainMesh(const int MeshIndex, const FVector2D Offset, const int GridSize, const float GridSectionSize) { // const float GridSectionSize = 100.0f; // In cm const float HeightScale = 3.0f; - UWorld* World = GetWorld(); - + UWorld* World = UEditorLevelLibrary::GetEditorWorld(); // Creation of the procedural mesh - AProceduralMeshActor* MeshActor = World->SpawnActor(); + AStaticMeshActor* MeshActor = World->SpawnActor(); MeshActor->SetActorLocation(FVector(Offset.X, Offset.Y, 0)); - UProceduralMeshComponent* Mesh = MeshActor->MeshComponent; + UStaticMeshComponent* Mesh = MeshActor->GetStaticMeshComponent(); TArray Vertices; TArray Triangles; @@ -208,7 +206,9 @@ void UOpenDriveToMap::CreateTerrainMesh(const int MeshIndex, const FVector2D Off TArray Tangents; TArray UVs; + int VerticesInLine = (GridSize / GridSectionSize) + 1.0f; + static int StaticMeshIndex = 0; for( int i = 0; i < VerticesInLine; i++ ) { float X = (i * GridSectionSize); @@ -222,12 +222,11 @@ void UOpenDriveToMap::CreateTerrainMesh(const int MeshIndex, const FVector2D Off (Offset.Y + Y), 0)); Vertices.Add(FVector( X, Y, HeightValue)); + UVs.Add(FVector2D(i, j)); } } - Normals.Init(FVector(0.0f, 0.0f, 1.0f), Vertices.Num()); //// Triangles formation. 2 triangles per section. - for(int i = 0; i < VerticesInLine - 1; i++) { for(int j = 0; j < VerticesInLine - 1; j++) @@ -242,19 +241,154 @@ void UOpenDriveToMap::CreateTerrainMesh(const int MeshIndex, const FVector2D Off } } - if( DefaultLandscapeMaterial ) - { - Mesh->SetMaterial(0, DefaultLandscapeMaterial); + UKismetProceduralMeshLibrary::CalculateTangentsForMesh( + Vertices, + Triangles, + UVs, + Normals, + Tangents + ); + + FProceduralCustomMesh MeshData; + MeshData.Vertices = Vertices; + MeshData.Triangles = Triangles; + MeshData.Normals = Normals; + MeshData.UV0 = UVs; + UStaticMesh* MeshToSet = UMapGenFunctionLibrary::CreateMesh(MeshData, Tangents, DefaultLandscapeMaterial, MapName, "Terrain", FName(TEXT("SM_LandscapeMesh" + FString::FromInt(StaticMeshIndex) + GetStringForCurrentTile() ))); + Mesh->SetStaticMesh(MeshToSet); + MeshActor->SetActorLabel("SM_LandscapeActor" + FString::FromInt(StaticMeshIndex) + GetStringForCurrentTile() ); + MeshActor->Tags.Add(FName("LandscapeToMove")); + Mesh->CastShadow = false; + Landscapes.Add(MeshActor); + StaticMeshIndex++; +} + +AActor* UOpenDriveToMap::SpawnActorWithCheckNoCollisions(UClass* ActorClassToSpawn, FTransform Transform) +{ + UWorld* World = UEditorLevelLibrary::GetEditorWorld(); + FActorSpawnParameters SpawnParameters; + SpawnParameters.bNoFail = true; + SpawnParameters.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + + // Creation of the procedural mesh + return World->SpawnActor(ActorClassToSpawn, Transform, SpawnParameters); + +} +void UOpenDriveToMap::GenerateTileStandalone(){ + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("UOpenDriveToMap::GenerateTileStandalone Function called")); + + ExecuteTileCommandlet(); + + UEditorLoadingAndSavingUtils::SaveDirtyPackages(true, true); + UEditorLevelLibrary::SaveCurrentLevel(); + +} + +void UOpenDriveToMap::GenerateTile(){ + + if( FilePath.IsEmpty() ){ + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("UOpenDriveToMap::GenerateTile(): Failed to load %s"), *FilePath ); + return; } - Mesh->CreateMeshSection_LinearColor(0, Vertices, Triangles, Normals, - TArray(), // UV0 - TArray(), // VertexColor - TArray(), // Tangents - true); // Create collision); + FString FileContent; + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("UOpenDriveToMap::GenerateTile(): File to load %s"), *FilePath ); + FFileHelper::LoadFileToString(FileContent, *FilePath); + std::string opendrive_xml = carla::rpc::FromLongFString(FileContent); + CarlaMap = carla::opendrive::OpenDriveParser::Load(opendrive_xml); - MeshActor->SetActorLabel("SM_Landscape" + FString::FromInt(MeshIndex) ); - Landscapes.Add(MeshActor); + if (!CarlaMap.has_value()) + { + UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Invalid Map")); + } + else + { + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Valid Map loaded")); + MapName = FPaths::GetCleanFilename(FilePath); + MapName.RemoveFromEnd(".xodr", ESearchCase::Type::IgnoreCase); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("MapName %s"), *MapName); + UEditorLevelLibrary::LoadLevel(*BaseLevelName); + + AActor* QueryActor = UGameplayStatics::GetActorOfClass( + GEditor->GetEditorWorldContext().World(), + ALargeMapManager::StaticClass() ); + if( QueryActor != nullptr ){ + ALargeMapManager* LmManager = Cast(QueryActor); + LmManager->GenerateMap_Editor(); + NumTilesInXY = LmManager->GetNumTilesInXY(); + TileSize = LmManager->GetTileSize(); + Tile0Offset = LmManager->GetTile0Offset(); + + FCarlaMapTile& CarlaTile = LmManager->GetCarlaMapTile(CurrentTilesInXY); + UEditorLevelLibrary::SaveCurrentLevel(); + + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Current Tile is %s"), *( CurrentTilesInXY.ToString() ) ); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("NumTilesInXY is %s"), *( NumTilesInXY.ToString() ) ); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("TileSize is %f"), ( TileSize ) ); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Tile0Offset is %s"), *( Tile0Offset.ToString() ) ); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Tile Name is %s"), *(CarlaTile.Name) ); + + UEditorLevelLibrary::LoadLevel(CarlaTile.Name); + + MinPosition = FVector(CurrentTilesInXY.X * TileSize, CurrentTilesInXY.Y * -TileSize, 0.0f); + MaxPosition = FVector((CurrentTilesInXY.X + 1.0f ) * TileSize, (CurrentTilesInXY.Y + 1.0f) * -TileSize, 0.0f); + + GenerateAll(CarlaMap, MinPosition, MaxPosition); + Landscapes.Empty(); + bHasStarted = true; + bRoadsFinished = true; + bMapLoaded = true; + bTileFinished = false; + }else{ + UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Largemapmanager not found ") ); + } + + UEditorLoadingAndSavingUtils::SaveDirtyPackages(true, true); + UEditorLevelLibrary::SaveCurrentLevel(); + RemoveFromRoot(); + } +} + +bool UOpenDriveToMap::GoNextTile(){ + CurrentTilesInXY.X++; + if( CurrentTilesInXY.X >= NumTilesInXY.X ){ + CurrentTilesInXY.X = 0; + CurrentTilesInXY.Y++; + if( CurrentTilesInXY.Y >= NumTilesInXY.Y ){ + return false; + } + } + return true; +} + +void UOpenDriveToMap::ReturnToMainLevel(){ + Landscapes.Empty(); + FEditorFileUtils::SaveDirtyPackages(false, true, true, false, false, false, nullptr); + UEditorLevelLibrary::LoadLevel(*BaseLevelName); +} + +void UOpenDriveToMap::CorrectPositionForAllActorsInCurrentTile(){ + TArray FoundActors; + UGameplayStatics::GetAllActorsOfClass(UEditorLevelLibrary::GetEditorWorld(), AActor::StaticClass(), FoundActors); + for( AActor* Current : FoundActors){ + Current->AddActorWorldOffset(-MinPosition, false); + if( AStaticMeshActor* MeshActor = Cast(Current) ){ + UStaticMesh* StaticMesh = MeshActor->GetStaticMeshComponent()->GetStaticMesh(); + if(StaticMesh) + StaticMesh->ClearFlags(RF_Standalone); + } + } + CollectGarbage(GARBAGE_COLLECTION_KEEPFLAGS); + GEngine->PerformGarbageCollectionAndCleanupActors(); +} + +FString UOpenDriveToMap::GetStringForCurrentTile(){ + return FString("_X_") + FString::FromInt(CurrentTilesInXY.X) + FString("_Y_") + FString::FromInt(CurrentTilesInXY.Y); +} + +AActor* UOpenDriveToMap::SpawnActorInEditorWorld(UClass* Class, FVector Location, FRotator Rotation){ + return UEditorLevelLibrary::GetEditorWorld()->SpawnActor(Class, + Location, Rotation); } void UOpenDriveToMap::OpenFileDialog() @@ -275,6 +409,10 @@ void UOpenDriveToMap::OpenFileDialog() void UOpenDriveToMap::LoadMap() { + if( FilePath.IsEmpty() ){ + return; + } + FString FileContent; UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("UOpenDriveToMap::LoadMap(): File to load %s"), *FilePath ); FFileHelper::LoadFileToString(FileContent, *FilePath); @@ -287,22 +425,41 @@ void UOpenDriveToMap::LoadMap() } else { - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Valid Map loaded")); - } - MapName = FPaths::GetCleanFilename(FilePath); - MapName.RemoveFromEnd(".xodr", ESearchCase::Type::IgnoreCase); - UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("MapName %s"), *MapName); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Valid Map loaded")); + MapName = FPaths::GetCleanFilename(FilePath); + MapName.RemoveFromEnd(".xodr", ESearchCase::Type::IgnoreCase); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("MapName %s"), *MapName); - GenerateAll(CarlaMap); - GenerationFinished(); + AActor* QueryActor = UGameplayStatics::GetActorOfClass( + UEditorLevelLibrary::GetEditorWorld(), + ALargeMapManager::StaticClass() ); + + if( QueryActor != nullptr ) + { + ALargeMapManager* LargeMapManager = Cast(QueryActor); + NumTilesInXY = LargeMapManager->GetNumTilesInXY(); + TileSize = LargeMapManager->GetTileSize(); + Tile0Offset = LargeMapManager->GetTile0Offset(); + CurrentTilesInXY = FIntVector(0,0,0); + ULevel* PersistantLevel = UEditorLevelLibrary::GetEditorWorld()->PersistentLevel; + BaseLevelName = LargeMapManager->LargeMapTilePath + "/" + LargeMapManager->LargeMapName; + do{ + GenerateTileStandalone(); + }while(GoNextTile()); + ReturnToMainLevel(); + } + } } -TArray UOpenDriveToMap::GenerateMiscActors(float Offset) +TArray UOpenDriveToMap::GenerateMiscActors(float Offset, FVector MinLocation, FVector MaxLocation ) { + carla::geom::Vector3D CarlaMinLocation(MinLocation.X / 100, MinLocation.Y / 100, MinLocation.Z /100); + carla::geom::Vector3D CarlaMaxLocation(MaxLocation.X / 100, MaxLocation.Y / 100, MaxLocation.Z /100); + std::vector> - Locations = CarlaMap->GetTreesTransform(DistanceBetweenTrees, DistanceFromRoadEdge, Offset); + Locations = CarlaMap->GetTreesTransform(CarlaMinLocation, CarlaMaxLocation, DistanceBetweenTrees, DistanceFromRoadEdge, Offset); TArray Returning; - int i = 0; + static int i = 0; for (auto& cl : Locations) { const FVector scale{ 1.0f, 1.0f, 1.0f }; @@ -311,7 +468,7 @@ TArray UOpenDriveToMap::GenerateMiscActors(float Offset) NewTransform = GetSnappedPosition(NewTransform); - AActor* Spawner = GetWorld()->SpawnActor(AStaticMeshActor::StaticClass(), + AActor* Spawner = UEditorLevelLibrary::GetEditorWorld()->SpawnActor(AStaticMeshActor::StaticClass(), NewTransform.GetLocation(), NewTransform.Rotator()); Spawner->Tags.Add(FName("MiscSpawnPosition")); Spawner->Tags.Add(FName(cl.second.c_str())); @@ -321,38 +478,40 @@ TArray UOpenDriveToMap::GenerateMiscActors(float Offset) } return Returning; } -void UOpenDriveToMap::GenerateAll(const boost::optional& CarlaMap ) + +void UOpenDriveToMap::GenerateAll(const boost::optional& ParamCarlaMap, + FVector MinLocation, + FVector MaxLocation ) { - if (!CarlaMap.has_value()) + if (!ParamCarlaMap.has_value()) { UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Invalid Map")); }else { - if(DefaultHeightmap && !Heightmap){ - Heightmap = DefaultHeightmap; - } - - GenerateRoadMesh(CarlaMap); - GenerateLaneMarks(CarlaMap); - GenerateSpawnPoints(CarlaMap); - CreateTerrain(12800, 256, nullptr); - GenerateTreePositions(CarlaMap); + GenerateRoadMesh(ParamCarlaMap, MinLocation, MaxLocation); + GenerateLaneMarks(ParamCarlaMap, MinLocation, MaxLocation); + //GenerateSpawnPoints(ParamCarlaMap); + CreateTerrain(12800, 256); + GenerateTreePositions(ParamCarlaMap, MinLocation, MaxLocation); + GenerationFinished(MinLocation, MaxLocation); } } -void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& CarlaMap ) +void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation ) { opg_parameters.vertex_distance = 0.5f; opg_parameters.vertex_width_resolution = 8.0f; opg_parameters.simplification_percentage = 50.0f; - double start = FPlatformTime::Seconds(); - const auto Meshes = CarlaMap->GenerateOrderedChunkedMesh(opg_parameters); + + carla::geom::Vector3D CarlaMinLocation(MinLocation.X / 100, MinLocation.Y / 100, MinLocation.Z /100); + carla::geom::Vector3D CarlaMaxLocation(MaxLocation.X / 100, MaxLocation.Y / 100, MaxLocation.Z /100); + const auto Meshes = ParamCarlaMap->GenerateOrderedChunkedMeshInLocations(opg_parameters, CarlaMinLocation, CarlaMaxLocation); double end = FPlatformTime::Seconds(); UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" GenerateOrderedChunkedMesh code executed in %f seconds. Simplification percentage is %f"), end - start, opg_parameters.simplification_percentage); start = FPlatformTime::Seconds(); - int index = 0; + static int index = 0; for (const auto &PairMap : Meshes) { for( auto& Mesh : PairMap.second ) @@ -370,34 +529,32 @@ void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& for( auto& Vertex : Mesh->GetVertices() ) { FVector VertexFVector = Vertex.ToFVector(); - Vertex.z += GetHeight(Vertex.x, Vertex.y, DistanceToLaneBorder(CarlaMap,VertexFVector) > 65.0f ); + Vertex.z += GetHeight(Vertex.x, Vertex.y, DistanceToLaneBorder(ParamCarlaMap,VertexFVector) > 65.0f ); } carla::geom::Simplification Simplify(0.15); Simplify.Simplificate(Mesh); }else{ for( auto& Vertex : Mesh->GetVertices() ) { - Vertex.z += GetHeight(Vertex.x, Vertex.y, false) + 0.10; + Vertex.z += GetHeight(Vertex.x, Vertex.y, false) + 0.15f; } } - AProceduralMeshActor* TempActor = GetWorld()->SpawnActor(); - + AStaticMeshActor* TempActor = UEditorLevelLibrary::GetEditorWorld()->SpawnActor(); + UStaticMeshComponent* StaticMeshComponent = TempActor->GetStaticMeshComponent(); TempActor->SetActorLabel(FString("SM_Lane_") + FString::FromInt(index)); - UProceduralMeshComponent *TempPMC = TempActor->MeshComponent; - TempPMC->bUseAsyncCooking = true; - TempPMC->bUseComplexAsSimpleCollision = true; - TempPMC->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics); + StaticMeshComponent->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics); if(DefaultRoadMaterial && PairMap.first == carla::road::Lane::LaneType::Driving) { - TempPMC->SetMaterial(0, DefaultRoadMaterial); + StaticMeshComponent->SetMaterial(0, DefaultRoadMaterial); + StaticMeshComponent->CastShadow = false; TempActor->SetActorLabel(FString("SM_DrivingLane_") + FString::FromInt(index)); } if(DefaultSidewalksMaterial && PairMap.first == carla::road::Lane::LaneType::Sidewalk) { - TempPMC->SetMaterial(0, DefaultSidewalksMaterial); + StaticMeshComponent->SetMaterial(0, DefaultSidewalksMaterial); TempActor->SetActorLabel(FString("SM_Sidewalk_") + FString::FromInt(index)); } FVector MeshCentroid = FVector(0,0,0); @@ -427,20 +584,22 @@ void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& Tangents ); - TempPMC->CreateMeshSection_LinearColor( - 0, - MeshData.Vertices, - MeshData.Triangles, - MeshData.Normals, - MeshData.UV0, // UV0 - TArray(), // VertexColor - Tangents, // Tangents - true); // Create collision - TempActor->SetActorLocation(MeshCentroid * 100); - // ActorMeshList.Add(TempActor); + if(PairMap.first == carla::road::Lane::LaneType::Sidewalk) + { + UStaticMesh* MeshToSet = UMapGenFunctionLibrary::CreateMesh(MeshData, Tangents, DefaultSidewalksMaterial, MapName, "DrivingLane", FName(TEXT("SM_SidewalkMesh" + FString::FromInt(index) + GetStringForCurrentTile() ))); + StaticMeshComponent->SetStaticMesh(MeshToSet); + } - RoadType.Add(LaneTypeToFString(PairMap.first)); - // RoadMesh.Add(TempPMC); + if(PairMap.first == carla::road::Lane::LaneType::Driving) + { + UStaticMesh* MeshToSet = UMapGenFunctionLibrary::CreateMesh(MeshData, Tangents, DefaultRoadMaterial, MapName, "DrivingLane", FName(TEXT("SM_DrivingLaneMesh" + FString::FromInt(index) + GetStringForCurrentTile() ))); + StaticMeshComponent->SetStaticMesh(MeshToSet); + } + TempActor->SetActorLocation(MeshCentroid * 100); + TempActor->Tags.Add(FName("RoadLane")); + // ActorMeshList.Add(TempActor); + StaticMeshComponent->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics); + TempActor->SetActorEnableCollision(true); index++; } } @@ -449,17 +608,21 @@ void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Mesh spawnning and translation code executed in %f seconds."), end - start); } -void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& CarlaMap) +void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation ) { opg_parameters.vertex_distance = 0.5f; opg_parameters.vertex_width_resolution = 8.0f; opg_parameters.simplification_percentage = 15.0f; std::vector lanemarkinfo; - auto MarkingMeshes = CarlaMap->GenerateLineMarkings(opg_parameters, lanemarkinfo); - + carla::geom::Vector3D CarlaMinLocation(MinLocation.X / 100, MinLocation.Y / 100, MinLocation.Z /100); + carla::geom::Vector3D CarlaMaxLocation(MaxLocation.X / 100, MaxLocation.Y / 100, MaxLocation.Z /100); + auto MarkingMeshes = ParamCarlaMap->GenerateLineMarkings(opg_parameters, CarlaMinLocation, CarlaMaxLocation, lanemarkinfo); + TArray LaneMarkerActorList; + static int meshindex = 0; int index = 0; for (const auto& Mesh : MarkingMeshes) { + if ( !Mesh->GetVertices().size() ) { index++; @@ -474,7 +637,7 @@ void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& for (auto& Vertex : Mesh->GetVertices()) { FVector VertexFVector = Vertex.ToFVector(); - Vertex.z += GetHeight(Vertex.x, Vertex.y, DistanceToLaneBorder(CarlaMap,VertexFVector) > 65.0f ) + 0.0001f; + Vertex.z += GetHeight(Vertex.x, Vertex.y, DistanceToLaneBorder(ParamCarlaMap,VertexFVector) > 65.0f ) + 0.0001f; MeshCentroid += Vertex.ToFVector(); } @@ -500,24 +663,21 @@ void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& if(MinDistance < 250) { - UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Skkipped is %f."), MinDistance); + UE_LOG(LogCarlaToolsMapGenerator, VeryVerbose, TEXT("Skkipped is %f."), MinDistance); index++; continue; } - AProceduralMeshActor* TempActor = GetWorld()->SpawnActor(); - TempActor->SetActorLabel(FString("SM_LaneMark_") + FString::FromInt(index)); - UProceduralMeshComponent* TempPMC = TempActor->MeshComponent; - TempPMC->bUseAsyncCooking = true; - TempPMC->bUseComplexAsSimpleCollision = true; - TempPMC->SetCollisionEnabled(ECollisionEnabled::NoCollision); - TempPMC->CastShadow = false; + AStaticMeshActor* TempActor = UEditorLevelLibrary::GetEditorWorld()->SpawnActor(); + UStaticMeshComponent* StaticMeshComponent = TempActor->GetStaticMeshComponent(); + TempActor->SetActorLabel(FString("SM_LaneMark_") + FString::FromInt(meshindex)); + StaticMeshComponent->CastShadow = false; if (lanemarkinfo[index].find("yellow") != std::string::npos) { if(DefaultLaneMarksYellowMaterial) - TempPMC->SetMaterial(0, DefaultLaneMarksYellowMaterial); + StaticMeshComponent->SetMaterial(0, DefaultLaneMarksYellowMaterial); }else{ if(DefaultLaneMarksWhiteMaterial) - TempPMC->SetMaterial(0, DefaultLaneMarksWhiteMaterial); + StaticMeshComponent->SetMaterial(0, DefaultLaneMarksWhiteMaterial); } @@ -531,39 +691,43 @@ void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& Normals, Tangents ); - TempPMC->CreateMeshSection_LinearColor( - 0, - MeshData.Vertices, - MeshData.Triangles, - Normals, - MeshData.UV0, // UV0 - TArray(), // VertexColor - Tangents, // Tangents - true); // Create collision + + UStaticMesh* MeshToSet = UMapGenFunctionLibrary::CreateMesh(MeshData, Tangents, DefaultLandscapeMaterial, MapName, "LaneMark", FName(TEXT("SM_LaneMarkMesh" + FString::FromInt(meshindex) + GetStringForCurrentTile() ))); + StaticMeshComponent->SetStaticMesh(MeshToSet); TempActor->SetActorLocation(MeshCentroid * 100); TempActor->Tags.Add(*FString(lanemarkinfo[index].c_str())); + TempActor->Tags.Add(FName("RoadLane")); LaneMarkerActorList.Add(TempActor); index++; + meshindex++; + TempActor->SetActorEnableCollision(false); + StaticMeshComponent->SetCollisionEnabled(ECollisionEnabled::NoCollision); + } } -void UOpenDriveToMap::GenerateSpawnPoints( const boost::optional& CarlaMap ) +void UOpenDriveToMap::GenerateSpawnPoints( const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation ) { float SpawnersHeight = 300.f; - const auto Waypoints = CarlaMap->GenerateWaypointsOnRoadEntries(); + const auto Waypoints = ParamCarlaMap->GenerateWaypointsOnRoadEntries(); + TArray ActorsToMove; for (const auto &Wp : Waypoints) { - const FTransform Trans = CarlaMap->ComputeTransform(Wp); - AVehicleSpawnPoint *Spawner = GetWorld()->SpawnActor(); + const FTransform Trans = ParamCarlaMap->ComputeTransform(Wp); + AVehicleSpawnPoint *Spawner = UEditorLevelLibrary::GetEditorWorld()->SpawnActor(); Spawner->SetActorRotation(Trans.GetRotation()); Spawner->SetActorLocation(Trans.GetTranslation() + FVector(0.f, 0.f, SpawnersHeight)); + ActorsToMove.Add(Spawner); } } -void UOpenDriveToMap::GenerateTreePositions( const boost::optional& CarlaMap ) +void UOpenDriveToMap::GenerateTreePositions( const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation ) { + carla::geom::Vector3D CarlaMinLocation(MinLocation.X / 100, MinLocation.Y / 100, MinLocation.Z /100); + carla::geom::Vector3D CarlaMaxLocation(MaxLocation.X / 100, MaxLocation.Y / 100, MaxLocation.Z /100); + std::vector> Locations = - CarlaMap->GetTreesTransform(DistanceBetweenTrees, DistanceFromRoadEdge ); + ParamCarlaMap->GetTreesTransform(CarlaMinLocation, CarlaMaxLocation,DistanceBetweenTrees, DistanceFromRoadEdge ); int i = 0; for (auto &cl : Locations) { @@ -572,271 +736,81 @@ void UOpenDriveToMap::GenerateTreePositions( const boost::optionalSpawnActor(AStaticMeshActor::StaticClass(), + AActor* Spawner = UEditorLevelLibrary::GetEditorWorld()->SpawnActor(AStaticMeshActor::StaticClass(), NewTransform.GetLocation(), NewTransform.Rotator()); Spawner->Tags.Add(FName("TreeSpawnPosition")); Spawner->Tags.Add(FName(cl.second.c_str())); - Spawner->SetActorLabel("TreeSpawnPosition" + FString::FromInt(i) ); + Spawner->SetActorLabel("TreeSpawnPosition" + FString::FromInt(i) + GetStringForCurrentTile() ); ++i; } } -UStaticMesh* UOpenDriveToMap::CreateStaticMeshAsset( UProceduralMeshComponent* ProcMeshComp, int32 MeshIndex, FString FolderName ) -{ - FMeshDescription MeshDescription = BuildMeshDescription(ProcMeshComp); - - IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile(); - - // If we got some valid data. - if (MeshDescription.Polygons().Num() > 0) - { - FString MeshName = *(FolderName + FString::FromInt(MeshIndex) ); - FString PackageName = "/Game/CustomMaps/" + MapName + "/Static/" + FolderName + "/" + MeshName; - - if( !PlatformFile.DirectoryExists(*PackageName) ) - { - PlatformFile.CreateDirectory(*PackageName); - } - - // Then find/create it. - UPackage* Package = CreatePackage(*PackageName); - check(Package); - // Create StaticMesh object - UStaticMesh* StaticMesh = NewObject(Package, *MeshName, RF_Public | RF_Standalone); - StaticMesh->InitResources(); - - StaticMesh->LightingGuid = FGuid::NewGuid(); - - // Add source to new StaticMesh - FStaticMeshSourceModel& SrcModel = StaticMesh->AddSourceModel(); - SrcModel.BuildSettings.bRecomputeNormals = false; - SrcModel.BuildSettings.bRecomputeTangents = false; - SrcModel.BuildSettings.bRemoveDegenerates = false; - SrcModel.BuildSettings.bUseHighPrecisionTangentBasis = false; - SrcModel.BuildSettings.bUseFullPrecisionUVs = false; - SrcModel.BuildSettings.bGenerateLightmapUVs = true; - SrcModel.BuildSettings.SrcLightmapIndex = 0; - SrcModel.BuildSettings.DstLightmapIndex = 1; - SrcModel.BuildSettings.DistanceFieldResolutionScale = 0; - StaticMesh->CreateMeshDescription(0, MoveTemp(MeshDescription)); - StaticMesh->CommitMeshDescription(0); - - //// SIMPLE COLLISION - if (!ProcMeshComp->bUseComplexAsSimpleCollision ) - { - StaticMesh->CreateBodySetup(); - UBodySetup* NewBodySetup = StaticMesh->BodySetup; - NewBodySetup->BodySetupGuid = FGuid::NewGuid(); - NewBodySetup->AggGeom.ConvexElems = ProcMeshComp->ProcMeshBodySetup->AggGeom.ConvexElems; - NewBodySetup->bGenerateMirroredCollision = false; - NewBodySetup->bDoubleSidedGeometry = true; - NewBodySetup->CollisionTraceFlag = CTF_UseDefault; - NewBodySetup->CreatePhysicsMeshes(); - } - - //// MATERIALS - TSet UniqueMaterials; - const int32 NumSections = ProcMeshComp->GetNumSections(); - for (int32 SectionIdx = 0; SectionIdx < NumSections; SectionIdx++) - { - FProcMeshSection *ProcSection = - ProcMeshComp->GetProcMeshSection(SectionIdx); - UMaterialInterface *Material = ProcMeshComp->GetMaterial(SectionIdx); - UniqueMaterials.Add(Material); - } - // Copy materials to new mesh - for (auto* Material : UniqueMaterials) - { - StaticMesh->StaticMaterials.Add(FStaticMaterial(Material)); - } - - //Set the Imported version before calling the build - StaticMesh->ImportVersion = EImportStaticMeshVersion::LastVersion; - StaticMesh->Build(false); - StaticMesh->PostEditChange(); - - // Notify asset registry of new asset - FAssetRegistryModule::AssetCreated(StaticMesh); - UPackage::SavePackage(Package, StaticMesh, EObjectFlags::RF_Public | EObjectFlags::RF_Standalone, *MeshName, GError, nullptr, true, true, SAVE_NoError); - return StaticMesh; - } - return nullptr; -} - -TArray UOpenDriveToMap::CreateStaticMeshAssets() -{ - double start = FPlatformTime::Seconds(); - double end = FPlatformTime::Seconds(); - - IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile(); - TArray StaticMeshes; - - double BuildMeshDescriptionTime = 0.0f; - double PackgaesCreatingTime = 0.0f; - double MeshInitTime = 0.0f; - double MatAndCollInitTime = 0.0f; - double MeshBuildTime = 0.0f; - double PackSaveTime = 0.0f; - - - for (int i = 0; i < RoadMesh.Num(); ++i) - { - FString MeshName = RoadType[i] + FString::FromInt(i); - FString PackageName = "/Game/CustomMaps/" + MapName + "/Static/" + RoadType[i] + "/" + MeshName; - - if (!PlatformFile.DirectoryExists(*PackageName)) - { - PlatformFile.CreateDirectory(*PackageName); - } - - UProceduralMeshComponent* ProcMeshComp = RoadMesh[i]; - start = FPlatformTime::Seconds(); - FMeshDescription MeshDescription = BuildMeshDescription(ProcMeshComp); - end = FPlatformTime::Seconds(); - BuildMeshDescriptionTime += end - start; - // If we got some valid data. - if (MeshDescription.Polygons().Num() > 0) - { - start = FPlatformTime::Seconds(); - // Then find/create it. - UPackage* Package = CreatePackage(*PackageName); - check(Package); - end = FPlatformTime::Seconds(); - PackgaesCreatingTime += end - start; - - start = FPlatformTime::Seconds(); - - // Create StaticMesh object - UStaticMesh* CurrentStaticMesh = NewObject(Package, *MeshName, RF_Public | RF_Standalone); - CurrentStaticMesh->InitResources(); - - CurrentStaticMesh->LightingGuid = FGuid::NewGuid(); - - // Add source to new StaticMesh - FStaticMeshSourceModel& SrcModel = CurrentStaticMesh->AddSourceModel(); - SrcModel.BuildSettings.bRecomputeNormals = false; - SrcModel.BuildSettings.bRecomputeTangents = false; - SrcModel.BuildSettings.bRemoveDegenerates = false; - SrcModel.BuildSettings.bUseHighPrecisionTangentBasis = false; - SrcModel.BuildSettings.bUseFullPrecisionUVs = false; - SrcModel.BuildSettings.bGenerateLightmapUVs = true; - SrcModel.BuildSettings.SrcLightmapIndex = 0; - SrcModel.BuildSettings.DstLightmapIndex = 1; - CurrentStaticMesh->CreateMeshDescription(0, MoveTemp(MeshDescription)); - CurrentStaticMesh->CommitMeshDescription(0); - end = FPlatformTime::Seconds(); - MeshInitTime += end - start; - start = FPlatformTime::Seconds(); - - //// SIMPLE COLLISION - if (!ProcMeshComp->bUseComplexAsSimpleCollision) - { - CurrentStaticMesh->CreateBodySetup(); - UBodySetup* NewBodySetup = CurrentStaticMesh->BodySetup; - NewBodySetup->BodySetupGuid = FGuid::NewGuid(); - NewBodySetup->AggGeom.ConvexElems = ProcMeshComp->ProcMeshBodySetup->AggGeom.ConvexElems; - NewBodySetup->bGenerateMirroredCollision = false; - NewBodySetup->bDoubleSidedGeometry = true; - NewBodySetup->CollisionTraceFlag = CTF_UseDefault; - NewBodySetup->CreatePhysicsMeshes(); - } - - //// MATERIALS - TSet UniqueMaterials; - const int32 NumSections = ProcMeshComp->GetNumSections(); - for (int32 SectionIdx = 0; SectionIdx < NumSections; SectionIdx++) - { - FProcMeshSection* ProcSection = - ProcMeshComp->GetProcMeshSection(SectionIdx); - UMaterialInterface* Material = ProcMeshComp->GetMaterial(SectionIdx); - UniqueMaterials.Add(Material); - } - // Copy materials to new mesh - for (auto* Material : UniqueMaterials) - { - CurrentStaticMesh->StaticMaterials.Add(FStaticMaterial(Material)); - } - - end = FPlatformTime::Seconds(); - MatAndCollInitTime += end - start; - start = FPlatformTime::Seconds(); - //Set the Imported version before calling the build - CurrentStaticMesh->ImportVersion = EImportStaticMeshVersion::LastVersion; - CurrentStaticMesh->Build(false); - CurrentStaticMesh->PostEditChange(); - - end = FPlatformTime::Seconds(); - MeshBuildTime += end - start; - start = FPlatformTime::Seconds(); - - FString RoadName = *(RoadType[i] + FString::FromInt(i)); - // Notify asset registry of new asset - FAssetRegistryModule::AssetCreated(CurrentStaticMesh); - UPackage::SavePackage(Package, CurrentStaticMesh, EObjectFlags::RF_Public | EObjectFlags::RF_Standalone, *RoadName, GError, nullptr, true, true, SAVE_NoError); - end = FPlatformTime::Seconds(); - PackSaveTime += end - start; - - - StaticMeshes.Add( CurrentStaticMesh ); - } - } - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in BuildMeshDescriptionTime %f. Time per mesh %f"), BuildMeshDescriptionTime, BuildMeshDescriptionTime/ RoadMesh.Num()); - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in PackgaesCreatingTime %f. Time per mesh %f"), PackgaesCreatingTime, PackgaesCreatingTime / RoadMesh.Num()); - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in MeshInitTime %f. Time per mesh %f"), MeshInitTime, MeshInitTime / RoadMesh.Num()); - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in MatAndCollInitTime %f. Time per mesh %f"), MatAndCollInitTime, MatAndCollInitTime / RoadMesh.Num()); - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in MeshBuildTime %f. Time per mesh %f"), MeshBuildTime, MeshBuildTime / RoadMesh.Num()); - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in PackSaveTime %f. Time per mesh %f"), PackSaveTime, PackSaveTime / RoadMesh.Num()); - return StaticMeshes; -} - -void UOpenDriveToMap::SaveMap() -{ - double start = FPlatformTime::Seconds(); - - MeshesToSpawn = CreateStaticMeshAssets(); - - double end = FPlatformTime::Seconds(); - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" Meshes created static mesh code executed in %f seconds."), end - start); - - start = FPlatformTime::Seconds(); - - for (int i = 0; i < MeshesToSpawn.Num(); ++i) - { - AStaticMeshActor* TempActor = GetWorld()->SpawnActor(); - // Build mesh from source - TempActor->GetStaticMeshComponent()->SetStaticMesh(MeshesToSpawn[i]); - TempActor->SetActorLabel(FString("SM_") + MeshesToSpawn[i]->GetName()); - TempActor->SetActorTransform(ActorMeshList[i]->GetActorTransform()); - } - - for (auto CurrentActor : ActorMeshList) - { - CurrentActor->Destroy(); - } - - end = FPlatformTime::Seconds(); - UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" Spawning Static Meshes code executed in %f seconds."), end - start); -} float UOpenDriveToMap::GetHeight(float PosX, float PosY, bool bDrivingLane){ - if( bDrivingLane ){ - return carla::geom::deformation::GetZPosInDeformation(PosX, PosY) - - carla::geom::deformation::GetBumpDeformation(PosX,PosY); + if( DefaultHeightmap ){ + const FColor* FormatedImageData = static_cast( DefaultHeightmap->PlatformData->Mips[0].BulkData.LockReadOnly()); + + int32 TextureSizeX = DefaultHeightmap->GetSizeX(); + int32 TextureSizeY = DefaultHeightmap->GetSizeY(); + + int32 PixelX = ( ( PosX - WorldOriginPosition.X/100) / (WorldEndPosition.X/100 - WorldOriginPosition.X/100) ) * ((float)TextureSizeX); + int32 PixelY = ( ( PosY - WorldOriginPosition.Y/100) / (WorldEndPosition.Y/100 - WorldOriginPosition.Y/100) ) * ((float)TextureSizeY); + + if( PixelX < 0 ){ + PixelX += TextureSizeX; + } + + if( PixelY < 0 ){ + PixelY += TextureSizeY; + } + + if( PixelX > TextureSizeX ){ + PixelX -= TextureSizeX; + } + + if( PixelY > TextureSizeY ){ + PixelY -= TextureSizeY; + } + + FColor PixelColor = FormatedImageData[PixelY * TextureSizeX + PixelX]; + + //UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("PosX %f PosY %f "), PosX, PosY ); + //UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("WorldOriginPosition %s "), *WorldOriginPosition.ToString() ); + //UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("WorldEndPosition %s "), *WorldEndPosition.ToString() ); + //UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("PixelColor %s "), *WorldEndPosition.ToString() ); + //UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Reading Pixel X: %d Y %d Total Size X %d Y %d"), PixelX, PixelY, TextureSizeX, TextureSizeY ); + + DefaultHeightmap->PlatformData->Mips[0].BulkData.Unlock(); + + float LandscapeHeight = ( (PixelColor.R/255.0f ) * ( MaxHeight - MinHeight ) ) + MinHeight; + + if( bDrivingLane ){ + return LandscapeHeight - + carla::geom::deformation::GetBumpDeformation(PosX,PosY); + }else{ + return LandscapeHeight; + } }else{ - return carla::geom::deformation::GetZPosInDeformation(PosX, PosY) + - (carla::geom::deformation::GetZPosInDeformation(PosX, PosY) * -0.15f); + if( bDrivingLane ){ + return carla::geom::deformation::GetZPosInDeformation(PosX, PosY) + + (carla::geom::deformation::GetZPosInDeformation(PosX, PosY) * -0.3f) - + carla::geom::deformation::GetBumpDeformation(PosX,PosY); + }else{ + return carla::geom::deformation::GetZPosInDeformation(PosX, PosY) + (carla::geom::deformation::GetZPosInDeformation(PosX, PosY) * -0.3f); + } } } FTransform UOpenDriveToMap::GetSnappedPosition( FTransform Origin ){ FTransform ToReturn = Origin; - FVector Start = Origin.GetLocation() + FVector( 0, 0, 1000); - FVector End = Origin.GetLocation() - FVector( 0, 0, 1000); + FVector Start = Origin.GetLocation() + FVector( 0, 0, 10000); + FVector End = Origin.GetLocation() - FVector( 0, 0, 10000); FHitResult HitResult; FCollisionQueryParams CollisionQuery; + CollisionQuery.bTraceComplex = true; FCollisionResponseParams CollisionParams; - if( GetWorld()->LineTraceSingleByChannel( + if( UEditorLevelLibrary::GetEditorWorld()->LineTraceSingleByChannel( HitResult, Start, End, @@ -858,34 +832,65 @@ float UOpenDriveToMap::GetHeightForLandscape( FVector Origin ){ CollisionQuery.AddIgnoredActors(Landscapes); FCollisionResponseParams CollisionParams; - if( GetWorld()->LineTraceSingleByChannel( + if( UEditorLevelLibrary::GetEditorWorld()->LineTraceSingleByChannel( HitResult, Start, End, ECollisionChannel::ECC_WorldStatic, CollisionQuery, - CollisionParams - ) ) + CollisionParams) ) { - return GetHeight(Origin.X * 0.01f, Origin.Y * 0.01f, true) * 100.0f - 25.0f; + return GetHeight(Origin.X * 0.01f, Origin.Y * 0.01f, true) * 100.0f - 80.0f; }else{ - return GetHeight(Origin.X * 0.01f, Origin.Y * 0.01f, true) * 100.0f; + return GetHeight(Origin.X * 0.01f, Origin.Y * 0.01f, true) * 100.0f - 1.0f; } return 0.0f; } -float UOpenDriveToMap::DistanceToLaneBorder(const boost::optional& CarlaMap, +float UOpenDriveToMap::DistanceToLaneBorder(const boost::optional& ParamCarlaMap, FVector &location, int32_t lane_type ) const { carla::geom::Location cl(location); //wp = GetClosestWaypoint(pos). if distance wp - pos == lane_width --> estas al borde de la carretera - auto wp = CarlaMap->GetClosestWaypointOnRoad(cl, lane_type); + auto wp = ParamCarlaMap->GetClosestWaypointOnRoad(cl, lane_type); if(wp) { - carla::geom::Transform ct = CarlaMap->ComputeTransform(*wp); - double LaneWidth = CarlaMap->GetLaneWidth(*wp); + carla::geom::Transform ct = ParamCarlaMap->ComputeTransform(*wp); + double LaneWidth = ParamCarlaMap->GetLaneWidth(*wp); return cl.Distance(ct.location) - LaneWidth; } return 100000.0f; } +bool UOpenDriveToMap::IsInRoad( + const boost::optional& ParamCarlaMap, + FVector &location) const +{ + int32_t start = static_cast(carla::road::Lane::LaneType::Driving); + int32_t end = static_cast(carla::road::Lane::LaneType::Sidewalk); + for( int32_t i = start; i < end; ++i) + { + if(ParamCarlaMap->GetWaypoint(location, i)) + { + return true; + } + } + return false; +} + +void UOpenDriveToMap::MoveActorsToSubLevels(TArray ActorsToMove) +{ + AActor* QueryActor = UGameplayStatics::GetActorOfClass( + UEditorLevelLibrary::GetEditorWorld(), + ALargeMapManager::StaticClass() ); + + if( QueryActor != nullptr ){ + ALargeMapManager* LmManager = Cast(QueryActor); + if( LmManager ){ + UEditorLevelLibrary::SaveCurrentLevel(); + UHoudiniImporterWidget::MoveActorsToSubLevelWithLargeMap(ActorsToMove, LmManager); + } + } +} + +#endif diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/USDImporterWidget.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/USDImporterWidget.cpp index 697839a11..b47302939 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/USDImporterWidget.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/USDImporterWidget.cpp @@ -64,10 +64,10 @@ void UUSDImporterWidget::ImportUSDVehicle( } // Import Wheel and suspension data TArray WheelsData = UUSDCARLAInterface::GetUSDWheelData(USDPath); - auto CreateVehicleWheel = - [&](const FUSDCARLAWheelData& WheelData, + auto CreateVehicleWheel = + [&](const FUSDCARLAWheelData& WheelData, TSubclassOf TemplateClass, - const FString &PackagePathName) + const FString &PackagePathName) -> TSubclassOf { // Get a reference to the editor subsystem @@ -161,7 +161,7 @@ bool UUSDImporterWidget::MergeStaticMeshComponents( } TArray UUSDImporterWidget::MergeMeshComponents( - TArray ComponentsToMerge, + TArray ComponentsToMerge, const FString& DestMesh) { if(!ComponentsToMerge.Num()) @@ -275,7 +275,7 @@ FVehicleMeshParts UUSDImporterWidget::SplitVehicleParts( } else if (ComponentName.Contains("Collision")) { - + } else { @@ -286,7 +286,7 @@ FVehicleMeshParts UUSDImporterWidget::SplitVehicleParts( } } - if(ComponentName.Contains("glass") || + if(ComponentName.Contains("glass") || IsChildrenOf(Component, "glass")) { GlassComponents.Add(Component); @@ -330,7 +330,7 @@ FMergedVehicleMeshParts UUSDImporterWidget::GenerateVehicleMeshes( const FVehicleMeshParts& VehicleMeshParts, const FString& DestPath) { FMergedVehicleMeshParts Result; - auto MergePart = + auto MergePart = [](TArray Components, const FString& DestMeshPath) -> UStaticMesh* { @@ -430,7 +430,7 @@ AActor* UUSDImporterWidget::GenerateNewVehicleBlueprint( UWorld* World, UClass* BaseClass, USkeletalMesh* NewSkeletalMesh, - const FString &DestPath, + const FString &DestPath, const FMergedVehicleMeshParts& VehicleMeshes, const FWheelTemplates& WheelTemplates) { @@ -468,11 +468,11 @@ AActor* UUSDImporterWidget::GenerateNewVehicleBlueprint( } // Get the skeletal mesh and modify it to match the vehicle parameters - USkeletalMeshComponent* SkeletalMeshComponent = Cast( + USkeletalMeshComponent* SkeletalMeshComponent = Cast( TemplateActor->GetComponentByClass(USkeletalMeshComponent::StaticClass())); if(!SkeletalMeshComponent) { - UE_LOG(LogCarlaTools, Log, TEXT("Skeletal mesh component not found")); + UE_LOG(LogCarlaTools, Log, TEXT("Skeletal mesh component not found")); return nullptr; } USkeletalMesh* SkeletalMesh = SkeletalMeshComponent->SkeletalMesh; @@ -506,7 +506,7 @@ AActor* UUSDImporterWidget::GenerateNewVehicleBlueprint( ULocalLightComponent * LightComponent = NewObject(TemplateActor, LightClass, FName(*FixedLightName)); LightComponent->RegisterComponent(); LightComponent->AttachToComponent( - TemplateActor->GetRootComponent(), + TemplateActor->GetRootComponent(), FAttachmentTransformRules::KeepRelativeTransform); LightComponent->SetRelativeLocation(Light.Location); // Set the position of the light relative to the actor LightComponent->SetIntensityUnits(ELightUnits::Lumens); @@ -514,7 +514,7 @@ AActor* UUSDImporterWidget::GenerateNewVehicleBlueprint( LightComponent->SetVolumetricScatteringIntensity(0.f); if (FixedLightName.Contains("high_beam")) { - USpotLightComponent* SpotLight = + USpotLightComponent* SpotLight = Cast(LightComponent); SpotLight->SetRelativeRotation(FRotator(-1.5f, 0, 0)); SpotLight->SetAttenuationRadius(5000.f); @@ -525,7 +525,7 @@ AActor* UUSDImporterWidget::GenerateNewVehicleBlueprint( } else if (FixedLightName.Contains("low_beam")) { - USpotLightComponent* SpotLight = + USpotLightComponent* SpotLight = Cast(LightComponent); LightComponent->SetRelativeRotation(FRotator(-3.f, 0, 0)); LightComponent->SetAttenuationRadius(3000.f); @@ -553,11 +553,11 @@ AActor* UUSDImporterWidget::GenerateNewVehicleBlueprint( Setup.WheelClass = WheelTemplates.WheelRR; Setup.BoneName = "Wheel_Rear_Right"; WheelSetups.Add(Setup); - ACarlaWheeledVehicle* CarlaVehicle = + ACarlaWheeledVehicle* CarlaVehicle = Cast(TemplateActor); if(CarlaVehicle) { - UWheeledVehicleMovementComponent4W* MovementComponent = + UWheeledVehicleMovementComponent4W* MovementComponent = Cast( CarlaVehicle->GetVehicleMovementComponent()); MovementComponent->WheelSetups = WheelSetups; @@ -582,12 +582,12 @@ AActor* UUSDImporterWidget::GenerateNewVehicleBlueprint( } bool UUSDImporterWidget::EditSkeletalMeshBones( - USkeletalMesh* NewSkeletalMesh, + USkeletalMesh* NewSkeletalMesh, const TMap &NewBoneTransforms) { if(!NewSkeletalMesh) { - UE_LOG(LogCarlaTools, Log, TEXT("Skeletal mesh invalid")); + UE_LOG(LogCarlaTools, Log, TEXT("Skeletal mesh invalid")); return false; } FReferenceSkeleton& ReferenceSkeleton = NewSkeletalMesh->RefSkeleton; @@ -599,7 +599,7 @@ bool UUSDImporterWidget::EditSkeletalMeshBones( int32 BoneIdx = SkeletonModifier.FindBoneIndex(FName(*BoneName)); if (BoneIdx == INDEX_NONE) { - UE_LOG(LogCarlaTools, Log, TEXT("Bone %s not found"), *BoneName); + UE_LOG(LogCarlaTools, Log, TEXT("Bone %s not found"), *BoneName); } UE_LOG(LogCarlaTools, Log, TEXT("Bone %s corresponds to index %d"), *BoneName, BoneIdx); SkeletonModifier.UpdateRefPoseTransform(BoneIdx, BoneTransform); @@ -608,7 +608,7 @@ bool UUSDImporterWidget::EditSkeletalMeshBones( NewSkeletalMesh->MarkPackageDirty(); UPackage* Package = NewSkeletalMesh->GetOutermost(); return UPackage::SavePackage( - Package, NewSkeletalMesh, - EObjectFlags::RF_Public | EObjectFlags::RF_Standalone, + Package, NewSkeletalMesh, + EObjectFlags::RF_Public | EObjectFlags::RF_Standalone, *(Package->GetName()), GError, nullptr, true, true, SAVE_NoError); } diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/Commandlet/GenerateTileCommandlet.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/Commandlet/GenerateTileCommandlet.h new file mode 100644 index 000000000..e5a6af39f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/Commandlet/GenerateTileCommandlet.h @@ -0,0 +1,46 @@ +// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Runtime/Engine/Classes/Engine/ObjectLibrary.h" +#include "Commandlets/Commandlet.h" + +#include + +#include + +#include "OpenDriveToMap.h" + +#include "GenerateTileCommandlet.generated.h" + +// Each commandlet should generate only 1 Tile + +DECLARE_LOG_CATEGORY_EXTERN(LogCarlaToolsMapGenerateTileCommandlet, Log, All); + + +UCLASS() +class CARLATOOLS_API UGenerateTileCommandlet + : public UCommandlet +{ + GENERATED_BODY() + +public: + + /// Default constructor. + UGenerateTileCommandlet(); + UGenerateTileCommandlet(const FObjectInitializer &); + +#if WITH_EDITORONLY_DATA + + virtual int32 Main(const FString &Params) override; + +#endif // WITH_EDITORONLY_DATA + UPROPERTY() + UOpenDriveToMap* OpenDriveMap; + UPROPERTY() + UClass* OpenDriveClass; +}; diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/DigitalTwinsBaseWidget.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/DigitalTwinsBaseWidget.h new file mode 100644 index 000000000..1eb345175 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/DigitalTwinsBaseWidget.h @@ -0,0 +1,34 @@ +// Copyright (c) 2023 Computer Vision Center (CVC) at the Universitat Autonoma +// de Barcelona (UAB). +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "CoreMinimal.h" +#include "EditorUtilityWidget.h" +#include "CarlaTools.h" + +#include "DigitalTwinsBaseWidget.generated.h" + +class UOpenDriveToMap; + +UCLASS(BlueprintType) +class CARLATOOLS_API UDigitalTwinsBaseWidget : public UEditorUtilityWidget +{ + GENERATED_BODY() +public: + + UFUNCTION(BlueprintCallable) + UOpenDriveToMap* InitializeOpenDriveToMap(TSubclassOf BaseClass); + + UFUNCTION(BlueprintPure) + UOpenDriveToMap* GetOpenDriveToMap(); + + UFUNCTION(BlueprintCallable) + void SetOpenDriveToMap(UOpenDriveToMap* ToSet); + + UFUNCTION(BlueprintCallable) + void DestroyOpenDriveToMap(); +}; diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h index 032590f11..c655966e7 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h @@ -18,13 +18,16 @@ UCLASS(BlueprintType) class CARLATOOLS_API UHoudiniImporterWidget : public UEditorUtilityWidget { GENERATED_BODY() - +public: UFUNCTION(BlueprintCallable, Category="HoudiniImporterWidget") static void CreateSubLevels(ALargeMapManager* LargeMapManager); UFUNCTION(BlueprintCallable, Category="HoudiniImporterWidget") static void MoveActorsToSubLevelWithLargeMap(TArray Actors, ALargeMapManager* LargeMapManager); + UFUNCTION(BlueprintCallable, Category="HoudiniImporterWidget") + static void ForceStreamingLevelsToUnload( ALargeMapManager* LargeMapManager ); + UFUNCTION(BlueprintCallable, Category="HoudiniImporterWidget") static void MoveActorsToSubLevel(TArray Actors, ULevelStreaming* Level); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/Online/CustomFileDownloader.h similarity index 88% rename from Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.h rename to Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/Online/CustomFileDownloader.h index a2b4780f5..ab1a91fb8 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/Online/CustomFileDownloader.h @@ -6,45 +6,26 @@ #include "Interfaces/IHttpRequest.h" #include "CustomFileDownloader.generated.h" /** - * + * */ DECLARE_DELEGATE(FDownloadComplete) -UCLASS(Blueprintable) -class CARLA_API UCustomFileDownloader : public UObject -{ - GENERATED_BODY() -public: - UFUNCTION(BlueprintCallable) - void StartDownload(); - UFUNCTION(BlueprintCallable) - void ConvertOSMInOpenDrive(FString FilePath, float Lat_0 = 0, float Lon_0 = 0); - FString ResultFileName; - - FString Url; - - FDownloadComplete DownloadDelegate; - -private: - void RequestComplete(FHttpRequestPtr HttpRequest, FHttpResponsePtr HttpResponse, bool bSucceeded); - - FString Payload; -}; - -class FHttpDownloader +struct FHttpDownloader { public: + FHttpDownloader(); + /** * * @param Verb - verb to use for request (GET,POST,DELETE,etc) * @param Url - url address to connect to */ - FHttpDownloader( const FString& InVerb, const FString& InUrl, const FString& InFilename, FDownloadComplete& Delegate ); + FHttpDownloader( const FString& InVerb, const FString& InUrl, const FString& InFilename, FDownloadComplete& Delegate ); // Kick off the Http request and wait for delegate to be called - void Run(void); + void Run(void); /** * Delegate called when the request completes @@ -61,3 +42,26 @@ private: FString Filename; FDownloadComplete DelegateToCall; }; + +UCLASS(Blueprintable) +class CARLATOOLS_API UCustomFileDownloader : public UObject +{ + GENERATED_BODY() +public: + UFUNCTION(BlueprintCallable) + void StartDownload(); + UFUNCTION(BlueprintCallable) + void ConvertOSMInOpenDrive(FString FilePath, float Lat_0 = 0, float Lon_0 = 0); + + FString ResultFileName; + + FString Url; + + FDownloadComplete DownloadDelegate; +private: + void RequestComplete(FHttpRequestPtr HttpRequest, FHttpResponsePtr HttpResponse, bool bSucceeded); + + FString Payload; +}; + + diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h index 4c6eea9c7..2002c8275 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h @@ -6,6 +6,8 @@ #include "Blueprint/UserWidget.h" #include "ProceduralMeshComponent.h" #include "Math/Vector2D.h" +#include "EditorUtilityActor.h" +#include "EditorUtilityObject.h" #include #include @@ -23,11 +25,13 @@ class UMaterialInstance; * */ UCLASS(Blueprintable, BlueprintType) -class CARLATOOLS_API UOpenDriveToMap : public UObject +class CARLATOOLS_API UOpenDriveToMap : public UEditorUtilityObject { GENERATED_BODY() - +#if WITH_EDITOR public: + UOpenDriveToMap(); + ~UOpenDriveToMap(); UFUNCTION() void ConvertOSMInOpenDrive(); @@ -36,12 +40,47 @@ public: void CreateMap(); UFUNCTION(BlueprintCallable) - void CreateTerrain(const int MeshGridSize, const float MeshGridSectionSize, - const class UTexture2D* HeightmapTexture); + void CreateTerrain(const int MeshGridSize, const float MeshGridSectionSize); UFUNCTION(BlueprintCallable) - void CreateTerrainMesh(const int MeshIndex, const FVector2D Offset, const int GridSize, const float GridSectionSize, - const class UTexture2D* HeightmapTexture, class UTextureRenderTarget2D* RoadMask); + void CreateTerrainMesh(const int MeshIndex, const FVector2D Offset, const int GridSize, const float GridSectionSize); + + UFUNCTION(BlueprintCallable) + float GetHeight(float PosX, float PosY,bool bDrivingLane = false); + + UFUNCTION(BlueprintCallable) + static AActor* SpawnActorWithCheckNoCollisions(UClass* ActorClassToSpawn, FTransform Transform); + + UFUNCTION(BlueprintCallable) + float GetDistanceToDrivingLaneBorder(FVector Location) const{ + return DistanceToLaneBorder(CarlaMap, Location); + } + + UFUNCTION(BlueprintCallable) + bool GetIsInRoad(FVector Location) const { + return IsInRoad(CarlaMap, Location); + } + + UFUNCTION(BlueprintCallable) + void GenerateTileStandalone(); + + UFUNCTION(BlueprintCallable) + void GenerateTile(); + + UFUNCTION(BlueprintCallable) + bool GoNextTile(); + + UFUNCTION(BlueprintCallable) + void ReturnToMainLevel(); + + UFUNCTION(BlueprintCallable) + void CorrectPositionForAllActorsInCurrentTile(); + + UFUNCTION(BlueprintCallable) + FString GetStringForCurrentTile(); + + UFUNCTION(BlueprintCallable) + AActor* SpawnActorInEditorWorld(UClass* Class, FVector Location, FRotator Rotation); UPROPERTY(EditAnywhere, BlueprintReadWrite, Category="File") FString FilePath; @@ -70,45 +109,97 @@ public: UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Defaults") UMaterialInstance* DefaultLandscapeMaterial; - UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Defaults") - UTexture2D* DefaultHeightmap; + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Settings" ) float DistanceBetweenTrees = 50.0f; UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Settings" ) float DistanceFromRoadEdge = 3.0f; + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Stage" ) + bool bHasStarted = false; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Stage" ) + bool bRoadsFinished = false; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Stage" ) + bool bMapLoaded = false; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + FVector MinPosition; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + FVector MaxPosition; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + float TileSize; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + FVector Tile0Offset; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + bool bTileFinished; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + FIntVector NumTilesInXY; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + FIntVector CurrentTilesInXY; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="TileGeneration" ) + FString BaseLevelName; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Heightmap") + UTexture2D* DefaultHeightmap; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Heightmap" ) + FVector WorldEndPosition; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Heightmap" ) + FVector WorldOriginPosition; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Heightmap" ) + float MinHeight; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Heightmap" ) + float MaxHeight; + + protected: - UFUNCTION( BlueprintCallable ) - void SaveMap(); - UFUNCTION(BlueprintCallable) - TArray GenerateMiscActors(float Offset); + TArray GenerateMiscActors(float Offset, FVector MinLocation, FVector MaxLocation ); UFUNCTION( BlueprintImplementableEvent ) - void GenerationFinished(); + void GenerationFinished(FVector MinLocation, FVector MaxLocation); + + UFUNCTION( BlueprintImplementableEvent ) + void DownloadFinished(); + + + UFUNCTION( BlueprintImplementableEvent ) + void ExecuteTileCommandlet(); + + UFUNCTION( BlueprintCallable ) + void MoveActorsToSubLevels(TArray ActorsToMove); + private: UFUNCTION() void OpenFileDialog(); - UFUNCTION() + UFUNCTION(BlueprintCallable) void LoadMap(); - void GenerateAll(const boost::optional& CarlaMap); - void GenerateRoadMesh(const boost::optional& CarlaMap); - void GenerateSpawnPoints(const boost::optional& CarlaMap); - void GenerateTreePositions(const boost::optional& CarlaMap); - void GenerateLaneMarks(const boost::optional& CarlaMap); + void GenerateAll(const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation); + void GenerateRoadMesh(const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation); + void GenerateSpawnPoints(const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation); + void GenerateTreePositions(const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation); + void GenerateLaneMarks(const boost::optional& ParamCarlaMap, FVector MinLocation, FVector MaxLocation); - float GetHeight(float PosX, float PosY,bool bDrivingLane = false); carla::rpc::OpendriveGenerationParameters opg_parameters; boost::optional CarlaMap; - UStaticMesh* CreateStaticMeshAsset(UProceduralMeshComponent* ProcMeshComp, int32 MeshIndex, FString FolderName); - TArray CreateStaticMeshAssets(); - FTransform GetSnappedPosition(FTransform Origin); float GetHeightForLandscape(FVector Origin); @@ -117,20 +208,16 @@ private: FVector &location, int32_t lane_type = static_cast(carla::road::Lane::LaneType::Driving)) const; + bool IsInRoad(const boost::optional& ParamCarlaMap, + FVector &location) const; + + void InitTextureData(); + UPROPERTY() UCustomFileDownloader* FileDownloader; UPROPERTY() - TArray ActorMeshList; - UPROPERTY() - TArray LaneMarkerActorList; - UPROPERTY() - TArray MeshesToSpawn; - UPROPERTY() - TArray RoadType; - UPROPERTY() - TArray RoadMesh; - UPROPERTY() TArray Landscapes; UPROPERTY() UTexture2D* Heightmap; +#endif }; diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/RegionOfInterest.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/RegionOfInterest.h index 46ad378b1..e5226c58c 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/RegionOfInterest.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/RegionOfInterest.h @@ -20,8 +20,8 @@ UENUM(BlueprintType) enum ERegionOfInterestType { NONE_REGION, - TERRAIN_REGION, - WATERBODIES_REGION, // Not Supported yet + TERRAIN_REGION, + WATERBODIES_REGION, // Not Supported yet VEGETATION_REGION, MISC_SPREADED_ACTORS_REGION, MISC_SPECIFIC_LOCATION_ACTORS_REGION, @@ -47,7 +47,7 @@ struct CARLATOOLS_API FRoiTile int Y; public: - FRoiTile() : X(-1), Y(-1) + FRoiTile() : X(-1), Y(-1) {}; FRoiTile(int X, int Y) @@ -108,7 +108,7 @@ FORCEINLINE uint32 GetTypeHash(const FRoiTile& Thing) } /** - * + * */ USTRUCT(BlueprintType) struct CARLATOOLS_API FCarlaRegionOfInterest @@ -141,8 +141,8 @@ struct CARLATOOLS_API FCarlaRegionOfInterest template static FORCEINLINE bool IsTileInRegionsSet(FRoiTile RoiTile, TMap RoisMap) { - static_assert(TIsDerivedFrom::IsDerived, - "ROIs Map Value type is not an URegionOfInterest derived type."); + static_assert(TIsDerivedFrom::IsDerived, + "ROIs Map Value type is not an URegionOfInterest derived type."); return RoisMap.Contains(RoiTile); } @@ -154,7 +154,7 @@ struct CARLATOOLS_API FCarlaRegionOfInterest { return false; } - + // Checking if the two regions have the same tiles. TMap TileCount; for(FRoiTile Tile : Other.TilesList) @@ -191,7 +191,7 @@ struct CARLATOOLS_API FVegetationROI : public FCarlaRegionOfInterest UPROPERTY(BlueprintReadWrite) TArray FoliageSpawners; - + FVegetationROI() : FCarlaRegionOfInterest() { this->FoliageSpawners.Empty(); @@ -230,12 +230,12 @@ struct CARLATOOLS_API FTerrainROI : public FCarlaRegionOfInterest UPROPERTY(BlueprintReadWrite) UTextureRenderTarget2D* RoiHeightmapRenderTarget; - FTerrainROI() : FCarlaRegionOfInterest(), RoiMaterialInstance() + FTerrainROI() : FCarlaRegionOfInterest(), RoiMaterialInstance(), RoiHeightmapRenderTarget() {} /** * This function checks if a tile is on the boundary of a region of interest - * + * * @param RoiTile The tile we're checking * @param RoisMap The map of RoiTiles to Rois. * @param OutUp Is there a tile above this one? @@ -276,7 +276,7 @@ struct CARLATOOLS_API FMiscSpreadedActorsROI : public FCarlaRegionOfInterest {} }; -/// A struct that is used to store the information of a region of interest that is used to +/// A struct that is used to store the information of a region of interest that is used to /// spawn actors in specific locations. USTRUCT(BlueprintType) struct CARLATOOLS_API FMiscSpecificLocationActorsROI : public FCarlaRegionOfInterest @@ -295,7 +295,11 @@ struct CARLATOOLS_API FMiscSpecificLocationActorsROI : public FCarlaRegionOfInte UPROPERTY(BlueprintReadWrite) float MaxRotationRange; - FMiscSpecificLocationActorsROI() : FCarlaRegionOfInterest(), ActorClass(), ActorLocation(0.0f) + FMiscSpecificLocationActorsROI() : FCarlaRegionOfInterest(), + ActorClass(), + ActorLocation(0.0f), + MinRotationRange(0.0f), + MaxRotationRange(0.0f) {} }; diff --git a/Util/BuildTools/BuildOSM2ODR.bat b/Util/BuildTools/BuildOSM2ODR.bat index 63cc613ef..7d2a7df60 100644 --- a/Util/BuildTools/BuildOSM2ODR.bat +++ b/Util/BuildTools/BuildOSM2ODR.bat @@ -20,8 +20,8 @@ set USAGE_STRING=Usage: %FILE_N% [-h^|--help] [--rebuild] [--build] [--clean] [- set REMOVE_INTERMEDIATE=false set BUILD_OSM2ODR=false set GIT_PULL=true -set CURRENT_OSM2ODR_COMMIT=03f2f1de6dcbfde41f2af464829d96b582fc2909 -set OSM2ODR_BRANCH=carla_osm2odr +set CURRENT_OSM2ODR_COMMIT=1835e1e9538d0778971acc8b19b111834aae7261 +set OSM2ODR_BRANCH=aaron/defaultsidewalkwidth set OSM2ODR_REPO=https://github.com/carla-simulator/sumo.git :arg-parse diff --git a/Util/BuildTools/BuildOSM2ODR.sh b/Util/BuildTools/BuildOSM2ODR.sh index c5a6a65ea..61e7d449f 100755 --- a/Util/BuildTools/BuildOSM2ODR.sh +++ b/Util/BuildTools/BuildOSM2ODR.sh @@ -14,8 +14,8 @@ END REMOVE_INTERMEDIATE=false BUILD_OSM2ODR=false GIT_PULL=true -CURRENT_OSM2ODR_COMMIT=03f2f1de6dcbfde41f2af464829d96b582fc2909 -OSM2ODR_BRANCH=carla_osm2odr +CURRENT_OSM2ODR_COMMIT=1835e1e9538d0778971acc8b19b111834aae7261 +OSM2ODR_BRANCH=aaron/defaultsidewalkwidth OSM2ODR_REPO=https://github.com/carla-simulator/sumo.git OPTS=`getopt -o h --long help,rebuild,build,clean,carsim,no-pull -n 'parse-options' -- "$@"` @@ -101,7 +101,7 @@ if ${BUILD_OSM2ODR} ; then -DPROJ_LIBRARY=${CARLA_BUILD_FOLDER}/proj-install/lib/libproj.a \ -DXercesC_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install/include \ -DXercesC_LIBRARY=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install/lib/libxerces-c.a - + ninja osm2odr ninja install @@ -111,7 +111,7 @@ if ${BUILD_OSM2ODR} ; then LLVM_BASENAME=llvm-8.0 LLVM_INCLUDE="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/include/c++/v1" LLVM_LIBPATH="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/lib/Linux/x86_64-unknown-linux-gnu" - + echo $LLVM_INCLUDE echo $LLVM_LIBPATH diff --git a/Util/BuildTools/BuildUE4Plugins.bat b/Util/BuildTools/BuildUE4Plugins.bat new file mode 100755 index 000000000..a3330a0b1 --- /dev/null +++ b/Util/BuildTools/BuildUE4Plugins.bat @@ -0,0 +1,89 @@ +@REM @echo off +setlocal enabledelayedexpansion + +rem Run it through a cmd with the x64 Visual C++ Toolset enabled. + +set LOCAL_PATH=%~dp0 +set FILE_N=-[%~n0]: + +rem Print batch params (debug purpose) +echo %FILE_N% [Batch params]: %* + +rem ============================================================================ +rem -- Parse arguments --------------------------------------------------------- +rem ============================================================================ + +set DOC_STRING=Build LibCarla. +set USAGE_STRING=Usage: %FILE_N% [-h^|--help] [--rebuild] [--build] [--clean] [--no-pull] + +set BUILD_STREETMAP=false +set GIT_PULL=true +set CURRENT_STREETMAP_COMMIT=260273d6b7c3f28988cda31fd33441de7e272958 +set STREETMAP_BRANCH=master +set STREETMAP_REPO=https://github.com/carla-simulator/StreetMap.git + +:arg-parse +if not "%1"=="" ( + if "%1"=="--rebuild" ( + set REMOVE_INTERMEDIATE=true + set BUILD_STREETMAP=true + ) + if "%1"=="--build" ( + set BUILD_STREETMAP=true + ) + if "%1"=="--no-pull" ( + set GIT_PULL=false + ) + if "%1"=="--clean" ( + set REMOVE_INTERMEDIATE=true + ) + if "%1"=="-h" ( + echo %DOC_STRING% + echo %USAGE_STRING% + GOTO :eof + ) + if "%1"=="--help" ( + echo %DOC_STRING% + echo %USAGE_STRING% + GOTO :eof + ) + shift + goto :arg-parse +) + +rem ============================================================================ +rem -- Local Variables --------------------------------------------------------- +rem ============================================================================ + +rem Set the visual studio solution directory +rem +set CARLA_PLUGINS_PATH=%ROOT_PATH:/=\%Unreal\CarlaUE4\Plugins\ +set CARLA_STREETMAP_PLUGINS_PATH=%ROOT_PATH:/=\%Unreal\CarlaUE4\Plugins\StreetMap\ + +rem Build STREETMAP + +if %GIT_PULL% == true ( + if not exist "%CARLA_STREETMAP_PLUGINS_PATH%" git clone -b %STREETMAP_BRANCH% %STREETMAP_REPO% %CARLA_STREETMAP_PLUGINS_PATH% + cd "%CARLA_STREETMAP_PLUGINS_PATH%" + git fetch + git checkout %CURRENT_STREETMAP_COMMIT% +) + + +goto success + +rem ============================================================================ +rem -- Messages and Errors ----------------------------------------------------- +rem ============================================================================ + +:success + if %BUILD_STREETMAP% == true echo %FILE_N% STREETMAP has been successfully installed in "%CARLA_PLUGINS_PATH%"! + goto good_exit + +:good_exit + endlocal + exit /b 0 + +:bad_exit + endlocal + exit /b %errorlevel% diff --git a/Util/BuildTools/BuildUE4Plugins.sh b/Util/BuildTools/BuildUE4Plugins.sh new file mode 100755 index 000000000..4bb412eca --- /dev/null +++ b/Util/BuildTools/BuildUE4Plugins.sh @@ -0,0 +1,90 @@ +#! /bin/bash +DOC_STRING="Download StreetMapUE4 Plugin." + +USAGE_STRING=$(cat <<- END +Usage: $0 [-h|--help] + +commands + + [--clean] Clean intermediate files. + [--rebuild] Clean and rebuild both configurations. +END +) + +REMOVE_INTERMEDIATE=false +BUILD_STREETMAP=false +GIT_PULL=true +CURRENT_STREETMAP_COMMIT=260273d6b7c3f28988cda31fd33441de7e272958 +STREETMAP_BRANCH=master +STREETMAP_REPO=https://github.com/carla-simulator/StreetMap.git + +OPTS=`getopt -o h --long build,rebuild,clean, -n 'parse-options' -- "$@"` + +eval set -- "$OPTS" + +while [[ $# -gt 0 ]]; do + case "$1" in + --rebuild ) + REMOVE_INTERMEDIATE=true; + BUILD_STREETMAP=true; + shift ;; + --build ) + BUILD_STREETMAP=true; + shift ;; + --no-pull ) + GIT_PULL=false + shift ;; + --clean ) + REMOVE_INTERMEDIATE=true; + shift ;; + -h | --help ) + echo "$DOC_STRING" + echo "$USAGE_STRING" + exit 1 + ;; + * ) + shift ;; + esac +done + +source $(dirname "$0")/Environment.sh + +if ! { ${REMOVE_INTERMEDIATE} || ${BUILD_STREETMAP}; }; then + fatal_error "Nothing selected to be done." +fi + +# ============================================================================== +# -- Clean intermediate files -------------------------------------------------- +# ============================================================================== + +if ${REMOVE_INTERMEDIATE} ; then + + log "Cleaning intermediate files and folders." + + UE4_INTERMEDIATE_FOLDERS="Binaries Build Intermediate DerivedDataCache" + + pushd "${CARLAUE4_STREETMAP_FOLDER}" >/dev/null + + rm -Rf ${UE4_INTERMEDIATE_FOLDERS} + + popd >/dev/null + +fi + +# ============================================================================== +# -- Build library ------------------------------------------------------------- +# ============================================================================== + +if ${BUILD_STREETMAP} ; then + log "Downloading STREETMAP plugin." + if ${GIT_PULL} ; then + if [ ! -d ${CARLAUE4_STREETMAP_FOLDER} ] ; then + git clone -b ${STREETMAP_BRANCH} ${STREETMAP_REPO} ${CARLAUE4_STREETMAP_FOLDER} + fi + cd ${CARLAUE4_STREETMAP_FOLDER} + git fetch + git checkout ${CURRENT_STREETMAP_COMMIT} + fi +fi + +log "StreetMap Success!" diff --git a/Util/BuildTools/Linux.mk b/Util/BuildTools/Linux.mk index e8b39e1c9..8e64f7689 100644 --- a/Util/BuildTools/Linux.mk +++ b/Util/BuildTools/Linux.mk @@ -3,7 +3,8 @@ default: help help: @less ${CARLA_BUILD_TOOLS_FOLDER}/Linux.mk.help -launch: LibCarla.server.release osm2odr +launch: LibCarla.server.release osm2odr downloadplugins + @${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --build $(ARGS) @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --build --launch $(ARGS) launch-only: @@ -27,6 +28,7 @@ clean.LibCarla: clean.PythonAPI: @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.sh --clean clean.CarlaUE4Editor: + @${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --clean $(ARGS) @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --clean clean.osm2odr: @${CARLA_BUILD_TOOLS_FOLDER}/BuildOSM2ODR.sh --clean @@ -36,9 +38,11 @@ rebuild: setup @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.sh --rebuild @${CARLA_BUILD_TOOLS_FOLDER}/BuildOSM2ODR.sh --rebuild @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.sh --rebuild $(ARGS) + @${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --rebuild $(ARGS) @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --rebuild $(ARGS) hard-clean: + @${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --clean $(ARGS) @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --hard-clean @${CARLA_BUILD_TOOLS_FOLDER}/BuildOSM2ODR.sh --clean @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.sh --clean @@ -79,7 +83,8 @@ examples: run-examples: @for D in ${CARLA_EXAMPLES_FOLDER}/*; do [ -d "$${D}" ] && make -C $${D} run.only; done -CarlaUE4Editor: LibCarla.server.release osm2odr +CarlaUE4Editor: LibCarla.server.release osm2odr downloadplugins + @${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --build $(ARGS) @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --build $(ARGS) .PHONY: PythonAPI @@ -125,7 +130,7 @@ LibCarla.client.release: setup LibCarla.client.rss: LibCarla.client.rss.debug LibCarla.client.rss.release LibCarla.client.rss.debug: setup ad-rss - @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.sh --client --debug --rss + @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.sh --client --debug --rss LibCarla.client.rss.release: setup ad-rss @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.sh --client --release --rss @@ -133,7 +138,7 @@ LibCarla.client.rss.release: setup ad-rss plugins: @${CARLA_BUILD_TOOLS_FOLDER}/Plugins.sh $(ARGS) -setup: +setup downloadplugins: @${CARLA_BUILD_TOOLS_FOLDER}/Setup.sh $(ARGS) ad-rss: @@ -153,3 +158,6 @@ osm2odr: osmrenderer: @${CARLA_BUILD_TOOLS_FOLDER}/BuildOSMRenderer.sh + +downloadplugins: + @${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --build $(ARGS) diff --git a/Util/BuildTools/Package.sh b/Util/BuildTools/Package.sh index 72e39a4b1..b8fffb76a 100755 --- a/Util/BuildTools/Package.sh +++ b/Util/BuildTools/Package.sh @@ -289,7 +289,7 @@ for PACKAGE_NAME in "${PACKAGES[@]}" ; do if [[ ${PACKAGE_NAME} != "Carla" ]] ; # MAPS_TO_COOK is read into an array as tokens separated by IFS read -ra ADDR <<< "$MAPS_TO_COOK" for i in "${ADDR[@]}"; do # access each element of array - + XODR_FILE_PATH="${CARLAUE4_ROOT_FOLDER}/Content${i:5}" MAP_NAME=${XODR_FILE_PATH##*/} XODR_FILE=$(find "${CARLAUE4_ROOT_FOLDER}/Content" -name "${MAP_NAME}.xodr" -print -quit) @@ -318,13 +318,13 @@ for PACKAGE_NAME in "${PACKAGES[@]}" ; do if [[ ${PACKAGE_NAME} != "Carla" ]] ; done done - rm -Rf "./CarlaUE4/Metadata" - rm -Rf "./CarlaUE4/Plugins" - rm -Rf "./CarlaUE4/Content/${PACKAGE_NAME}/Maps/${PROPS_MAP_NAME}" - rm -f "./CarlaUE4/AssetRegistry.bin" + rm -Rf "./CarlaUE4/Metadata" + rm -Rf "./CarlaUE4/Plugins" + rm -Rf "./CarlaUE4/Content/${PACKAGE_NAME}/Maps/${PROPS_MAP_NAME}" + rm -f "./CarlaUE4/AssetRegistry.bin" if ${DO_TARBALL} ; then - + if ${SINGLE_PACKAGE} ; then tar -rf ${DESTINATION} * else diff --git a/Util/BuildTools/Setup.bat b/Util/BuildTools/Setup.bat index 95b0ccb43..92434a00d 100644 --- a/Util/BuildTools/Setup.bat +++ b/Util/BuildTools/Setup.bat @@ -1,5 +1,5 @@ @echo off -setlocal +setlocal enabledelayedexpansion rem BAT script that downloads and generates rem rpclib, gtest and boost libraries for CARLA (carla.org). @@ -54,7 +54,7 @@ if not "%1"=="" ( if "%1"=="--help" ( goto help ) - + shift goto :arg-parse ) diff --git a/Util/BuildTools/Vars.mk b/Util/BuildTools/Vars.mk index 4b57a1f3e..d544ce53b 100644 --- a/Util/BuildTools/Vars.mk +++ b/Util/BuildTools/Vars.mk @@ -32,3 +32,6 @@ CMAKE_CONFIG_FILE=${CARLA_BUILD_FOLDER}/CMakeLists.txt.in LIBCARLA_TEST_CONTENT_FOLDER=${CARLA_BUILD_FOLDER}/test-content CARLA_EXAMPLES_FOLDER=${CURDIR}/Examples + +CARLAUE4_ADDPLUGINS_FOLDER=${CURDIR}/Unreal/CarlaUE4/Plugins +CARLAUE4_STREETMAP_FOLDER=${CARLAUE4_ADDPLUGINS_FOLDER}/Streetmap diff --git a/Util/BuildTools/Windows.mk b/Util/BuildTools/Windows.mk index 071c63054..1728f1a72 100644 --- a/Util/BuildTools/Windows.mk +++ b/Util/BuildTools/Windows.mk @@ -71,9 +71,10 @@ client: setup LibCarla: setup @"${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --server --client --generator "$(GENERATOR)" -setup: +setup: downloadplugin @"${CARLA_BUILD_TOOLS_FOLDER}/Setup.bat" --boost-toolset msvc-14.2 --generator "$(GENERATOR)" $(ARGS) + .PHONY: Plugins plugins: @"${CARLA_BUILD_TOOLS_FOLDER}/Plugins.bat" $(ARGS) @@ -86,3 +87,6 @@ osm2odr: osmrenderer: @"${CARLA_BUILD_TOOLS_FOLDER}/BuildOSMRenderer.bat" + +downloadplugin: + @"${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.bat" --build $(ARGS)