From 40ee5705d67852b1357ac399e8c0034cbd753aef Mon Sep 17 00:00:00 2001 From: Marc Garcia Puig Date: Wed, 8 Apr 2020 19:33:46 +0200 Subject: [PATCH] Added walls to the mesh chunks --- LibCarla/source/carla/geom/MeshFactory.cpp | 218 ++++++++++++++++++++- LibCarla/source/carla/geom/MeshFactory.h | 36 +++- LibCarla/source/carla/road/Lane.cpp | 2 +- LibCarla/source/carla/road/Map.cpp | 132 +------------ 4 files changed, 251 insertions(+), 137 deletions(-) diff --git a/LibCarla/source/carla/geom/MeshFactory.cpp b/LibCarla/source/carla/geom/MeshFactory.cpp index 392fb912b..afe994839 100644 --- a/LibCarla/source/carla/geom/MeshFactory.cpp +++ b/LibCarla/source/carla/geom/MeshFactory.cpp @@ -57,7 +57,6 @@ namespace geom { if (lane.IsStraight()) { // Mesh optimization: If the lane is straight just add vertices at the // begining and at the end of it - const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); vertices.push_back(edges.first); vertices.push_back(edges.second); @@ -90,6 +89,130 @@ namespace geom { return std::make_unique(out_mesh); } + std::unique_ptr MeshFactory::GenerateWalls(const road::LaneSection &lane_section) const { + Mesh out_mesh; + + 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; + + for (auto &&lane_pair : lane_section.GetLanes()) { + const auto &lane = lane_pair.second; + const double s_start = lane.GetDistance() + EPSILON; + const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON; + if (lane.GetId() == max_lane) { + out_mesh += *GenerateLeftWall(lane, s_start, s_end); + } + if (lane.GetId() == min_lane) { + out_mesh += *GenerateRightWall(lane, s_start, s_end); + } + } + return std::make_unique(out_mesh); + } + + std::unique_ptr MeshFactory::GenerateRightWall( + const road::Lane &lane, const double s_start, const double s_end) const { + RELEASE_ASSERT(road_param.resolution > 0.0); + DEBUG_ASSERT(s_start >= 0.0); + DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength()); + DEBUG_ASSERT(s_end >= EPSILON); + DEBUG_ASSERT(s_start < s_end); + // The lane with lane_id 0 have no physical representation in OpenDRIVE + Mesh out_mesh; + if (lane.GetId() == 0) { + return std::make_unique(out_mesh); + } + double s_current = s_start; + const geom::Vector3D height_vector = geom::Vector3D(0.f, 0.f, road_param.wall_height); + + std::vector r_vertices; + if (lane.IsStraight()) { + // Mesh optimization: If the lane is straight just add vertices at the + // begining and at the end of it + const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); + r_vertices.push_back(edges.first + height_vector); + r_vertices.push_back(edges.first); + } else { + // 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 + const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); + r_vertices.push_back(edges.first + height_vector); + r_vertices.push_back(edges.first); + + // Update the current waypoint's "s" + s_current += road_param.resolution; + } while(s_current < s_end); + } + + // This ensures the mesh is constant and have no gaps between roads, + // adding geometry at the very end of the lane + if (s_end - (s_current - road_param.resolution) > EPSILON) { + const auto edges = lane.GetCornerPositions(s_end, road_param.extra_lane_width); + r_vertices.push_back(edges.first + height_vector); + r_vertices.push_back(edges.first); + } + + // Add the adient material, create the strip and close the material + out_mesh.AddMaterial( + lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road"); + out_mesh.AddTriangleStrip(r_vertices); + out_mesh.EndMaterial(); + return std::make_unique(out_mesh); + } + + std::unique_ptr MeshFactory::GenerateLeftWall( + const road::Lane &lane, const double s_start, const double s_end) const { + RELEASE_ASSERT(road_param.resolution > 0.0); + DEBUG_ASSERT(s_start >= 0.0); + DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength()); + DEBUG_ASSERT(s_end >= EPSILON); + DEBUG_ASSERT(s_start < s_end); + // The lane with lane_id 0 have no physical representation in OpenDRIVE + Mesh out_mesh; + if (lane.GetId() == 0) { + return std::make_unique(out_mesh); + } + double s_current = s_start; + const geom::Vector3D height_vector = geom::Vector3D(0.f, 0.f, road_param.wall_height); + + std::vector l_vertices; + if (lane.IsStraight()) { + // Mesh optimization: If the lane is straight just add vertices at the + // begining and at the end of it + const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); + l_vertices.push_back(edges.second); + l_vertices.push_back(edges.second + height_vector); + } else { + // 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 + const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); + l_vertices.push_back(edges.second); + l_vertices.push_back(edges.second + height_vector); + + // Update the current waypoint's "s" + s_current += road_param.resolution; + } while(s_current < s_end); + } + + // This ensures the mesh is constant and have no gaps between roads, + // adding geometry at the very end of the lane + if (s_end - (s_current - road_param.resolution) > EPSILON) { + const auto edges = lane.GetCornerPositions(s_end, road_param.extra_lane_width); + l_vertices.push_back(edges.second); + l_vertices.push_back(edges.second + height_vector); + } + + // Add the adient material, create the strip and close the material + out_mesh.AddMaterial( + lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road"); + out_mesh.AddTriangleStrip(l_vertices); + out_mesh.EndMaterial(); + return std::make_unique(out_mesh); + } + std::vector> MeshFactory::GenerateWithMaxLen( const road::Road &road, const double max_len) const { std::vector> mesh_uptr_list; @@ -121,13 +244,104 @@ namespace geom { s_current = s_until; } if (s_end - s_current > EPSILON) { + Mesh lane_section_mesh; for (auto &&lane_pair : lane_section.GetLanes()) { - mesh_uptr_list.emplace_back(Generate(lane_pair.second, s_current, s_end)); + lane_section_mesh += *Generate(lane_pair.second, s_current, s_end); } + mesh_uptr_list.emplace_back(std::make_unique(lane_section_mesh)); } } return mesh_uptr_list; } + std::vector> MeshFactory::GenerateWallsWithMaxLen( + const road::Road &road, const double max_len) const { + std::vector> mesh_uptr_list; + for (auto &&lane_section : road.GetLaneSections()) { + auto section_uptr_list = GenerateWallsWithMaxLen(lane_section, max_len); + mesh_uptr_list.insert( + mesh_uptr_list.end(), + std::make_move_iterator(section_uptr_list.begin()), + std::make_move_iterator(section_uptr_list.end())); + } + return mesh_uptr_list; + } + + std::vector> MeshFactory::GenerateWallsWithMaxLen( + const road::LaneSection &lane_section, const double max_len) const { + std::vector> mesh_uptr_list; + + 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; + + if (lane_section.GetLength() < max_len) { + mesh_uptr_list.emplace_back(GenerateWalls(lane_section)); + } else { + double s_current = lane_section.GetDistance() + EPSILON; + const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON; + while(s_current + max_len < s_end) { + const auto s_until = s_current + max_len; + Mesh lane_section_mesh; + for (auto &&lane_pair : lane_section.GetLanes()) { + const auto &lane = lane_pair.second; + if (lane.GetId() == max_lane) { + lane_section_mesh += *GenerateLeftWall(lane, s_current, s_until); + } + if (lane.GetId() == min_lane) { + lane_section_mesh += *GenerateRightWall(lane, s_current, s_until); + } + } + mesh_uptr_list.emplace_back(std::make_unique(lane_section_mesh)); + s_current = s_until; + } + if (s_end - s_current > EPSILON) { + Mesh lane_section_mesh; + for (auto &&lane_pair : lane_section.GetLanes()) { + const auto &lane = lane_pair.second; + if (lane.GetId() == max_lane) { + lane_section_mesh += *GenerateLeftWall(lane, s_current, s_end); + } + if (lane.GetId() == min_lane) { + lane_section_mesh += *GenerateRightWall(lane, s_current, s_end); + } + } + mesh_uptr_list.emplace_back(std::make_unique(lane_section_mesh)); + } + } + return mesh_uptr_list; + } + + std::vector> MeshFactory::GenerateAllWithMaxLen( + const road::Road &road, const double max_len) const { + std::vector> mesh_uptr_list; + + // Get road meshes + auto roads = GenerateWithMaxLen(road, max_len); + mesh_uptr_list.insert( + mesh_uptr_list.end(), + std::make_move_iterator(roads.begin()), + std::make_move_iterator(roads.end())); + + // Get wall meshes only if is not a junction + if (!road.IsJunction()) { + auto walls = GenerateWallsWithMaxLen(road, max_len); + + if (roads.size() == walls.size()) { + for (size_t i = 0; i < walls.size(); ++i) { + *mesh_uptr_list[i] += *walls[i]; + } + } else { + mesh_uptr_list.insert( + mesh_uptr_list.end(), + std::make_move_iterator(walls.begin()), + std::make_move_iterator(walls.end())); + } + } + + return mesh_uptr_list; + } + } // namespace geom } // namespace carla diff --git a/LibCarla/source/carla/geom/MeshFactory.h b/LibCarla/source/carla/geom/MeshFactory.h index de135e399..dfa13037d 100644 --- a/LibCarla/source/carla/geom/MeshFactory.h +++ b/LibCarla/source/carla/geom/MeshFactory.h @@ -27,6 +27,8 @@ namespace geom { // -- Map Related ---------------------------------------------------------- // ========================================================================= + // -- Basic -- + /// Generates a mesh that defines a road std::unique_ptr Generate(const road::Road &road) const; @@ -40,19 +42,47 @@ namespace geom { /// Generates a mesh that defines the whole lane std::unique_ptr Generate(const road::Lane &lane) const; - /// Generates a mesh that defines a lane + // -- Walls -- + + /// Genrates a mesh representing a wall on the road corners to avoid + /// cars falling down + std::unique_ptr GenerateWalls(const road::LaneSection &lane_section) const; + + /// Generates a wall-like mesh at the right side of the lane + std::unique_ptr GenerateRightWall( + const road::Lane &lane, const double s_start, const double s_end) const; + + /// Generates a wall-like mesh at the left side of the lane + std::unique_ptr GenerateLeftWall( + const road::Lane &lane, const double s_start, const double s_end) const; + + // -- Chunked -- + + /// Generates list of meshes that defines a single road with a maximum length std::vector> GenerateWithMaxLen( const road::Road &road, const double max_len) const; - /// Generates a mesh that defines a lane + /// Generates list of meshes that defines a single lane_section with a maximum length std::vector> GenerateWithMaxLen( const road::LaneSection &lane_section, const double max_len) const; + std::vector> GenerateWallsWithMaxLen( + const road::Road &road, const double max_len) const; + + std::vector> GenerateWallsWithMaxLen( + const road::LaneSection &lane_section, const double max_len) const; + + // -- Util -- + + /// Generates a chunked road with all the features needed for simulation + std::vector> GenerateAllWithMaxLen( + const road::Road &road, const double max_len) const; + // ========================================================================= // -- Generation parameters ------------------------------------------------ // ========================================================================= - /// Generates a mesh that defines a lane + /// Parameters for the road generation struct RoadParameters { float resolution = 2.0f; float extra_lane_width = 1.0f; diff --git a/LibCarla/source/carla/road/Lane.cpp b/LibCarla/source/carla/road/Lane.cpp index a4e3e7c4b..bb45cb49c 100644 --- a/LibCarla/source/carla/road/Lane.cpp +++ b/LibCarla/source/carla/road/Lane.cpp @@ -129,7 +129,7 @@ namespace road { const Road *road = GetRoad(); DEBUG_ASSERT(road != nullptr); - // must s be smaller (or eq) than road lenght and bigger (or eq) than 0? + // must s be smaller (or eq) than road length and bigger (or eq) than 0? RELEASE_ASSERT(s <= road->GetLength()); RELEASE_ASSERT(s >= 0.0); diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index a8fe638e6..c6f14ec85 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -932,38 +932,6 @@ namespace road { return _data.GetJunction(id); } - /// Computes the location of the edges of the current lane at the current waypoint - static std::pair GetWaypointCornerPositions( - const Map &map, const Waypoint &waypoint, const Lane &lane, const float extra_width=0.f) { - float lane_width = static_cast(map.GetLaneWidth(waypoint)) / 2.0f; - - DEBUG_ASSERT(lane.GetRoad() != nullptr); - if (extra_width != 0.f && lane.GetRoad()->IsJunction() && lane.GetType() == Lane::LaneType::Driving) { - lane_width += extra_width; - } - lane_width = waypoint.lane_id > 0 ? -lane_width : lane_width; - - const geom::Transform wp_trnasf = map.ComputeTransform(waypoint); - auto loc_r = static_cast(wp_trnasf.location) + - (wp_trnasf.GetRightVector() * lane_width); - auto loc_l = static_cast(wp_trnasf.location) + - (wp_trnasf.GetRightVector() * -lane_width); - - if (lane.GetType() == Lane::LaneType::Driving) { - - } - // Apply an offset to the Sidewalks - else if (lane.GetType() == Lane::LaneType::Sidewalk) { - // RoadRunner doesn't export it right now and as a workarround where 15.24 cm - // is the exact height that match with most of the RoadRunner sidewalks - loc_r.z += 0.1524f; - loc_l.z += 0.1524f; - /// TODO: use the OpenDRIVE 5.3.7.2.1.1.9 Lane Height Record - } - - return std::make_pair(loc_r, loc_l); - } - geom::Mesh Map::GenerateMesh(const double distance, const float extra_width) const { RELEASE_ASSERT(distance > 0.0); geom::MeshFactory mesh_factory; @@ -994,7 +962,7 @@ namespace road { for (auto &&pair : _data.GetRoads()) { const auto &road = pair.second; std::vector> road_mesh_list = - mesh_factory.GenerateWithMaxLen(road, max_road_len); + mesh_factory.GenerateAllWithMaxLen(road, max_road_len); // If the road in in a junction, add the road to the junction mesh instead of // doing it separately, this is needed for the road mesh smooth algorithm @@ -1063,103 +1031,5 @@ namespace road { return out_mesh; } - geom::Mesh Map::GenerateWalls( - const double distance, const float wall_height) const { - RELEASE_ASSERT(distance > 0.0); - geom::Mesh out_mesh; - if (wall_height == 0.0f) { - return out_mesh; - } - // Iterate each lane in each lane_section in each road - for (const auto &pair : _data.GetRoads()) { - const auto &road = pair.second; - if (road.IsJunction()) { - continue; - } - for (const 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; - for (const auto &lane_pair : lane_section.GetLanes()) { - // Get the lane reference - const auto &lane = lane_pair.second; - // The lane with lane_id 0 have no physical representation in OpenDRIVE - if (lane.GetId() == 0) { - continue; - } - - const auto end_distance = lane.GetDistance() + lane.GetLength() - EPSILON; - Waypoint current_wp { - road.GetId(), - lane_section.GetId(), - lane.GetId(), - lane_section.GetDistance() + EPSILON }; - - std::vector r_vertices; - std::vector l_vertices; - if (lane.IsStraight()) { - // Mesh optimization: If the lane is straight just add vertices at the - // begining and at the end of it - const auto edges = GetWaypointCornerPositions(*this, current_wp, lane); - // vertices.push_back(edges.first); - // vertices.push_back(edges.second); - if (lane.GetId() == min_lane) { - r_vertices.push_back(edges.first + geom::Vector3D(0.f, 0.f, wall_height)); - r_vertices.push_back(edges.first); - } - if (lane.GetId() == max_lane) { - l_vertices.push_back(edges.second); - l_vertices.push_back(edges.second + geom::Vector3D(0.f, 0.f, wall_height)); - } - } else { - // 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 - const auto edges = GetWaypointCornerPositions(*this, current_wp, lane); - // vertices.push_back(edges.first); - // vertices.push_back(edges.second); - if (lane.GetId() == min_lane) { - r_vertices.push_back(edges.first + geom::Vector3D(0.f, 0.f, wall_height)); - r_vertices.push_back(edges.first); - } - if (lane.GetId() == max_lane) { - l_vertices.push_back(edges.second); - l_vertices.push_back(edges.second + geom::Vector3D(0.f, 0.f, wall_height)); - } - // Update the current waypoint's "s" - current_wp.s += distance; - } while(current_wp.s < end_distance); - } - - // This ensures the mesh is constant and have no gaps between roads, - // adding geometry at the very end of the lane - if (end_distance - (current_wp.s - distance) > EPSILON) { - current_wp.s = end_distance; - const auto edges = GetWaypointCornerPositions(*this, current_wp, lane); - // vertices.push_back(edges.first); - // vertices.push_back(edges.second); - if (lane.GetId() == min_lane) { - r_vertices.push_back(edges.first + geom::Vector3D(0.f, 0.f, wall_height)); - r_vertices.push_back(edges.first); - } - if (lane.GetId() == max_lane) { - l_vertices.push_back(edges.second); - l_vertices.push_back(edges.second + geom::Vector3D(0.f, 0.f, wall_height)); - } - } - // Add the adient material, create the strip and close the material - out_mesh.AddMaterial( - lane.GetType() == Lane::LaneType::Sidewalk ? "sidewalk" : "road"); - out_mesh.AddTriangleStrip(r_vertices); - out_mesh.AddTriangleStrip(l_vertices); - out_mesh.EndMaterial(); - } - } - } - - return out_mesh; - } - } // namespace road } // namespace carla