diff --git a/LibCarla/source/carla/road/LaneSection.cpp b/LibCarla/source/carla/road/LaneSection.cpp index 8ef5e2735..9421bba91 100644 --- a/LibCarla/source/carla/road/LaneSection.cpp +++ b/LibCarla/source/carla/road/LaneSection.cpp @@ -29,6 +29,14 @@ namespace road { return nullptr; } + const Lane *LaneSection::GetLane(const LaneId id) const { + auto search = _lanes.find(id); + if (search != _lanes.end()) { + return &search->second; + } + return nullptr; + } + std::map &LaneSection::GetLanes() { return _lanes; } diff --git a/LibCarla/source/carla/road/LaneSection.h b/LibCarla/source/carla/road/LaneSection.h index 7ec14b4b9..8626fb3c4 100644 --- a/LibCarla/source/carla/road/LaneSection.h +++ b/LibCarla/source/carla/road/LaneSection.h @@ -32,6 +32,8 @@ namespace road { Lane *GetLane(const LaneId id); + const Lane *GetLane(const LaneId id) const; + bool ContainsLane(LaneId id) const { return (_lanes.find(id) != _lanes.end()); } diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index 7b1f4b4ce..9d188fdc5 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -1070,7 +1070,7 @@ namespace road { return std::make_pair(loc_r, loc_l); } - geom::Mesh Map::GenerateMesh(double distance) const { + geom::Mesh Map::GenerateMesh(const double distance) const { RELEASE_ASSERT(distance > 0.0); geom::Mesh out_mesh; // Iterate each lane in each lane_section in each road @@ -1167,5 +1167,109 @@ namespace road { return out_mesh; } + geom::Mesh Map::GenerateWalls(const double distance, const float wall_height) const { + RELEASE_ASSERT(distance > 0.0); + // TODO: clean up this code + geom::Mesh 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()) { + // Get the lane reference + // -- RIGHT WALL -- + const auto *lane0 = lane_section.GetLane(lane_section.GetLanes().begin()->first); + // The lane with lane_id 0 have no physical representation in OpenDRIVE + if (lane0 == nullptr || lane0->GetId() == 0) { + continue; + } + const auto end_distance = lane0->GetDistance() + lane0->GetLength() - EPSILON; + Waypoint current_wp0 { + road.GetId(), + lane_section.GetId(), + lane0->GetId(), + lane_section.GetDistance() + EPSILON }; + + std::vector vertices; + if (IsLaneStraight(*lane0)) { + // 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_wp0, *lane0); + vertices.push_back(edges.first + geom::Vector3D(0.f, 0.f, wall_height)); + 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 = GetWaypointCornerPositions(*this, current_wp0, *lane0); + vertices.push_back(edges.first + geom::Vector3D(0.f, 0.f, wall_height)); + vertices.push_back(edges.first); + + // Update the current waypoint's "s" + current_wp0.s += distance; + } while(current_wp0.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_wp0.s - distance) > EPSILON) { + current_wp0.s = end_distance; + const auto edges = GetWaypointCornerPositions(*this, current_wp0, *lane0); + vertices.push_back(edges.first + geom::Vector3D(0.f, 0.f, wall_height)); + vertices.push_back(edges.first); + } + out_mesh.AddMaterial("security_wall"); + out_mesh.AddTriangleStrip(vertices); + vertices.clear(); + // -- LEFT WALL -- + const auto *lane1 = lane_section.GetLane(lane_section.GetLanes().rbegin()->first); + // The lane with lane_id 0 have no physical representation in OpenDRIVE + if (lane1 == nullptr || lane1->GetId() == 0) { + continue; + } + Waypoint current_wp1 { + road.GetId(), + lane_section.GetId(), + lane1->GetId(), + lane_section.GetDistance() + EPSILON }; + + if (IsLaneStraight(*lane1)) { + // 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_wp1, *lane1); + vertices.push_back(edges.second); + 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_wp1, *lane1); + vertices.push_back(edges.second); + vertices.push_back(edges.second + geom::Vector3D(0.f, 0.f, wall_height)); + + // Update the current waypoint's "s" + current_wp1.s += distance; + } while(current_wp1.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_wp1.s - distance) > EPSILON) { + current_wp1.s = end_distance; + const auto edges = GetWaypointCornerPositions(*this, current_wp1, *lane1); + vertices.push_back(edges.second); + vertices.push_back(edges.second + geom::Vector3D(0.f, 0.f, wall_height)); + } + + out_mesh.AddTriangleStrip(vertices); + out_mesh.EndMaterial(); + } + } + + return out_mesh; + } + } // namespace road } // namespace carla diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index c53253adc..8d8770ce2 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -146,11 +146,13 @@ namespace road { ComputeJunctionConflicts(JuncId id) const; /// Buids a mesh based on the OpenDRIVE - geom::Mesh GenerateMesh(double distance) const; + geom::Mesh GenerateMesh(const double distance) const; /// Buids a mesh of all crosswalks based on the OpenDRIVE geom::Mesh GetAllCrosswalkMesh() const; + geom::Mesh GenerateWalls(const double distance, const float wall_height) const; + const std::unordered_map>& GetSignals() const { return _data.GetSignals(); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp index 861caffd3..ec767accb 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDrive/OpenDriveGenerator.cpp @@ -70,7 +70,7 @@ void AOpenDriveGenerator::GenerateRoadMesh() return; } - const auto MeshData = CarlaMap->GenerateMesh(2); + const auto MeshData = CarlaMap->GenerateMesh(2) + CarlaMap->GenerateWalls(2, 1); // Build the mesh TArray Vertices;