diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index f67e2bf3..a773d2b8 100644 --- a/tmp/lanelet2_extension/CMakeLists.txt +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -55,7 +55,7 @@ target_link_libraries(lanelet2_extension_lib get_target_property(lanelet2_core_INCLUDE_DIRECTORIES lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES) target_include_directories(lanelet2_extension_lib SYSTEM PRIVATE - ${lanelet2_core_INCLUDE_DIRECTORIES} + ${lanelet2_core_INCLUDE_DIRECTORIES} ) ament_auto_add_executable(lanelet2_extension_sample src/sample_code.cpp) @@ -64,9 +64,9 @@ target_link_libraries(lanelet2_extension_sample lanelet2_extension_lib ) -ament_auto_add_executable(autoware_lanelet2_validation src/validation.cpp) -add_dependencies(autoware_lanelet2_validation lanelet2_extension_lib) -target_link_libraries(autoware_lanelet2_validation +ament_auto_add_executable(simple_lanelet2_validation src/validation.cpp) +add_dependencies(simple_lanelet2_validation lanelet2_extension_lib) +target_link_libraries(simple_lanelet2_validation ${catkin_LIBRARIES} ${PUGIXML_LIBRARIES} lanelet2_extension_lib @@ -80,6 +80,21 @@ target_link_libraries(check_right_of_way lanelet2_extension_lib ) +file(GLOB_RECURSE autoware_lanelet2_validation_lib_src src/autoware_lanelet2_validation/lib/*.cpp) +ament_auto_add_library(autoware_lanelet2_validation_lib SHARED + ${autoware_lanelet2_validation_lib_src} +) + +target_link_libraries(autoware_lanelet2_validation_lib + lanelet2_extension_lib +) + +ament_auto_add_executable(autoware_lanelet2_validation src/autoware_lanelet2_validation/main.cpp) +add_dependencies(autoware_lanelet2_validation autoware_lanelet2_validation_lib) +target_link_libraries(autoware_lanelet2_validation + autoware_lanelet2_validation_lib +) + if(BUILD_TESTING) ament_add_ros_isolated_gtest(message_conversion-test test/src/test_message_conversion.cpp) target_link_libraries(message_conversion-test lanelet2_extension_lib) @@ -92,11 +107,25 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(utilities-test test/src/test_utilities.cpp) target_link_libraries(utilities-test lanelet2_extension_lib) target_include_directories(utilities-test - SYSTEM PRIVATE + SYSTEM PRIVATE ${lanelet2_core_INCLUDE_DIRECTORIES} ) ament_add_ros_isolated_gtest(route-test test/src/test_route_checker.cpp) target_link_libraries(route-test lanelet2_extension_lib) + + function(add_validation_test VALIDATION_NAME) + ament_add_ros_isolated_gtest( + ${VALIDATION_NAME}_test + test/src/autoware_lanelet2_validation/test_${VALIDATION_NAME}.cpp + ) + target_link_libraries( + ${VALIDATION_NAME}_test + autoware_lanelet2_validation_lib + ) + endfunction() + + add_validation_test(missing_regulatory_elements) + add_validation_test(regulatory_element_details) endif() ament_auto_package() diff --git a/tmp/lanelet2_extension/README.md b/tmp/lanelet2_extension/README.md index 85e360b7..a5899299 100644 --- a/tmp/lanelet2_extension/README.md +++ b/tmp/lanelet2_extension/README.md @@ -91,11 +91,56 @@ Currently it contains following conversions: Code for this explains how this lanelet2_extension library is used. The executable is not meant to do anything. -### autoware_lanelet2_extension +### simple_lanelet2_validation This node checks if an .osm file follows the Autoware version of Lanelet2 format. You can check by running: ```sh -ros2 run lanelet2_extension autoware_lanelet2_validation --ros-args -p map_file:= +ros2 run lanelet2_extension simple_lanelet2_validation --ros-args -p map_file:= ``` + +### autoware_lanelet2_validation + +This node validates if the provided lanelet2 map is usable with Autoware. +You can check by running: + +```sh +ros2 run lanelet2_extension autoware_lanelet2_validation --map_file --validator +``` + +Example: + +```sh +ros2 run lanelet2_extension autoware_lanelet2_validation --map_file ~/autoware_map/sample-map-planning/lanelet2_map.osm --validator mapping.missing_regulatory_elements +``` + +Output of above example: + +```sh +Set to default projector: MGRS projector +Error: linestring 9776 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: linestring 9774 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: linestring 9771 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: linestring 9769 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: linestring 340 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: linestring 342 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: linestring 345 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: linestring 347 Traffic light must have a regulatory element. [mapping.missing_regulatory_elements] +Error: lanelet 163 Crosswalk must have a regulatory element. [mapping.missing_regulatory_elements] +Error: lanelet 164 Crosswalk must have a regulatory element. [mapping.missing_regulatory_elements] +Error: lanelet 165 Crosswalk must have a regulatory element. [mapping.missing_regulatory_elements] +Error: lanelet 166 Crosswalk must have a regulatory element. [mapping.missing_regulatory_elements] +``` + +For more information, please refer to help. You can check by running: + +```sh +ros2 run lanelet2_extension autoware_lanelet2_validation --help +``` + +#### Avalilable validators + +##### mapping.missing_regulatory_elements + +This validate if given lanelet2 map has the traffic light or crosswalk which is not associated with a regulatory element. diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp new file mode 100644 index 00000000..8b57f992 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 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_EXTENSION__AUTOWARE_LANELET2_VALIDATION__CLI_HPP_ +#define LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__CLI_HPP_ + +#include + +#include + +#include +#include + +namespace lanelet +{ +namespace autoware +{ +namespace validation +{ +struct MetaConfig +{ + lanelet::validation::CommandLineConfig command_line_config; + std::string projector_type; +}; + +MetaConfig parseCommandLine(int argc, const char * argv[]); + +} // namespace validation +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__CLI_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp new file mode 100644 index 00000000..af705d51 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp @@ -0,0 +1,85 @@ +// Copyright 2023 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_EXTENSION__AUTOWARE_LANELET2_VALIDATION__UTILS_HPP_ +#define LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__UTILS_HPP_ + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace autoware +{ +namespace validation +{ +template +void appendIssues(std::vector & to, std::vector && from) +{ + to.insert(to.end(), std::make_move_iterator(from.begin()), std::make_move_iterator(from.end())); +} + +template +void checkPrimitivesType( + std::vector & in_vec, const std::string & expected_type, + const lanelet::validation::Issue & issue, lanelet::validation::Issues & issues) +{ + for (auto iter = in_vec.begin(); iter != in_vec.end(); ++iter) { + const auto & item = *iter; + const auto & attrs = item.attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Type); + if (it == attrs.end() || it->second != expected_type) { + issues.emplace_back(issue.severity, issue.primitive, item.id(), issue.message); + const auto new_it = in_vec.erase(iter); + if (new_it != in_vec.end()) { + iter = new_it; + } else { + break; + } + } + } +} + +template +void checkPrimitivesType( + std::vector & in_vec, const std::string & expected_type, const std::string & expected_subtype, + const lanelet::validation::Issue & issue, lanelet::validation::Issues & issues) +{ + for (auto iter = in_vec.begin(); iter != in_vec.end(); ++iter) { + const auto & item = *iter; + const auto & attrs = item.attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Type); + const auto & it_sub = attrs.find(lanelet::AttributeName::Subtype); + if ( + it == attrs.end() || it->second != expected_type || it_sub == attrs.end() || + it_sub->second != expected_subtype) { + issues.emplace_back(issue.severity, issue.primitive, item.id(), issue.message); + const auto new_it = in_vec.erase(iter); + if (new_it != in_vec.end()) { + iter = new_it; + } else { + break; + } + } + } +} + +} // namespace validation +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__UTILS_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp new file mode 100644 index 00000000..9e7ef8f5 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALIDATION_HPP_ +#define LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALIDATION_HPP_ + +#include "lanelet2_extension/autoware_lanelet2_validation/cli.hpp" +#include "lanelet2_extension/autoware_lanelet2_validation/utils.hpp" +#include "lanelet2_extension/projection/mgrs_projector.hpp" +#include "lanelet2_extension/projection/transverse_mercator_projector.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ +namespace projector_names +{ +constexpr const char * mgrs = "mgrs"; +constexpr const char * transverse_mercator = "transverse_mercator"; +constexpr const char * utm = "utm"; +} // namespace projector_names +} // namespace + +namespace lanelet +{ +namespace autoware +{ +namespace validation +{ +std::unique_ptr getProjector(const MetaConfig & config); +std::vector validateMap(const MetaConfig & config); +} // namespace validation +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALIDATION_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp new file mode 100644 index 00000000..2419d4a9 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__MISSING_REGULATORY_ELEMENTS_HPP_ +#define LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__MISSING_REGULATORY_ELEMENTS_HPP_ + +#include "lanelet2_extension/autoware_lanelet2_validation/utils.hpp" +#include "lanelet2_extension/regulatory_elements/crosswalk.hpp" + +#include +#include +#include + +#include +#include + +#include + +namespace lanelet +{ +namespace validation +{ + +class MissingRegulatoryElementsChecker : public lanelet::validation::MapValidator +{ +public: + constexpr static const char * name() { return "mapping.missing_regulatory_elements"; } + + lanelet::validation::Issues operator()(const lanelet::LaneletMap & map) override; + +private: + lanelet::validation::Issues checkMissingReglatoryElementsInTrafficLight( + const lanelet::LaneletMap & map); + lanelet::validation::Issues checkMissingReglatoryElementsInCrosswalk( + const lanelet::LaneletMap & map); + lanelet::validation::Issues checkMissingReglatoryElementsInStopLine( + const lanelet::LaneletMap & map); + std::set tl_elem_with_cw_; +}; +} // namespace validation +} // namespace lanelet + +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__MISSING_REGULATORY_ELEMENTS_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/regulatory_element_details.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/regulatory_element_details.hpp new file mode 100644 index 00000000..ae59ed8a --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/regulatory_element_details.hpp @@ -0,0 +1,50 @@ +// Copyright 2023 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_extension/autoware_lanelet2_validation/utils.hpp" +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "lanelet2_extension/regulatory_elements/crosswalk.hpp" + +#include + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace validation +{ + +#ifndef LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__REGULATORY_ELEMENT_DETAILS_HPP_ +#define LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__REGULATORY_ELEMENT_DETAILS_HPP_ + +class RegulatoryElementDetailsChecker : public lanelet::validation::MapValidator +{ +public: + constexpr static const char * name() { return "mapping.regulatory_elements_details"; } + + lanelet::validation::Issues operator()(const lanelet::LaneletMap & map) override; + +private: + bool isPedestrianTrafficLight(const std::vector & traffic_lights); + lanelet::validation::Issues checkRegulatoryElementOfTrafficLight(const lanelet::LaneletMap & map); + lanelet::validation::Issues checkRegulatoryElementOfCrosswalk(const lanelet::LaneletMap & map); +}; +} // namespace validation +} // namespace lanelet + +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__REGULATORY_ELEMENT_DETAILS_HPP_ diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp new file mode 100644 index 00000000..6bc6f899 --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 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_extension/autoware_lanelet2_validation/cli.hpp" + +namespace po = boost::program_options; + +namespace lanelet +{ +namespace autoware +{ +namespace validation +{ + +MetaConfig parseCommandLine(int argc, const char * argv[]) +{ + MetaConfig config; + auto & validation_config = config.command_line_config.validationConfig; + po::options_description desc( + "Runs a set of validators on a map. Think of it like a linter. The following checks are " + "available:"); + desc.add_options()("help,h", "this help message") + + ("map_file,m", po::value(), "Path to the map to be validated") + + ("validator,v", po::value(&validation_config.checksFilter), + "Comma separated list of regexes to filter the applicable validators. Will run all " + "validators by " + "default. Example: " + "routing_graph.* to run all checks for the routing graph") + + ("projector,p", po::value(&config.projector_type)->composing(), + "Projector used for loading lanelet map. Available projectors are: mgrs, utm, " + "transverse_mercator. (default: mgrs)") + + ("location,l", + po::value(&validation_config.location)->default_value(validation_config.location), + "Location of the map (for instanciating the traffic rules), e.g. de for Germany") + + ("participants", po::value(&validation_config.participants)->composing(), + "Participants for which the routing graph will be instanciated (default: vehicle)") + + ("lat", + po::value(&validation_config.origin.lat) + ->default_value(validation_config.origin.lat), + "latitude coordinate of map origin. This is reguired for the transverse mercator " + "and utm projector.") + + ("lon", + po::value(&validation_config.origin.lon) + ->default_value(validation_config.origin.lon), + "longitude coofdinate of map origin. This is reguired for the transverse mercator " + "and utm projector.") + + ("print", "Only print all avalible checker, but dont run them"); + po::variables_map vm; + po::positional_options_description pos; + pos.add("map_file", 1); + po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm); + po::notify(vm); + config.command_line_config.help = vm.count("help") != 0; + config.command_line_config.print = vm.count("print") != 0; + if (vm.count("map_file") != 0) { + config.command_line_config.mapFile = + vm["map_file"].as(); + } + if ( + (vm.count("lat") != 0 && vm.count("lon") != 0) && + (config.projector_type == "tm" || config.projector_type == "utm")) { + throw std::runtime_error( + "Please set latitude and longitude. These are required for " + config.projector_type + + " projector. Please refer to the help message."); + } + if (config.command_line_config.help) { + std::cout << '\n' << desc; + } else if (config.command_line_config.mapFile.empty() && !config.command_line_config.print) { + std::cout << "Please pass either a valid file or '--print' or '--help'!\n"; + } + return config; +} + +} // namespace validation +} // namespace autoware +} // namespace lanelet diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp new file mode 100644 index 00000000..9a1766c2 --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 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 + +namespace lanelet +{ +namespace autoware +{ +namespace validation +{ + +std::unique_ptr getProjector(const MetaConfig & config) +{ + const auto & val_config = config.command_line_config.validationConfig; + if (config.projector_type == projector_names::mgrs) { + return std::make_unique(); + } else if (config.projector_type == projector_names::transverse_mercator) { + return std::make_unique( + lanelet::Origin{val_config.origin}); + } else if (config.projector_type == projector_names::utm) { + return std::make_unique(lanelet::Origin{val_config.origin}); + } else { + std::cerr << "Set to default projector: MGRS projector" << std::endl; + return std::make_unique(); + } +} + +std::vector validateMap(const MetaConfig & config) +{ + const auto & cm_config = config.command_line_config; + const auto & val_config = config.command_line_config.validationConfig; + + const auto & parse_filter = [](const std::string & str) { + std::vector regexes; + std::stringstream ss(str); + + while (ss.good()) { + std::string substr; + getline(ss, substr, ','); + if (substr.empty()) { + continue; + } + regexes.emplace_back(substr, std::regex::basic | std::regex::icase); + } + return regexes; + }; + + auto checks = parse_filter(val_config.checksFilter); + + std::vector issues; + lanelet::LaneletMapPtr map; + lanelet::validation::Strings errors; + try { + const auto & projector = getProjector(config); + map = lanelet::load(cm_config.mapFile, *projector, &errors); + if (!errors.empty()) { + issues.emplace_back("general", utils::transform(errors, [](auto & error) { + return lanelet::validation::Issue( + lanelet::validation::Severity::Error, error); + })); + } + } catch (lanelet::LaneletError & err) { + issues.emplace_back("general", utils::transform(errors, [](auto & error) { + return lanelet::validation::Issue( + lanelet::validation::Severity::Error, error); + })); + } + + appendIssues(issues, lanelet::validation::validateMap(*map, val_config)); + return issues; +} + +} // namespace validation +} // namespace autoware +} // namespace lanelet diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/missing_regulatory_elements.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/missing_regulatory_elements.cpp new file mode 100644 index 00000000..40a5f55f --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/missing_regulatory_elements.cpp @@ -0,0 +1,194 @@ +// Copyright 2023 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_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp" + +namespace lanelet +{ +namespace validation +{ +namespace +{ +lanelet::validation::RegisterMapValidator reg; +} // namespace + +lanelet::validation::Issues MissingRegulatoryElementsChecker::operator()( + const lanelet::LaneletMap & map) +{ + // All issues found by all validators + lanelet::validation::Issues issues; + + // Append issues found by each validator + lanelet::autoware::validation::appendIssues( + issues, checkMissingReglatoryElementsInTrafficLight(map)); + lanelet::autoware::validation::appendIssues( + issues, checkMissingReglatoryElementsInCrosswalk(map)); + lanelet::autoware::validation::appendIssues(issues, checkMissingReglatoryElementsInStopLine(map)); + return issues; +} + +lanelet::validation::Issues +MissingRegulatoryElementsChecker::checkMissingReglatoryElementsInTrafficLight( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + + // Get all line strings whose type is traffic light + auto tl_ids = + map.lineStringLayer | ranges::views::filter([](auto && ls) { + const auto & attrs = ls.attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Type); + return it != attrs.end() && it->second == lanelet::AttributeValueString::TrafficLight; + }) | + ranges::views::transform([](auto && ls) { return ls.id(); }) | ranges::views::unique; + + // Filter regulatory elements whose type is traffic light and has refers + auto reg_elem_tl = map.regulatoryElementLayer | ranges::views::filter([](auto && elem) { + const auto & attrs = elem->attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Subtype); + const auto & params = elem->getParameters(); + return it != attrs.end() && + it->second == lanelet::AttributeValueString::TrafficLight && + params.find(lanelet::RoleNameString::Refers) != params.end(); + }); + // Get all line strings of traffic light referred by regulatory elements + std::set tl_ids_reg_elem; + for (const auto & elem : reg_elem_tl) { + const auto & refers = + elem->getParameters(lanelet::RoleName::Refers); + for (const auto & refer : refers) { + tl_ids_reg_elem.insert(refer.id()); + } + } + + // Check if all line strings of traffic light referred by regulatory elements + for (const auto & tl_id : tl_ids) { + if (tl_ids_reg_elem.find(tl_id) == tl_ids_reg_elem.end()) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::LineString, tl_id, + "Traffic light must have a regulatory element."); + } + } + + return issues; +} + +lanelet::validation::Issues +MissingRegulatoryElementsChecker::checkMissingReglatoryElementsInCrosswalk( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + + // Get all lanelets whose type is crosswalk + std::set cw_ids; + for (const auto & ll : map.laneletLayer) { + const auto & attrs = ll.attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Subtype); + // Check if this lanelet is crosswalk + if (it != attrs.end() && it->second == lanelet::AttributeValueString::Crosswalk) { + cw_ids.insert(ll.id()); + + // Check if crosswalk has reg elem of traffic light + for (const auto & elem : ll.regulatoryElements()) { + const auto & attrs = elem->attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Subtype); + if (it != attrs.end() && it->second == lanelet::AttributeValueString::TrafficLight) { + tl_elem_with_cw_.insert(elem->id()); + } + } + } + } + + // Filter regulatory elements whose type is crosswalk and has refers + auto reg_elem_cw = + map.regulatoryElementLayer | ranges::views::filter([](auto && elem) { + const auto & attrs = elem->attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Subtype); + return it != attrs.end() && it->second == lanelet::AttributeValueString::Crosswalk; + }) | + ranges::views::filter([](auto && elem) { + const auto & param = elem->getParameters(); + return param.find(lanelet::RoleNameString::Refers) != param.end(); + }); + + // Get all lanelets of crosswalk referred by regulatory elements + std::set cw_ids_reg_elem; + for (const auto & elem : reg_elem_cw) { + const auto & refers = elem->getParameters(lanelet::RoleName::Refers); + for (const lanelet::ConstLanelet & refer : refers) { + cw_ids_reg_elem.insert(refer.id()); + } + } + + // Check if all lanelets of crosswalk referred by regulatory elements + for (const auto & cw_id : cw_ids) { + if (cw_ids_reg_elem.find(cw_id) == cw_ids_reg_elem.end()) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Lanelet, cw_id, + "Crosswalk must have a regulatory element."); + } + } + + return issues; +} + +lanelet::validation::Issues +MissingRegulatoryElementsChecker::checkMissingReglatoryElementsInStopLine( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + + // Get all line strings whose type is stop line + auto sl_ids = map.lineStringLayer | ranges::views::filter([](auto && ls) { + const auto & attrs = ls.attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Type); + return it != attrs.end() && it->second == lanelet::AttributeValueString::StopLine; + }) | + ranges::views::transform([](auto && ls) { return ls.id(); }) | + ranges::views::unique; + + // Filter regulatory elements whose refline type is stop line + auto reg_elem_sl = map.regulatoryElementLayer | ranges::views::filter([](auto && elem) { + const auto & params = elem->getParameters(); + return params.find(lanelet::RoleNameString::RefLine) != params.end(); + }); + + // Get all line strings of stop line referred by regulatory elements + std::set sl_ids_reg_elem; + for (const auto & elem : reg_elem_sl) { + const auto & ref_lines = + elem->getParameters(lanelet::RoleName::RefLine); + for (const auto & ref_line : ref_lines) { + const auto & attrs = ref_line.attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Type); + if (it != attrs.end() && it->second == lanelet::AttributeValueString::StopLine) { + sl_ids_reg_elem.insert(ref_line.id()); + } + } + } + + // Check if all line strings of stop line referred by regulatory elements + for (const auto & sl_id : sl_ids) { + if (sl_ids_reg_elem.find(sl_id) == sl_ids_reg_elem.end()) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::LineString, sl_id, + "Stop Line must have a regulatory element."); + } + } + + return issues; +} + +} // namespace validation +} // namespace lanelet diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/regulatory_element_details.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/regulatory_element_details.cpp new file mode 100644 index 00000000..7f1bba9e --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/regulatory_element_details.cpp @@ -0,0 +1,183 @@ +// Copyright 2023 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_extension/autoware_lanelet2_validation/vals/regulatory_element_details.hpp" + +namespace lanelet +{ +namespace validation +{ +namespace +{ +lanelet::validation::RegisterMapValidator reg; +} // namespace + +lanelet::validation::Issues RegulatoryElementDetailsChecker::operator()( + const lanelet::LaneletMap & map) +{ + // All issues found by all validators + lanelet::validation::Issues issues; + + // Append issues found by each validator + lanelet::autoware::validation::appendIssues(issues, checkRegulatoryElementOfTrafficLight(map)); + lanelet::autoware::validation::appendIssues(issues, checkRegulatoryElementOfCrosswalk(map)); + return issues; +} + +bool RegulatoryElementDetailsChecker::isPedestrianTrafficLight( + const std::vector & traffic_lights) +{ + for (const auto & tl : traffic_lights) { + const auto & attrs = tl.attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Subtype); + if (it == attrs.end() || it->second != "red_green") { + return false; + } + } + return true; +} + +lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElementOfTrafficLight( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + // filter regulatory element whose Subtype is traffic light + auto elems = + map.regulatoryElementLayer | ranges::views::filter([](auto && elem) { + const auto & attrs = elem->attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Subtype); + return it != attrs.end() && it->second == lanelet::AttributeValueString::TrafficLight; + }); + + for (const auto & elem : elems) { + // Get line strings of traffic light referred by regulatory element + auto refers = elem->getParameters(lanelet::RoleName::Refers); + // Get stop line referred by regulatory element + auto ref_lines = elem->getParameters(lanelet::RoleName::RefLine); + const auto & issue_tl = lanelet::validation::Issue( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::LineString, + lanelet::utils::getId(), + "Refers of traffic light regulatory element must have type of traffic_light."); + lanelet::autoware::validation::checkPrimitivesType( + refers, lanelet::AttributeValueString::TrafficLight, issue_tl, issues); + + const auto & issue_sl = lanelet::validation::Issue( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::LineString, + lanelet::utils::getId(), + "Refline of traffic light regulatory element must have type of stop_line."); + lanelet::autoware::validation::checkPrimitivesType( + ref_lines, lanelet::AttributeValueString::StopLine, issue_sl, issues); + + if (refers.empty()) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of traffic light must have a traffic light(refers)."); + } + // TODO(sgk-000): Check correct behavior if regulatory element has two or more traffic light + // else if (refers.size() != 1) { + // issues.emplace_back( + // lanelet::validation::Severity::Error, + // lanelet::validation::Primitive::RegulatoryElement, elem->id(), "Regulatory element of + // traffic light must have only one traffic light(refers)."); + // } + + // Report error if regulatory element does not have stop line and this is not a pedestrian + // traffic light + if (ref_lines.empty() && !isPedestrianTrafficLight(refers)) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of traffic light must have a stop line(ref_line)."); + } + } + return issues; +} + +lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElementOfCrosswalk( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + // filter elem whose Subtype is crosswalk + auto elems = map.regulatoryElementLayer | ranges::views::filter([](auto && elem) { + const auto & attrs = elem->attributes(); + const auto & it = attrs.find(lanelet::AttributeName::Subtype); + return it != attrs.end() && it->second == lanelet::AttributeValueString::Crosswalk; + }); + + for (const auto & elem : elems) { + // Get lanelet of crosswalk referred by regulatory element + auto refers = elem->getParameters(lanelet::RoleName::Refers); + // Get stop line referred by regulatory element + auto ref_lines = elem->getParameters(lanelet::RoleName::RefLine); + // Get crosswalk polygon referred by regulatory element + auto crosswalk_polygons = elem->getParameters( + lanelet::autoware::Crosswalk::AutowareRoleNameString::CrosswalkPolygon); + + const auto & issue_cw = lanelet::validation::Issue( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Lanelet, + lanelet::utils::getId(), + "Refers of crosswalk regulatory element must have type of crosswalk."); + lanelet::autoware::validation::checkPrimitivesType( + refers, lanelet::AttributeValueString::Lanelet, lanelet::AttributeValueString::Crosswalk, + issue_cw, issues); + + const auto & issue_sl = lanelet::validation::Issue( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::LineString, + lanelet::utils::getId(), + "Refline of crosswalk regulatory element must have type of stopline."); + lanelet::autoware::validation::checkPrimitivesType( + ref_lines, lanelet::AttributeValueString::StopLine, issue_sl, issues); + + const auto & issue_poly = lanelet::validation::Issue( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Polygon, + lanelet::utils::getId(), + "Crosswalk polygon of crosswalk regulatory element must have type of Crosswalk_polygon."); + lanelet::autoware::validation::checkPrimitivesType( + crosswalk_polygons, lanelet::autoware::Crosswalk::AutowareRoleNameString::CrosswalkPolygon, + issue_poly, issues); + + // Report warning if regulatory element does not have crosswalk polygon + if (crosswalk_polygons.empty()) { + issues.emplace_back( + lanelet::validation::Severity::Warning, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of cross walk is nice to have crosswalk_polygon."); + } else if (crosswalk_polygons.size() > 1) { // Report error if regulatory element has two or + // more crosswalk polygon + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of cross walk must have only one crosswalk_polygon."); + } + // Report Info if regulatory element does not have stop line + if (ref_lines.empty()) { + issues.emplace_back( + lanelet::validation::Severity::Info, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of cross walk does not have stop line(ref_line)."); + } + // Report error if regulatory element does not have lanelet of crosswalk + if (refers.empty()) { + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of cross walk must have lanelet of crosswalk(refers)."); + } else if (refers.size() > 1) { // Report error if regulatory element has two or more lanelet + // of crosswalk + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), + "Regulatory element of cross walk must have only one lanelet of crosswalk(refers)."); + } + } + return issues; +} + +} // namespace validation +} // namespace lanelet diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp new file mode 100644 index 00000000..9f80c8a2 --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp @@ -0,0 +1,54 @@ +// Copyright 2023 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_extension/autoware_lanelet2_validation/validation.hpp" +#include "lanelet2_validation/Validation.h" + +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("autoware_lanelet2_validation"); + + auto config = lanelet::autoware::validation::parseCommandLine( + argc, const_cast(argv)); // NOLINT + + auto command_line_config = config.command_line_config; + if (command_line_config.help) { + return 0; + } + if (command_line_config.print) { + auto checks = + lanelet::validation::availabeChecks(command_line_config.validationConfig.checksFilter); + if (checks.empty()) { + std::cout << "No checks found matching '" << command_line_config.validationConfig.checksFilter + << "'\n"; + } else { + std::cout << "Will use following checks:\n"; + for (auto & check : checks) { + std::cout << check << '\n'; + } + } + return 0; + } + if (command_line_config.mapFile.empty()) { + std::cout << "No map file specified" << std::endl; + return 1; + } + + auto issues = lanelet::autoware::validation::validateMap(config); + lanelet::validation::printAllIssues(issues); + return static_cast(!issues.empty()); +} diff --git a/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_missing_regulatory_elements.cpp b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_missing_regulatory_elements.cpp new file mode 100644 index 00000000..0c326a27 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_missing_regulatory_elements.cpp @@ -0,0 +1,215 @@ +// Copyright 2023 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_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp" + +#include + +using lanelet::AttributeMap; +using lanelet::AttributeName; +using lanelet::AttributeValueString; +using lanelet::Lanelet; +using lanelet::LaneletMapPtr; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::Polygon3d; +using lanelet::RegulatoryElementPtr; +using lanelet::TrafficLight; +using lanelet::autoware::Crosswalk; +using lanelet::utils::getId; +class TestSuite : public ::testing::Test +{ +public: + TestSuite() { initializeAttributes(); } + + ~TestSuite() override = default; + + void initializeAttributes() + { + // Stop Line + sl_attr_[AttributeName::Type] = AttributeValueString::StopLine; + + // Traffic Light + tl_attr_[AttributeName::Type] = AttributeValueString::TrafficLight; + tl_attr_[AttributeName::Subtype] = "red_yellow_green"; + tl_attr_["height"] = "0.5"; + + // Crosswalk polygon + cw_poly_attr_[AttributeName::Type] = + lanelet::autoware::Crosswalk::AutowareRoleNameString::CrosswalkPolygon; + + // Crosswalk + cw_attr_[AttributeName::Type] = AttributeValueString::Lanelet; + cw_attr_[AttributeName::Subtype] = AttributeValueString::Crosswalk; + cw_attr_[AttributeName::SpeedLimit] = "10"; + cw_attr_[AttributeName::OneWay] = "no"; + cw_attr_[AttributeName::Location] = AttributeValueString::Urban; + cw_attr_[AttributeName::ParticipantPedestrian] = "yes"; + + // Regulatory element of traffic light + AttributeMap tl_re_attr_ = AttributeMap(); + tl_re_attr_[AttributeName::Type] = AttributeValueString::RegulatoryElement; + tl_re_attr_[AttributeName::Subtype] = AttributeValueString::TrafficLight; + + // Regulatory element of crosswalk + AttributeMap cw_re_attr_ = AttributeMap(); + cw_re_attr_[AttributeName::Type] = AttributeValueString::RegulatoryElement; + cw_re_attr_[AttributeName::Subtype] = AttributeValueString::Crosswalk; + } + + void addTestMap(LaneletMapPtr in_map_ptr) + { + // Create test map + + // Points for stop line + Point3d p0 = Point3d(getId(), 0.0, 0.0, 0.1); + Point3d p1 = Point3d(getId(), 0.0, 1.0, 0.1); + Point3d p2 = Point3d(getId(), 0.0, 2.0, 0.1); + // Points for traffic light + Point3d p3 = Point3d(getId(), 0.0, 0.0, 5.0); + Point3d p4 = Point3d(getId(), 0.0, 1.0, 5.0); + Point3d p5 = Point3d(getId(), 0.0, 2.0, 5.0); + // Points for crosswalk + Point3d p6 = Point3d(getId(), 1.0, 0.0, 0.1); + Point3d p7 = Point3d(getId(), 1.0, 1.0, 0.1); + Point3d p8 = Point3d(getId(), 1.0, 2.0, 0.1); + Point3d p9 = Point3d(getId(), 2.0, 0.0, 0.1); + Point3d p10 = Point3d(getId(), 2.0, 1.0, 0.1); + Point3d p11 = Point3d(getId(), 2.0, 2.0, 0.1); + + // Stop line + LineString3d sl1 = LineString3d(getId(), {p0, p1}, sl_attr_); + LineString3d sl2 = LineString3d(getId(), {p1, p2}, sl_attr_); + + LineString3d tl1 = LineString3d(getId(), {p3, p4}, tl_attr_); + LineString3d tl2 = LineString3d(getId(), {p4, p5}, tl_attr_); + + // LineStrings for crosswalk + LineString3d cw_ls1 = LineString3d(getId(), {p6, p7}); + LineString3d cw_ls2 = LineString3d(getId(), {p7, p8}); + LineString3d cw_ls3 = LineString3d(getId(), {p9, p10}); + LineString3d cw_ls4 = LineString3d(getId(), {p10, p11}); + + Polygon3d cw_poly1 = Polygon3d(getId(), {p7, p6, p9, p10, p7}, cw_poly_attr_); + Polygon3d cw_poly2 = Polygon3d(getId(), {p8, p7, p10, p11, p8}, cw_poly_attr_); + // Lanelets for crosswalk + Lanelet cw1 = Lanelet(getId(), cw_ls1, cw_ls3, cw_attr_); + Lanelet cw2 = Lanelet(getId(), cw_ls2, cw_ls4, cw_attr_); + + // Traffic light regulatory element + RegulatoryElementPtr tl_reg_elem1, tl_reg_elem2; + tl_reg_elem1 = TrafficLight::make(getId(), tl_re_attr_, {tl1}, {sl1}); + tl_reg_elem2 = TrafficLight::make(getId(), tl_re_attr_, {tl2}, {sl2}); + + // Crosswalk regulatory element + RegulatoryElementPtr cw_reg_elem1, cw_reg_elem2; + cw_reg_elem1 = Crosswalk::make(getId(), cw_re_attr_, cw1, cw_poly1, {sl1}); + cw_reg_elem2 = Crosswalk::make(getId(), cw_re_attr_, cw2, cw_poly2, {sl2}); + + cw1.addRegulatoryElement(cw_reg_elem1); + cw2.addRegulatoryElement(cw_reg_elem2); + + // Add elements to map + for (const auto & re : {tl_reg_elem1, tl_reg_elem2}) { + in_map_ptr->add(re); + } + for (const auto & cw : {cw1, cw2}) { + in_map_ptr->add(cw); + } + } + AttributeMap sl_attr_, tl_attr_, cw_attr_, cw_poly_attr_, tl_re_attr_, cw_re_attr_; + lanelet::validation::MissingRegulatoryElementsChecker checker_; + +private: +}; + +TEST_F(TestSuite, ValidatorAvailability) // NOLINT for gtest +{ + lanelet::validation::Strings validators = lanelet::validation::availabeChecks( + lanelet::validation::MissingRegulatoryElementsChecker::name()); + uint8_t expected_num_validators = 1; + EXPECT_EQ(expected_num_validators, validators.size()); + for (const auto & v : validators) { + EXPECT_EQ(lanelet::validation::MissingRegulatoryElementsChecker::name(), v); + } +} + +TEST_F(TestSuite, MissingRegulatoryElementOfTrafficLight) // NOLINT for gtest +{ + // Check missing regulatory element of traffic light + + const auto & tl_no_reg_elem = LineString3d( + 99999, {Point3d(getId(), 0.0, 3.0, 5.0), Point3d(getId(), 0.0, 4.0, 5.0)}, tl_attr_); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({tl_no_reg_elem}); + addTestMap(test_map_ptr); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 1; + static constexpr const char * expected_message = "Traffic light must have a regulatory element."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + EXPECT_EQ(99999, issue.id); + EXPECT_EQ(expected_message, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } +} + +TEST_F(TestSuite, MissingRegulatoryElementOfCrosswalk) // NOLINT for gtest +{ + // Check missing regulatory element of crosswalk + + const auto & cw_no_reg_elem = Lanelet( + 99999, + LineString3d(getId(), {Point3d(getId(), 3.0, 0.0, 0.1), Point3d(getId(), 3.0, 1.0, 0.1)}), + LineString3d(getId(), {Point3d(getId(), 3.0, 1.0, 0.1), Point3d(getId(), 3.0, 2.0, 0.1)}), + cw_attr_); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({cw_no_reg_elem}); + addTestMap(test_map_ptr); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 1; + static constexpr const char * expected_message = "Crosswalk must have a regulatory element."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + EXPECT_EQ(99999, issue.id); + EXPECT_EQ(expected_message, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::Lanelet, issue.primitive); + } +} + +TEST_F(TestSuite, MissingRegulatoryElementOfStopLine) // NOLINT for gtest +{ + // Check missing regulatory element of stop line + + const auto & sl_no_reg_elem = LineString3d( + 99999, {Point3d(getId(), 0.0, 3.0, 5.0), Point3d(getId(), 0.0, 4.0, 5.0)}, sl_attr_); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({sl_no_reg_elem}); + addTestMap(test_map_ptr); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 1; + static constexpr const char * expected_message = "Stop Line must have a regulatory element."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + EXPECT_EQ(99999, issue.id); + EXPECT_EQ(expected_message, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } +} diff --git a/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_regulatory_element_details.cpp b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_regulatory_element_details.cpp new file mode 100644 index 00000000..d12067ee --- /dev/null +++ b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_regulatory_element_details.cpp @@ -0,0 +1,343 @@ +// Copyright 2023 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_extension/autoware_lanelet2_validation/vals/regulatory_element_details.hpp" + +#include + +using lanelet::AttributeMap; +using lanelet::AttributeName; +using lanelet::AttributeValueString; +using lanelet::Lanelet; +using lanelet::LaneletMapPtr; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::Polygon3d; +using lanelet::RegulatoryElementPtr; +using lanelet::TrafficLight; +using lanelet::autoware::Crosswalk; +using lanelet::utils::getId; +class TestSuite : public ::testing::Test +{ +public: + TestSuite() { initializeAttributes(); } + + ~TestSuite() override = default; + + void initializeAttributes() + { + // Stop Line + sl_attr[AttributeName::Type] = AttributeValueString::StopLine; + + // Traffic Light + tl_attr[AttributeName::Type] = AttributeValueString::TrafficLight; + tl_attr[AttributeName::Subtype] = "red_yellow_green"; + tl_attr["height"] = "0.5"; + + // Crosswalk polygon + cw_poly_attr[AttributeName::Type] = + lanelet::autoware::Crosswalk::AutowareRoleNameString::CrosswalkPolygon; + + // Crosswalk + cw_attr[AttributeName::Type] = AttributeValueString::Lanelet; + cw_attr[AttributeName::Subtype] = AttributeValueString::Crosswalk; + cw_attr[AttributeName::SpeedLimit] = "10"; + cw_attr[AttributeName::OneWay] = "no"; + cw_attr[AttributeName::Location] = AttributeValueString::Urban; + cw_attr[AttributeName::ParticipantPedestrian] = "yes"; + + // Regulatory element of traffic light + AttributeMap tl_re_attr = AttributeMap(); + tl_re_attr[AttributeName::Type] = AttributeValueString::RegulatoryElement; + tl_re_attr[AttributeName::Subtype] = AttributeValueString::TrafficLight; + + // Regulatory element of crosswalk + AttributeMap cw_re_attr = AttributeMap(); + cw_re_attr[AttributeName::Type] = AttributeValueString::RegulatoryElement; + cw_re_attr[AttributeName::Subtype] = AttributeValueString::Crosswalk; + } + + void addTestMap(LaneletMapPtr in_map_ptr) + { + // Create test map + + // Points for stop line + Point3d p0 = Point3d(getId(), 0.0, 0.0, 0.1); + Point3d p1 = Point3d(getId(), 0.0, 1.0, 0.1); + Point3d p2 = Point3d(getId(), 0.0, 2.0, 0.1); + // Points for traffic light + Point3d p3 = Point3d(getId(), 0.0, 0.0, 5.0); + Point3d p4 = Point3d(getId(), 0.0, 1.0, 5.0); + Point3d p5 = Point3d(getId(), 0.0, 2.0, 5.0); + // Points for crosswalk + Point3d p6 = Point3d(getId(), 1.0, 0.0, 0.1); + Point3d p7 = Point3d(getId(), 1.0, 1.0, 0.1); + Point3d p8 = Point3d(getId(), 1.0, 2.0, 0.1); + Point3d p9 = Point3d(getId(), 2.0, 0.0, 0.1); + Point3d p10 = Point3d(getId(), 2.0, 1.0, 0.1); + Point3d p11 = Point3d(getId(), 2.0, 2.0, 0.1); + + // Stop line + LineString3d sl1 = LineString3d(getId(), {p0, p1}, sl_attr); + LineString3d sl2 = LineString3d(getId(), {p1, p2}, sl_attr); + + LineString3d tl1 = LineString3d(getId(), {p3, p4}, tl_attr); + LineString3d tl2 = LineString3d(getId(), {p4, p5}, tl_attr); + + // LineStrings for crosswalk + LineString3d cw_ls1 = LineString3d(getId(), {p6, p7}); + LineString3d cw_ls2 = LineString3d(getId(), {p7, p8}); + LineString3d cw_ls3 = LineString3d(getId(), {p9, p10}); + LineString3d cw_ls4 = LineString3d(getId(), {p10, p11}); + + Polygon3d cw_poly1 = Polygon3d(getId(), {p7, p6, p9, p10, p7}, cw_poly_attr); + Polygon3d cw_poly2 = Polygon3d(getId(), {p8, p7, p10, p11, p8}, cw_poly_attr); + // Lanelets for crosswalk + Lanelet cw1 = Lanelet(getId(), cw_ls1, cw_ls3, cw_attr); + Lanelet cw2 = Lanelet(getId(), cw_ls2, cw_ls4, cw_attr); + + // Traffic light regulatory element + RegulatoryElementPtr tl_reg_elem1, tl_reg_elem2; + tl_reg_elem1 = TrafficLight::make(getId(), tl_re_attr, {tl1}, {sl1}); + tl_reg_elem2 = TrafficLight::make(getId(), tl_re_attr, {tl2}, {sl2}); + + // Crosswalk regulatory element + RegulatoryElementPtr cw_reg_elem1, cw_reg_elem2; + cw_reg_elem1 = Crosswalk::make(getId(), cw_re_attr, cw1, cw_poly1, {sl1}); + cw_reg_elem2 = Crosswalk::make(getId(), cw_re_attr, cw2, cw_poly2, {sl2}); + + cw1.addRegulatoryElement(cw_reg_elem1); + cw2.addRegulatoryElement(cw_reg_elem2); + + // Add elements to map + for (const auto & re : {tl_reg_elem1, tl_reg_elem2}) { + in_map_ptr->add(re); + } + for (const auto & cw : {cw1, cw2}) { + in_map_ptr->add(cw); + } + } + AttributeMap sl_attr, tl_attr, cw_attr, cw_poly_attr, tl_re_attr, cw_re_attr; + lanelet::validation::RegulatoryElementDetailsChecker checker_; + +private: +}; + +TEST_F(TestSuite, ValidatorAvailability) // NOLINT for gtest +{ + lanelet::validation::Strings validators = lanelet::validation::availabeChecks( + lanelet::validation::RegulatoryElementDetailsChecker::name()); + uint8_t expected_num_validators = 1; + EXPECT_EQ(expected_num_validators, validators.size()); + for (const auto & v : validators) { + EXPECT_EQ(lanelet::validation::RegulatoryElementDetailsChecker::name(), v); + } +} + +TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutTrafficLight) // NOLINT for gtest +{ + // Check regulatory element of traffic light without traffic light + + RegulatoryElementPtr tl_reg_elem_no_tl; + // Line string without traffic light attribute + const auto & ls = LineString3d(99998, {}); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({ls}); + // Traffic light regulatory element without traffic light. It refers to the line string without + // traffic light attribute. + tl_reg_elem_no_tl = TrafficLight::make( + 99999, tl_re_attr, {ls}, + {LineString3d( + getId(), {Point3d(getId(), 3.0, 3.0, 0.1), Point3d(getId(), 3.0, 4.0, 0.1)}, sl_attr)}); + test_map_ptr->add(tl_reg_elem_no_tl); + addTestMap(test_map_ptr); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 2; + static constexpr const char * expected_message1 = + "Refers of traffic light regulatory element must have type of traffic_light."; + static constexpr const char * expected_message2 = + "Regulatory element of traffic light must have a traffic light(refers)."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + if (issue.id == 99998) { + EXPECT_EQ(expected_message1, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } else if (issue.id == 99999) { + EXPECT_EQ(expected_message2, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::RegulatoryElement, issue.primitive); + } else { + FAIL() << "Unexpected issue id: " << issue.id; + } + } +} + +TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutStopLine) // NOLINT for gtest +{ + // Check regulatory element of traffic light without stop line + + RegulatoryElementPtr tl_reg_elem_no_sl; + // Line string without stop line attribute + const auto & ls = LineString3d(99998, {}); + const auto & tl = LineString3d( + getId(), {Point3d(getId(), 0.0, 3.0, 5.0), Point3d(getId(), 0.0, 4.0, 5.0)}, tl_attr); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({tl}); + // Traffic light regulatory element without stop line. It refers to the line string without stop + // line attribute. + tl_reg_elem_no_sl = TrafficLight::make(99999, tl_re_attr, {tl}, {ls}); + test_map_ptr->add(tl_reg_elem_no_sl); + addTestMap(test_map_ptr); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 2; + static constexpr const char * expected_message1 = + "Refline of traffic light regulatory element must have type of stop_line."; + static constexpr const char * expected_message2 = + "Regulatory element of traffic light must have a stop line(ref_line)."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + if (issue.id == 99998) { + EXPECT_EQ(expected_message1, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } else if (issue.id == 99999) { + EXPECT_EQ(expected_message2, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::RegulatoryElement, issue.primitive); + } else { + FAIL() << "Unexpected issue id: " << issue.id; + } + } +} + +TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutPolygon) // NOLINT for gtest +{ + // Check regulatory element of crosswalk without polygon + + RegulatoryElementPtr cw_reg_elem_no_poly; + Lanelet cw_no_poly = Lanelet( + getId(), + LineString3d(getId(), {Point3d(getId(), 3.0, 0.0, 0.1), Point3d(getId(), 3.0, 1.0, 0.1)}), + LineString3d(getId(), {Point3d(getId(), 3.0, 1.0, 0.1), Point3d(getId(), 3.0, 2.0, 0.1)}), + cw_attr); + + // Crosswalk regulatory element without cross walk polygon. It refers to the polygon without cross + // walk polygon attribute. + RegulatoryElementPtr reg_elem = Crosswalk::make( + 99999, cw_re_attr, cw_no_poly, Polygon3d(99998), + {LineString3d( + getId(), {Point3d(getId(), 3.0, 3.0, 0.1), Point3d(getId(), 3.0, 4.0, 0.1)}, sl_attr)}); + cw_no_poly.addRegulatoryElement(reg_elem); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({cw_no_poly}); + addTestMap(test_map_ptr); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 2; + static constexpr const char * expected_message1 = + "Crosswalk polygon of crosswalk regulatory element must have type of Crosswalk_polygon."; + static constexpr const char * expected_message2 = + "Regulatory element of cross walk is nice to have crosswalk_polygon."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + if (issue.id == 99998) { + EXPECT_EQ(expected_message1, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::Polygon, issue.primitive); + } else if (issue.id == 99999) { + EXPECT_EQ(expected_message2, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Warning, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::RegulatoryElement, issue.primitive); + } else { + FAIL() << "Unexpected issue id: " << issue.id; + } + } +} + +TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutStopline) // NOLINT for gtest +{ + // Check regulatory element of crosswalk without stop line + + Lanelet cw_no_sl = Lanelet( + getId(), + LineString3d(getId(), {Point3d(getId(), 3.0, 0.0, 0.1), Point3d(getId(), 3.0, 1.0, 0.1)}), + LineString3d(getId(), {Point3d(getId(), 3.0, 1.0, 0.1), Point3d(getId(), 3.0, 2.0, 0.1)}), + cw_attr); + + // Crosswalk regulatory element without stop line. + RegulatoryElementPtr reg_elem = Crosswalk::make( + 99999, cw_re_attr, cw_no_sl, + Polygon3d(getId(), {Point3d(getId(), 3.0, 3.0, 0.1)}, cw_poly_attr), {}); + cw_no_sl.addRegulatoryElement(reg_elem); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({cw_no_sl}); + addTestMap(test_map_ptr); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 1; + static constexpr const char * expected_message = + "Regulatory element of cross walk does not have stop line(ref_line)."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + EXPECT_EQ(expected_message, issue.message); + EXPECT_EQ(99999, issue.id); + EXPECT_EQ(lanelet::validation::Severity::Info, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::RegulatoryElement, issue.primitive); + } +} + +TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutCrosswalk) // NOLINT for gtest +{ + // Check regulatory element of crosswalk without crosswalk + + const auto poly = Polygon3d(getId(), {Point3d(getId(), 3.0, 3.0, 0.1)}, cw_poly_attr); + // Lanelet without crosswalk attribute + const auto ll = Lanelet( + 99998, + LineString3d(getId(), {Point3d(getId(), 3.0, 0.0, 0.1), Point3d(getId(), 3.0, 1.0, 0.1)}), + LineString3d(getId(), {Point3d(getId(), 3.0, 1.0, 0.1), Point3d(getId(), 3.0, 2.0, 0.1)})); + // Crosswalk regulatory element without crosswalk. It refers to the lanelet without crosswalk + RegulatoryElementPtr reg_elem = Crosswalk::make( + 99999, cw_re_attr, ll, poly, + {LineString3d( + getId(), {Point3d(getId(), 3.0, 3.0, 0.1), Point3d(getId(), 3.0, 4.0, 0.1)}, sl_attr)}); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({poly}); + addTestMap(test_map_ptr); + test_map_ptr->add(reg_elem); + + const auto & issues = checker_(*test_map_ptr); + + uint8_t expected_num_issues = 2; + static constexpr const char * expected_message1 = + "Refers of crosswalk regulatory element must have type of crosswalk."; + static constexpr const char * expected_message2 = + "Regulatory element of cross walk must have lanelet of crosswalk(refers)."; + EXPECT_EQ(expected_num_issues, issues.size()); + for (const auto & issue : issues) { + if (issue.id == 99998) { + EXPECT_EQ(expected_message1, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::Lanelet, issue.primitive); + } else if (issue.id == 99999) { + EXPECT_EQ(expected_message2, issue.message); + EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::RegulatoryElement, issue.primitive); + } else { + FAIL() << "Unexpected issue id: " << issue.id; + } + } +}