From 791e28556e9311bd2b003c942e3d1867782a8522 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Mon, 12 Dec 2022 19:25:52 -0300 Subject: [PATCH 1/5] Adds a adjacency checker for the builder. Signed-off-by: Franco Cipollone --- .../geometry/utility/geometry.h | 17 ++ include/maliput_sparse/parser/validator.h | 115 ++++++++ src/geometry/utility/geometry.cc | 11 + src/loader/road_geometry_loader.cc | 24 ++ src/parser/CMakeLists.txt | 2 + src/parser/validator.cc | 218 ++++++++++++++ test/geometry/utility/geometry_test.cc | 41 +++ test/parser/CMakeLists.txt | 2 + test/parser/validator_test.cc | 272 ++++++++++++++++++ 9 files changed, 702 insertions(+) create mode 100644 include/maliput_sparse/parser/validator.h create mode 100644 src/parser/validator.cc create mode 100644 test/parser/validator_test.cc diff --git a/include/maliput_sparse/geometry/utility/geometry.h b/include/maliput_sparse/geometry/utility/geometry.h index 5e688f2..f020359 100644 --- a/include/maliput_sparse/geometry/utility/geometry.h +++ b/include/maliput_sparse/geometry/utility/geometry.h @@ -120,6 +120,23 @@ ClosestPointResult GetClosestPoint(const std::pair +#include + +#include "maliput_sparse/parser/parser.h" + +namespace maliput_sparse { +namespace parser { + +/// ValidatorOptions struct that contains the options for the Validator. +struct ValidatorOptions { + /// Verifies adjacency for lanes in a segment. + bool lane_adjacency{true}; +}; + +/// ValidatorConfig struct that contains the configuration for the Validator. +struct ValidatorConfig { + double linear_tolerance{1e-12}; +}; + +/// After parsing a road network, the Validator can be used to check for errors before +/// creating a maliput::api::RoadNetwork via the maliput_sparse::loader::RoadNetworkLoader. +/// The Validator can be configured to check for different types of errors and provide an interface to retrieve +/// the errors found. +/// The errors are stored in a vector of Error structs. The Error struct contains a message, the type of error, and the +/// severity. It's on the user to decide how to handle the errors. +class Validator { + public: + /// Error struct that contains the error message, type, and severity. + struct Error { + /// The type of error. + enum class Type { + kLaneAdjacency, + }; + /// The severity of the error. + enum class Severity { + kWarning, + kError, + }; + + /// Equality operator for Error. + bool operator==(const Error& other) const; + /// Inequality operator for Error. + bool operator!=(const Error& other) const; + + /// Message describing the error. + std::string message; + /// The type of error. + Type type; + /// The severity of the error. + Severity severity; + }; + + /// Constructor for Validator. + /// During construction, the Validator will perform the validation checks. + //// + /// @param parser The maliput_sparse::parser::Parser instance to validate. + /// @param options The maliput_sparse::parser::ValidatorOptions to use. + /// @param config The maliput_sparse::parser::ValidatorConfig to use. + Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig config); + + /// Returns the errors found during validation. + const std::vector& GetErrors() const; + + private: + // Helper functions for reporting errors. + // @param message The error message. + // @Param type The type of error. + // @Param severity The severity of the error. + void Report(const std::string& message, const Error::Type& type, const Error::Severity& severity); + + // Method to validate lane adjacency. + // @param parser The maliput_sparse::parser::Parser instance to validate. + // @param config The maliput_sparse::parser::ValidatorConfig to use. + void ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config); + + // Helper function to geometrically check lane adjacency. + void CheckAdjacency(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance); + + // Holds the errors found during validation. + std::vector errors_; +}; + +} // namespace parser +} // namespace maliput_sparse diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc index 2037dd1..3fab368 100644 --- a/src/geometry/utility/geometry.cc +++ b/src/geometry/utility/geometry.cc @@ -374,6 +374,17 @@ ClosestPointResult GetClosestPoint(const LineString3d& line_string, const malipu return result; } +double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs) { + const LineString3d& base_ls = rhs.size() > lhs.size() ? rhs : lhs; + const LineString3d& other_ls = rhs.size() > lhs.size() ? lhs : rhs; + double sum_distances{}; + for (const auto& base_point : base_ls) { + const auto closest_point_res = GetClosestPoint(other_ls, base_point); + sum_distances += closest_point_res.distance; + } + return sum_distances / base_ls.size(); +} + } // namespace utility } // namespace geometry } // namespace maliput_sparse diff --git a/src/loader/road_geometry_loader.cc b/src/loader/road_geometry_loader.cc index 94c0819..c1504bc 100644 --- a/src/loader/road_geometry_loader.cc +++ b/src/loader/road_geometry_loader.cc @@ -29,7 +29,10 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "maliput_sparse/loader/road_geometry_loader.h" +#include + #include "maliput_sparse/builder/builder.h" +#include "maliput_sparse/parser/validator.h" namespace maliput_sparse { namespace loader { @@ -55,6 +58,27 @@ RoadGeometryLoader::RoadGeometryLoader(std::unique_ptr parser, } std::unique_ptr RoadGeometryLoader::operator()() { + // Validates the parsed data before building the RoadGeometry. + const parser::Validator validator(parser_.get(), parser::ValidatorOptions{true}, + parser::ValidatorConfig{builder_configuration_.linear_tolerance}); + const auto errors = validator.GetErrors(); + for (const auto& error : errors) { + if (error.severity == parser::Validator::Error::Severity::kError) { + maliput::log()->error("{}", error.message); + } + if (error.severity == parser::Validator::Error::Severity::kWarning) { + maliput::log()->warn("{}", error.message); + } + } + + if (std::find_if(errors.begin(), errors.end(), [](const parser::Validator::Error& error) { + return error.severity == parser::Validator::Error::Severity::kError; + }) != errors.end()) { + MALIPUT_THROW_MESSAGE("Errors(" + std::to_string(static_cast(errors.size())) + + ") found during validation. Aborting."); + } + + // Builds the RoadGeometry. const std::unordered_map& junctions = parser_->GetJunctions(); const std::vector& connections = parser_->GetConnections(); diff --git a/src/parser/CMakeLists.txt b/src/parser/CMakeLists.txt index e7b0836..9d2c48d 100644 --- a/src/parser/CMakeLists.txt +++ b/src/parser/CMakeLists.txt @@ -4,6 +4,7 @@ add_library(parser connection.cc lane.cc + validator.cc ) add_library(maliput_sparse::parser ALIAS parser) @@ -23,6 +24,7 @@ target_include_directories( target_link_libraries(parser PRIVATE maliput::common + maliput_sparse::geometry ) install(TARGETS parser diff --git a/src/parser/validator.cc b/src/parser/validator.cc new file mode 100644 index 0000000..60cfec2 --- /dev/null +++ b/src/parser/validator.cc @@ -0,0 +1,218 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput_sparse/parser/validator.h" + +#include "maliput_sparse/geometry/utility/geometry.h" +#include "maliput_sparse/parser/junction.h" +#include "maliput_sparse/parser/lane.h" +#include "maliput_sparse/parser/segment.h" + +namespace maliput_sparse { +namespace parser { + +namespace { + +// Convenient constant for indicating left or right. +static constexpr bool kLeft{true}; +static constexpr bool kRight{!kLeft}; + +} // namespace + +Validator::Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig config) { + MALIPUT_THROW_UNLESS(parser); + if (options.lane_adjacency) { + ValidateLaneAdjacency(parser, config); + } +} + +const std::vector& Validator::GetErrors() const { return errors_; } + +void Validator::CheckAdjacency(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance) { + const auto line_string_a = left ? lane.left : lane.right; + const auto line_string_b = left ? adjacent_lane.right : adjacent_lane.left; + const double distance = geometry::utility::ComputeDistance(line_string_a, line_string_b); + if (distance > tolerance) { + const std::string msg{"Lane " + lane.id + " and lane " + adjacent_lane.id + + " are not adjacent under the tolerance " + std::to_string(tolerance) + + ". (distance: " + std::to_string(distance) + ")"}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } +} + +void Validator::Report(const std::string& message, const Validator::Error::Type& type, + const Validator::Error::Severity& severity) { + errors_.push_back(Validator::Error{message, type, severity}); +} + +void Validator::ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config) { + const auto junctions = parser->GetJunctions(); + for (const auto junction : junctions) { + for (const auto segment : junction.second.segments) { + const std::vector& lanes = segment.second.lanes; + // Iterates lanes from right to left. + // | idx=N-1 | idx=3 | idx=2 | idx=1 | idx=0 | + for (int idx{}; idx < static_cast(lanes.size()); ++idx) { + const Lane& lane = lanes[idx]; + + // Note: The following code is a bit repetitive for left and right adjacency checks, but it is easier to read + // and understand. Check right adjacency <-------------------------------------------------- + if (lane.right_lane_id) { + // Check if there is a idx to the right. + if (idx == 0) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") but is the first lane in the segment " + + segment.first + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + // Check if right lane id is in the same segment + const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { + return lane.right_lane_id.value() == lane_it.id; + }); + if (adj_lane_it == lanes.end()) { + // Error. + const std::string msg{"Adjacent lane isn't part of the segment: Lane " + lane.id + + " has a right lane id (" + lane.right_lane_id.value() + + ") that is not part of the segment " + segment.first + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } else { + // Check if right lane id has the lane id as left lane id. + if (adj_lane_it->left_lane_id) { + if (adj_lane_it->left_lane_id.value() != lane.id) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that has a left lane id (" + + adj_lane_it->left_lane_id.value() + ") that is not the lane " + lane.id + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + } else { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that has no left lane id."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + + // Check geometrical adjacency. + CheckAdjacency(lane, *adj_lane_it, kRight, config.linear_tolerance); + } + + // Check if idx - 1 lane is the right lane id. + if ((idx - 1 >= 0) && (lanes[idx - 1].id != lane.right_lane_id.value())) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that is not the previous lane in the segment " + + segment.first + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + + } else { + // Check if idx is the first lane in the segment. + if (idx != 0) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + + " has no right lane id but it isn't the first lane in the segment " + segment.first + + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + } + + // Check left adjacency <-------------------------------------------------- + if (lane.left_lane_id) { + // Check if there is a idx to the left. + if (idx == static_cast(lanes.size()) - 1) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") but is the last lane in the segment " + segment.first + + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + // Check if left lane id is in the same segment + const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { + return lane.left_lane_id.value() == lane_it.id; + }); + if (adj_lane_it == lanes.end()) { + // Error. + const std::string msg{"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that is not part of the segment " + segment.first + + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } else { + // Check if left lane id has the lane id as right lane id. + if (adj_lane_it->right_lane_id) { + if (adj_lane_it->right_lane_id.value() != lane.id) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that has a right lane id (" + + adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + } else { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that has no right lane id."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + + // Check geometrical adjacency. + CheckAdjacency(lane, *adj_lane_it, kLeft, config.linear_tolerance); + } + + // Check if idx + 1 lane is the left lane id. + if ((idx + 1 <= static_cast(lanes.size()) - 1) && (lanes[idx + 1].id != lane.left_lane_id.value())) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that is not the next lane in the segment " + + segment.first + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + + } else { + // Check if idx is the last lane in the segment. + if (idx != static_cast(lanes.size()) - 1) { + // Error. + const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + + " has no left lane id but it isn't the last lane in the segment " + segment.first + + "."}; + Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + } + } + } + } + } +} + +bool Validator::Error::operator==(const Error& other) const { + return message == other.message && type == other.type && severity == other.severity; +} + +bool Validator::Error::operator!=(const Error& other) const { return !(*this == other); } + +} // namespace parser +} // namespace maliput_sparse diff --git a/test/geometry/utility/geometry_test.cc b/test/geometry/utility/geometry_test.cc index c09d3ff..de971d9 100644 --- a/test/geometry/utility/geometry_test.cc +++ b/test/geometry/utility/geometry_test.cc @@ -504,6 +504,47 @@ TEST_P(GetClosestPointLineStringTest, Test) { INSTANTIATE_TEST_CASE_P(GetClosestPointLineStringTestGroup, GetClosestPointLineStringTest, ::testing::ValuesIn(GetClosestPointLineStringTestCases())); +struct ComputeDistanceTestCase { + LineString3d lhs; + LineString3d rhs; + double expected_distance{0.}; +}; + +std::vector ComputeDistanceTestCases() { + return { + { + LineString3d{{0., 0., 0.}, {10., 0., 0.}} /* lhs */, LineString3d{{0., 0., 0.}, {10., 0., 0.}} /* rhs */, + 0. /* expected_distance */ + }, + { + LineString3d{{0., 5., 0.}, {10., 5., 0.}} /* lhs */, LineString3d{{0., 0., 0.}, {10., 0., 0.}} /* rhs */, + 5. /* expected_distance */ + }, + { + LineString3d{{0., 5., 0.}, {10., 5., 0.}} /* lhs */, + LineString3d{{0., 0., 0.}, {2., 0., 0.}, {4., 0., 0.}, {6., 0., 0.}, {8., 0., 0.}, {10., 0., 0.}} /* rhs */, + 5. /* expected_distance */ + }, + { + LineString3d{{0., 0., 0.}, {2., 0., 0.}, {4., 0., 0.}, {6., 0., 0.}, {8., 0., 0.}, {10., 0., 0.}} /* lhs */, + LineString3d{{0., 5., 0.}, {10., 5., 0.}} /* rhs */, 5. /* expected_distance */ + }, + }; +} + +class ComputeDistanceTest : public ::testing::TestWithParam { + public: + static constexpr double kTolerance{1e-12}; + ComputeDistanceTestCase case_ = GetParam(); +}; + +TEST_P(ComputeDistanceTest, Test) { + const auto dut = ComputeDistance(case_.lhs, case_.rhs); + EXPECT_NEAR(case_.expected_distance, dut, kTolerance); +} + +INSTANTIATE_TEST_CASE_P(ComputeDistanceTestGroup, ComputeDistanceTest, ::testing::ValuesIn(ComputeDistanceTestCases())); + } // namespace } // namespace test } // namespace utility diff --git a/test/parser/CMakeLists.txt b/test/parser/CMakeLists.txt index a342d25..2c69470 100644 --- a/test/parser/CMakeLists.txt +++ b/test/parser/CMakeLists.txt @@ -1,5 +1,6 @@ ament_add_gtest(lane_parser_test lane_test.cc) ament_add_gtest(segment_parser_test segment_test.cc) +ament_add_gtest(validator_test validator_test.cc) macro(add_dependencies_to_test target) if (TARGET ${target}) @@ -20,5 +21,6 @@ if (TARGET ${target}) endif() endmacro() +add_dependencies_to_test(validator_test) add_dependencies_to_test(lane_parser_test) add_dependencies_to_test(segment_parser_test) diff --git a/test/parser/validator_test.cc b/test/parser/validator_test.cc new file mode 100644 index 0000000..aa25e44 --- /dev/null +++ b/test/parser/validator_test.cc @@ -0,0 +1,272 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput_sparse/parser/validator.h" + +#include + +#include + +#include "maliput_sparse/parser/parser.h" + +namespace maliput_sparse { +namespace parser { +namespace test { +namespace { + +using maliput_sparse::geometry::LineString3d; + +class MockParser : public Parser { + public: + MockParser(const std::unordered_map& junctions = {}, + const std::vector& connections = {}) + : junctions_(junctions), connections_(connections) {} + + private: + const std::unordered_map& DoGetJunctions() const override { return junctions_; } + const std::vector& DoGetConnections() const override { return connections_; } + const std::unordered_map junctions_; + const std::vector connections_; +}; + +Lane CreateLane(const Lane::Id& id, const LineString3d& left, const LineString3d& right, + const std::optional& left_lane_id, const std::optional& right_lane_id, + const std::unordered_map& successors, + const std::unordered_map& predecessors) { + return Lane{id, left, right, left_lane_id, right_lane_id, successors, predecessors}; +} + +class ValidatorTest : public ::testing::Test { + public: + static MockParser CreateMockParser(const std::vector& lanes) { + const Junction junction{"junction", {{"segment", Segment{"segment", lanes}}}}; + return MockParser({{junction.id, junction}}, {}); + } + + const Lane lane_0 = CreateLane("0", LineString3d{{1., 1., 1.}, {10., 1., 1.}}, + LineString3d{{1., -1., 1.}, {10., -1., 1.}}, {"1"}, std::nullopt, {}, {}); + const Lane lane_1 = CreateLane("1", LineString3d{{1., 3., 1.}, {10., 3., 1.}}, + LineString3d{{1., 1., 1.}, {10., 1., 1.}}, {"2"}, {"0"}, {}, {}); + const Lane lane_2 = CreateLane("2", LineString3d{{1., 5., 1.}, {10., 5., 1.}}, + LineString3d{{1., 3., 1.}, {10., 3., 1.}}, {"3"}, {"1"}, {}, {}); + const Lane lane_3 = CreateLane("3", LineString3d{{1., 7., 1.}, {10., 7., 1.}}, + LineString3d{{1., 5., 1.}, {10., 5., 1.}}, std::nullopt, {"2"}, {}, {}); +}; + +TEST_F(ValidatorTest, NoErrors) { + const Segment segment{"segment", {lane_0, lane_1, lane_2, lane_3}}; + const Junction junction{"junction", {{segment.id, segment}}}; + const MockParser parser({{junction.id, junction}}, {}); + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + EXPECT_EQ(0., errors.size()); +} + +TEST_F(ValidatorTest, NoValidRightLaneInTheSegment) { + const std::vector expected_errors{ + {"Wrong ordering of lanes: Lane 1 has a left lane id (2) that has a right lane id (41856) that is not the lane " + "1.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Adjacent lane isn't part of the segment: Lane 2 has a right lane id (41856) that is not part of the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 2 has a right lane id (41856) that is not the previous lane in the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + Lane lane_2_no_valid_right_id{lane_2}; + lane_2_no_valid_right_id.right_lane_id = "41856"; + const Segment segment{"segment", {lane_0, lane_1, lane_2_no_valid_right_id, lane_3}}; + const Junction junction{"junction", {{segment.id, segment}}}; + const MockParser parser({{junction.id, junction}}, {}); + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoValidLeftLaneInTheSegment) { + const std::vector expected_errors{ + {"Adjacent lane isn't part of the segment: Lane 2 has a left lane id (41856) that is not part of the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 2 has a left lane id (41856) that is not the next lane in the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 3 has a right lane id (2) that has a left lane id (41856) that is not the lane " + "3.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + Lane lane_2_no_valid_left_id{lane_2}; + lane_2_no_valid_left_id.left_lane_id = "41856"; + const MockParser parser{CreateMockParser({lane_0, lane_1, lane_2_no_valid_left_id, lane_3})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoCorrespondenceForRightLane) { + const std::vector expected_errors{ + {"Adjacent lane isn't part of the segment: Lane 0 has a left lane id (54656465) that is not part of the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 0 has a left lane id (54656465) that is not the next lane in the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 1 has a right lane id (0) that has a left lane id (54656465) that is not the " + "lane 1.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 1 has no left lane id but it isn't the last lane in the segment segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 2 has a right lane id (1) that has no left lane id.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + Lane lane_0_with_other_left_lane_id{lane_0}; + lane_0_with_other_left_lane_id.left_lane_id = "54656465"; + Lane lane_1_without_left_lane_id{lane_1}; + lane_1_without_left_lane_id.left_lane_id = std::nullopt; + const MockParser parser{ + CreateMockParser({lane_0_with_other_left_lane_id, lane_1_without_left_lane_id, lane_2, lane_3})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoCorrespondenceForLeftLane) { + const std::vector expected_errors{ + {"Wrong ordering of lanes: Lane 1 has a left lane id (2) that has no right lane id.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 2 has no right lane id but it isn't the first lane in the segment segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 2 has a left lane id (3) that has a right lane id (54656465) that is not the " + "lane 2.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Adjacent lane isn't part of the segment: Lane 3 has a right lane id (54656465) that is not part of the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 3 has a right lane id (54656465) that is not the previous lane in the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + Lane lane_2_without_right_lane_id{lane_2}; + lane_2_without_right_lane_id.right_lane_id = std::nullopt; + Lane lane_3_with_other_right_lane_id{lane_3}; + lane_3_with_other_right_lane_id.right_lane_id = "54656465"; + const MockParser parser{ + CreateMockParser({lane_0, lane_1, lane_2_without_right_lane_id, lane_3_with_other_right_lane_id})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoRightLaneIdForANonFirstLane) { + const std::vector expected_errors{ + {"Wrong ordering of lanes: Lane 0 has a left lane id (1) that has no right lane id.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 1 has no right lane id but it isn't the first lane in the segment segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + Lane lane_1_without_right_lane_id{lane_1}; + lane_1_without_right_lane_id.right_lane_id = std::nullopt; + const MockParser parser{CreateMockParser({lane_0, lane_1_without_right_lane_id, lane_2, lane_3})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoLeftLaneIdForANonLastLane) { + const std::vector expected_errors{ + {"Wrong ordering of lanes: Lane 1 has no left lane id but it isn't the last lane in the segment segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Wrong ordering of lanes: Lane 2 has a right lane id (1) that has no left lane id.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + Lane lane_1_without_left_lane_id{lane_1}; + lane_1_without_left_lane_id.left_lane_id = std::nullopt; + const MockParser parser{CreateMockParser({lane_0, lane_1_without_left_lane_id, lane_2, lane_3})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoRightLanes) { + const std::vector expected_errors{ + {"Wrong ordering of lanes: Lane 3 has a right lane id (2) but is the first lane in the segment segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Adjacent lane isn't part of the segment: Lane 3 has a right lane id (2) that is not part of the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + const MockParser parser{CreateMockParser({lane_3})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoLeftLanes) { + const std::vector expected_errors{ + {"Wrong ordering of lanes: Lane 0 has a left lane id (1) but is the last lane in the segment segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Adjacent lane isn't part of the segment: Lane 0 has a left lane id (1) that is not part of the segment " + "segment.", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + const MockParser parser{CreateMockParser({lane_0})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +TEST_F(ValidatorTest, NoGeometricallyAdjacentLanes) { + Lane lane_0_no_geometrically_adj{lane_0}; + lane_0_no_geometrically_adj.left = LineString3d{{1., 7., 1.}, {10., 7., 1.}}; + const std::vector expected_errors{ + {"Lane 0 and lane 1 are not adjacent under the tolerance 0.001000. (distance: 6.000000)", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Lane 1 and lane 0 are not adjacent under the tolerance 0.001000. (distance: 6.000000)", + Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + }; + const MockParser parser{CreateMockParser({lane_0_no_geometrically_adj, lane_1, lane_2, lane_3})}; + const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); + const auto errors = dut.GetErrors(); + ASSERT_EQ(expected_errors.size(), errors.size()); + EXPECT_EQ(expected_errors, errors); +} + +} // namespace +} // namespace test +} // namespace parser +} // namespace maliput_sparse From ad67ef8d97e8839bdd4c9fb86d9c11c8cc3fae83 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Mon, 19 Dec 2022 16:57:13 -0300 Subject: [PATCH 2/5] Addresses comments. Signed-off-by: Franco Cipollone --- include/maliput_sparse/parser/validator.h | 35 ++-- src/geometry/utility/geometry.cc | 13 +- src/loader/road_geometry_loader.cc | 19 ++- src/parser/validator.cc | 184 +++++++++++----------- test/parser/validator_test.cc | 137 +++++++++------- 5 files changed, 204 insertions(+), 184 deletions(-) diff --git a/include/maliput_sparse/parser/validator.h b/include/maliput_sparse/parser/validator.h index 8ca0279..30724bf 100644 --- a/include/maliput_sparse/parser/validator.h +++ b/include/maliput_sparse/parser/validator.h @@ -60,7 +60,8 @@ class Validator { struct Error { /// The type of error. enum class Type { - kLaneAdjacency, + kLogicalLaneAdjacency, + kGeometricalLaneAdjacency, }; /// The severity of the error. enum class Severity { @@ -87,29 +88,25 @@ class Validator { /// @param parser The maliput_sparse::parser::Parser instance to validate. /// @param options The maliput_sparse::parser::ValidatorOptions to use. /// @param config The maliput_sparse::parser::ValidatorConfig to use. - Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig config); + Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig& config); /// Returns the errors found during validation. - const std::vector& GetErrors() const; + std::vector operator()() const; private: - // Helper functions for reporting errors. - // @param message The error message. - // @Param type The type of error. - // @Param severity The severity of the error. - void Report(const std::string& message, const Error::Type& type, const Error::Severity& severity); - - // Method to validate lane adjacency. - // @param parser The maliput_sparse::parser::Parser instance to validate. - // @param config The maliput_sparse::parser::ValidatorConfig to use. - void ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config); - - // Helper function to geometrically check lane adjacency. - void CheckAdjacency(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance); - - // Holds the errors found during validation. - std::vector errors_; + // The maliput_sparse::parser::Parser instance to validate. + const Parser* parser_{nullptr}; + // The maliput_sparse::parser::ValidatorOptions to use. + const ValidatorOptions options_; + // The maliput_sparse::parser::ValidatorConfig to use. + const ValidatorConfig config_; }; +/// Validates lane adjacency. +/// @param parser The maliput_sparse::parser::Parser instance to validate. +/// @param config The maliput_sparse::parser::ValidatorConfig to use. +/// @returns A vector of maliput_sparse::parser::Validator::Error. +std::vector ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config); + } // namespace parser } // namespace maliput_sparse diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc index 3fab368..db07fd5 100644 --- a/src/geometry/utility/geometry.cc +++ b/src/geometry/utility/geometry.cc @@ -60,6 +60,7 @@ #include #include +#include namespace maliput_sparse { namespace geometry { @@ -377,12 +378,12 @@ ClosestPointResult GetClosestPoint(const LineString3d& line_string, const malipu double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs) { const LineString3d& base_ls = rhs.size() > lhs.size() ? rhs : lhs; const LineString3d& other_ls = rhs.size() > lhs.size() ? lhs : rhs; - double sum_distances{}; - for (const auto& base_point : base_ls) { - const auto closest_point_res = GetClosestPoint(other_ls, base_point); - sum_distances += closest_point_res.distance; - } - return sum_distances / base_ls.size(); + const double sum_distances = + std::accumulate(base_ls.begin(), base_ls.end(), 0.0, [&other_ls](double sum, const auto& base_point) { + const auto closest_point_res = GetClosestPoint(other_ls, base_point); + return sum + closest_point_res.distance; + }); + return sum_distances / static_cast(base_ls.size()); } } // namespace utility diff --git a/src/loader/road_geometry_loader.cc b/src/loader/road_geometry_loader.cc index c1504bc..77716a2 100644 --- a/src/loader/road_geometry_loader.cc +++ b/src/loader/road_geometry_loader.cc @@ -59,15 +59,18 @@ RoadGeometryLoader::RoadGeometryLoader(std::unique_ptr parser, std::unique_ptr RoadGeometryLoader::operator()() { // Validates the parsed data before building the RoadGeometry. - const parser::Validator validator(parser_.get(), parser::ValidatorOptions{true}, - parser::ValidatorConfig{builder_configuration_.linear_tolerance}); - const auto errors = validator.GetErrors(); + const auto errors = parser::Validator(parser_.get(), parser::ValidatorOptions{true}, + parser::ValidatorConfig{builder_configuration_.linear_tolerance})(); for (const auto& error : errors) { - if (error.severity == parser::Validator::Error::Severity::kError) { - maliput::log()->error("{}", error.message); - } - if (error.severity == parser::Validator::Error::Severity::kWarning) { - maliput::log()->warn("{}", error.message); + switch (error.severity) { + case parser::Validator::Error::Severity::kError: + maliput::log()->error("{}", error.message); + break; + case parser::Validator::Error::Severity::kWarning: + maliput::log()->warn("{}", error.message); + break; + default: + MALIPUT_THROW_MESSAGE("Unknown parser::Validator::Error::Severity value: " + static_cast(error.severity)); } } diff --git a/src/parser/validator.cc b/src/parser/validator.cc index 60cfec2..7ffe885 100644 --- a/src/parser/validator.cc +++ b/src/parser/validator.cc @@ -43,35 +43,41 @@ namespace { static constexpr bool kLeft{true}; static constexpr bool kRight{!kLeft}; -} // namespace - -Validator::Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig config) { - MALIPUT_THROW_UNLESS(parser); - if (options.lane_adjacency) { - ValidateLaneAdjacency(parser, config); - } -} +// Helper function to geometrically check lane adjacency. +// It relies on geometry::utility::ComputeDistance to compute the distance between the two adjacent line strings. +// Returns true if the lanes are adjacent under certain tolerance, false otherwise. -const std::vector& Validator::GetErrors() const { return errors_; } - -void Validator::CheckAdjacency(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance) { +bool AreAdjacent(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance) { const auto line_string_a = left ? lane.left : lane.right; const auto line_string_b = left ? adjacent_lane.right : adjacent_lane.left; const double distance = geometry::utility::ComputeDistance(line_string_a, line_string_b); - if (distance > tolerance) { - const std::string msg{"Lane " + lane.id + " and lane " + adjacent_lane.id + - " are not adjacent under the tolerance " + std::to_string(tolerance) + - ". (distance: " + std::to_string(distance) + ")"}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + return distance <= tolerance; } -void Validator::Report(const std::string& message, const Validator::Error::Type& type, - const Validator::Error::Severity& severity) { - errors_.push_back(Validator::Error{message, type, severity}); +} // namespace + +Validator::Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig& config) + : parser_{parser}, options_{options}, config_{config} { + MALIPUT_THROW_UNLESS(parser_); +} + +std::vector Validator::operator()() const { + std::vector errors; + if (options_.lane_adjacency) { + const auto lane_adjacency_errors = ValidateLaneAdjacency(parser_, config_); + errors.insert(errors.end(), lane_adjacency_errors.begin(), lane_adjacency_errors.end()); + } + return errors; } -void Validator::ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config) { +std::vector ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config) { + std::vector errors; + auto evaluate = [&errors](bool condition, const std::string& message, const Validator::Error::Type& error_type) { + if (condition) { + errors.push_back({message, error_type, Validator::Error::Severity::kError}); + } + return condition; + }; const auto junctions = parser->GetJunctions(); for (const auto junction : junctions) { for (const auto segment : junction.second.segments) { @@ -82,130 +88,122 @@ void Validator::ValidateLaneAdjacency(const Parser* parser, const ValidatorConfi const Lane& lane = lanes[idx]; // Note: The following code is a bit repetitive for left and right adjacency checks, but it is easier to read - // and understand. Check right adjacency <-------------------------------------------------- + // and understand. + // + // Check right adjacency <-------------------------------------------------- if (lane.right_lane_id) { // Check if there is a idx to the right. - if (idx == 0) { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + - lane.right_lane_id.value() + ") but is the first lane in the segment " + - segment.first + "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + evaluate(idx == 0, + {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() + + ") but is the first lane in the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); // Check if right lane id is in the same segment const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { return lane.right_lane_id.value() == lane_it.id; }); if (adj_lane_it == lanes.end()) { // Error. - const std::string msg{"Adjacent lane isn't part of the segment: Lane " + lane.id + - " has a right lane id (" + lane.right_lane_id.value() + - ") that is not part of the segment " + segment.first + "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + evaluate(true, + {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); } else { // Check if right lane id has the lane id as left lane id. if (adj_lane_it->left_lane_id) { - if (adj_lane_it->left_lane_id.value() != lane.id) { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + - lane.right_lane_id.value() + ") that has a left lane id (" + - adj_lane_it->left_lane_id.value() + ") that is not the lane " + lane.id + "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + evaluate(adj_lane_it->left_lane_id.value() != lane.id, + {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that has a left lane id (" + adj_lane_it->left_lane_id.value() + + ") that is not the lane " + lane.id + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); + } else { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + - lane.right_lane_id.value() + ") that has no left lane id."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + evaluate(true, + {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that has no left lane id."}, + Validator::Error::Type::kLogicalLaneAdjacency); } // Check geometrical adjacency. - CheckAdjacency(lane, *adj_lane_it, kRight, config.linear_tolerance); + evaluate(!AreAdjacent(lane, *adj_lane_it, kRight, config.linear_tolerance), + {"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " + + std::to_string(config.linear_tolerance) + "."}, + Validator::Error::Type::kGeometricalLaneAdjacency); } // Check if idx - 1 lane is the right lane id. - if ((idx - 1 >= 0) && (lanes[idx - 1].id != lane.right_lane_id.value())) { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + - lane.right_lane_id.value() + ") that is not the previous lane in the segment " + - segment.first + "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + evaluate((idx - 1 >= 0) && (lanes[idx - 1].id != lane.right_lane_id.value()), + {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() + + ") that is not the previous lane in the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); } else { // Check if idx is the first lane in the segment. - if (idx != 0) { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + - " has no right lane id but it isn't the first lane in the segment " + segment.first + - "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + evaluate(idx != 0, + {"Wrong ordering of lanes: Lane " + lane.id + + " has no right lane id but it isn't the first lane in the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); } // Check left adjacency <-------------------------------------------------- if (lane.left_lane_id) { // Check if there is a idx to the left. - if (idx == static_cast(lanes.size()) - 1) { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") but is the last lane in the segment " + segment.first + - "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + evaluate(idx == static_cast(lanes.size()) - 1, + {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() + + ") but is the last lane in the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); + // Check if left lane id is in the same segment const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { return lane.left_lane_id.value() == lane_it.id; }); if (adj_lane_it == lanes.end()) { - // Error. - const std::string msg{"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") that is not part of the segment " + segment.first + - "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + evaluate(true, + {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); } else { // Check if left lane id has the lane id as right lane id. if (adj_lane_it->right_lane_id) { - if (adj_lane_it->right_lane_id.value() != lane.id) { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") that has a right lane id (" + - adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + evaluate(adj_lane_it->right_lane_id.value() != lane.id, + {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that has a right lane id (" + + adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); } else { // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") that has no right lane id."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + evaluate(true, + {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that has no right lane id."}, + Validator::Error::Type::kLogicalLaneAdjacency); } // Check geometrical adjacency. - CheckAdjacency(lane, *adj_lane_it, kLeft, config.linear_tolerance); + evaluate(!AreAdjacent(lane, *adj_lane_it, kLeft, config.linear_tolerance), + {"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " + + std::to_string(config.linear_tolerance) + "."}, + Validator::Error::Type::kGeometricalLaneAdjacency); } // Check if idx + 1 lane is the left lane id. if ((idx + 1 <= static_cast(lanes.size()) - 1) && (lanes[idx + 1].id != lane.left_lane_id.value())) { // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") that is not the next lane in the segment " + - segment.first + "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); + evaluate(true, + {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() + + ") that is not the next lane in the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); } } else { // Check if idx is the last lane in the segment. - if (idx != static_cast(lanes.size()) - 1) { - // Error. - const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + - " has no left lane id but it isn't the last lane in the segment " + segment.first + - "."}; - Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError); - } + evaluate(idx != static_cast(lanes.size()) - 1, + {"Wrong ordering of lanes: Lane " + lane.id + + " has no left lane id but it isn't the last lane in the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); } } } } + return errors; } bool Validator::Error::operator==(const Error& other) const { diff --git a/test/parser/validator_test.cc b/test/parser/validator_test.cc index aa25e44..aed6494 100644 --- a/test/parser/validator_test.cc +++ b/test/parser/validator_test.cc @@ -32,6 +32,7 @@ #include #include +#include #include "maliput_sparse/parser/parser.h" @@ -69,6 +70,36 @@ class ValidatorTest : public ::testing::Test { return MockParser({{junction.id, junction}}, {}); } + protected: + const Lane wrong_adj_lane{CreateLane("0", LineString3d{{1., 1., 1.}, {10., 1., 1.}}, + LineString3d{{1., -1., 1.}, {10., -1., 1.}}, {"23123"}, std::nullopt, {}, {})}; +}; + +TEST_F(ValidatorTest, InvalidParser) { + EXPECT_THROW(Validator(nullptr, {}, ValidatorConfig{1e-3}), maliput::common::assertion_error); +} + +TEST_F(ValidatorTest, AllOptionsDisabled) { + ValidatorOptions options; + options.lane_adjacency = false; + const MockParser parser = CreateMockParser({wrong_adj_lane}); + const auto errors = Validator(&parser, options, ValidatorConfig{1e-3})(); + ASSERT_EQ(0., errors.size()); +} + +TEST_F(ValidatorTest, LaneAdjacencyOption) { + ValidatorOptions options; + options.lane_adjacency = true; + const MockParser parser = CreateMockParser({wrong_adj_lane}); + const auto errors = Validator(&parser, options, ValidatorConfig{1e-3})(); + ASSERT_NE(0., errors.size()); + for (const auto& error : errors) { + EXPECT_TRUE(error.type == Validator::Error::Type::kLogicalLaneAdjacency); + } +} + +class ValidateLaneAdjacencyTest : public ValidatorTest { + public: const Lane lane_0 = CreateLane("0", LineString3d{{1., 1., 1.}, {10., 1., 1.}}, LineString3d{{1., -1., 1.}, {10., -1., 1.}}, {"1"}, std::nullopt, {}, {}); const Lane lane_1 = CreateLane("1", LineString3d{{1., 3., 1.}, {10., 3., 1.}}, @@ -79,74 +110,71 @@ class ValidatorTest : public ::testing::Test { LineString3d{{1., 5., 1.}, {10., 5., 1.}}, std::nullopt, {"2"}, {}, {}); }; -TEST_F(ValidatorTest, NoErrors) { +TEST_F(ValidateLaneAdjacencyTest, NoErrors) { const Segment segment{"segment", {lane_0, lane_1, lane_2, lane_3}}; const Junction junction{"junction", {{segment.id, segment}}}; const MockParser parser({{junction.id, junction}}, {}); - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); EXPECT_EQ(0., errors.size()); } -TEST_F(ValidatorTest, NoValidRightLaneInTheSegment) { +TEST_F(ValidateLaneAdjacencyTest, NoValidRightLaneInTheSegment) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 1 has a left lane id (2) that has a right lane id (41856) that is not the lane " "1.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 2 has a right lane id (41856) that is not part of the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a right lane id (41856) that is not the previous lane in the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_2_no_valid_right_id{lane_2}; lane_2_no_valid_right_id.right_lane_id = "41856"; const Segment segment{"segment", {lane_0, lane_1, lane_2_no_valid_right_id, lane_3}}; const Junction junction{"junction", {{segment.id, segment}}}; const MockParser parser({{junction.id, junction}}, {}); - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoValidLeftLaneInTheSegment) { +TEST_F(ValidateLaneAdjacencyTest, NoValidLeftLaneInTheSegment) { const std::vector expected_errors{ {"Adjacent lane isn't part of the segment: Lane 2 has a left lane id (41856) that is not part of the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a left lane id (41856) that is not the next lane in the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 3 has a right lane id (2) that has a left lane id (41856) that is not the lane " "3.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_2_no_valid_left_id{lane_2}; lane_2_no_valid_left_id.left_lane_id = "41856"; const MockParser parser{CreateMockParser({lane_0, lane_1, lane_2_no_valid_left_id, lane_3})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoCorrespondenceForRightLane) { +TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForRightLane) { const std::vector expected_errors{ {"Adjacent lane isn't part of the segment: Lane 0 has a left lane id (54656465) that is not part of the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 0 has a left lane id (54656465) that is not the next lane in the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 1 has a right lane id (0) that has a left lane id (54656465) that is not the " "lane 1.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 1 has no left lane id but it isn't the last lane in the segment segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a right lane id (1) that has no left lane id.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_0_with_other_left_lane_id{lane_0}; lane_0_with_other_left_lane_id.left_lane_id = "54656465"; @@ -154,27 +182,26 @@ TEST_F(ValidatorTest, NoCorrespondenceForRightLane) { lane_1_without_left_lane_id.left_lane_id = std::nullopt; const MockParser parser{ CreateMockParser({lane_0_with_other_left_lane_id, lane_1_without_left_lane_id, lane_2, lane_3})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoCorrespondenceForLeftLane) { +TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForLeftLane) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 1 has a left lane id (2) that has no right lane id.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has no right lane id but it isn't the first lane in the segment segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a left lane id (3) that has a right lane id (54656465) that is not the " "lane 2.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 3 has a right lane id (54656465) that is not part of the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 3 has a right lane id (54656465) that is not the previous lane in the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_2_without_right_lane_id{lane_2}; lane_2_without_right_lane_id.right_lane_id = std::nullopt; @@ -182,86 +209,80 @@ TEST_F(ValidatorTest, NoCorrespondenceForLeftLane) { lane_3_with_other_right_lane_id.right_lane_id = "54656465"; const MockParser parser{ CreateMockParser({lane_0, lane_1, lane_2_without_right_lane_id, lane_3_with_other_right_lane_id})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoRightLaneIdForANonFirstLane) { +TEST_F(ValidateLaneAdjacencyTest, NoRightLaneIdForANonFirstLane) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 0 has a left lane id (1) that has no right lane id.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 1 has no right lane id but it isn't the first lane in the segment segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_1_without_right_lane_id{lane_1}; lane_1_without_right_lane_id.right_lane_id = std::nullopt; const MockParser parser{CreateMockParser({lane_0, lane_1_without_right_lane_id, lane_2, lane_3})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoLeftLaneIdForANonLastLane) { +TEST_F(ValidateLaneAdjacencyTest, NoLeftLaneIdForANonLastLane) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 1 has no left lane id but it isn't the last lane in the segment segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a right lane id (1) that has no left lane id.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_1_without_left_lane_id{lane_1}; lane_1_without_left_lane_id.left_lane_id = std::nullopt; const MockParser parser{CreateMockParser({lane_0, lane_1_without_left_lane_id, lane_2, lane_3})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoRightLanes) { +TEST_F(ValidateLaneAdjacencyTest, NoRightLanes) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 3 has a right lane id (2) but is the first lane in the segment segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 3 has a right lane id (2) that is not part of the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; const MockParser parser{CreateMockParser({lane_3})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoLeftLanes) { +TEST_F(ValidateLaneAdjacencyTest, NoLeftLanes) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 0 has a left lane id (1) but is the last lane in the segment segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 0 has a left lane id (1) that is not part of the segment " "segment.", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; const MockParser parser{CreateMockParser({lane_0})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } -TEST_F(ValidatorTest, NoGeometricallyAdjacentLanes) { +TEST_F(ValidateLaneAdjacencyTest, NoGeometricallyAdjacentLanes) { Lane lane_0_no_geometrically_adj{lane_0}; lane_0_no_geometrically_adj.left = LineString3d{{1., 7., 1.}, {10., 7., 1.}}; const std::vector expected_errors{ - {"Lane 0 and lane 1 are not adjacent under the tolerance 0.001000. (distance: 6.000000)", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, - {"Lane 1 and lane 0 are not adjacent under the tolerance 0.001000. (distance: 6.000000)", - Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError}, + {"Lane 0 and lane 1 are not adjacent under the tolerance 0.001000.", + Validator::Error::Type::kGeometricalLaneAdjacency, Validator::Error::Severity::kError}, + {"Lane 1 and lane 0 are not adjacent under the tolerance 0.001000.", + Validator::Error::Type::kGeometricalLaneAdjacency, Validator::Error::Severity::kError}, }; const MockParser parser{CreateMockParser({lane_0_no_geometrically_adj, lane_1, lane_2, lane_3})}; - const Validator dut(&parser, ValidatorOptions{true}, ValidatorConfig{1e-3}); - const auto errors = dut.GetErrors(); + const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } From 68c48cfa130e943d56c01610faa71029217872c9 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Wed, 21 Dec 2022 18:07:13 -0300 Subject: [PATCH 3/5] Addresses comments Signed-off-by: Franco Cipollone --- include/maliput_sparse/parser/validator.h | 6 --- src/parser/validation_methods.h | 47 ++++++++++++++++ src/parser/validator.cc | 66 ++++++++++------------- test/parser/validator_test.cc | 1 + 4 files changed, 75 insertions(+), 45 deletions(-) create mode 100644 src/parser/validation_methods.h diff --git a/include/maliput_sparse/parser/validator.h b/include/maliput_sparse/parser/validator.h index 30724bf..7668edd 100644 --- a/include/maliput_sparse/parser/validator.h +++ b/include/maliput_sparse/parser/validator.h @@ -102,11 +102,5 @@ class Validator { const ValidatorConfig config_; }; -/// Validates lane adjacency. -/// @param parser The maliput_sparse::parser::Parser instance to validate. -/// @param config The maliput_sparse::parser::ValidatorConfig to use. -/// @returns A vector of maliput_sparse::parser::Validator::Error. -std::vector ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config); - } // namespace parser } // namespace maliput_sparse diff --git a/src/parser/validation_methods.h b/src/parser/validation_methods.h new file mode 100644 index 0000000..55f96ef --- /dev/null +++ b/src/parser/validation_methods.h @@ -0,0 +1,47 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include + +#include "maliput_sparse/parser/parser.h" +#include "maliput_sparse/parser/validator.h" + +namespace maliput_sparse { +namespace parser { + +/// Validates lane adjacency. +/// @param parser The maliput_sparse::parser::Parser instance to validate. +/// @param config The maliput_sparse::parser::ValidatorConfig to use. +/// @returns A vector of maliput_sparse::parser::Validator::Error. +std::vector ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config); + +} // namespace parser +} // namespace maliput_sparse diff --git a/src/parser/validator.cc b/src/parser/validator.cc index 7ffe885..190f562 100644 --- a/src/parser/validator.cc +++ b/src/parser/validator.cc @@ -33,6 +33,7 @@ #include "maliput_sparse/parser/junction.h" #include "maliput_sparse/parser/lane.h" #include "maliput_sparse/parser/segment.h" +#include "parser/validation_methods.h" namespace maliput_sparse { namespace parser { @@ -101,27 +102,20 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { return lane.right_lane_id.value() == lane_it.id; }); - if (adj_lane_it == lanes.end()) { - // Error. - evaluate(true, - {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a right lane id (" + - lane.right_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); - } else { + if (!evaluate(adj_lane_it == lanes.end(), + {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency)) { // Check if right lane id has the lane id as left lane id. - if (adj_lane_it->left_lane_id) { - evaluate(adj_lane_it->left_lane_id.value() != lane.id, - {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + - lane.right_lane_id.value() + ") that has a left lane id (" + adj_lane_it->left_lane_id.value() + - ") that is not the lane " + lane.id + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); - - } else { - evaluate(true, - {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + - lane.right_lane_id.value() + ") that has no left lane id."}, - Validator::Error::Type::kLogicalLaneAdjacency); - } + !evaluate(!adj_lane_it->left_lane_id.has_value(), + {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that has no left lane id."}, + Validator::Error::Type::kLogicalLaneAdjacency) && + evaluate(adj_lane_it->left_lane_id.value() != lane.id, + {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + + lane.right_lane_id.value() + ") that has a left lane id (" + + adj_lane_it->left_lane_id.value() + ") that is not the lane " + lane.id + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); // Check geometrical adjacency. evaluate(!AreAdjacent(lane, *adj_lane_it, kRight, config.linear_tolerance), @@ -156,26 +150,20 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { return lane.left_lane_id.value() == lane_it.id; }); - if (adj_lane_it == lanes.end()) { - evaluate(true, - {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); - } else { + if (!evaluate(adj_lane_it == lanes.end(), + {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, + Validator::Error::Type::kLogicalLaneAdjacency)) { // Check if left lane id has the lane id as right lane id. - if (adj_lane_it->right_lane_id) { - evaluate(adj_lane_it->right_lane_id.value() != lane.id, - {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") that has a right lane id (" + - adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); - } else { - // Error. - evaluate(true, - {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + - lane.left_lane_id.value() + ") that has no right lane id."}, - Validator::Error::Type::kLogicalLaneAdjacency); - } + !evaluate(!adj_lane_it->right_lane_id.has_value(), + {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that has no right lane id."}, + Validator::Error::Type::kLogicalLaneAdjacency) && + evaluate(adj_lane_it->right_lane_id.value() != lane.id, + {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + + lane.left_lane_id.value() + ") that has a right lane id (" + + adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."}, + Validator::Error::Type::kLogicalLaneAdjacency); // Check geometrical adjacency. evaluate(!AreAdjacent(lane, *adj_lane_it, kLeft, config.linear_tolerance), diff --git a/test/parser/validator_test.cc b/test/parser/validator_test.cc index aed6494..f9cd12e 100644 --- a/test/parser/validator_test.cc +++ b/test/parser/validator_test.cc @@ -35,6 +35,7 @@ #include #include "maliput_sparse/parser/parser.h" +#include "parser/validation_methods.h" namespace maliput_sparse { namespace parser { From be8e284e968d38d095a4006f9eddc1dd3e6a1d9d Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Thu, 22 Dec 2022 14:31:07 -0300 Subject: [PATCH 4/5] Addresses comments. Signed-off-by: Franco Cipollone --- include/maliput_sparse/parser/validator.h | 38 +++--- src/loader/road_geometry_loader.cc | 10 +- src/parser/validation_methods.h | 4 +- src/parser/validator.cc | 94 +++++++++++---- test/parser/validator_test.cc | 138 ++++++++++++++-------- 5 files changed, 191 insertions(+), 93 deletions(-) diff --git a/include/maliput_sparse/parser/validator.h b/include/maliput_sparse/parser/validator.h index 7668edd..f8dedb7 100644 --- a/include/maliput_sparse/parser/validator.h +++ b/include/maliput_sparse/parser/validator.h @@ -29,7 +29,10 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include #include +#include +#include #include #include "maliput_sparse/parser/parser.h" @@ -37,12 +40,6 @@ namespace maliput_sparse { namespace parser { -/// ValidatorOptions struct that contains the options for the Validator. -struct ValidatorOptions { - /// Verifies adjacency for lanes in a segment. - bool lane_adjacency{true}; -}; - /// ValidatorConfig struct that contains the configuration for the Validator. struct ValidatorConfig { double linear_tolerance{1e-12}; @@ -56,13 +53,14 @@ struct ValidatorConfig { /// severity. It's on the user to decide how to handle the errors. class Validator { public: + /// The type of validation. + enum class Type { + kLogicalLaneAdjacency, + kGeometricalLaneAdjacency, + }; + /// Error struct that contains the error message, type, and severity. struct Error { - /// The type of error. - enum class Type { - kLogicalLaneAdjacency, - kGeometricalLaneAdjacency, - }; /// The severity of the error. enum class Severity { kWarning, @@ -77,30 +75,38 @@ class Validator { /// Message describing the error. std::string message; /// The type of error. - Type type; + Validator::Type type; /// The severity of the error. Severity severity; }; + using Types = std::unordered_set; + /// Constructor for Validator. /// During construction, the Validator will perform the validation checks. //// /// @param parser The maliput_sparse::parser::Parser instance to validate. - /// @param options The maliput_sparse::parser::ValidatorOptions to use. + /// @param types The types of validation to perform. /// @param config The maliput_sparse::parser::ValidatorConfig to use. - Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig& config); + Validator(const Parser* parser, const Types& types, const ValidatorConfig& config); /// Returns the errors found during validation. std::vector operator()() const; private: + // Returns the types of validation that are dependent on the given type. + static const std::unordered_map> kDependentTypes; + // The maliput_sparse::parser::Parser instance to validate. const Parser* parser_{nullptr}; - // The maliput_sparse::parser::ValidatorOptions to use. - const ValidatorOptions options_; // The maliput_sparse::parser::ValidatorConfig to use. const ValidatorConfig config_; + // The types of validation to perform. + Types types_; }; +/// Serialize Validator::Type to ostream. +std::ostream& operator<<(std::ostream& os, const Validator::Type& type); + } // namespace parser } // namespace maliput_sparse diff --git a/src/loader/road_geometry_loader.cc b/src/loader/road_geometry_loader.cc index 77716a2..f2590cb 100644 --- a/src/loader/road_geometry_loader.cc +++ b/src/loader/road_geometry_loader.cc @@ -59,15 +59,17 @@ RoadGeometryLoader::RoadGeometryLoader(std::unique_ptr parser, std::unique_ptr RoadGeometryLoader::operator()() { // Validates the parsed data before building the RoadGeometry. - const auto errors = parser::Validator(parser_.get(), parser::ValidatorOptions{true}, - parser::ValidatorConfig{builder_configuration_.linear_tolerance})(); + const auto errors = parser::Validator( + parser_.get(), + {parser::Validator::Type::kLogicalLaneAdjacency, parser::Validator::Type::kGeometricalLaneAdjacency}, + parser::ValidatorConfig{builder_configuration_.linear_tolerance})(); for (const auto& error : errors) { switch (error.severity) { case parser::Validator::Error::Severity::kError: - maliput::log()->error("{}", error.message); + maliput::log()->error("[{}] {}", error.type, error.message); break; case parser::Validator::Error::Severity::kWarning: - maliput::log()->warn("{}", error.message); + maliput::log()->warn("[{}] {}", error.type, error.message); break; default: MALIPUT_THROW_MESSAGE("Unknown parser::Validator::Error::Severity value: " + static_cast(error.severity)); diff --git a/src/parser/validation_methods.h b/src/parser/validation_methods.h index 55f96ef..af73b00 100644 --- a/src/parser/validation_methods.h +++ b/src/parser/validation_methods.h @@ -39,9 +39,11 @@ namespace parser { /// Validates lane adjacency. /// @param parser The maliput_sparse::parser::Parser instance to validate. +/// @param validate_geometric_adjacency Whether to validate geometric adjacency. /// @param config The maliput_sparse::parser::ValidatorConfig to use. /// @returns A vector of maliput_sparse::parser::Validator::Error. -std::vector ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config); +std::vector ValidateLaneAdjacency(const Parser* parser, const bool& validate_geometric_adjacency, + const ValidatorConfig config); } // namespace parser } // namespace maliput_sparse diff --git a/src/parser/validator.cc b/src/parser/validator.cc index 190f562..ce38e9c 100644 --- a/src/parser/validator.cc +++ b/src/parser/validator.cc @@ -29,6 +29,8 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "maliput_sparse/parser/validator.h" +#include + #include "maliput_sparse/geometry/utility/geometry.h" #include "maliput_sparse/parser/junction.h" #include "maliput_sparse/parser/lane.h" @@ -57,23 +59,47 @@ bool AreAdjacent(const Lane& lane, const Lane& adjacent_lane, bool left, double } // namespace -Validator::Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig& config) - : parser_{parser}, options_{options}, config_{config} { +const std::unordered_map> Validator::kDependentTypes{ + {Validator::Type::kLogicalLaneAdjacency, {}}, + {Validator::Type::kGeometricalLaneAdjacency, {Validator::Type::kLogicalLaneAdjacency}}, +}; + +Validator::Validator(const Parser* parser, const Types& types, const ValidatorConfig& config) + : parser_{parser}, config_{config}, types_{types} { MALIPUT_THROW_UNLESS(parser_); + + // Evaluate dependencies of the validation types. + for (const auto& type : types_) { + const auto& dependencies = kDependentTypes.at(type); + for (const auto& dependency : dependencies) { + if (std::find(types_.begin(), types_.end(), dependency) == types_.end()) { + maliput::log()->debug("Validator: {} depends on {}, adding it to the validation types.", type, dependency); + types_.insert(dependency); + } + } + } } std::vector Validator::operator()() const { std::vector errors; - if (options_.lane_adjacency) { - const auto lane_adjacency_errors = ValidateLaneAdjacency(parser_, config_); + + // Lane adjacency validation. + const bool logical_lane_adjacency_enabled = + std::find(types_.begin(), types_.end(), Validator::Type::kLogicalLaneAdjacency) != types_.end(); + const bool geometrical_lane_adjacency_enabled = + std::find(types_.begin(), types_.end(), Validator::Type::kGeometricalLaneAdjacency) != types_.end(); + if (logical_lane_adjacency_enabled || geometrical_lane_adjacency_enabled) { + const auto lane_adjacency_errors = ValidateLaneAdjacency(parser_, geometrical_lane_adjacency_enabled, config_); errors.insert(errors.end(), lane_adjacency_errors.begin(), lane_adjacency_errors.end()); } + return errors; } -std::vector ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config) { +std::vector ValidateLaneAdjacency(const Parser* parser, const bool& validate_geometric_adjacency, + const ValidatorConfig config) { std::vector errors; - auto evaluate = [&errors](bool condition, const std::string& message, const Validator::Error::Type& error_type) { + auto evaluate = [&errors](bool condition, const std::string& message, const Validator::Type& error_type) { if (condition) { errors.push_back({message, error_type, Validator::Error::Severity::kError}); } @@ -97,7 +123,7 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const evaluate(idx == 0, {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() + ") but is the first lane in the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); // Check if right lane id is in the same segment const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { return lane.right_lane_id.value() == lane_it.id; @@ -105,37 +131,39 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const if (!evaluate(adj_lane_it == lanes.end(), {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency)) { + Validator::Type::kLogicalLaneAdjacency)) { // Check if right lane id has the lane id as left lane id. !evaluate(!adj_lane_it->left_lane_id.has_value(), {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() + ") that has no left lane id."}, - Validator::Error::Type::kLogicalLaneAdjacency) && + Validator::Type::kLogicalLaneAdjacency) && evaluate(adj_lane_it->left_lane_id.value() != lane.id, {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() + ") that has a left lane id (" + adj_lane_it->left_lane_id.value() + ") that is not the lane " + lane.id + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); // Check geometrical adjacency. - evaluate(!AreAdjacent(lane, *adj_lane_it, kRight, config.linear_tolerance), - {"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " + - std::to_string(config.linear_tolerance) + "."}, - Validator::Error::Type::kGeometricalLaneAdjacency); + if (validate_geometric_adjacency) { + evaluate(!AreAdjacent(lane, *adj_lane_it, kRight, config.linear_tolerance), + {"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " + + std::to_string(config.linear_tolerance) + "."}, + Validator::Type::kGeometricalLaneAdjacency); + } } // Check if idx - 1 lane is the right lane id. evaluate((idx - 1 >= 0) && (lanes[idx - 1].id != lane.right_lane_id.value()), {"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() + ") that is not the previous lane in the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); } else { // Check if idx is the first lane in the segment. evaluate(idx != 0, {"Wrong ordering of lanes: Lane " + lane.id + " has no right lane id but it isn't the first lane in the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); } // Check left adjacency <-------------------------------------------------- @@ -144,7 +172,7 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const evaluate(idx == static_cast(lanes.size()) - 1, {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() + ") but is the last lane in the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); // Check if left lane id is in the same segment const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) { @@ -153,23 +181,25 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const if (!evaluate(adj_lane_it == lanes.end(), {"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() + ") that is not part of the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency)) { + Validator::Type::kLogicalLaneAdjacency)) { // Check if left lane id has the lane id as right lane id. !evaluate(!adj_lane_it->right_lane_id.has_value(), {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() + ") that has no right lane id."}, - Validator::Error::Type::kLogicalLaneAdjacency) && + Validator::Type::kLogicalLaneAdjacency) && evaluate(adj_lane_it->right_lane_id.value() != lane.id, {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() + ") that has a right lane id (" + adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); // Check geometrical adjacency. - evaluate(!AreAdjacent(lane, *adj_lane_it, kLeft, config.linear_tolerance), - {"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " + - std::to_string(config.linear_tolerance) + "."}, - Validator::Error::Type::kGeometricalLaneAdjacency); + if (validate_geometric_adjacency) { + evaluate(!AreAdjacent(lane, *adj_lane_it, kLeft, config.linear_tolerance), + {"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " + + std::to_string(config.linear_tolerance) + "."}, + Validator::Type::kGeometricalLaneAdjacency); + } } // Check if idx + 1 lane is the left lane id. @@ -178,7 +208,7 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const evaluate(true, {"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() + ") that is not the next lane in the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); } } else { @@ -186,7 +216,7 @@ std::vector ValidateLaneAdjacency(const Parser* parser, const evaluate(idx != static_cast(lanes.size()) - 1, {"Wrong ordering of lanes: Lane " + lane.id + " has no left lane id but it isn't the last lane in the segment " + segment.first + "."}, - Validator::Error::Type::kLogicalLaneAdjacency); + Validator::Type::kLogicalLaneAdjacency); } } } @@ -200,5 +230,17 @@ bool Validator::Error::operator==(const Error& other) const { bool Validator::Error::operator!=(const Error& other) const { return !(*this == other); } +std::ostream& operator<<(std::ostream& os, const Validator::Type& type) { + switch (type) { + case Validator::Type::kLogicalLaneAdjacency: + os << "Logical Lane Adjacency"; + break; + case Validator::Type::kGeometricalLaneAdjacency: + os << "Geometrical Lane Adjacency"; + break; + } + return os; +} + } // namespace parser } // namespace maliput_sparse diff --git a/test/parser/validator_test.cc b/test/parser/validator_test.cc index f9cd12e..dbcc543 100644 --- a/test/parser/validator_test.cc +++ b/test/parser/validator_test.cc @@ -74,6 +74,11 @@ class ValidatorTest : public ::testing::Test { protected: const Lane wrong_adj_lane{CreateLane("0", LineString3d{{1., 1., 1.}, {10., 1., 1.}}, LineString3d{{1., -1., 1.}, {10., -1., 1.}}, {"23123"}, std::nullopt, {}, {})}; + const Lane lane_0{CreateLane("0", LineString3d{{1., 1., 1.}, {10., 1., 1.}}, + LineString3d{{1., -1., 1.}, {10., -1., 1.}}, {"1"}, std::nullopt, {}, {})}; + const Lane not_geometrical_adj_lane_to_lane_0{CreateLane("1", LineString3d{{1., 3., 1.}, {10., 3., 1.}}, + LineString3d{{1., 2., 1.}, {10., 2., 1.}}, std::nullopt, + {"0"}, {}, {})}; }; TEST_F(ValidatorTest, InvalidParser) { @@ -81,21 +86,51 @@ TEST_F(ValidatorTest, InvalidParser) { } TEST_F(ValidatorTest, AllOptionsDisabled) { - ValidatorOptions options; - options.lane_adjacency = false; + const Validator::Types types{/* No validation selected */}; const MockParser parser = CreateMockParser({wrong_adj_lane}); - const auto errors = Validator(&parser, options, ValidatorConfig{1e-3})(); + const auto errors = Validator(&parser, types, ValidatorConfig{1e-3})(); ASSERT_EQ(0., errors.size()); } -TEST_F(ValidatorTest, LaneAdjacencyOption) { - ValidatorOptions options; - options.lane_adjacency = true; +// Wrong adjacent lane id. Only logical errors will be found. +TEST_F(ValidatorTest, LogicalLaneAdjacencyOptionCaseA) { + const Validator::Types types{Validator::Type::kLogicalLaneAdjacency}; const MockParser parser = CreateMockParser({wrong_adj_lane}); - const auto errors = Validator(&parser, options, ValidatorConfig{1e-3})(); + const auto errors = Validator(&parser, types, ValidatorConfig{1e-3})(); ASSERT_NE(0., errors.size()); for (const auto& error : errors) { - EXPECT_TRUE(error.type == Validator::Error::Type::kLogicalLaneAdjacency); + EXPECT_TRUE(error.type == Validator::Type::kLogicalLaneAdjacency); + } +} + +// As GeometryLaneAdjacency depends on LogicalLaneAdjacency, and there is only one lane then a logical error will be +// found. +TEST_F(ValidatorTest, GeometricalLaneAdjacencyOptionCaseA) { + const Validator::Types types{Validator::Type::kGeometricalLaneAdjacency}; + const MockParser parser = CreateMockParser({wrong_adj_lane}); + const auto errors = Validator(&parser, types, ValidatorConfig{1e-3})(); + ASSERT_NE(0., errors.size()); + for (const auto& error : errors) { + EXPECT_TRUE(error.type == Validator::Type::kLogicalLaneAdjacency); + } +} + +// Wrong geometric adjacency. No errors will be found +TEST_F(ValidatorTest, LogicalLaneAdjacencyOptionCaseB) { + const Validator::Types types{Validator::Type::kLogicalLaneAdjacency}; + const MockParser parser = CreateMockParser({lane_0, not_geometrical_adj_lane_to_lane_0}); + const auto errors = Validator(&parser, types, ValidatorConfig{1e-3})(); + ASSERT_EQ(0., errors.size()); +} + +// Wrong geometric adjacency. Only geometrical errors will be found. +TEST_F(ValidatorTest, GeometricalLaneAdjacencyOptionCaseB) { + const Validator::Types types{Validator::Type::kGeometricalLaneAdjacency}; + const MockParser parser = CreateMockParser({lane_0, not_geometrical_adj_lane_to_lane_0}); + const auto errors = Validator(&parser, types, ValidatorConfig{1e-3})(); + ASSERT_NE(0., errors.size()); + for (const auto& error : errors) { + EXPECT_TRUE(error.type == Validator::Type::kGeometricalLaneAdjacency); } } @@ -109,13 +144,15 @@ class ValidateLaneAdjacencyTest : public ValidatorTest { LineString3d{{1., 3., 1.}, {10., 3., 1.}}, {"3"}, {"1"}, {}, {}); const Lane lane_3 = CreateLane("3", LineString3d{{1., 7., 1.}, {10., 7., 1.}}, LineString3d{{1., 5., 1.}, {10., 5., 1.}}, std::nullopt, {"2"}, {}, {}); + const bool kEnableGeometricalValidation = true; + const bool kDisableGeometricalValidation = !kEnableGeometricalValidation; }; TEST_F(ValidateLaneAdjacencyTest, NoErrors) { const Segment segment{"segment", {lane_0, lane_1, lane_2, lane_3}}; const Junction junction{"junction", {{segment.id, segment}}}; const MockParser parser({{junction.id, junction}}, {}); - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); EXPECT_EQ(0., errors.size()); } @@ -123,20 +160,20 @@ TEST_F(ValidateLaneAdjacencyTest, NoValidRightLaneInTheSegment) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 1 has a left lane id (2) that has a right lane id (41856) that is not the lane " "1.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 2 has a right lane id (41856) that is not part of the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a right lane id (41856) that is not the previous lane in the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_2_no_valid_right_id{lane_2}; lane_2_no_valid_right_id.right_lane_id = "41856"; const Segment segment{"segment", {lane_0, lane_1, lane_2_no_valid_right_id, lane_3}}; const Junction junction{"junction", {{segment.id, segment}}}; const MockParser parser({{junction.id, junction}}, {}); - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -145,18 +182,18 @@ TEST_F(ValidateLaneAdjacencyTest, NoValidLeftLaneInTheSegment) { const std::vector expected_errors{ {"Adjacent lane isn't part of the segment: Lane 2 has a left lane id (41856) that is not part of the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a left lane id (41856) that is not the next lane in the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 3 has a right lane id (2) that has a left lane id (41856) that is not the lane " "3.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_2_no_valid_left_id{lane_2}; lane_2_no_valid_left_id.left_lane_id = "41856"; const MockParser parser{CreateMockParser({lane_0, lane_1, lane_2_no_valid_left_id, lane_3})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -165,17 +202,17 @@ TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForRightLane) { const std::vector expected_errors{ {"Adjacent lane isn't part of the segment: Lane 0 has a left lane id (54656465) that is not part of the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 0 has a left lane id (54656465) that is not the next lane in the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 1 has a right lane id (0) that has a left lane id (54656465) that is not the " "lane 1.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 1 has no left lane id but it isn't the last lane in the segment segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a right lane id (1) that has no left lane id.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_0_with_other_left_lane_id{lane_0}; lane_0_with_other_left_lane_id.left_lane_id = "54656465"; @@ -183,7 +220,7 @@ TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForRightLane) { lane_1_without_left_lane_id.left_lane_id = std::nullopt; const MockParser parser{ CreateMockParser({lane_0_with_other_left_lane_id, lane_1_without_left_lane_id, lane_2, lane_3})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -191,18 +228,18 @@ TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForRightLane) { TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForLeftLane) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 1 has a left lane id (2) that has no right lane id.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has no right lane id but it isn't the first lane in the segment segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a left lane id (3) that has a right lane id (54656465) that is not the " "lane 2.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 3 has a right lane id (54656465) that is not part of the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 3 has a right lane id (54656465) that is not the previous lane in the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_2_without_right_lane_id{lane_2}; lane_2_without_right_lane_id.right_lane_id = std::nullopt; @@ -210,7 +247,7 @@ TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForLeftLane) { lane_3_with_other_right_lane_id.right_lane_id = "54656465"; const MockParser parser{ CreateMockParser({lane_0, lane_1, lane_2_without_right_lane_id, lane_3_with_other_right_lane_id})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -218,14 +255,14 @@ TEST_F(ValidateLaneAdjacencyTest, NoCorrespondenceForLeftLane) { TEST_F(ValidateLaneAdjacencyTest, NoRightLaneIdForANonFirstLane) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 0 has a left lane id (1) that has no right lane id.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 1 has no right lane id but it isn't the first lane in the segment segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_1_without_right_lane_id{lane_1}; lane_1_without_right_lane_id.right_lane_id = std::nullopt; const MockParser parser{CreateMockParser({lane_0, lane_1_without_right_lane_id, lane_2, lane_3})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -233,14 +270,14 @@ TEST_F(ValidateLaneAdjacencyTest, NoRightLaneIdForANonFirstLane) { TEST_F(ValidateLaneAdjacencyTest, NoLeftLaneIdForANonLastLane) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 1 has no left lane id but it isn't the last lane in the segment segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Wrong ordering of lanes: Lane 2 has a right lane id (1) that has no left lane id.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; Lane lane_1_without_left_lane_id{lane_1}; lane_1_without_left_lane_id.left_lane_id = std::nullopt; const MockParser parser{CreateMockParser({lane_0, lane_1_without_left_lane_id, lane_2, lane_3})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -248,13 +285,13 @@ TEST_F(ValidateLaneAdjacencyTest, NoLeftLaneIdForANonLastLane) { TEST_F(ValidateLaneAdjacencyTest, NoRightLanes) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 3 has a right lane id (2) but is the first lane in the segment segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 3 has a right lane id (2) that is not part of the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; const MockParser parser{CreateMockParser({lane_3})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -262,13 +299,13 @@ TEST_F(ValidateLaneAdjacencyTest, NoRightLanes) { TEST_F(ValidateLaneAdjacencyTest, NoLeftLanes) { const std::vector expected_errors{ {"Wrong ordering of lanes: Lane 0 has a left lane id (1) but is the last lane in the segment segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, {"Adjacent lane isn't part of the segment: Lane 0 has a left lane id (1) that is not part of the segment " "segment.", - Validator::Error::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, + Validator::Type::kLogicalLaneAdjacency, Validator::Error::Severity::kError}, }; const MockParser parser{CreateMockParser({lane_0})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } @@ -277,17 +314,26 @@ TEST_F(ValidateLaneAdjacencyTest, NoGeometricallyAdjacentLanes) { Lane lane_0_no_geometrically_adj{lane_0}; lane_0_no_geometrically_adj.left = LineString3d{{1., 7., 1.}, {10., 7., 1.}}; const std::vector expected_errors{ - {"Lane 0 and lane 1 are not adjacent under the tolerance 0.001000.", - Validator::Error::Type::kGeometricalLaneAdjacency, Validator::Error::Severity::kError}, - {"Lane 1 and lane 0 are not adjacent under the tolerance 0.001000.", - Validator::Error::Type::kGeometricalLaneAdjacency, Validator::Error::Severity::kError}, + {"Lane 0 and lane 1 are not adjacent under the tolerance 0.001000.", Validator::Type::kGeometricalLaneAdjacency, + Validator::Error::Severity::kError}, + {"Lane 1 and lane 0 are not adjacent under the tolerance 0.001000.", Validator::Type::kGeometricalLaneAdjacency, + Validator::Error::Severity::kError}, }; const MockParser parser{CreateMockParser({lane_0_no_geometrically_adj, lane_1, lane_2, lane_3})}; - const auto errors = ValidateLaneAdjacency(&parser, ValidatorConfig{1e-3}); + const auto errors = ValidateLaneAdjacency(&parser, kEnableGeometricalValidation, ValidatorConfig{1e-3}); ASSERT_EQ(expected_errors.size(), errors.size()); EXPECT_EQ(expected_errors, errors); } +TEST_F(ValidateLaneAdjacencyTest, NoGeometricallyAdjacentLanesButNoGeometricalValidation) { + Lane lane_0_no_geometrically_adj{lane_0}; + lane_0_no_geometrically_adj.left = LineString3d{{1., 7., 1.}, {10., 7., 1.}}; + const std::vector expected_errors{/* empty */}; + const MockParser parser{CreateMockParser({lane_0_no_geometrically_adj, lane_1, lane_2, lane_3})}; + const auto errors = ValidateLaneAdjacency(&parser, kDisableGeometricalValidation, ValidatorConfig{1e-3}); + ASSERT_EQ(expected_errors.size(), errors.size()); +} + } // namespace } // namespace test } // namespace parser From eeb7c16eed13af570daf42ec4067bd9c0434ef10 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Fri, 23 Dec 2022 11:18:39 -0300 Subject: [PATCH 5/5] Addresses final comments. Signed-off-by: Franco Cipollone --- src/parser/validation_methods.h | 4 ++-- src/parser/validator.cc | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/parser/validation_methods.h b/src/parser/validation_methods.h index af73b00..6348f6f 100644 --- a/src/parser/validation_methods.h +++ b/src/parser/validation_methods.h @@ -42,8 +42,8 @@ namespace parser { /// @param validate_geometric_adjacency Whether to validate geometric adjacency. /// @param config The maliput_sparse::parser::ValidatorConfig to use. /// @returns A vector of maliput_sparse::parser::Validator::Error. -std::vector ValidateLaneAdjacency(const Parser* parser, const bool& validate_geometric_adjacency, - const ValidatorConfig config); +std::vector ValidateLaneAdjacency(const Parser* parser, bool validate_geometric_adjacency, + const ValidatorConfig& config); } // namespace parser } // namespace maliput_sparse diff --git a/src/parser/validator.cc b/src/parser/validator.cc index ce38e9c..64bfbf7 100644 --- a/src/parser/validator.cc +++ b/src/parser/validator.cc @@ -96,8 +96,8 @@ std::vector Validator::operator()() const { return errors; } -std::vector ValidateLaneAdjacency(const Parser* parser, const bool& validate_geometric_adjacency, - const ValidatorConfig config) { +std::vector ValidateLaneAdjacency(const Parser* parser, bool validate_geometric_adjacency, + const ValidatorConfig& config) { std::vector errors; auto evaluate = [&errors](bool condition, const std::string& message, const Validator::Type& error_type) { if (condition) {