diff --git a/LibCarla/source/carla/geom/Mesh.cpp b/LibCarla/source/carla/geom/Mesh.cpp index 747c76dd4..b2dfeac85 100644 --- a/LibCarla/source/carla/geom/Mesh.cpp +++ b/LibCarla/source/carla/geom/Mesh.cpp @@ -106,6 +106,10 @@ namespace geom { _uvs.push_back(uv); } + void Mesh::AddUVs(const std::vector & uv) { + std::copy(uv.begin(), uv.end(), std::back_inserter(_uvs)); + } + void Mesh::AddMaterial(const std::string &material_name) { const size_t open_index = _indexes.size(); if (!_materials.empty()) { @@ -306,7 +310,6 @@ namespace geom { rhs.GetNormals().end()); const size_t vertex_to_start_concating = v_num - num_vertices_to_link; - for( size_t i = 1; i < num_vertices_to_link; ++i ) { _indexes.push_back( vertex_to_start_concating + i ); _indexes.push_back( vertex_to_start_concating + i + 1 ); diff --git a/LibCarla/source/carla/geom/Mesh.h b/LibCarla/source/carla/geom/Mesh.h index 326b0d561..581f4e589 100644 --- a/LibCarla/source/carla/geom/Mesh.h +++ b/LibCarla/source/carla/geom/Mesh.h @@ -95,6 +95,9 @@ namespace geom { /// Appends a vertex to the vertices list, they will be read 3 in 3. void AddUV(uv_type uv); + /// Appends uvs. + void AddUVs(const std::vector & uv); + /// Starts applying a new material to the new added triangles. void AddMaterial(const std::string &material_name); @@ -212,6 +215,12 @@ namespace geom { } } + for (const auto uv : GetUVs()) + { + // From meters to centimeters + Mesh.UV0.Add(FVector2D{uv.x, uv.y}); + } + return Mesh; } diff --git a/LibCarla/source/carla/geom/Simplification.cpp b/LibCarla/source/carla/geom/Simplification.cpp new file mode 100644 index 000000000..c18b5f528 --- /dev/null +++ b/LibCarla/source/carla/geom/Simplification.cpp @@ -0,0 +1,56 @@ +// Copyright (c) 2017 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 "carla/geom/Simplification.h" +#include "simplify/Simplify.h" + +namespace carla { +namespace geom { + + void Simplification::Simplificate(const std::unique_ptr& pmesh){ + Simplify::SimplificationObject Simplification; + for (carla::geom::Vector3D& current_vertex : pmesh->GetVertices()) { + Simplify::Vertex v; + v.p.x = current_vertex.x; + v.p.y = current_vertex.y; + v.p.z = current_vertex.z; + Simplification.vertices.push_back(v); + } + + for (size_t i = 0; i < pmesh->GetIndexes().size() - 2; i += 3) { + Simplify::Triangle t; + t.material = 0; + auto indices = pmesh->GetIndexes(); + t.v[0] = (indices[i]) - 1; + t.v[1] = (indices[i + 1]) - 1; + t.v[2] = (indices[i + 2]) - 1; + Simplification.triangles.push_back(t); + } + + // Reduce to the X% of the polys + float target_size = Simplification.triangles.size(); + Simplification.simplify_mesh((target_size * simplification_percentage)); + + pmesh->GetVertices().clear(); + pmesh->GetIndexes().clear(); + + for (Simplify::Vertex& current_vertex : Simplification.vertices) { + carla::geom::Vector3D v; + v.x = current_vertex.p.x; + v.y = current_vertex.p.y; + v.z = current_vertex.p.z; + pmesh->AddVertex(v); + } + + for (size_t i = 0; i < Simplification.triangles.size(); ++i) { + pmesh->GetIndexes().push_back((Simplification.triangles[i].v[0]) + 1); + pmesh->GetIndexes().push_back((Simplification.triangles[i].v[1]) + 1); + pmesh->GetIndexes().push_back((Simplification.triangles[i].v[2]) + 1); + } + } + +} // namespace geom +} // namespace carla diff --git a/LibCarla/source/carla/geom/Simplification.h b/LibCarla/source/carla/geom/Simplification.h new file mode 100644 index 000000000..2667bbac7 --- /dev/null +++ b/LibCarla/source/carla/geom/Simplification.h @@ -0,0 +1,29 @@ +// Copyright (c) 2017 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 "carla/geom/Mesh.h" + +namespace carla { +namespace geom { + + class Simplification { + public: + + Simplification() = default; + + Simplification(float simplificationrate) + : simplification_percentage(simplificationrate) + {} + + float simplification_percentage; + + void Simplificate(const std::unique_ptr& pmesh); + }; + +} // namespace geom +} // namespace carla diff --git a/LibCarla/source/carla/road/Deformation.h b/LibCarla/source/carla/road/Deformation.h index 791271fad..1df6ca79f 100644 --- a/LibCarla/source/carla/road/Deformation.h +++ b/LibCarla/source/carla/road/Deformation.h @@ -20,10 +20,10 @@ namespace geom { namespace deformation { inline float GetZPosInDeformation(float posx, float posy){ // Amplitud - const float A1 = 0.3f; - const float A2 = 0.5f; + const float A1 = 0.6f; + const float A2 = 1.1f; // Fases - const float F1 = 100.0; + const float F1 = 1000.0; const float F2 = -1500.0; // Modifiers const float Kx1 = 0.035f; @@ -37,23 +37,23 @@ namespace deformation { } inline float GetBumpDeformation(float posx, float posy){ - const float A3 = 0.15f; + const float A3 = 0.10f; float bumpsoffset = 0; - const float constraintX = 15.0f; - const float constraintY = 15.0f; + float constraintX = 17.0f; + float constraintY = 12.0f; - float BumpX = std::round(posx / constraintX); - float BumpY = std::round(posy / constraintX); + float BumpX = std::ceil(posx / constraintX); + float BumpY = std::floor(posy / constraintY); BumpX *= constraintX; BumpY *= constraintY; float DistanceToBumpOrigin = sqrt(pow(BumpX - posx, 2) + pow(BumpY - posy, 2) ); - float MaxDistance = 2; + float MaxDistance = 2.0; if (DistanceToBumpOrigin <= MaxDistance) { - bumpsoffset = abs((1.0f / MaxDistance) * DistanceToBumpOrigin * DistanceToBumpOrigin - MaxDistance); + bumpsoffset = sin(DistanceToBumpOrigin); } return A3 * bumpsoffset; diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index 35d85b66d..549f5dda2 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -20,7 +20,6 @@ #include "carla/road/element/RoadInfoSpeed.h" #include "carla/road/element/RoadInfoSignal.h" -#include "simplify/Simplify.h" #include "marchingcube/MeshReconstruction.h" #include @@ -1145,9 +1144,8 @@ namespace road { std::thread juntction_thread( &Map::GenerateJunctions, this, mesh_factory, params, &junction_out_mesh_list); - float simplificationrate = params.simplification_percentage * 0.01f; size_t num_roads = _data.GetRoads().size(); - size_t num_roads_per_thread = 100; + size_t num_roads_per_thread = 20; size_t num_threads = (num_roads / num_roads_per_thread) + 1; num_threads = num_threads > 1 ? num_threads : 1; std::vector workers; @@ -1176,29 +1174,6 @@ namespace road { workers[i].join(); } workers.clear(); - for (auto& current_mesh_vector : road_out_mesh_list) { - if (current_mesh_vector.first != road::Lane::LaneType::Driving) { - continue; - } - - size_t num_roads = current_mesh_vector.second.size(); - size_t num_roads_per_thread = 15; - size_t num_threads = (num_roads / num_roads_per_thread) + 1; - num_threads = num_threads > 1 ? num_threads : 1; - - for (size_t i = 0; i < num_threads; ++i) { - std::vector RoadsMesh; - for (std::unique_ptr& current_mesh : current_mesh_vector.second) { - if(current_mesh) { - RoadsMesh.push_back( current_mesh.get() ); - } - } - - std::thread neworker( &Map::DeformateRoadsMultithreaded, this, RoadsMesh, i, num_roads_per_thread, simplificationrate ); - workers.push_back( std::move(neworker) ); - } - } - for (size_t i = 0; i < workers.size(); ++i) { if (workers[i].joinable()) { workers[i].join(); @@ -1221,6 +1196,45 @@ namespace road { return road_out_mesh_list; } + std::vector> Map::GetTreesTransform( + float distancebetweentrees, + float distancefromdrivinglineborder, + float s_offset) const { + + std::vector> transforms; + for (auto &&pair : _data.GetRoads()) { + const auto &road = pair.second; + if (!road.IsJunction()) { + for (auto &&lane_section : road.GetLaneSections()) { + LaneId min_lane = 0; + for (auto &pairlane : lane_section.GetLanes()) { + if (min_lane > pairlane.first && pairlane.second.GetType() == Lane::LaneType::Driving) { + 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())); + s_current += distancebetweentrees; + } + + } + } + } + } + return transforms; + } geom::Mesh Map::GetAllCrosswalkMesh() const { geom::Mesh out_mesh; @@ -1259,7 +1273,9 @@ namespace road { } /// Buids a list of meshes related with LineMarkings - std::vector> Map::GenerateLineMarkings( const rpc::OpendriveGenerationParameters& params ) const + std::vector> Map::GenerateLineMarkings( + const rpc::OpendriveGenerationParameters& params, + std::vector& outinfo ) const { std::vector> LineMarks; geom::MeshFactory mesh_factory(params); @@ -1267,18 +1283,9 @@ namespace road { if ( pair.second.IsJunction() ) { continue; } - mesh_factory.GenerateLaneMarkForRoad(pair.second, LineMarks); + mesh_factory.GenerateLaneMarkForRoad(pair.second, LineMarks, outinfo); } - for (auto& Mesh : LineMarks) { - if (!Mesh->IsValid()) { - continue; - } - - for (carla::geom::Vector3D& current_vertex : Mesh->GetVertices()) { - current_vertex.z = GetZPosInDeformation(current_vertex.x, current_vertex.y) + 0.01; - } - } return std::move(LineMarks); } @@ -1294,39 +1301,6 @@ namespace road { return returning; } - std::vector> Map::GetTreesPosition( - float distancebetweentrees, - float distancefromdrivinglineborder) const { - - std::vector> positions; - for (auto &&pair : _data.GetRoads()) { - const auto &road = pair.second; - if (!road.IsJunction()) { - for (auto &&lane_section : road.GetLaneSections()) { - const auto min_lane = lane_section.GetLanes().begin()->first == 0 ? - 1 : lane_section.GetLanes().begin()->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(); - 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; - const carla::road::element::RoadInfoSpeed* roadinfo = lane->GetInfo(s_current); - positions.push_back(std::make_pair(treeposition,roadinfo->GetType())); - s_current += distancebetweentrees; - } - - } - } - } - } - return positions; - } - inline float Map::GetZPosInDeformation(float posx, float posy) const { return geom::deformation::GetZPosInDeformation(posx, posy) + geom::deformation::GetBumpDeformation(posx,posy); @@ -1359,7 +1333,6 @@ namespace road { std::map>>* junction_out_mesh_list) const { - float simplificationrate = params.simplification_percentage * 0.01f; for (const auto& junc_pair : _data.GetJunctions()) { const auto& junction = junc_pair.second; if (junction.GetConnections().size() > 2) { @@ -1368,46 +1341,6 @@ namespace road { std::vector perimeterpoints; auto pmesh = SDFToMesh(junction, perimeterpoints, 75); - Simplify::SimplificationObject Simplification; - for (carla::geom::Vector3D& current_vertex : pmesh->GetVertices()) { - Simplify::Vertex v; - v.p.x = current_vertex.x; - v.p.y = current_vertex.y; - v.p.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); - Simplification.vertices.push_back(v); - } - - for (size_t i = 0; i < pmesh->GetIndexes().size() - 2; i += 3) { - Simplify::Triangle t; - t.material = 0; - auto indices = pmesh->GetIndexes(); - t.v[0] = (indices[i]) - 1; - t.v[1] = (indices[i + 1]) - 1; - t.v[2] = (indices[i + 2]) - 1; - Simplification.triangles.push_back(t); - } - - // Reduce to the X% of the polys - float target_size = Simplification.triangles.size(); - Simplification.simplify_mesh((target_size * simplificationrate)); - - pmesh->GetVertices().clear(); - pmesh->GetIndexes().clear(); - - for (Simplify::Vertex& current_vertex : Simplification.vertices) { - carla::geom::Vector3D v; - v.x = current_vertex.p.x; - v.y = current_vertex.p.y; - v.z = current_vertex.p.z; - pmesh->AddVertex(v); - } - - for (size_t i = 0; i < Simplification.triangles.size(); ++i) { - pmesh->GetIndexes().push_back((Simplification.triangles[i].v[0]) + 1); - pmesh->GetIndexes().push_back((Simplification.triangles[i].v[1]) + 1); - pmesh->GetIndexes().push_back((Simplification.triangles[i].v[2]) + 1); - } - (*junction_out_mesh_list)[road::Lane::LaneType::Driving].push_back(std::move(pmesh)); for (const auto& connection_pair : junction.GetConnections()) { @@ -1449,9 +1382,6 @@ namespace road { for (auto& lane : lane_meshes) { *merged_mesh += *lane; } - for (carla::geom::Vector3D& current_vertex : merged_mesh->GetVertices()) { - current_vertex.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); - } std::unique_ptr sidewalk_mesh = std::make_unique(); for (auto& lane : sidewalk_lane_meshes) { *sidewalk_mesh += *lane; @@ -1463,72 +1393,12 @@ namespace road { } } - void Map::DeformateRoadsMultithreaded(const std::vector& roadsmesh, - const size_t index, const size_t number_of_roads_per_thread, const float simplificationrate) const - { - auto start = std::next( roadsmesh.begin(), ( index ) * number_of_roads_per_thread); - size_t endoffset = (index+1) * number_of_roads_per_thread; - if( endoffset >= roadsmesh.size() ) { - endoffset = roadsmesh.size(); - } - auto end = std::next( roadsmesh.begin(), endoffset ); - for ( auto it = start; it != end && it != roadsmesh.end(); ++it ) { - geom::Mesh* current_mesh = *it; - if( current_mesh == nullptr ) { - continue; - } - - if ( !current_mesh->IsValid() ) { - continue; - } - Simplify::SimplificationObject Simplification; - for (carla::geom::Vector3D& current_vertex : current_mesh->GetVertices()) { - Simplify::Vertex v; - v.p.x = current_vertex.x; - v.p.y = current_vertex.y; - v.p.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); - Simplification.vertices.push_back(v); - } - - - for (size_t i = 0; i < current_mesh->GetIndexes().size() - 2; i += 3) { - Simplify::Triangle t; - t.material = 0; - auto indices = current_mesh->GetIndexes(); - t.v[0] = (indices[i]) - 1; - t.v[1] = (indices[i + 1]) - 1; - t.v[2] = (indices[i + 2]) - 1; - Simplification.triangles.push_back(t); - } - - // Reduce to the X% of the polys - float target_size = Simplification.triangles.size(); - Simplification.simplify_mesh((target_size * simplificationrate)); - - current_mesh->GetVertices().clear(); - current_mesh->GetIndexes().clear(); - for (Simplify::Vertex& current_vertex : Simplification.vertices) { - carla::geom::Vector3D v; - v.x = current_vertex.p.x; - v.y = current_vertex.p.y; - v.z = current_vertex.p.z; - current_mesh->AddVertex(v); - } - - for (size_t i = 0; i < Simplification.triangles.size(); ++i) { - current_mesh->GetIndexes().push_back((Simplification.triangles[i].v[0]) + 1); - current_mesh->GetIndexes().push_back((Simplification.triangles[i].v[1]) + 1); - current_mesh->GetIndexes().push_back((Simplification.triangles[i].v[2]) + 1); - } - } - } - std::unique_ptr Map::SDFToMesh(const road::Junction& jinput, const std::vector& sdfinput, int grid_cells_per_dim) const { int junctionid = jinput.GetId(); - float box_extraextension_factor = 1.5f; + float box_extraextension_factor = 1.2f; const double CubeSize = 0.5; carla::geom::BoundingBox bb = jinput.GetBoundingBox(); carla::geom::Vector3D MinOffset = bb.location - geom::Location(bb.extent * box_extraextension_factor); @@ -1539,7 +1409,6 @@ namespace road { { geom::Vector3D worldloc(pos.x, pos.y, pos.z); boost::optional CheckingWaypoint = GetWaypoint(geom::Location(worldloc), 0x1 << 1); - if (CheckingWaypoint) { if ( pos.z < 0.2) { return 0.0; @@ -1547,7 +1416,6 @@ namespace road { return -abs(pos.z); } } - boost::optional InRoadWaypoint = GetClosestWaypointOnRoad(geom::Location(worldloc), 0x1 << 1); geom::Transform InRoadWPTransform = ComputeTransform(*InRoadWaypoint); diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index a868e15cc..681968c34 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -166,14 +166,17 @@ namespace road { /// Buids a mesh of all crosswalks based on the OpenDRIVE geom::Mesh GetAllCrosswalkMesh() const; - std::vector> GetTreesPosition( + std::vector> GetTreesTransform( float distancebetweentrees, - float distancefromdrivinglineborder) const; + float distancefromdrivinglineborder, + float s_offset = 0) const; geom::Mesh GenerateWalls(const double distance, const float wall_height) const; /// Buids a list of meshes related with LineMarkings - std::vector> GenerateLineMarkings( const rpc::OpendriveGenerationParameters& params ) const; + std::vector> GenerateLineMarkings( + const rpc::OpendriveGenerationParameters& params, + std::vector& outinfo ) const; const std::unordered_map>& GetSignals() const { return _data.GetSignals(); @@ -227,9 +230,6 @@ public: std::map>>* juntion_out_mesh_list) const; - void DeformateRoadsMultithreaded(const std::vector& roadsmesh, const size_t index, - const size_t number_of_roads_per_thread, const float simplificationrate) const; - std::unique_ptr SDFToMesh(const road::Junction& jinput, const std::vector& sdfinput, int grid_cells_per_dim) const; }; diff --git a/LibCarla/source/carla/road/MeshFactory.cpp b/LibCarla/source/carla/road/MeshFactory.cpp index ce753d287..43eef0617 100644 --- a/LibCarla/source/carla/road/MeshFactory.cpp +++ b/LibCarla/source/carla/road/MeshFactory.cpp @@ -128,17 +128,23 @@ namespace geom { const int vertices_in_width = road_param.vertex_width_resolution >= 2 ? road_param.vertex_width_resolution : 2; const int segments_number = vertices_in_width - 1; + std::vector uvs; + int uvx = 0; + int uvy = 0; // Iterate over the lane's 's' and store the vertices based on it's width do { // Get the location of the edges of the current lane at the current waypoint std::pair edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); const geom::Vector3D segments_size = ( edges.second - edges.first ) / segments_number; geom::Vector3D current_vertex = edges.first; - + uvx = 0; for (int i = 0; i < vertices_in_width; ++i) { + uvs.push_back(geom::Vector2D(uvx, uvy)); vertices.push_back(current_vertex); current_vertex = current_vertex + segments_size; + uvx++; } + uvy++; // Update the current waypoint's "s" s_current += road_param.resolution; } while (s_current < s_end); @@ -151,14 +157,17 @@ namespace geom { lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width); const geom::Vector3D segments_size = (edges.second - edges.first) / segments_number; geom::Vector3D current_vertex = edges.first; - + uvx = 0; for (int i = 0; i < vertices_in_width; ++i) { + uvs.push_back(geom::Vector2D(uvx, uvy)); vertices.push_back(current_vertex); current_vertex = current_vertex + segments_size; + uvx++; } } out_mesh.AddVertices(vertices); + out_mesh.AddUVs(uvs); // Add the adient material, create the strip and close the material out_mesh.AddMaterial( @@ -194,7 +203,6 @@ namespace geom { redirections.push_back(lane_pair.first); it = std::find(redirections.begin(), redirections.end(), lane_pair.first); } - size_t PosToAdd = it - redirections.begin(); Mesh out_mesh; @@ -211,7 +219,7 @@ namespace geom { if(lane_pair.second.GetType() == road::Lane::LaneType::Driving) { verticesinwidth = vertices_in_width; }else if(lane_pair.second.GetType() == road::Lane::LaneType::Sidewalk){ - verticesinwidth = 4; + verticesinwidth = 6; }else{ verticesinwidth = 2; } @@ -253,25 +261,40 @@ namespace geom { std::vector vertices; // Ensure minimum vertices in width are two - const int vertices_in_width = 4; + const int vertices_in_width = 6; const int segments_number = vertices_in_width - 1; + std::vector uvs; + int uvy = 0; // Iterate over the lane's 's' and store the vertices based on it's width do { // Get the location of the edges of the current lane at the current waypoint std::pair edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); - edges.first.z += deformation::GetZPosInDeformation(edges.first.x, edges.first.y); - edges.second.z += deformation::GetZPosInDeformation(edges.second.x, edges.second.y); + geom::Vector3D low_vertex_first = edges.first - geom::Vector3D(0,0,1); geom::Vector3D low_vertex_second = edges.second - geom::Vector3D(0,0,1); vertices.push_back(low_vertex_first); + uvs.push_back(geom::Vector2D(0, uvy)); + vertices.push_back(edges.first); + uvs.push_back(geom::Vector2D(1, uvy)); + + vertices.push_back(edges.first); + uvs.push_back(geom::Vector2D(1, uvy)); + vertices.push_back(edges.second); + uvs.push_back(geom::Vector2D(2, uvy)); + + vertices.push_back(edges.second); + uvs.push_back(geom::Vector2D(2, uvy)); + vertices.push_back(low_vertex_second); + uvs.push_back(geom::Vector2D(3, uvy)); // Update the current waypoint's "s" s_current += road_param.resolution; + uvy++; } while (s_current < s_end); // This ensures the mesh is constant and have no gaps between roads, @@ -281,17 +304,31 @@ namespace geom { std::pair edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width); - edges.first.z += deformation::GetZPosInDeformation(edges.first.x, edges.first.y); - edges.second.z += deformation::GetZPosInDeformation(edges.second.x, edges.second.y); geom::Vector3D low_vertex_first = edges.first - geom::Vector3D(0,0,1); geom::Vector3D low_vertex_second = edges.second - geom::Vector3D(0,0,1); - vertices.push_back(low_vertex_first); - vertices.push_back(edges.first); - vertices.push_back(edges.second); - vertices.push_back(low_vertex_second); - } - out_mesh.AddVertices(vertices); + vertices.push_back(low_vertex_first); + uvs.push_back(geom::Vector2D(0, uvy)); + + vertices.push_back(edges.first); + uvs.push_back(geom::Vector2D(1, uvy)); + + vertices.push_back(edges.first); + uvs.push_back(geom::Vector2D(1, uvy)); + + vertices.push_back(edges.second); + uvs.push_back(geom::Vector2D(2, uvy)); + + vertices.push_back(edges.second); + uvs.push_back(geom::Vector2D(2, uvy)); + + vertices.push_back(low_vertex_second); + uvs.push_back(geom::Vector2D(3, uvy)); + + } + + out_mesh.AddVertices(vertices); + out_mesh.AddUVs(uvs); // Add the adient material, create the strip and close the material out_mesh.AddMaterial( lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road"); @@ -300,6 +337,11 @@ namespace geom { for (size_t i = 0; i < (number_of_rows - 1); ++i) { for (size_t j = 0; j < vertices_in_width - 1; ++j) { + + if(j == 1 || j == 3){ + continue; + } + out_mesh.AddIndex( j + ( i * vertices_in_width ) + 1); out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1); out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1); @@ -307,6 +349,7 @@ namespace geom { out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1); out_mesh.AddIndex( ( j + 1 ) + ( ( i + 1 ) * vertices_in_width ) + 1); out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1); + } } out_mesh.EndMaterial(); @@ -525,7 +568,7 @@ std::map>> MeshFactory: if(lane_pair.second.GetType() == road::Lane::LaneType::Driving) { verticesinwidth = vertices_in_width; }else if(lane_pair.second.GetType() == road::Lane::LaneType::Sidewalk){ - verticesinwidth = 4; + verticesinwidth = 6; }else{ verticesinwidth = 2; } @@ -558,11 +601,11 @@ std::map>> MeshFactory: if(lane_pair.second.GetType() == road::Lane::LaneType::Driving) { verticesinwidth = vertices_in_width; }else if(lane_pair.second.GetType() == road::Lane::LaneType::Sidewalk){ - verticesinwidth = 4; + verticesinwidth = 6; }else{ verticesinwidth = 2; } - (mesh_uptr_list[lane_pair.second.GetType()][PosToAdd])->ConcatMesh(lane_section_mesh, verticesinwidth); + *(mesh_uptr_list[lane_pair.second.GetType()][PosToAdd]) += lane_section_mesh; } } } @@ -675,16 +718,22 @@ std::map>> MeshFactory: } void MeshFactory::GenerateLaneMarkForRoad( - const road::Road& road, std::vector>& inout) const + const road::Road& road, + std::vector>& inout, + std::vector& outinfo ) const { for (auto&& lane_section : road.GetLaneSections()) { for (auto&& lane : lane_section.GetLanes()) { if (lane.first != 0) { if(lane.second.GetType() == road::Lane::LaneType::Driving ){ - GenerateLaneMarksForNotCenterLine(lane_section, lane.second, inout); + GenerateLaneMarksForNotCenterLine(lane_section, lane.second, inout, outinfo); + outinfo.push_back("white"); } } else { - GenerateLaneMarksForCenterLine(road, lane_section, lane.second, inout); + if(lane.second.GetType() == road::Lane::LaneType::None ){ + GenerateLaneMarksForCenterLine(road, lane_section, lane.second, inout, outinfo); + outinfo.push_back("yellow"); + } } } } @@ -693,8 +742,8 @@ std::map>> MeshFactory: void MeshFactory::GenerateLaneMarksForNotCenterLine( const road::LaneSection& lane_section, const road::Lane& lane, - std::vector>& inout) const { - + std::vector>& inout, + std::vector& outinfo ) const { Mesh out_mesh; const double s_start = lane_section.GetDistance(); const double s_end = lane_section.GetDistance() + lane_section.GetLength(); @@ -831,7 +880,8 @@ std::map>> MeshFactory: const road::Road& road, const road::LaneSection& lane_section, const road::Lane& lane, - std::vector>& inout) const + std::vector>& inout, + std::vector& outinfo ) const { Mesh out_mesh; const double s_start = lane_section.GetDistance(); @@ -970,6 +1020,7 @@ std::map>> MeshFactory: out_mesh.AddVertex(rightpoint.location); out_mesh.AddVertex(leftpoint.location); + } inout.push_back(std::make_unique(out_mesh)); } diff --git a/LibCarla/source/carla/road/MeshFactory.h b/LibCarla/source/carla/road/MeshFactory.h index 6cd9bb101..0334d01d4 100644 --- a/LibCarla/source/carla/road/MeshFactory.h +++ b/LibCarla/source/carla/road/MeshFactory.h @@ -111,21 +111,24 @@ namespace geom { std::unique_ptr MergeAndSmooth(std::vector> &lane_meshes) const; // -- LaneMarks -- - - void GenerateLaneMarkForRoad(const road::Road& road, std::vector>& inout) const; + void GenerateLaneMarkForRoad(const road::Road& road, + std::vector>& inout, + std::vector& outinfo ) const; // Generate for NOT center line AKA All lines but the one which id 0 void GenerateLaneMarksForNotCenterLine( const road::LaneSection& lane_section, const road::Lane& lane, - std::vector>& inout) const; + std::vector>& inout, + std::vector& outinfo ) const; // Generate marks ONLY for line with ID 0 void GenerateLaneMarksForCenterLine( const road::Road& road, const road::LaneSection& lane_section, const road::Lane& lane, - std::vector>& inout) const; + std::vector>& inout, + std::vector& outinfo ) const; // ========================================================================= // -- Generation parameters ------------------------------------------------ // ========================================================================= diff --git a/LibCarla/source/carla/road/element/LaneMarking.h b/LibCarla/source/carla/road/element/LaneMarking.h index 997dad4e0..feb865fde 100644 --- a/LibCarla/source/carla/road/element/LaneMarking.h +++ b/LibCarla/source/carla/road/element/LaneMarking.h @@ -7,6 +7,7 @@ #pragma once #include +#include namespace carla { namespace road { @@ -62,6 +63,21 @@ namespace element { LaneChange lane_change = LaneChange::None; double width = 0.0; + + std::string GetColorInfoAsString(){ + switch(color){ + case Color::Yellow: + return std::string("yellow"); + break; + case Color::Standard: + return std::string("white"); + break; + default: + return std::string("white"); + break; + } + return std::string("white"); + } }; } // namespace element diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h index 570846ec4..4a889c980 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h @@ -145,9 +145,9 @@ public: FTransform LocalToGlobalTransform(const FTransform& InTransform) const; FVector LocalToGlobalLocation(const FVector& InLocation) const; - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") FString LargeMapTilePath = "/Game/Carla/Maps/testmap"; - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") FString LargeMapName = "testmap"; void SetTile0Offset(const FVector& Offset); @@ -285,32 +285,32 @@ protected: FDVector CurrentOriginD; - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") FVector Tile0Offset = FVector(0,0,0); - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") float TickInterval = 0.0f; - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") float LayerStreamingDistance = 3.0f * 1000.0f * 100.0f; - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") float ActorStreamingDistance = 2.0f * 1000.0f * 100.0f; - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") float RebaseOriginDistance = 2.0f * 1000.0f * 100.0f; float LayerStreamingDistanceSquared = LayerStreamingDistance * LayerStreamingDistance; float ActorStreamingDistanceSquared = ActorStreamingDistance * ActorStreamingDistance; float RebaseOriginDistanceSquared = RebaseOriginDistance * RebaseOriginDistance; - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") float TileSide = 2.0f * 1000.0f * 100.0f; // 2km - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") FVector LocalTileOffset = FVector(0,0,0); - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") bool ShouldTilesBlockOnLoad = false; @@ -324,7 +324,7 @@ protected: void PrintMapInfo(); - UPROPERTY(EditAnywhere, Category = "Large Map Manager") + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Large Map Manager") FString AssetsPath = ""; FString BaseTileMapPath = "/Game/Carla/Maps/LargeMap/EmptyTileBase"; diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/MapGenerator/Materials/MI_HeightMapNoiseMat.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/MapGenerator/Materials/MI_HeightMapNoiseMat.uasset index 0a77603e7..69b3787ff 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/MapGenerator/Materials/MI_HeightMapNoiseMat.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/MapGenerator/Materials/MI_HeightMapNoiseMat.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_InstancedMesh.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_InstancedMesh.uasset new file mode 100644 index 000000000..5deb1b4e8 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_InstancedMesh.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_OpenDriveToMap.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/BP_OpenDriveToMap.uasset index e7452710e..b4fdfca20 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/BuildingStyleHolder.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/Blueprints/BuildingStyleHolder.uasset new file mode 100644 index 000000000..8a83e1967 Binary files /dev/null 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 new file mode 100644 index 000000000..fe643a78a Binary files /dev/null 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 189b78916..b4876f094 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 new file mode 100644 index 000000000..dd6086ea2 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/LevelCreator.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/MapPreview/W_MapPreview.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/MapPreview/W_MapPreview.uasset index aa2e38dfc..3014c4eed 100644 Binary files a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/MapPreview/W_MapPreview.uasset and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/OnroadMapGenerator/MapPreview/W_MapPreview.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 556c48e3f..ebdaf3219 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 c411a694d..1db1b85fa 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/Source/CarlaTools/Private/HoudiniImporterWidget.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/HoudiniImporterWidget.cpp index 8e5e7a6bd..155bf6305 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/HoudiniImporterWidget.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/HoudiniImporterWidget.cpp @@ -17,11 +17,14 @@ void UHoudiniImporterWidget::CreateSubLevels(ALargeMapManager* LargeMapManager) } -void UHoudiniImporterWidget::MoveActorsToSubLevel(TArray Actors, ALargeMapManager* LargeMapManager) +void UHoudiniImporterWidget::MoveActorsToSubLevelWithLargeMap(TArray Actors, ALargeMapManager* LargeMapManager) { TMap> ActorsToMove; for (AActor* Actor : Actors) { + if (Actor == nullptr) { + continue; + } UHierarchicalInstancedStaticMeshComponent* Component = Cast( Actor->GetComponentByClass( @@ -77,7 +80,6 @@ void UHoudiniImporterWidget::MoveActorsToSubLevel(TArray Actors, ALarge ULevelStreaming* Level = UEditorLevelUtils::AddLevelToWorld( World, *Tile->Name, ULevelStreamingDynamic::StaticClass(), FTransform()); - int MovedActors = UEditorLevelUtils::MoveActorsToLevel(ActorList, Level, false, false); // StreamingLevel->SetShouldBeLoaded(false); UE_LOG(LogCarlaTools, Log, TEXT("Moved %d actors"), MovedActors); @@ -86,6 +88,15 @@ void UHoudiniImporterWidget::MoveActorsToSubLevel(TArray Actors, ALarge } } +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()); +} + void UHoudiniImporterWidget::UpdateGenericActorCoordinates( AActor* Actor, FVector TileOrigin) { diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp index 1b09a839c..eb1d8304e 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/MapPreviewUserWidget.cpp @@ -8,7 +8,6 @@ #endif #include "GenericPlatform/GenericPlatformMath.h" -//#include "GenericPlatform/GenericPlatformProcess.h" #include "GenericPlatform/GenericPlatformFile.h" #include "Sockets.h" @@ -33,10 +32,12 @@ #include "Misc/Paths.h" - - - -//#include "Unix/UnixPlatformProcess.h" +namespace Asio = boost::asio; +using AsioStreamBuf = boost::asio::streambuf; +using AsioTCP = boost::asio::ip::tcp; +using AsioSocket = boost::asio::ip::tcp::socket; +using AsioAcceptor = boost::asio::ip::tcp::acceptor; +using AsioEndpoint = boost::asio::ip::tcp::endpoint; void UMapPreviewUserWidget::CreateTexture() { @@ -49,17 +50,10 @@ void UMapPreviewUserWidget::CreateTexture() void UMapPreviewUserWidget::ConnectToSocket(FString DatabasePath, FString StylesheetPath, int Size) { - //FSocket* Socket = FSocket::CreateTCPConnection(nullptr, TEXT("OSMRendererSocket")); - Socket = FTcpSocketBuilder(TEXT("OSMRendererSocket")).AsReusable(); - FIPv4Address RemoteAddress; - FIPv4Address::Parse(FIPv4Address::InternalLoopback.ToString(), RemoteAddress); - TSharedRef RemoteAddr = ISocketSubsystem::Get(PLATFORM_SOCKETSUBSYSTEM)->CreateInternetAddr(); - RemoteAddr->SetIp(RemoteAddress.Value); - RemoteAddr->SetPort(5000); - - // Connect to the remote server - bool Connected = Socket->Connect(*RemoteAddr); - if (!Connected) + const unsigned int PORT = 5000; + SocketPtr = std::make_unique(io_service); + SocketPtr->connect(AsioEndpoint(AsioTCP::v4(), PORT)); + if(!SocketPtr->is_open()) { UE_LOG(LogTemp, Error, TEXT("Error connecting to remote server")); return; @@ -73,33 +67,29 @@ void UMapPreviewUserWidget::ConnectToSocket(FString DatabasePath, FString Styles void UMapPreviewUserWidget::RenderMap(FString Latitude, FString Longitude, FString Zoom) { - FString Message = "-R " + Latitude + " " + Longitude + " " + Zoom; - //FString Message = "-R 40.415 -3.702 100000"; + FString Message = "-R " + Latitude + " " + Longitude + " " + Zoom; SendStr(Message); TArray ReceivedData; - uint32 ReceivedDataSize; + uint32 ReceivedDataSize = 0; - if(Socket->Wait(ESocketWaitConditions::WaitForRead, FTimespan::FromSeconds(5))) { - while(Socket->HasPendingData(ReceivedDataSize)) + SocketPtr->wait(boost::asio::ip::tcp::socket::wait_read); + while (SocketPtr->available()) { - int32 BytesReceived = 0; + AsioStreamBuf Buffer; + std::size_t BytesReceived = + Asio::read(*SocketPtr, Buffer, Asio::transfer_at_least(2)); TArray ThisReceivedData; - ThisReceivedData.Init(0, FMath::Min(ReceivedDataSize, uint32(512*512*4))); - bool bRecv = Socket->Recv(ThisReceivedData.GetData(), ThisReceivedData.Num(), BytesReceived); - if (!bRecv) + const char* DataPtr = Asio::buffer_cast(Buffer.data()); + for (std::size_t i = 0; i < Buffer.size(); ++i) { - UE_LOG(LogTemp, Error, TEXT("Error receiving message")); - } - else - { - UE_LOG(LogTemp, Log, TEXT("Received %d bytes. %d"), BytesReceived, ReceivedDataSize); - ReceivedData.Append(ThisReceivedData); - UE_LOG(LogTemp, Log, TEXT("Size of Data: %d"), ReceivedData.Num()); + ThisReceivedData.Add(DataPtr[i]); } + ReceivedData.Append(ThisReceivedData); } - + UE_LOG(LogTemp, Log, TEXT("Size of Data: %d"), ReceivedData.Num()); + // TODO: Move to function if(ReceivedData.Num() > 0) { @@ -127,44 +117,26 @@ void UMapPreviewUserWidget::RenderMap(FString Latitude, FString Longitude, FStri FString UMapPreviewUserWidget::RecvCornersLatLonCoords() { SendStr("-L"); - uint8 TempBuffer[40]; - int32 BytesReceived = 0; - if(Socket->Wait(ESocketWaitConditions::WaitForRead, FTimespan::FromSeconds(5))) - { - bool bRecv = Socket->Recv(TempBuffer, 40, BytesReceived); - if(!bRecv) - { - UE_LOG(LogTemp, Error, TEXT("Error receiving LatLon message")); - return ""; - } - - FString CoordStr = FString(UTF8_TO_TCHAR((char*)TempBuffer)); - return CoordStr; - } - return ""; + + AsioStreamBuf Buffer; + 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; } void UMapPreviewUserWidget::Shutdown() { // Close the socket - Socket->Close(); + SocketPtr->close(); } void UMapPreviewUserWidget::OpenServer() { - - /*FPlatformProcess::CreateProc( - TEXT("/home/adas/carla/osm-world-renderer/build/osm-world-renderer"), - nullptr, // Args - true, // if true process will have its own window - false, // if true it will be minimized - false, // if true it will be hidden in task bar - nullptr, // filled with PID - 0, // priority - nullptr, // directory to place after running the program - nullptr // redirection pipe - );*/ - + // Todo: automatically spawn the osm renderer process } void UMapPreviewUserWidget::CloseServer() @@ -174,24 +146,33 @@ void UMapPreviewUserWidget::CloseServer() bool UMapPreviewUserWidget::SendStr(FString Msg) { - if(!Socket) + if(!SocketPtr) { UE_LOG(LogTemp, Error, TEXT("Error. No socket.")); return false; } std::string MessageStr = std::string(TCHAR_TO_UTF8(*Msg)); - int32 BytesSent = 0; - bool bSent = Socket->Send((uint8*)MessageStr.c_str(), MessageStr.size(), BytesSent); - if (!bSent) + std::size_t BytesSent = 0; + try { - UE_LOG(LogTemp, Error, TEXT("Error sending message")); + BytesSent = Asio::write(*SocketPtr, Asio::buffer(MessageStr)); + } + catch (const boost::system::system_error& e) + { + FString ErrorMessage = e.what(); + UE_LOG(LogTemp, Error, TEXT("Error sending message: %s"), *ErrorMessage); + } + if (BytesSent != MessageStr.size()) + { + UE_LOG(LogTemp, Error, TEXT("Error sending message: num bytes mismatch")); + return true; } else { UE_LOG(LogTemp, Log, TEXT("Sent %d bytes"), BytesSent); + return false; } - return bSent; } void UMapPreviewUserWidget::UpdateLatLonCoordProperties() diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp index b485caf89..13e243913 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp @@ -5,7 +5,11 @@ #include "DesktopPlatform/Public/IDesktopPlatform.h" #include "DesktopPlatform/Public/DesktopPlatformModule.h" #include "Misc/FileHelper.h" +#include "Engine/LevelBounds.h" +#include "Engine/SceneCapture2D.h" #include "Runtime/Core/Public/Async/ParallelFor.h" +#include "Kismet/KismetRenderingLibrary.h" +#include "KismetProceduralMeshLibrary.h" #include "Carla/Game/CarlaStatics.h" #include "Traffic/TrafficLightManager.h" @@ -17,6 +21,8 @@ #include #include #include +#include +#include #include #include #include @@ -32,9 +38,13 @@ #include "MeshDescription.h" #include "EditorLevelLibrary.h" #include "ProceduralMeshConversion.h" + #include "ContentBrowserModule.h" #include "Materials/MaterialInstanceConstant.h" #include "Math/Vector.h" +#include "GameFramework/Actor.h" + +#include "DrawDebugHelpers.h" FString LaneTypeToFString(carla::road::Lane::LaneType LaneType) { @@ -141,6 +151,112 @@ void UOpenDriveToMap::CreateMap() ActorMeshList.Empty(); } +void UOpenDriveToMap::CreateTerrain( const int MeshGridSize, const float MeshGridSectionSize, const class UTexture2D* HeightmapTexture) +{ + TArray FoundActors; + UGameplayStatics::GetAllActorsOfClass(GetWorld(), AProceduralMeshActor::StaticClass(), FoundActors); + FVector BoxOrigin; + FVector BoxExtent; + UGameplayStatics::GetActorArrayBounds(FoundActors, false, BoxOrigin, BoxExtent); + FVector MinBox = BoxOrigin - BoxExtent; + + 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; + + /* Blueprint darfted code should be here */ + for( int i = 0; i < NumI; i++ ) + { + 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 ); + } + } +} + +void UOpenDriveToMap::CreateTerrainMesh(const int MeshIndex, const FVector2D Offset, const int GridSize, const float GridSectionSize, const UTexture2D* HeightmapTexture, UTextureRenderTarget2D* RoadMask) +{ + // const float GridSectionSize = 100.0f; // In cm + const float HeightScale = 3.0f; + + UWorld* World = GetWorld(); + + // Creation of the procedural mesh + AProceduralMeshActor* MeshActor = World->SpawnActor(); + MeshActor->SetActorLocation(FVector(Offset.X, Offset.Y, 0)); + UProceduralMeshComponent* Mesh = MeshActor->MeshComponent; + + TArray Vertices; + TArray Triangles; + + TArray Normals; + TArray Colors; + TArray Tangents; + TArray UVs; + + int VerticesInLine = (GridSize / GridSectionSize) + 1.0f; + for( int i = 0; i < VerticesInLine; i++ ) + { + float X = (i * GridSectionSize); + const int RoadMapX = i * 255 / VerticesInLine; + for( int j = 0; j < VerticesInLine; j++ ) + { + float Y = (j * GridSectionSize); + const int RoadMapY = j * 255 / VerticesInLine; + const int CellIndex = RoadMapY + 255 * RoadMapX; + float HeightValue = GetHeightForLandscape( FVector( (Offset.X + X), + (Offset.Y + Y), + 0)); + Vertices.Add(FVector( X, Y, HeightValue)); + } + } + + 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++) + { + Triangles.Add( j + ( i * VerticesInLine ) ); + Triangles.Add( ( j + 1 ) + ( i * VerticesInLine ) ); + Triangles.Add( j + ( ( i + 1 ) * VerticesInLine ) ); + + Triangles.Add( ( j + 1 ) + ( i * VerticesInLine ) ); + Triangles.Add( ( j + 1 ) + ( ( i + 1 ) * VerticesInLine ) ); + Triangles.Add( j + ( ( i + 1 ) * VerticesInLine ) ); + } + } + + if( DefaultLandscapeMaterial ) + { + Mesh->SetMaterial(0, DefaultLandscapeMaterial); + } + + Mesh->CreateMeshSection_LinearColor(0, Vertices, Triangles, Normals, + TArray(), // UV0 + TArray(), // VertexColor + TArray(), // Tangents + true); // Create collision); + + MeshActor->SetActorLabel("SM_Landscape" + FString::FromInt(MeshIndex) ); + Landscapes.Add(MeshActor); +} + void UOpenDriveToMap::OpenFileDialog() { TArray OutFileNames; @@ -163,7 +279,7 @@ void UOpenDriveToMap::LoadMap() UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("UOpenDriveToMap::LoadMap(): File to load %s"), *FilePath ); FFileHelper::LoadFileToString(FileContent, *FilePath); std::string opendrive_xml = carla::rpc::FromLongFString(FileContent); - boost::optional CarlaMap = carla::opendrive::OpenDriveParser::Load(opendrive_xml); + CarlaMap = carla::opendrive::OpenDriveParser::Load(opendrive_xml); if (!CarlaMap.has_value()) { @@ -181,6 +297,30 @@ void UOpenDriveToMap::LoadMap() GenerationFinished(); } +TArray UOpenDriveToMap::GenerateMiscActors(float Offset) +{ + std::vector> + Locations = CarlaMap->GetTreesTransform(DistanceBetweenTrees, DistanceFromRoadEdge, Offset); + TArray Returning; + int i = 0; + for (auto& cl : Locations) + { + const FVector scale{ 1.0f, 1.0f, 1.0f }; + cl.first.location.z = GetHeight(cl.first.location.x, cl.first.location.y) + 0.3f; + FTransform NewTransform ( FRotator(cl.first.rotation), FVector(cl.first.location), scale ); + + NewTransform = GetSnappedPosition(NewTransform); + + AActor* Spawner = GetWorld()->SpawnActor(AStaticMeshActor::StaticClass(), + NewTransform.GetLocation(), NewTransform.Rotator()); + Spawner->Tags.Add(FName("MiscSpawnPosition")); + Spawner->Tags.Add(FName(cl.second.c_str())); + Spawner->SetActorLabel("MiscSpawnPosition" + FString::FromInt(i)); + ++i; + Returning.Add(Spawner); + } + return Returning; +} void UOpenDriveToMap::GenerateAll(const boost::optional& CarlaMap ) { if (!CarlaMap.has_value()) @@ -188,10 +328,15 @@ void UOpenDriveToMap::GenerateAll(const boost::optional& Carla UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Invalid Map")); }else { + if(DefaultHeightmap && !Heightmap){ + Heightmap = DefaultHeightmap; + } + GenerateRoadMesh(CarlaMap); - GenerateSpawnPoints(CarlaMap); - GenerateTreePositions(CarlaMap); GenerateLaneMarks(CarlaMap); + GenerateSpawnPoints(CarlaMap); + CreateTerrain(12800, 256, nullptr); + GenerateTreePositions(CarlaMap); } } @@ -210,7 +355,7 @@ void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& int index = 0; for (const auto &PairMap : Meshes) { - for( const auto &Mesh : PairMap.second ) + for( auto& Mesh : PairMap.second ) { if (!Mesh->GetVertices().size()) { @@ -220,6 +365,22 @@ void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& continue; } + if(PairMap.first == carla::road::Lane::LaneType::Driving) + { + for( auto& Vertex : Mesh->GetVertices() ) + { + FVector VertexFVector = Vertex.ToFVector(); + Vertex.z += GetHeight(Vertex.x, Vertex.y, DistanceToLaneBorder(CarlaMap,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; + } + } + AProceduralMeshActor* TempActor = GetWorld()->SpawnActor(); TempActor->SetActorLabel(FString("SM_Lane_") + FString::FromInt(index)); @@ -255,14 +416,25 @@ void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& } const FProceduralCustomMesh MeshData = *Mesh; + TArray Normals; + TArray Tangents; + + UKismetProceduralMeshLibrary::CalculateTangentsForMesh( + MeshData.Vertices, + MeshData.Triangles, + MeshData.UV0, + Normals, + Tangents + ); + TempPMC->CreateMeshSection_LinearColor( 0, MeshData.Vertices, MeshData.Triangles, MeshData.Normals, - TArray(), // UV0 + MeshData.UV0, // UV0 TArray(), // VertexColor - TArray(), // Tangents + Tangents, // Tangents true); // Create collision TempActor->SetActorLocation(MeshCentroid * 100); // ActorMeshList.Add(TempActor); @@ -275,7 +447,6 @@ void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& end = FPlatformTime::Seconds(); UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Mesh spawnning and translation code executed in %f seconds."), end - start); - } void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& CarlaMap) @@ -283,23 +454,27 @@ void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& opg_parameters.vertex_distance = 0.5f; opg_parameters.vertex_width_resolution = 8.0f; opg_parameters.simplification_percentage = 15.0f; - - auto MarkingMeshes = CarlaMap->GenerateLineMarkings(opg_parameters); + std::vector lanemarkinfo; + auto MarkingMeshes = CarlaMap->GenerateLineMarkings(opg_parameters, lanemarkinfo); int index = 0; for (const auto& Mesh : MarkingMeshes) { if ( !Mesh->GetVertices().size() ) { + index++; continue; } if ( !Mesh->IsValid() ) { + index++; continue; } FVector MeshCentroid = FVector(0, 0, 0); - for (auto Vertex : Mesh->GetVertices()) + for (auto& Vertex : Mesh->GetVertices()) { + FVector VertexFVector = Vertex.ToFVector(); + Vertex.z += GetHeight(Vertex.x, Vertex.y, DistanceToLaneBorder(CarlaMap,VertexFVector) > 65.0f ) + 0.0001f; MeshCentroid += Vertex.ToFVector(); } @@ -326,6 +501,7 @@ void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& if(MinDistance < 250) { UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("Skkipped is %f."), MinDistance); + index++; continue; } @@ -335,21 +511,37 @@ void UOpenDriveToMap::GenerateLaneMarks(const boost::optional& TempPMC->bUseAsyncCooking = true; TempPMC->bUseComplexAsSimpleCollision = true; TempPMC->SetCollisionEnabled(ECollisionEnabled::NoCollision); + TempPMC->CastShadow = false; + if (lanemarkinfo[index].find("yellow") != std::string::npos) { + if(DefaultLaneMarksYellowMaterial) + TempPMC->SetMaterial(0, DefaultLaneMarksYellowMaterial); + }else{ + if(DefaultLaneMarksWhiteMaterial) + TempPMC->SetMaterial(0, DefaultLaneMarksWhiteMaterial); - if(DefaultLaneMarksMaterial) - TempPMC->SetMaterial(0, DefaultLaneMarksMaterial); + } const FProceduralCustomMesh MeshData = *Mesh; + TArray Normals; + TArray Tangents; + UKismetProceduralMeshLibrary::CalculateTangentsForMesh( + MeshData.Vertices, + MeshData.Triangles, + MeshData.UV0, + Normals, + Tangents + ); TempPMC->CreateMeshSection_LinearColor( 0, MeshData.Vertices, MeshData.Triangles, - MeshData.Normals, - TArray(), // UV0 + Normals, + MeshData.UV0, // UV0 TArray(), // VertexColor - TArray(), // Tangents + Tangents, // Tangents true); // Create collision TempActor->SetActorLocation(MeshCentroid * 100); + TempActor->Tags.Add(*FString(lanemarkinfo[index].c_str())); LaneMarkerActorList.Add(TempActor); index++; } @@ -370,12 +562,19 @@ void UOpenDriveToMap::GenerateSpawnPoints( const boost::optional& CarlaMap ) { - const std::vector> Locations = - CarlaMap->GetTreesPosition(DistanceBetweenTrees, DistanceFromRoadEdge ); + std::vector> Locations = + CarlaMap->GetTreesTransform(DistanceBetweenTrees, DistanceFromRoadEdge ); int i = 0; - for (const auto &cl : Locations) + for (auto &cl : Locations) { - AActor *Spawner = GetWorld()->SpawnActor(AStaticMeshActor::StaticClass(), cl.first.ToFVector() * 100, FRotator(0,0,0)); + const FVector scale{ 1.0f, 1.0f, 1.0f }; + cl.first.location.z = GetHeight(cl.first.location.x, cl.first.location.y) + 0.3f; + FTransform NewTransform ( FRotator(cl.first.rotation), FVector(cl.first.location), scale ); + NewTransform = GetSnappedPosition(NewTransform); + + AActor* Spawner = GetWorld()->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) ); @@ -618,3 +817,75 @@ void UOpenDriveToMap::SaveMap() 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); + }else{ + return carla::geom::deformation::GetZPosInDeformation(PosX, PosY) + + (carla::geom::deformation::GetZPosInDeformation(PosX, PosY) * -0.15f); + } +} + +FTransform UOpenDriveToMap::GetSnappedPosition( FTransform Origin ){ + FTransform ToReturn = Origin; + FVector Start = Origin.GetLocation() + FVector( 0, 0, 1000); + FVector End = Origin.GetLocation() - FVector( 0, 0, 1000); + FHitResult HitResult; + FCollisionQueryParams CollisionQuery; + FCollisionResponseParams CollisionParams; + + if( GetWorld()->LineTraceSingleByChannel( + HitResult, + Start, + End, + ECollisionChannel::ECC_WorldStatic, + CollisionQuery, + CollisionParams + ) ) + { + ToReturn.SetLocation(HitResult.Location); + } + return ToReturn; +} + +float UOpenDriveToMap::GetHeightForLandscape( FVector Origin ){ + FVector Start = Origin + FVector( 0, 0, 10000); + FVector End = Origin - FVector( 0, 0, 10000); + FHitResult HitResult; + FCollisionQueryParams CollisionQuery; + CollisionQuery.AddIgnoredActors(Landscapes); + FCollisionResponseParams CollisionParams; + + if( GetWorld()->LineTraceSingleByChannel( + HitResult, + Start, + End, + ECollisionChannel::ECC_WorldStatic, + CollisionQuery, + CollisionParams + ) ) + { + return GetHeight(Origin.X * 0.01f, Origin.Y * 0.01f, true) * 100.0f - 25.0f; + }else{ + return GetHeight(Origin.X * 0.01f, Origin.Y * 0.01f, true) * 100.0f; + } + return 0.0f; +} + +float UOpenDriveToMap::DistanceToLaneBorder(const boost::optional& CarlaMap, + 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); + if(wp) + { + carla::geom::Transform ct = CarlaMap->ComputeTransform(*wp); + double LaneWidth = CarlaMap->GetLaneWidth(*wp); + return cl.Distance(ct.location) - LaneWidth; + } + return 100000.0f; +} + diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h index f1a74da88..032590f11 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/HoudiniImporterWidget.h @@ -23,8 +23,12 @@ class CARLATOOLS_API UHoudiniImporterWidget : public UEditorUtilityWidget static void CreateSubLevels(ALargeMapManager* LargeMapManager); UFUNCTION(BlueprintCallable, Category="HoudiniImporterWidget") - static void MoveActorsToSubLevel(TArray Actors, ALargeMapManager* LargeMapManager); + static void MoveActorsToSubLevelWithLargeMap(TArray Actors, ALargeMapManager* LargeMapManager); + UFUNCTION(BlueprintCallable, Category="HoudiniImporterWidget") + static void MoveActorsToSubLevel(TArray Actors, ULevelStreaming* Level); + + UFUNCTION(BlueprintCallable, Category="HoudiniImporterWidget") static void UpdateGenericActorCoordinates(AActor* Actor, FVector TileOrigin); static void UpdateInstancedMeshCoordinates( diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/MapPreviewUserWidget.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/MapPreviewUserWidget.h index 0d792af94..00b0a1a96 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/MapPreviewUserWidget.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/MapPreviewUserWidget.h @@ -4,6 +4,12 @@ #include "CoreMinimal.h" #include "Blueprint/UserWidget.h" + +THIRD_PARTY_INCLUDES_START +#include +THIRD_PARTY_INCLUDES_END +#include + #include "MapPreviewUserWidget.generated.h" class FSocket; @@ -15,7 +21,10 @@ class CARLATOOLS_API UMapPreviewUserWidget : public UUserWidget GENERATED_BODY() private: - FSocket* Socket; + // Boost socket + boost::asio::io_service io_service; + std::unique_ptr SocketPtr; + bool SendStr(FString Msg); FString RecvCornersLatLonCoords(); diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h index e2955d94f..4c6eea9c7 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h @@ -35,6 +35,14 @@ public: UFUNCTION( BlueprintCallable ) void CreateMap(); + UFUNCTION(BlueprintCallable) + void CreateTerrain(const int MeshGridSize, const float MeshGridSectionSize, + const class UTexture2D* HeightmapTexture); + + UFUNCTION(BlueprintCallable) + void CreateTerrainMesh(const int MeshIndex, const FVector2D Offset, const int GridSize, const float GridSectionSize, + const class UTexture2D* HeightmapTexture, class UTextureRenderTarget2D* RoadMask); + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category="File") FString FilePath; @@ -47,15 +55,24 @@ public: UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Settings") FVector2D OriginGeoCoordinates; - UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Defaults") UMaterialInstance* DefaultRoadMaterial; - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UMaterialInstance* DefaultLaneMarksMaterial; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Defaults") + UMaterialInstance* DefaultLaneMarksWhiteMaterial; - UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Defaults") + UMaterialInstance* DefaultLaneMarksYellowMaterial; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Defaults") UMaterialInstance* DefaultSidewalksMaterial; + 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" ) @@ -66,6 +83,9 @@ protected: UFUNCTION( BlueprintCallable ) void SaveMap(); + UFUNCTION(BlueprintCallable) + TArray GenerateMiscActors(float Offset); + UFUNCTION( BlueprintImplementableEvent ) void GenerationFinished(); private: @@ -82,11 +102,21 @@ private: void GenerateTreePositions(const boost::optional& CarlaMap); void GenerateLaneMarks(const boost::optional& CarlaMap); + 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); + + float DistanceToLaneBorder(const boost::optional& CarlaMap, + FVector &location, + int32_t lane_type = static_cast(carla::road::Lane::LaneType::Driving)) const; + UPROPERTY() UCustomFileDownloader* FileDownloader; UPROPERTY() @@ -99,4 +129,8 @@ private: TArray RoadType; UPROPERTY() TArray RoadMesh; + UPROPERTY() + TArray Landscapes; + UPROPERTY() + UTexture2D* Heightmap; }; diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/TreeTableRow.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/TreeTableRow.h index 8c97713b6..7821316b3 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/TreeTableRow.h +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/TreeTableRow.h @@ -26,10 +26,10 @@ struct FTreeTableRow : public FTableRowBase { public: - UPROPERTY(EditAnywhere, BlueprintReadWrite, Category= "Category") - ELaneDescriptor TreesCategory; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category= "Meshes") + TArray> Meshes; - UPROPERTY(EditAnywhere, BlueprintReadWrite, Category= "Trees") - TArray> Trees; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Blueprints") + TArray> Blueprints; }; diff --git a/osm-world-renderer/CMakeLists.txt b/osm-world-renderer/CMakeLists.txt index 806509ca1..e62a9dcc1 100644 --- a/osm-world-renderer/CMakeLists.txt +++ b/osm-world-renderer/CMakeLists.txt @@ -3,6 +3,7 @@ project(OsmMapRenderer) # Libosmscout and Luna SVG Library include_directories(ThirdParties/include) +include_directories(ThirdParties/include/lunasvg) link_directories(ThirdParties/lib) add_definitions(-D_USE_MATH_DEFINES) diff --git a/osm-world-renderer/OsmRenderer/include/MapDrawer.h b/osm-world-renderer/OsmRenderer/include/MapDrawer.h index 4f1859f99..cc7c08c52 100644 --- a/osm-world-renderer/OsmRenderer/include/MapDrawer.h +++ b/osm-world-renderer/OsmRenderer/include/MapDrawer.h @@ -11,7 +11,9 @@ #include #include #include - +#if defined(_MSC_VER) + #include +#endif class MapDrawer { public: @@ -52,7 +54,7 @@ private: void SetDrawParameters(); void DrawMap(std::uint8_t* OutMap); - + }; #endif \ No newline at end of file diff --git a/osm-world-renderer/OsmRenderer/src/OsmRenderer.cpp b/osm-world-renderer/OsmRenderer/src/OsmRenderer.cpp index 1adef4711..a9dba18bd 100644 --- a/osm-world-renderer/OsmRenderer/src/OsmRenderer.cpp +++ b/osm-world-renderer/OsmRenderer/src/OsmRenderer.cpp @@ -68,10 +68,11 @@ void OsmRenderer::RunCmd(string Cmd) if(CmdType == "-R") // Render Command { - std::uint8_t* RenderedMap = new uint8_t[Drawer->GetImgSizeSqr() * 4]; - RenderMapCmd(CmdVector, RenderedMap); + std::unique_ptr RenderedMap = std::unique_ptr(new uint8_t[Drawer->GetImgSizeSqr() * 4]); + RenderMapCmd(CmdVector, RenderedMap.get()); - Asio::write(*SocketPtr, Asio::buffer(RenderedMap, (Drawer->GetImgSizeSqr() * 4 * sizeof(uint8_t)))); + std::cout << LOG_PRFX << "Sending image data: " << (Drawer->GetImgSizeSqr() * 4 * sizeof(uint8_t)) << " bytes" << std::endl; + Asio::write(*SocketPtr, Asio::buffer(RenderedMap.get(), (Drawer->GetImgSizeSqr() * 4 * sizeof(uint8_t)))); // TODO: delete RenderedMap after is sent }