diff --git a/map/autoware_lanelet2_map_validator/docs/lane/local_coordinates_declaration.md b/map/autoware_lanelet2_map_validator/docs/lane/local_coordinates_declaration.md new file mode 100644 index 00000000..23da6360 --- /dev/null +++ b/map/autoware_lanelet2_map_validator/docs/lane/local_coordinates_declaration.md @@ -0,0 +1,24 @@ +# local_coordinates_declaration + +## Validator name + +mapping.lane.local_coordinates_declaration + +## Feature + +This validator checks whether local coordinates are declared to each point in a Lanelet2 map if one or more points have local coordinates. If a Lanelet2 map is made to have local coordinates, each point MUST have both a `local_x` tag and a `local_y` tag. + +If the Lanelet2 map is NOT made to have local coordinates and doesn't have a single point with `local_x` or `local_y` declared, this validator ignores the map and outputs no issues. + +The validator outputs the following issue with the corresponding ID of the primitive. + +| Issue Code | Message | Severity | Primitive | Description | Approach | +| ------------------------------------ | ---------------------------------------------------------- | -------- | --------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------ | ------------------------------------------------------------------------------------------ | +| Lane.LocalCoordinatesDeclaration-001 | This point doesn't have local coordinates while others do. | Error | Point | The map has a point that has tags `local_x` and `local_y` but this point doesn't have them. All points in a map must be set to have both tags or not at all. | Be sure to align whether each point has or does not have both tabs `local_x` and `local_y` | +| Lane.LocalCoordinatesDeclaration-002 | "local_x" is declared but "local_y" is not." | Error | Point | The point has a `local_x` tag but doesn't have a `local_y` tag. | Be sure that all points have both tags `local_x` and `local_y`. | +| Lane.LocalCoordinatesDeclaration-003 | "local_y" is declared but "local_x" is not." | Error | Point | The point has a `local_y` tag but doesn't have a `local_x` tag. | Be sure that all points have both tags `local_x` and `local_y`. | + +## Related source codes + +- local_coordinates_declaration.cpp +- local_coordinates_declaration.hpp diff --git a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validators/lane/local_coordinates_declaration.hpp b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validators/lane/local_coordinates_declaration.hpp new file mode 100644 index 00000000..1f181e76 --- /dev/null +++ b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validators/lane/local_coordinates_declaration.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELET2_MAP_VALIDATOR__VALIDATORS__LANE__LOCAL_COORDINATES_DECLARATION_HPP_ +#define LANELET2_MAP_VALIDATOR__VALIDATORS__LANE__LOCAL_COORDINATES_DECLARATION_HPP_ + +#include +#include + +namespace lanelet::autoware::validation +{ +class LocalCoordinatesDeclarationValidator : public lanelet::validation::MapValidator +{ +public: + // Write the validator's name here + constexpr static const char * name() { return "mapping.lane.local_coordinates_declaration"; } + + lanelet::validation::Issues operator()(const lanelet::LaneletMap & map) override; + +private: + lanelet::validation::Issues check_local_coordinates_declaration(const lanelet::LaneletMap & map); +}; +} // namespace lanelet::autoware::validation + +#endif // LANELET2_MAP_VALIDATOR__VALIDATORS__LANE__LOCAL_COORDINATES_DECLARATION_HPP_ diff --git a/map/autoware_lanelet2_map_validator/src/validators/lane/local_coordinates_declaration.cpp b/map/autoware_lanelet2_map_validator/src/validators/lane/local_coordinates_declaration.cpp new file mode 100644 index 00000000..e562caca --- /dev/null +++ b/map/autoware_lanelet2_map_validator/src/validators/lane/local_coordinates_declaration.cpp @@ -0,0 +1,96 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lanelet2_map_validator/validators/lane/local_coordinates_declaration.hpp" + +#include "lanelet2_core/LaneletMap.h" +#include "lanelet2_map_validator/utils.hpp" + +#include + +namespace lanelet::autoware::validation +{ +namespace +{ +lanelet::validation::RegisterMapValidator reg; +} + +lanelet::validation::Issues LocalCoordinatesDeclarationValidator::operator()( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + + lanelet::autoware::validation::appendIssues(issues, check_local_coordinates_declaration(map)); + + return issues; +} + +lanelet::validation::Issues +LocalCoordinatesDeclarationValidator::check_local_coordinates_declaration( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + + bool is_local_mode = false; + + std::set non_local_point_ids; + + for (const lanelet::ConstPoint3d & point : map.pointLayer) { + bool local_x = point.hasAttribute("local_x"); + bool local_y = point.hasAttribute("local_y"); + + if (!is_local_mode) { + if (local_x || local_y) { + is_local_mode = true; + for (const lanelet::Id & id : non_local_point_ids) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Point, id, + append_issue_code_prefix( + this->name(), 1, "This point doesn't have local coordinates while others do.")); + } + } else { + non_local_point_ids.insert(point.id()); + } + continue; + } + + // Points are assumed to have local_x and local_y from here + if (!local_x && !local_y) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Point, point.id(), + append_issue_code_prefix( + this->name(), 1, "This point doesn't have local coordinates while others do.")); + } + + // Only one coordinate (local_x or local_y) is defined + if (local_x && !local_y) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Point, point.id(), + append_issue_code_prefix( + this->name(), 2, "\"local_x\" is declared but \"local_y\" is not.")); + continue; + } + + if (!local_x && local_y) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Point, point.id(), + append_issue_code_prefix( + this->name(), 3, "\"local_y\" is declared but \"local_x\" is not.")); + continue; + } + } + + return issues; +} +} // namespace lanelet::autoware::validation diff --git a/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet.osm b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet.osm new file mode 100644 index 00000000..5f34499f --- /dev/null +++ b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet.osm @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_first_three_points_missing_local_coordinates.osm b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_first_three_points_missing_local_coordinates.osm new file mode 100644 index 00000000..50c631c6 --- /dev/null +++ b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_first_three_points_missing_local_coordinates.osm @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_partially_defected_local_coordinates.osm b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_partially_defected_local_coordinates.osm new file mode 100644 index 00000000..7bc75f1f --- /dev/null +++ b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_partially_defected_local_coordinates.osm @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_without_local_coordinates.osm b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_without_local_coordinates.osm new file mode 100644 index 00000000..d7cf0386 --- /dev/null +++ b/map/autoware_lanelet2_map_validator/test/data/map/lane/single_lanelet_without_local_coordinates.osm @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/autoware_lanelet2_map_validator/test/src/lane/test_local_coordinates_declaration.cpp b/map/autoware_lanelet2_map_validator/test/src/lane/test_local_coordinates_declaration.cpp new file mode 100644 index 00000000..43022d72 --- /dev/null +++ b/map/autoware_lanelet2_map_validator/test/src/lane/test_local_coordinates_declaration.cpp @@ -0,0 +1,130 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lanelet2_map_validator/validators/lane/local_coordinates_declaration.hpp" +#include "map_validation_tester.hpp" + +#include +#include + +#include + +class TestLocalCoordinatesDeclarationValidator : public MapValidationTester +{ +private: +}; + +TEST_F(TestLocalCoordinatesDeclarationValidator, ValidatorAvailability) // NOLINT for gtest +{ + std::string expected_validator_name = + lanelet::autoware::validation::LocalCoordinatesDeclarationValidator::name(); + + lanelet::validation::Strings validators = + lanelet::validation::availabeChecks(expected_validator_name); // cspell:disable-line + + const uint32_t expected_validator_num = 1; + EXPECT_EQ(expected_validator_num, validators.size()); + EXPECT_EQ(expected_validator_name, validators[0]); +} + +TEST_F(TestLocalCoordinatesDeclarationValidator, NotLocalFromStart) // NOLINT for gtest +{ + load_target_map("lane/single_lanelet_first_three_points_missing_local_coordinates.osm"); + + lanelet::autoware::validation::LocalCoordinatesDeclarationValidator checker; + const auto & issues = checker(*map_); + + EXPECT_EQ(issues.size(), 3); + + EXPECT_EQ(issues[0].id, 8); + EXPECT_EQ(issues[0].severity, lanelet::validation::Severity::Error); + EXPECT_EQ(issues[0].primitive, lanelet::validation::Primitive::Point); + EXPECT_EQ( + issues[0].message, + "[Lane.LocalCoordinatesDeclaration-001] This point doesn't have local coordinates while others " + "do."); + + EXPECT_EQ(issues[1].id, 9); + EXPECT_EQ(issues[1].severity, lanelet::validation::Severity::Error); + EXPECT_EQ(issues[1].primitive, lanelet::validation::Primitive::Point); + EXPECT_EQ( + issues[1].message, + "[Lane.LocalCoordinatesDeclaration-001] This point doesn't have local coordinates while others " + "do."); + + EXPECT_EQ(issues[2].id, 11); + EXPECT_EQ(issues[2].severity, lanelet::validation::Severity::Error); + EXPECT_EQ(issues[2].primitive, lanelet::validation::Primitive::Point); + EXPECT_EQ( + issues[2].message, + "[Lane.LocalCoordinatesDeclaration-001] This point doesn't have local coordinates while others " + "do."); +} + +TEST_F(TestLocalCoordinatesDeclarationValidator, PartiallyDefected) // NOLINT for gtest +{ + load_target_map("lane/single_lanelet_partially_defected_local_coordinates.osm"); + + lanelet::autoware::validation::LocalCoordinatesDeclarationValidator checker; + const auto & issues = checker(*map_); + + EXPECT_EQ(issues.size(), 2); + + EXPECT_EQ(issues[0].id, 8); + EXPECT_EQ(issues[0].severity, lanelet::validation::Severity::Error); + EXPECT_EQ(issues[0].primitive, lanelet::validation::Primitive::Point); + EXPECT_EQ( + issues[0].message, + "[Lane.LocalCoordinatesDeclaration-002] \"local_x\" is declared but \"local_y\" is not."); + + EXPECT_EQ(issues[1].id, 17); + EXPECT_EQ(issues[1].severity, lanelet::validation::Severity::Error); + EXPECT_EQ(issues[1].primitive, lanelet::validation::Primitive::Point); + EXPECT_EQ( + issues[1].message, + "[Lane.LocalCoordinatesDeclaration-003] \"local_y\" is declared but \"local_x\" is not."); +} + +TEST_F( + TestLocalCoordinatesDeclarationValidator, NormalLaneletWithLocalCoordinates) // NOLINT for gtest +{ + load_target_map("lane/single_lanelet.osm"); + + lanelet::autoware::validation::LocalCoordinatesDeclarationValidator checker; + const auto & issues = checker(*map_); + + EXPECT_EQ(issues.size(), 0); +} + +TEST_F( + TestLocalCoordinatesDeclarationValidator, + NormalLaneletWithoutLocalCoordinates) // NOLINT for gtest +{ + load_target_map("lane/single_lanelet_without_local_coordinates.osm"); + + lanelet::autoware::validation::LocalCoordinatesDeclarationValidator checker; + const auto & issues = checker(*map_); + + EXPECT_EQ(issues.size(), 0); +} + +TEST_F(TestLocalCoordinatesDeclarationValidator, SampleMap) // NOLINT for gtest +{ + load_target_map("sample_map.osm"); + + lanelet::autoware::validation::LocalCoordinatesDeclarationValidator checker; + const auto & issues = checker(*map_); + + EXPECT_EQ(issues.size(), 0); +}