From e3e81ac68c3a841cecd8cf61a5c45e1ff44efbb3 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 09:59:32 +0900 Subject: [PATCH 01/34] Added autoware_lanenet2_validation Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/cli.hpp | 23 ++ .../unconnected_relations.hpp | 60 ++++ .../autoware_lanelet2_validation/utils.hpp | 17 + .../validation.hpp | 35 ++ .../autoware_lanelet2_validation/lib/cli.cpp | 70 ++++ .../lib/utils.cpp | 20 ++ .../lib/validation.cpp | 76 ++++ .../lib/validators/unconnected_relations.cpp | 294 ++++++++++++++++ .../src/autoware_lanelet2_validation/main.cpp | 41 +++ .../test_unconnected_relations.cpp | 329 ++++++++++++++++++ 10 files changed, 965 insertions(+) create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp create mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp create mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp create mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp create mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/unconnected_relations.cpp create mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp create mode 100644 tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp 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..b7350476 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp @@ -0,0 +1,23 @@ +#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 diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp new file mode 100644 index 00000000..42220ad5 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp @@ -0,0 +1,60 @@ +// 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 +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace lanelet +{ +namespace validation +{ + +// TODO: update description +//! This check looks for points within linestrings or polygons that appear two times in succession. +//! These are not allowed because they often confuse geometry algorithms. +class UnconnectedRelationsChecker : public lanelet::validation::MapValidator +{ +public: + constexpr static const char * name() { return "mapping.unconnected_relations"; } + + lanelet::validation::Issues operator()(const lanelet::LaneletMap & map) override; + +private: + lanelet::validation::Issues checkRegElemExistsInAllTrafficLight(const lanelet::LaneletMap & map); + lanelet::validation::Issues checkRegElemExistsInAllCrosswalk(const lanelet::LaneletMap & map); + lanelet::validation::Issues checkRegElemOfTrafficLight(const lanelet::LaneletMap & map); + lanelet::validation::Issues checkRegElemOfCrosswalk(const lanelet::LaneletMap & map); + template + void checkPrimitiveType( + std::vector & in_vec, const std::string & expected_type, const std::string & message, + lanelet::validation::Issues & issues); + template + void checkPrimitiveType( + std::vector & in_vec, const std::string & expected_type, + const std::string & expected_subtype, const std::string & message, + lanelet::validation::Issues & issues); + + std::set tl_elem_with_cw_; +}; +} // namespace validation +} // namespace lanelet 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..509c8245 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp @@ -0,0 +1,17 @@ +#include +#include + +namespace lanelet +{ +namespace validation +{ +std::vector parseFilterString(const std::string & str); + +void appendIssues( + std::vector & to, + std::vector && from); + +void appendIssues(lanelet::validation::Issues & to, lanelet::validation::Issues && from); + +} // namespace validation +} // namespace lanelet 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..97e34990 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp @@ -0,0 +1,35 @@ +#include "lanelet2_extension/autoware_lanelet2_validation/cli.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace +{ +namespace projector_names +{ +constexpr const char * transverse_mercator = "tm"; +constexpr const char * mgrs = "mgrs"; +constexpr const char * utm = "utm"; +constexpr const char * local = "local"; +} // 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 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..ed72eeb6 --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp @@ -0,0 +1,70 @@ +#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", po::value(), "Path to the map to be validated") + + ("filter,f", po::value(&validation_config.checksFilter), + "Comma separated list of regexes to filter the applicable tests. Will run all tests by " + "default. Example: " + "routing_graph.* to run all checks for the routing graph") + + ("projector,p", po::value(&config.projector_type)->composing(), + "Participants for which the routing graph will be instanciated (default: vehicle)") + + ("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") + + ("lon", + po::value(&validation_config.origin.lon) + ->default_value(validation_config.origin.lon), + "longitude coofdinate of map origin") + + ("print", "Only print the checks that will be run, 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 (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/utils.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp new file mode 100644 index 00000000..e0b0559b --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp @@ -0,0 +1,20 @@ +#include + +namespace lanelet +{ +namespace validation +{ + +void appendIssues( + std::vector & to, + std::vector && from) +{ + to.insert(to.end(), std::make_move_iterator(from.begin()), std::make_move_iterator(from.end())); +} +void appendIssues(lanelet::validation::Issues & to, lanelet::validation::Issues && from) +{ + to.insert(to.end(), from.begin(), from.end()); +} + +} // namespace validation +} // 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..32e51420 --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp @@ -0,0 +1,76 @@ +#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::transverse_mercator) { + return std::make_unique( + lanelet::Origin{val_config.origin}); + } else if (config.projector_type == projector_names::mgrs) { + return std::make_unique(); + } else if (config.projector_type == projector_names::utm) { + return std::make_unique(lanelet::Origin{val_config.origin}); + } else { + std::cerr << "Unknown projector type: " << config.projector_type << "'\n"; + std::cerr << "Set to default projector: Transverse Mercator projector" << std::endl; + return std::make_unique( + lanelet::Origin{val_config.origin}); + } +} + +std::vector validateMap(const MetaConfig & config) +{ + const auto & cm_config = config.command_line_config; + const auto & val_config = config.command_line_config.validationConfig; + + using namespace std::string_literals; + 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/validators/unconnected_relations.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/unconnected_relations.cpp new file mode 100644 index 00000000..15bbd76c --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/unconnected_relations.cpp @@ -0,0 +1,294 @@ +#include + +namespace lanelet +{ +namespace validation +{ +namespace +{ +lanelet::validation::RegisterMapValidator reg; +} // namespace + +lanelet::validation::Issues UnconnectedRelationsChecker::operator()(const lanelet::LaneletMap & map) +{ + // All issues found by all validators + lanelet::validation::Issues issues; + + // Append issues found by each validator + appendIssues(issues, checkRegElemExistsInAllTrafficLight(map)); + appendIssues(issues, checkRegElemExistsInAllCrosswalk(map)); + appendIssues(issues, checkRegElemOfTrafficLight(map)); + appendIssues(issues, checkRegElemOfCrosswalk(map)); + return issues; +} + +template +void UnconnectedRelationsChecker::checkPrimitiveType( + std::vector & in_vec, const std::string & expected_type, const std::string & message, + 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) { + // Report warning if crosswalk polygon does not have type of crosswalk_polygon + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, + item.id(), message); + const auto new_it = in_vec.erase(iter); + if (new_it != in_vec.end()) { + iter = new_it; + } else { + break; + } + } + } +} + +template +void UnconnectedRelationsChecker::checkPrimitiveType( + std::vector & in_vec, const std::string & expected_type, const std::string & expected_subtype, + const std::string & message, 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) { + // Report warning if crosswalk polygon does not have type of crosswalk_polygon + issues.emplace_back( + lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, + item.id(), message); + const auto new_it = in_vec.erase(iter); + if (new_it != in_vec.end()) { + iter = new_it; + } else { + break; + } + } + } +} + +lanelet::validation::Issues UnconnectedRelationsChecker::checkRegElemExistsInAllTrafficLight( + 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 and + 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 UnconnectedRelationsChecker::checkRegElemExistsInAllCrosswalk( + 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::LineString, cw_id, + "Crosswalk must have a regulatory element."); + } + } + + return issues; +} + +lanelet::validation::Issues UnconnectedRelationsChecker::checkRegElemOfTrafficLight( + 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); + + checkPrimitiveType( + refers, lanelet::AttributeValueString::TrafficLight, + "Refers of traffic light regulatory element must have type of traffic_light", issues); + checkPrimitiveType( + ref_lines, lanelet::AttributeValueString::StopLine, + "Refline of traffic light regulatory element must have type of stop_line", 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: 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 crosswalk + if (ref_lines.empty() && tl_elem_with_cw_.find(elem->id()) == tl_elem_with_cw_.end()) { + 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 UnconnectedRelationsChecker::checkRegElemOfCrosswalk( + const lanelet::LaneletMap & map) +{ + lanelet::validation::Issues issues; + // filter elem whose Subtype is crosswalk and has crosswalk polygon + 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); + + checkPrimitiveType( + refers, lanelet::AttributeValueString::Lanelet, lanelet::AttributeValueString::Crosswalk, + "Refers of crosswalk regulatory element must have type of crosswalk", issues); + checkPrimitiveType( + ref_lines, lanelet::AttributeValueString::StopLine, + "Refline of crosswalk regulatory element must have type of stopline", issues); + checkPrimitiveType( + crosswalk_polygons, lanelet::autoware::Crosswalk::AutowareRoleNameString::CrosswalkPolygon, + "Crosswalk polygon of crosswalk regulatory element must have type of Crosswalk_polygon", + 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."); + } + // Report warning if regulatory element has two or more crosswalk polygon + else if (crosswalk_polygons.size() != 1) { + issues.emplace_back( + lanelet::validation::Severity::Warning, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of cross walk must have only one crosswalk_polygon."); + } + // Report warning if regulatory element does not have stop line + if (ref_lines.empty()) { + issues.emplace_back( + lanelet::validation::Severity::Warning, lanelet::validation::Primitive::RegulatoryElement, + elem->id(), "Regulatory element of cross walk is nice to 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)."); + } + // Report error if regulatory element has two or more lanelet of crosswalk + else if (refers.size() != 1) { + 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..bd621a39 --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp @@ -0,0 +1,41 @@ +#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"); + // lanelet::validation::RegisterMapValidator(); + // lanelet::RegisterRegulatoryElement(); + + auto config = lanelet::autoware::validation::parseCommandLine( + argc, const_cast(argv)); // NOLINT + auto command_line_config = config.command_line_config; + // return runFromConfig(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 int(!issues.empty()); +} diff --git a/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp new file mode 100644 index 00000000..b9e36558 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp @@ -0,0 +1,329 @@ +// TODO: Add license header +// TODO: remove unused includes +#include "lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.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() + { + // TODO: remove + lanelet::validation::RegisterMapValidator(); + + initializeAttributes(); + config.checksFilter = "mapping.unconnected_relations"; + validators = lanelet::validation::availabeChecks(config.checksFilter); + } + + ~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::ValidationConfig config; + lanelet::validation::Strings validators; + +private: +}; + +// TEST_F(TestSuite, SaveMap) // NOLINT for gtest +// { +// // Save map +// lanelet::write("/tmp/test_map.osm", *test_map_ptr); +// } + +TEST_F(TestSuite, AvailableValidator) // NOLINT for gtest +{ + unsigned int expected_num_validators = 1; + EXPECT_EQ(expected_num_validators, validators.size()); + for (const auto & v : validators) { + EXPECT_EQ("mapping.unconnected_relations", 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); + + auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); + for (const auto & detect_issue : detect_issues) { + for (const auto & issue : detect_issue.issues) { + std::cout << "message = " << issue.message << std::endl; + EXPECT_EQ(99999, issue.id); + 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); + + auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); + for (const auto & detect_issue : detect_issues) { + for (const auto & issue : detect_issue.issues) { + std::cout << "message = " << issue.message << std::endl; + EXPECT_EQ(99999, issue.id); + // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + // EXPECT_EQ(lanelet::validation::Primitive::Lanelet, issue.primitive); + } + } +} + +TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutTrafficLight) // NOLINT for gtest +{ + // Check regulatory element of traffic light without stop line + + RegulatoryElementPtr tl_reg_elem_no_tl; + const auto & tl = LineString3d(99998, {}); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({tl}); + tl_reg_elem_no_tl = TrafficLight::make( + 99999, tl_re_attr, {tl}, + {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); + + auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); + for (const auto & detect_issue : detect_issues) { + for (const auto & issue : detect_issue.issues) { + std::cout << "message = " << issue.message << std::endl; + EXPECT_TRUE(issue.id == 99999 || issue.id == 99998); + // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } + } +} + +TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutStopLine) // NOLINT for gtest +{ + // Check regulatory element of traffic light without stop line + + RegulatoryElementPtr tl_reg_elem_no_sl; + 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}); + tl_reg_elem_no_sl = TrafficLight::make(99999, tl_re_attr, {tl}, {}); + test_map_ptr->add(tl_reg_elem_no_sl); + addTestMap(test_map_ptr); + + auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); + for (const auto & detect_issue : detect_issues) { + for (const auto & issue : detect_issue.issues) { + std::cout << "message = " << issue.message << std::endl; + EXPECT_EQ(99999, issue.id); + // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } + } +} + +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); + + 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); + + auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); + for (const auto & detect_issue : detect_issues) { + for (const auto & issue : detect_issue.issues) { + std::cout << "message = " << issue.message << std::endl; + EXPECT_TRUE(issue.id == 99999 || issue.id == 99998); + // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } + } +} + +TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutStopline) // NOLINT for gtest +{ + // Check regulatory element of crosswalk without stop line + + RegulatoryElementPtr cw_reg_elem_no_sl; + 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); + + 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); + + auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); + for (const auto & detect_issue : detect_issues) { + for (const auto & issue : detect_issue.issues) { + std::cout << "message = " << issue.message << std::endl; + EXPECT_EQ(99999, issue.id); + // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } + } +} + +TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutCrosswalk) // NOLINT for gtest +{ + // Check regulatory element of crosswalk without crosswalk + + const auto poly_no_cw = Polygon3d(getId(), {Point3d(getId(), 3.0, 3.0, 0.1)}, cw_poly_attr); + 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)})); + + RegulatoryElementPtr reg_elem = Crosswalk::make( + 99999, cw_re_attr, ll, poly_no_cw, + {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_no_cw}); + addTestMap(test_map_ptr); + test_map_ptr->add(reg_elem); + + // TODO: remove + // lanelet::write("/tmp/test_map.osm", *test_map_ptr); + + auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); + for (const auto & detect_issue : detect_issues) { + for (const auto & issue : detect_issue.issues) { + std::cout << "message = " << issue.message << std::endl; + EXPECT_TRUE(issue.id == 99999 || issue.id == 99998); + // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); + // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); + } + } +} From 546bf3c14109c57954d9dc3267cbf6d8acba2779 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 10:00:11 +0900 Subject: [PATCH 02/34] Added autoware_lanelet2_validation to cmake Signed-off-by: Shigeki Kobayashi --- tmp/lanelet2_extension/CMakeLists.txt | 44 +++++++++++++++++++++------ 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index f67e2bf3..82fc16b6 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,14 +64,6 @@ 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 - ${catkin_LIBRARIES} - ${PUGIXML_LIBRARIES} - lanelet2_extension_lib -) - ament_auto_add_executable(check_right_of_way src/check_right_of_way.cpp) add_dependencies(check_right_of_way lanelet2_extension_lib) target_link_libraries(check_right_of_way @@ -80,6 +72,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 +99,28 @@ 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 + ) + target_include_directories(${VALIDATION_NAME}_test + SYSTEM PRIVATE + ${lanelet2_core_INCLUDE_DIRECTORIES} + ) + endfunction() + + add_validation_test(unconnected_relations) endif() ament_auto_package() From b34790bbcfa17ec859d6920ad649278e3bef1ece Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 10:56:01 +0900 Subject: [PATCH 03/34] Splited unconnected_relations checker to missing regulatory elements checker and regulatory element details checker Signed-off-by: Shigeki Kobayashi --- tmp/lanelet2_extension/CMakeLists.txt | 2 +- .../unconnected_relations.hpp | 60 -------- .../missing_regulatory_elements.hpp | 46 +++++++ .../validators/regulatory_element_details.hpp | 42 ++++++ .../missing_regulatory_elements.cpp | 130 ++++++++++++++++++ ...ons.cpp => regulatory_element_details.cpp} | 124 ++--------------- 6 files changed, 229 insertions(+), 175 deletions(-) delete mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp create mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp rename tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/{unconnected_relations.cpp => regulatory_element_details.cpp} (60%) diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index 82fc16b6..ac4ca495 100644 --- a/tmp/lanelet2_extension/CMakeLists.txt +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -120,7 +120,7 @@ if(BUILD_TESTING) ) endfunction() - add_validation_test(unconnected_relations) + # add_validation_test(unconnected_relations) endif() ament_auto_package() diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp deleted file mode 100644 index 42220ad5..00000000 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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 -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace lanelet -{ -namespace validation -{ - -// TODO: update description -//! This check looks for points within linestrings or polygons that appear two times in succession. -//! These are not allowed because they often confuse geometry algorithms. -class UnconnectedRelationsChecker : public lanelet::validation::MapValidator -{ -public: - constexpr static const char * name() { return "mapping.unconnected_relations"; } - - lanelet::validation::Issues operator()(const lanelet::LaneletMap & map) override; - -private: - lanelet::validation::Issues checkRegElemExistsInAllTrafficLight(const lanelet::LaneletMap & map); - lanelet::validation::Issues checkRegElemExistsInAllCrosswalk(const lanelet::LaneletMap & map); - lanelet::validation::Issues checkRegElemOfTrafficLight(const lanelet::LaneletMap & map); - lanelet::validation::Issues checkRegElemOfCrosswalk(const lanelet::LaneletMap & map); - template - void checkPrimitiveType( - std::vector & in_vec, const std::string & expected_type, const std::string & message, - lanelet::validation::Issues & issues); - template - void checkPrimitiveType( - std::vector & in_vec, const std::string & expected_type, - const std::string & expected_subtype, const std::string & message, - lanelet::validation::Issues & issues); - - std::set tl_elem_with_cw_; -}; -} // namespace validation -} // namespace lanelet diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp new file mode 100644 index 00000000..da3161c9 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp @@ -0,0 +1,46 @@ +// 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 +#include +#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); + std::set tl_elem_with_cw_; +}; +} // namespace validation +} // namespace lanelet diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp new file mode 100644 index 00000000..f98c7db0 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp @@ -0,0 +1,42 @@ +#include +#include +#include + +#include +#include + +#include + +namespace lanelet +{ +namespace validation +{ + +// TODO: update description +//! This check looks for points within linestrings or polygons that appear two times in succession. +//! These are not allowed because they often confuse geometry algorithms. +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: + lanelet::validation::Issues checkRegulatoryElementsOfTrafficLight( + const lanelet::LaneletMap & map); + lanelet::validation::Issues checkRegulatoryElementsOfCrosswalk(const lanelet::LaneletMap & map); + template + void checkPrimitiveType( + std::vector & in_vec, const std::string & expected_type, const std::string & message, + lanelet::validation::Issues & issues); + template + void checkPrimitiveType( + std::vector & in_vec, const std::string & expected_type, + const std::string & expected_subtype, const std::string & message, + lanelet::validation::Issues & issues); + + std::set tl_elem_with_cw_; +}; +} // namespace validation +} // namespace lanelet diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp new file mode 100644 index 00000000..735a3f3b --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp @@ -0,0 +1,130 @@ +#include + +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 + appendIssues(issues, checkMissingReglatoryElementsInTrafficLight(map)); + appendIssues(issues, checkMissingReglatoryElementsInCrosswalk(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 and + 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::LineString, cw_id, + "Crosswalk must have a regulatory element."); + } + } + + return issues; +} + +} // namespace validation +} // namespace lanelet diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/unconnected_relations.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp similarity index 60% rename from tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/unconnected_relations.cpp rename to tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp index 15bbd76c..06485e7e 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/unconnected_relations.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp @@ -1,4 +1,4 @@ -#include +#include namespace lanelet { @@ -6,24 +6,23 @@ namespace validation { namespace { -lanelet::validation::RegisterMapValidator reg; +lanelet::validation::RegisterMapValidator reg; } // namespace -lanelet::validation::Issues UnconnectedRelationsChecker::operator()(const lanelet::LaneletMap & map) +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 - appendIssues(issues, checkRegElemExistsInAllTrafficLight(map)); - appendIssues(issues, checkRegElemExistsInAllCrosswalk(map)); - appendIssues(issues, checkRegElemOfTrafficLight(map)); - appendIssues(issues, checkRegElemOfCrosswalk(map)); + appendIssues(issues, checkRegulatoryElementsOfTrafficLight(map)); + appendIssues(issues, checkRegulatoryElementsOfCrosswalk(map)); return issues; } template -void UnconnectedRelationsChecker::checkPrimitiveType( +void RegulatoryElementDetailsChecker::checkPrimitiveType( std::vector & in_vec, const std::string & expected_type, const std::string & message, lanelet::validation::Issues & issues) { @@ -47,7 +46,7 @@ void UnconnectedRelationsChecker::checkPrimitiveType( } template -void UnconnectedRelationsChecker::checkPrimitiveType( +void RegulatoryElementDetailsChecker::checkPrimitiveType( std::vector & in_vec, const std::string & expected_type, const std::string & expected_subtype, const std::string & message, lanelet::validation::Issues & issues) { @@ -73,110 +72,7 @@ void UnconnectedRelationsChecker::checkPrimitiveType( } } -lanelet::validation::Issues UnconnectedRelationsChecker::checkRegElemExistsInAllTrafficLight( - 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 and - 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 UnconnectedRelationsChecker::checkRegElemExistsInAllCrosswalk( - 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::LineString, cw_id, - "Crosswalk must have a regulatory element."); - } - } - - return issues; -} - -lanelet::validation::Issues UnconnectedRelationsChecker::checkRegElemOfTrafficLight( +lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElementsOfTrafficLight( const lanelet::LaneletMap & map) { lanelet::validation::Issues issues; @@ -224,7 +120,7 @@ lanelet::validation::Issues UnconnectedRelationsChecker::checkRegElemOfTrafficLi return issues; } -lanelet::validation::Issues UnconnectedRelationsChecker::checkRegElemOfCrosswalk( +lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElementsOfCrosswalk( const lanelet::LaneletMap & map) { lanelet::validation::Issues issues; From e5d01f75df06e06b84eb6d5df6bbee552139b1b8 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 17:57:03 +0900 Subject: [PATCH 04/34] Added test for missing regulatory elements checker Signed-off-by: Shigeki Kobayashi --- .../missing_regulatory_elements.cpp | 2 +- .../test_missing_regulatory_elements.cpp | 181 ++++++++++++++++++ 2 files changed, 182 insertions(+), 1 deletion(-) create mode 100644 tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_missing_regulatory_elements.cpp diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp index 735a3f3b..d2cd11a3 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp @@ -118,7 +118,7 @@ MissingRegulatoryElementsChecker::checkMissingReglatoryElementsInCrosswalk( 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::LineString, cw_id, + lanelet::validation::Severity::Error, lanelet::validation::Primitive::Lanelet, cw_id, "Crosswalk must have a regulatory element."); } } 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..9fde67ff --- /dev/null +++ b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_missing_regulatory_elements.cpp @@ -0,0 +1,181 @@ +// TODO: Add license header +// TODO: remove unused includes +#include + +#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()); + unsigned int 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_.operator()(*test_map_ptr); + + unsigned int 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_.operator()(*test_map_ptr); + + unsigned int 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); + } +} From 1f0d92878e161739a6ee401d196bf4065605264a Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 17:57:56 +0900 Subject: [PATCH 05/34] Added test of missing regulatory elements checker to cmake Signed-off-by: Shigeki Kobayashi --- tmp/lanelet2_extension/CMakeLists.txt | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index ac4ca495..05f18315 100644 --- a/tmp/lanelet2_extension/CMakeLists.txt +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -114,13 +114,9 @@ if(BUILD_TESTING) ${VALIDATION_NAME}_test autoware_lanelet2_validation_lib ) - target_include_directories(${VALIDATION_NAME}_test - SYSTEM PRIVATE - ${lanelet2_core_INCLUDE_DIRECTORIES} - ) endfunction() - # add_validation_test(unconnected_relations) + add_validation_test(missing_regulatory_elements) endif() ament_auto_package() From aa0d33f8111d3a4b94ff7e4e1b4d50906669b590 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 18:06:15 +0900 Subject: [PATCH 06/34] Added license and copyright notice Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/cli.hpp | 14 ++++++++++++++ .../autoware_lanelet2_validation/utils.hpp | 14 ++++++++++++++ .../autoware_lanelet2_validation/validation.hpp | 14 ++++++++++++++ .../src/autoware_lanelet2_validation/lib/cli.cpp | 14 ++++++++++++++ .../autoware_lanelet2_validation/lib/utils.cpp | 14 ++++++++++++++ .../lib/validation.cpp | 14 ++++++++++++++ .../validators/missing_regulatory_elements.cpp | 14 ++++++++++++++ .../src/autoware_lanelet2_validation/main.cpp | 14 ++++++++++++++ .../test_missing_regulatory_elements.cpp | 16 ++++++++++++++-- 9 files changed, 126 insertions(+), 2 deletions(-) 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 index b7350476..fe196c24 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp @@ -1,3 +1,17 @@ +// 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 #include 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 index 509c8245..5f68ae02 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp @@ -1,3 +1,17 @@ +// 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 #include 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 index 97e34990..393232a8 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp @@ -1,3 +1,17 @@ +// 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" #include diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp index ed72eeb6..561e2027 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp @@ -1,3 +1,17 @@ +// 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; diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp index e0b0559b..7c460723 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp @@ -1,3 +1,17 @@ +// 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 diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp index 32e51420..1a4485fa 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp @@ -1,3 +1,17 @@ +// 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 diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp index d2cd11a3..d01d5971 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp @@ -1,3 +1,17 @@ +// 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 diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp index bd621a39..3cf461a0 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp @@ -1,3 +1,17 @@ +// 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" 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 index 9fde67ff..34f5fb23 100644 --- 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 @@ -1,5 +1,17 @@ -// TODO: Add license header -// TODO: remove unused includes +// 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 #include From da970b024c31dae0023f1ed63957330cee8ab2ad Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 18:53:16 +0900 Subject: [PATCH 07/34] Refactored Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/validation.hpp | 7 +++---- .../validators/missing_regulatory_elements.hpp | 5 +++-- .../validators/regulatory_element_details.hpp | 5 +++-- .../lib/validators/missing_regulatory_elements.cpp | 2 +- .../lib/validators/regulatory_element_details.cpp | 2 +- .../src/autoware_lanelet2_validation/main.cpp | 5 ++--- .../test_missing_regulatory_elements.cpp | 8 ++++---- 7 files changed, 17 insertions(+), 17 deletions(-) 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 index 393232a8..ac1b3e2f 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp @@ -13,10 +13,9 @@ // limitations under the License. #include "lanelet2_extension/autoware_lanelet2_validation/cli.hpp" - -#include -#include -#include +#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 diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp index da3161c9..fabdcae6 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include "lanelet2_extension/autoware_lanelet2_validation/utils.hpp" +#include "lanelet2_extension/regulatory_elements/crosswalk.hpp" + #include #include #include diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp index f98c7db0..bdb9a819 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp @@ -1,5 +1,6 @@ -#include -#include +#include "lanelet2_extension/autoware_lanelet2_validation/utils.hpp" +#include "lanelet2_extension/regulatory_elements/crosswalk.hpp" + #include #include diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp index d01d5971..60a40a81 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp" namespace lanelet { diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp index 06485e7e..fa0799be 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp @@ -1,4 +1,4 @@ -#include +#include "lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp" namespace lanelet { diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp index 3cf461a0..2f9563b5 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp @@ -21,13 +21,11 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("autoware_lanelet2_validation"); - // lanelet::validation::RegisterMapValidator(); - // lanelet::RegisterRegulatoryElement(); auto config = lanelet::autoware::validation::parseCommandLine( argc, const_cast(argv)); // NOLINT + auto command_line_config = config.command_line_config; - // return runFromConfig(config); if (command_line_config.help) { return 0; } @@ -49,6 +47,7 @@ int main(int argc, char * argv[]) std::cout << "No map file specified" << std::endl; return 1; } + auto issues = lanelet::autoware::validation::validateMap(config); lanelet::validation::printAllIssues(issues); return int(!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 index 34f5fb23..5491139f 100644 --- 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 @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp" #include @@ -138,7 +138,7 @@ TEST_F(TestSuite, ValidatorAvailability) // NOLINT for gtest { lanelet::validation::Strings validators = lanelet::validation::availabeChecks( lanelet::validation::MissingRegulatoryElementsChecker::name()); - unsigned int expected_num_validators = 1; + 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); @@ -156,7 +156,7 @@ TEST_F(TestSuite, MissingRegulatoryElementOfTrafficLight) // NOLINT for gtest const auto & issues = checker_.operator()(*test_map_ptr); - unsigned int expected_num_issues = 1; + 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) { @@ -181,7 +181,7 @@ TEST_F(TestSuite, MissingRegulatoryElementOfCrosswalk) // NOLINT for gtest const auto & issues = checker_.operator()(*test_map_ptr); - unsigned int expected_num_issues = 1; + 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) { From bea8df7a73ce21787fe168c59eac9c22fede5836 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 31 Jan 2024 18:55:10 +0900 Subject: [PATCH 08/34] Removed unnecessary files Signed-off-by: Shigeki Kobayashi --- .../validators/regulatory_element_details.hpp | 43 --- .../validators/regulatory_element_details.cpp | 190 ---------- .../test_unconnected_relations.cpp | 329 ------------------ 3 files changed, 562 deletions(-) delete mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp delete mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp delete mode 100644 tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp deleted file mode 100644 index bdb9a819..00000000 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/regulatory_element_details.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#include "lanelet2_extension/autoware_lanelet2_validation/utils.hpp" -#include "lanelet2_extension/regulatory_elements/crosswalk.hpp" - -#include - -#include -#include - -#include - -namespace lanelet -{ -namespace validation -{ - -// TODO: update description -//! This check looks for points within linestrings or polygons that appear two times in succession. -//! These are not allowed because they often confuse geometry algorithms. -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: - lanelet::validation::Issues checkRegulatoryElementsOfTrafficLight( - const lanelet::LaneletMap & map); - lanelet::validation::Issues checkRegulatoryElementsOfCrosswalk(const lanelet::LaneletMap & map); - template - void checkPrimitiveType( - std::vector & in_vec, const std::string & expected_type, const std::string & message, - lanelet::validation::Issues & issues); - template - void checkPrimitiveType( - std::vector & in_vec, const std::string & expected_type, - const std::string & expected_subtype, const std::string & message, - lanelet::validation::Issues & issues); - - std::set tl_elem_with_cw_; -}; -} // namespace validation -} // namespace lanelet diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp deleted file mode 100644 index fa0799be..00000000 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/regulatory_element_details.cpp +++ /dev/null @@ -1,190 +0,0 @@ -#include "lanelet2_extension/autoware_lanelet2_validation/validators/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 - appendIssues(issues, checkRegulatoryElementsOfTrafficLight(map)); - appendIssues(issues, checkRegulatoryElementsOfCrosswalk(map)); - return issues; -} - -template -void RegulatoryElementDetailsChecker::checkPrimitiveType( - std::vector & in_vec, const std::string & expected_type, const std::string & message, - 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) { - // Report warning if crosswalk polygon does not have type of crosswalk_polygon - issues.emplace_back( - lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, - item.id(), message); - const auto new_it = in_vec.erase(iter); - if (new_it != in_vec.end()) { - iter = new_it; - } else { - break; - } - } - } -} - -template -void RegulatoryElementDetailsChecker::checkPrimitiveType( - std::vector & in_vec, const std::string & expected_type, const std::string & expected_subtype, - const std::string & message, 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) { - // Report warning if crosswalk polygon does not have type of crosswalk_polygon - issues.emplace_back( - lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, - item.id(), message); - const auto new_it = in_vec.erase(iter); - if (new_it != in_vec.end()) { - iter = new_it; - } else { - break; - } - } - } -} - -lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElementsOfTrafficLight( - 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); - - checkPrimitiveType( - refers, lanelet::AttributeValueString::TrafficLight, - "Refers of traffic light regulatory element must have type of traffic_light", issues); - checkPrimitiveType( - ref_lines, lanelet::AttributeValueString::StopLine, - "Refline of traffic light regulatory element must have type of stop_line", 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: 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 crosswalk - if (ref_lines.empty() && tl_elem_with_cw_.find(elem->id()) == tl_elem_with_cw_.end()) { - 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::checkRegulatoryElementsOfCrosswalk( - const lanelet::LaneletMap & map) -{ - lanelet::validation::Issues issues; - // filter elem whose Subtype is crosswalk and has crosswalk polygon - 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); - - checkPrimitiveType( - refers, lanelet::AttributeValueString::Lanelet, lanelet::AttributeValueString::Crosswalk, - "Refers of crosswalk regulatory element must have type of crosswalk", issues); - checkPrimitiveType( - ref_lines, lanelet::AttributeValueString::StopLine, - "Refline of crosswalk regulatory element must have type of stopline", issues); - checkPrimitiveType( - crosswalk_polygons, lanelet::autoware::Crosswalk::AutowareRoleNameString::CrosswalkPolygon, - "Crosswalk polygon of crosswalk regulatory element must have type of Crosswalk_polygon", - 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."); - } - // Report warning if regulatory element has two or more crosswalk polygon - else if (crosswalk_polygons.size() != 1) { - issues.emplace_back( - lanelet::validation::Severity::Warning, lanelet::validation::Primitive::RegulatoryElement, - elem->id(), "Regulatory element of cross walk must have only one crosswalk_polygon."); - } - // Report warning if regulatory element does not have stop line - if (ref_lines.empty()) { - issues.emplace_back( - lanelet::validation::Severity::Warning, lanelet::validation::Primitive::RegulatoryElement, - elem->id(), "Regulatory element of cross walk is nice to 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)."); - } - // Report error if regulatory element has two or more lanelet of crosswalk - else if (refers.size() != 1) { - 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/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp deleted file mode 100644 index b9e36558..00000000 --- a/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_unconnected_relations.cpp +++ /dev/null @@ -1,329 +0,0 @@ -// TODO: Add license header -// TODO: remove unused includes -#include "lanelet2_extension/autoware_lanelet2_validation/unconnected_relations.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() - { - // TODO: remove - lanelet::validation::RegisterMapValidator(); - - initializeAttributes(); - config.checksFilter = "mapping.unconnected_relations"; - validators = lanelet::validation::availabeChecks(config.checksFilter); - } - - ~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::ValidationConfig config; - lanelet::validation::Strings validators; - -private: -}; - -// TEST_F(TestSuite, SaveMap) // NOLINT for gtest -// { -// // Save map -// lanelet::write("/tmp/test_map.osm", *test_map_ptr); -// } - -TEST_F(TestSuite, AvailableValidator) // NOLINT for gtest -{ - unsigned int expected_num_validators = 1; - EXPECT_EQ(expected_num_validators, validators.size()); - for (const auto & v : validators) { - EXPECT_EQ("mapping.unconnected_relations", 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); - - auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); - for (const auto & detect_issue : detect_issues) { - for (const auto & issue : detect_issue.issues) { - std::cout << "message = " << issue.message << std::endl; - EXPECT_EQ(99999, issue.id); - 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); - - auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); - for (const auto & detect_issue : detect_issues) { - for (const auto & issue : detect_issue.issues) { - std::cout << "message = " << issue.message << std::endl; - EXPECT_EQ(99999, issue.id); - // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); - // EXPECT_EQ(lanelet::validation::Primitive::Lanelet, issue.primitive); - } - } -} - -TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutTrafficLight) // NOLINT for gtest -{ - // Check regulatory element of traffic light without stop line - - RegulatoryElementPtr tl_reg_elem_no_tl; - const auto & tl = LineString3d(99998, {}); - LaneletMapPtr test_map_ptr = lanelet::utils::createMap({tl}); - tl_reg_elem_no_tl = TrafficLight::make( - 99999, tl_re_attr, {tl}, - {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); - - auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); - for (const auto & detect_issue : detect_issues) { - for (const auto & issue : detect_issue.issues) { - std::cout << "message = " << issue.message << std::endl; - EXPECT_TRUE(issue.id == 99999 || issue.id == 99998); - // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); - // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); - } - } -} - -TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutStopLine) // NOLINT for gtest -{ - // Check regulatory element of traffic light without stop line - - RegulatoryElementPtr tl_reg_elem_no_sl; - 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}); - tl_reg_elem_no_sl = TrafficLight::make(99999, tl_re_attr, {tl}, {}); - test_map_ptr->add(tl_reg_elem_no_sl); - addTestMap(test_map_ptr); - - auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); - for (const auto & detect_issue : detect_issues) { - for (const auto & issue : detect_issue.issues) { - std::cout << "message = " << issue.message << std::endl; - EXPECT_EQ(99999, issue.id); - // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); - // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); - } - } -} - -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); - - 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); - - auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); - for (const auto & detect_issue : detect_issues) { - for (const auto & issue : detect_issue.issues) { - std::cout << "message = " << issue.message << std::endl; - EXPECT_TRUE(issue.id == 99999 || issue.id == 99998); - // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); - // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); - } - } -} - -TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutStopline) // NOLINT for gtest -{ - // Check regulatory element of crosswalk without stop line - - RegulatoryElementPtr cw_reg_elem_no_sl; - 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); - - 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); - - auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); - for (const auto & detect_issue : detect_issues) { - for (const auto & issue : detect_issue.issues) { - std::cout << "message = " << issue.message << std::endl; - EXPECT_EQ(99999, issue.id); - // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); - // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); - } - } -} - -TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutCrosswalk) // NOLINT for gtest -{ - // Check regulatory element of crosswalk without crosswalk - - const auto poly_no_cw = Polygon3d(getId(), {Point3d(getId(), 3.0, 3.0, 0.1)}, cw_poly_attr); - 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)})); - - RegulatoryElementPtr reg_elem = Crosswalk::make( - 99999, cw_re_attr, ll, poly_no_cw, - {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_no_cw}); - addTestMap(test_map_ptr); - test_map_ptr->add(reg_elem); - - // TODO: remove - // lanelet::write("/tmp/test_map.osm", *test_map_ptr); - - auto detect_issues = lanelet::validation::validateMap(*test_map_ptr, config); - for (const auto & detect_issue : detect_issues) { - for (const auto & issue : detect_issue.issues) { - std::cout << "message = " << issue.message << std::endl; - EXPECT_TRUE(issue.id == 99999 || issue.id == 99998); - // EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); - // EXPECT_EQ(lanelet::validation::Primitive::LineString, issue.primitive); - } - } -} From c047c3d9d69ed00dd8037faf86e4ebc8cbf4afd6 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 13:40:18 +0900 Subject: [PATCH 09/34] Change default projector to MGRS projector Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/validation.hpp | 2 +- .../src/autoware_lanelet2_validation/lib/cli.cpp | 6 ++++++ .../autoware_lanelet2_validation/lib/validation.cpp | 12 +++++------- 3 files changed, 12 insertions(+), 8 deletions(-) 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 index ac1b3e2f..b48b8f6a 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp @@ -28,8 +28,8 @@ namespace { namespace projector_names { -constexpr const char * transverse_mercator = "tm"; constexpr const char * mgrs = "mgrs"; +constexpr const char * transverse_mercator = "transverse_mercator"; constexpr const char * utm = "utm"; constexpr const char * local = "local"; } // namespace projector_names diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp index 561e2027..065e5544 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp @@ -71,6 +71,12 @@ MetaConfig parseCommandLine(int argc, const char * argv[]) 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")) { + std::cerr << "lat and lon were not set, but are required for the " << config.projector_type + << " projector\n"; + } if (config.command_line_config.help) { std::cout << '\n' << desc; } else if (config.command_line_config.mapFile.empty() && !config.command_line_config.print) { diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp index 1a4485fa..eb875810 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp @@ -24,18 +24,16 @@ namespace validation std::unique_ptr getProjector(const MetaConfig & config) { const auto & val_config = config.command_line_config.validationConfig; - if (config.projector_type == projector_names::transverse_mercator) { + 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::mgrs) { - return std::make_unique(); } else if (config.projector_type == projector_names::utm) { return std::make_unique(lanelet::Origin{val_config.origin}); } else { - std::cerr << "Unknown projector type: " << config.projector_type << "'\n"; - std::cerr << "Set to default projector: Transverse Mercator projector" << std::endl; - return std::make_unique( - lanelet::Origin{val_config.origin}); + std::cerr << "Set to default projector: MGRS projector" << std::endl; + return std::make_unique(); } } From 9d8d286519ecfd5d51318513737d9b888a123868 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 14:03:36 +0900 Subject: [PATCH 10/34] Updated help messages in cli Signed-off-by: Shigeki Kobayashi --- .../src/autoware_lanelet2_validation/lib/cli.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp index 065e5544..1b96efad 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp @@ -40,7 +40,8 @@ MetaConfig parseCommandLine(int argc, const char * argv[]) "routing_graph.* to run all checks for the routing graph") ("projector,p", po::value(&config.projector_type)->composing(), - "Participants for which the routing graph will be instanciated (default: vehicle)") + "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), @@ -52,14 +53,16 @@ MetaConfig parseCommandLine(int argc, const char * argv[]) ("lat", po::value(&validation_config.origin.lat) ->default_value(validation_config.origin.lat), - "latitude coordinate of map origin") + "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") + "longitude coofdinate of map origin. This is reguired for the transverse mercator " + "and utm projector.") - ("print", "Only print the checks that will be run, but dont run them"); + ("print", "Only print all avalible checker, but dont run them"); po::variables_map vm; po::positional_options_description pos; pos.add("map_file", 1); @@ -74,8 +77,9 @@ MetaConfig parseCommandLine(int argc, const char * argv[]) if ( (vm.count("lat") != 0 && vm.count("lon") != 0) && (config.projector_type == "tm" || config.projector_type == "utm")) { - std::cerr << "lat and lon were not set, but are required for the " << config.projector_type - << " projector\n"; + throw std::runtime_error( + "latitude and longitude were not set, but these are required for " + config.projector_type + + " projector. Please refer to the help message."); } if (config.command_line_config.help) { std::cout << '\n' << desc; From 9646c3488578440e923e4a48580d48f1eddab264 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 14:03:53 +0900 Subject: [PATCH 11/34] Removed local from projector option Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/validation.hpp | 1 - 1 file changed, 1 deletion(-) 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 index b48b8f6a..628581e7 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp @@ -31,7 +31,6 @@ namespace projector_names constexpr const char * mgrs = "mgrs"; constexpr const char * transverse_mercator = "transverse_mercator"; constexpr const char * utm = "utm"; -constexpr const char * local = "local"; } // namespace projector_names } // namespace From 2dfe7e38a1fa5d6153ce9417d701001c82f1346a Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 15:13:14 +0900 Subject: [PATCH 12/34] Updated help message to be more readable Signed-off-by: Shigeki Kobayashi --- .../src/autoware_lanelet2_validation/lib/cli.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp index 1b96efad..ad1d2ae4 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp @@ -32,10 +32,11 @@ MetaConfig parseCommandLine(int argc, const char * argv[]) "available:"); desc.add_options()("help,h", "this help message") - ("map_file", po::value(), "Path to the map to be validated") + ("map_file,m", po::value(), "Path to the map to be validated") - ("filter,f", po::value(&validation_config.checksFilter), - "Comma separated list of regexes to filter the applicable tests. Will run all tests by " + ("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") From 3c7bf709b788e8d58ec123d4ddadc8768720df45 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 15:13:45 +0900 Subject: [PATCH 13/34] Added document for autoware_lanelet2_validation Signed-off-by: Shigeki Kobayashi --- tmp/lanelet2_extension/README.md | 44 ++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/tmp/lanelet2_extension/README.md b/tmp/lanelet2_extension/README.md index 85e360b7..dde5aa27 100644 --- a/tmp/lanelet2_extension/README.md +++ b/tmp/lanelet2_extension/README.md @@ -91,11 +91,51 @@ 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: +``` +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. From bafae9429e89ff61b9d006ecd6af8bb862794ecb Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 15:14:12 +0900 Subject: [PATCH 14/34] Added simple_lanelet2_validation Signed-off-by: Shigeki Kobayashi --- tmp/lanelet2_extension/CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index 05f18315..d7e6a2fe 100644 --- a/tmp/lanelet2_extension/CMakeLists.txt +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -64,6 +64,14 @@ target_link_libraries(lanelet2_extension_sample lanelet2_extension_lib ) +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 +) + ament_auto_add_executable(check_right_of_way src/check_right_of_way.cpp) add_dependencies(check_right_of_way lanelet2_extension_lib) target_link_libraries(check_right_of_way From 6a284375a25497a8b8f8bffd773d45443a056a67 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 16:28:02 +0900 Subject: [PATCH 15/34] Updated help description to warn if lat or lon is not set when projector is utm or transverse mercator Signed-off-by: Shigeki Kobayashi --- .../src/autoware_lanelet2_validation/lib/cli.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp index ad1d2ae4..6bc6f899 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/cli.cpp @@ -79,7 +79,7 @@ MetaConfig parseCommandLine(int argc, const char * argv[]) (vm.count("lat") != 0 && vm.count("lon") != 0) && (config.projector_type == "tm" || config.projector_type == "utm")) { throw std::runtime_error( - "latitude and longitude were not set, but these are required for " + config.projector_type + + "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) { From 54da3d663f324a1370fc83ef99a2745fca18a5ba Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 16:29:20 +0900 Subject: [PATCH 16/34] Used move_iterator Signed-off-by: Shigeki Kobayashi --- .../src/autoware_lanelet2_validation/lib/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp index 7c460723..3b183143 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp @@ -27,7 +27,7 @@ void appendIssues( } void appendIssues(lanelet::validation::Issues & to, lanelet::validation::Issues && from) { - to.insert(to.end(), from.begin(), from.end()); + to.insert(to.end(), std::make_move_iterator(from.begin()), std::make_move_iterator(from.end())); } } // namespace validation From 4e72c8dd45820ff6b582239a3f9f7b67833615d0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 1 Feb 2024 07:38:53 +0000 Subject: [PATCH 17/34] style(pre-commit): autofix --- tmp/lanelet2_extension/README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tmp/lanelet2_extension/README.md b/tmp/lanelet2_extension/README.md index dde5aa27..c0888cca 100644 --- a/tmp/lanelet2_extension/README.md +++ b/tmp/lanelet2_extension/README.md @@ -110,11 +110,13 @@ ros2 run lanelet2_extension autoware_lanelet2_validation --map_file Date: Thu, 1 Feb 2024 18:47:04 +0900 Subject: [PATCH 18/34] Made fenced code blocks to have language specified context Signed-off-by: Shigeki Kobayashi --- tmp/lanelet2_extension/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmp/lanelet2_extension/README.md b/tmp/lanelet2_extension/README.md index c0888cca..a5899299 100644 --- a/tmp/lanelet2_extension/README.md +++ b/tmp/lanelet2_extension/README.md @@ -117,7 +117,7 @@ ros2 run lanelet2_extension autoware_lanelet2_validation --map_file ~/autoware_m 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] From 874b4e2aa916fe4d8363be9ce2e546154377de84 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 18:51:13 +0900 Subject: [PATCH 19/34] Added include guard Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/cli.hpp | 6 ++++++ .../autoware_lanelet2_validation/utils.hpp | 8 ++++++++ .../autoware_lanelet2_validation/validation.hpp | 7 +++++++ 3 files changed, 21 insertions(+) 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 index fe196c24..8b57f992 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/cli.hpp @@ -12,11 +12,15 @@ // 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 { @@ -35,3 +39,5 @@ 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 index 5f68ae02..a038c991 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp @@ -12,9 +12,15 @@ // 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 validation @@ -29,3 +35,5 @@ void appendIssues(lanelet::validation::Issues & to, lanelet::validation::Issues } // namespace validation } // 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 index 628581e7..9e7ef8f5 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validation.hpp @@ -12,6 +12,9 @@ // 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" @@ -22,7 +25,9 @@ #include #include +#include #include +#include namespace { @@ -45,3 +50,5 @@ std::vector validateMap(const MetaConfig & } // namespace validation } // namespace autoware } // namespace lanelet + +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALIDATION_HPP_ From ac974dbdb57a321693f86ab267c6cab45fae69fe Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 18:53:27 +0900 Subject: [PATCH 20/34] Rename validators directory to vals because include guard name too long Signed-off-by: Shigeki Kobayashi --- .../{validators => vals}/missing_regulatory_elements.hpp | 5 +++++ .../lib/{validators => vals}/missing_regulatory_elements.cpp | 4 ++-- .../test_missing_regulatory_elements.cpp | 2 +- 3 files changed, 8 insertions(+), 3 deletions(-) rename tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/{validators => vals}/missing_regulatory_elements.hpp (83%) rename tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/{validators => vals}/missing_regulatory_elements.cpp (97%) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp similarity index 83% rename from tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp rename to tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp index fabdcae6..eee70eb5 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp @@ -12,6 +12,9 @@ // 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" @@ -45,3 +48,5 @@ class MissingRegulatoryElementsChecker : public lanelet::validation::MapValidato }; } // namespace validation } // namespace lanelet + +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALIDATORS__MISSING_REGULATORY_ELEMENTS_HPP_ diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/missing_regulatory_elements.cpp similarity index 97% rename from tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp rename to tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/missing_regulatory_elements.cpp index 60a40a81..f4aefd73 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validators/missing_regulatory_elements.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/missing_regulatory_elements.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp" +#include "lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp" namespace lanelet { @@ -56,7 +56,7 @@ MissingRegulatoryElementsChecker::checkMissingReglatoryElementsInTrafficLight( const auto & it = attrs.find(lanelet::AttributeName::Subtype); const auto & params = elem->getParameters(); return it != attrs.end() && - it->second == lanelet::AttributeValueString::TrafficLight and + it->second == lanelet::AttributeValueString::TrafficLight && params.find(lanelet::RoleNameString::Refers) != params.end(); }); // Get all line strings of traffic light referred by regulatory elements 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 index 5491139f..cfc06de8 100644 --- 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 @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lanelet2_extension/autoware_lanelet2_validation/validators/missing_regulatory_elements.hpp" +#include "lanelet2_extension/autoware_lanelet2_validation/vals/missing_regulatory_elements.hpp" #include From a34c8d3f714f8359014b6c82260db5c5fa7197f2 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 18:55:04 +0900 Subject: [PATCH 21/34] Removed unnecessary namespace Signed-off-by: Shigeki Kobayashi --- .../src/autoware_lanelet2_validation/lib/validation.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp index eb875810..9a1766c2 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/validation.cpp @@ -42,7 +42,6 @@ std::vector validateMap(const MetaConfig & const auto & cm_config = config.command_line_config; const auto & val_config = config.command_line_config.validationConfig; - using namespace std::string_literals; const auto & parse_filter = [](const std::string & str) { std::vector regexes; std::stringstream ss(str); From c59775270a3cef8ea3e4d8c786c033685a6a5f5e Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 18:55:28 +0900 Subject: [PATCH 22/34] Add static cast Signed-off-by: Shigeki Kobayashi --- .../src/autoware_lanelet2_validation/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp index 2f9563b5..9f80c8a2 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/main.cpp @@ -50,5 +50,5 @@ int main(int argc, char * argv[]) auto issues = lanelet::autoware::validation::validateMap(config); lanelet::validation::printAllIssues(issues); - return int(!issues.empty()); + return static_cast(!issues.empty()); } From ecc58a8ab7280fded929f8863f378570779a112c Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 18:56:32 +0900 Subject: [PATCH 23/34] Fixed include guard name Signed-off-by: Shigeki Kobayashi --- .../vals/missing_regulatory_elements.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index eee70eb5..aac7c0fa 100644 --- 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 @@ -49,4 +49,4 @@ class MissingRegulatoryElementsChecker : public lanelet::validation::MapValidato } // namespace validation } // namespace lanelet -#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALIDATORS__MISSING_REGULATORY_ELEMENTS_HPP_ +#endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__MISSING_REGULATORY_ELEMENTS_HPP_ From c2ac49d50a26c6166d9fad44b5ce17f52bdf8fba Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Thu, 1 Feb 2024 19:18:38 +0900 Subject: [PATCH 24/34] Removed explicitly operetor call Signed-off-by: Shigeki Kobayashi --- .../test_missing_regulatory_elements.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index cfc06de8..1f9bed25 100644 --- 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 @@ -154,7 +154,7 @@ TEST_F(TestSuite, MissingRegulatoryElementOfTrafficLight) // NOLINT for gtest LaneletMapPtr test_map_ptr = lanelet::utils::createMap({tl_no_reg_elem}); addTestMap(test_map_ptr); - const auto & issues = checker_.operator()(*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."; @@ -179,7 +179,7 @@ TEST_F(TestSuite, MissingRegulatoryElementOfCrosswalk) // NOLINT for gtest LaneletMapPtr test_map_ptr = lanelet::utils::createMap({cw_no_reg_elem}); addTestMap(test_map_ptr); - const auto & issues = checker_.operator()(*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."; From 4a1b85cbe8261eb7b59122a56b09444ea1c4674c Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Mon, 5 Feb 2024 11:31:11 +0900 Subject: [PATCH 25/34] Added missed namespace Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/utils.hpp | 3 +++ .../src/autoware_lanelet2_validation/lib/utils.cpp | 3 +++ .../lib/vals/missing_regulatory_elements.cpp | 6 ++++-- 3 files changed, 10 insertions(+), 2 deletions(-) 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 index a038c991..959d8fc6 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp @@ -23,6 +23,8 @@ namespace lanelet { +namespace autoware +{ namespace validation { std::vector parseFilterString(const std::string & str); @@ -34,6 +36,7 @@ void appendIssues( void appendIssues(lanelet::validation::Issues & to, lanelet::validation::Issues && from); } // namespace validation +} // namespace autoware } // namespace lanelet #endif // LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__UTILS_HPP_ diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp index 3b183143..69016a49 100644 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp @@ -16,6 +16,8 @@ namespace lanelet { +namespace autoware +{ namespace validation { @@ -31,4 +33,5 @@ void appendIssues(lanelet::validation::Issues & to, lanelet::validation::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 index f4aefd73..fdcf5037 100644 --- 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 @@ -30,8 +30,10 @@ lanelet::validation::Issues MissingRegulatoryElementsChecker::operator()( lanelet::validation::Issues issues; // Append issues found by each validator - appendIssues(issues, checkMissingReglatoryElementsInTrafficLight(map)); - appendIssues(issues, checkMissingReglatoryElementsInCrosswalk(map)); + lanelet::autoware::validation::appendIssues( + issues, checkMissingReglatoryElementsInTrafficLight(map)); + lanelet::autoware::validation::appendIssues( + issues, checkMissingReglatoryElementsInCrosswalk(map)); return issues; } From 160a9426dd693b13323c5aa982154b8f066883bf Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Mon, 5 Feb 2024 16:42:09 +0900 Subject: [PATCH 26/34] Added template for append_issue Signed-off-by: Shigeki Kobayashi --- .../autoware_lanelet2_validation/utils.hpp | 12 +++--- .../lib/utils.cpp | 37 ------------------- 2 files changed, 5 insertions(+), 44 deletions(-) delete mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp 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 index 959d8fc6..4d1a4abd 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp @@ -27,13 +27,11 @@ namespace autoware { namespace validation { -std::vector parseFilterString(const std::string & str); - -void appendIssues( - std::vector & to, - std::vector && from); - -void appendIssues(lanelet::validation::Issues & to, lanelet::validation::Issues && from); +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())); +} } // namespace validation } // namespace autoware diff --git a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp deleted file mode 100644 index 69016a49..00000000 --- a/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/utils.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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 -{ - -void appendIssues( - std::vector & to, - std::vector && from) -{ - to.insert(to.end(), std::make_move_iterator(from.begin()), std::make_move_iterator(from.end())); -} -void appendIssues(lanelet::validation::Issues & to, lanelet::validation::Issues && from) -{ - to.insert(to.end(), std::make_move_iterator(from.begin()), std::make_move_iterator(from.end())); -} - -} // namespace validation -} // namespace autoware -} // namespace lanelet From b4ca2fd1993052478fa2b05d3a7d99fba21bc387 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Mon, 5 Feb 2024 18:24:34 +0900 Subject: [PATCH 27/34] Check missing regulatory elements in stop lines Signed-off-by: Shigeki Kobayashi --- .../vals/missing_regulatory_elements.hpp | 2 + .../lib/vals/missing_regulatory_elements.cpp | 48 +++++++++++++++++++ .../test_missing_regulatory_elements.cpp | 22 +++++++++ 3 files changed, 72 insertions(+) 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 index aac7c0fa..2419d4a9 100644 --- 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 @@ -44,6 +44,8 @@ class MissingRegulatoryElementsChecker : public lanelet::validation::MapValidato 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 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 index fdcf5037..40a5f55f 100644 --- 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 @@ -34,6 +34,7 @@ lanelet::validation::Issues MissingRegulatoryElementsChecker::operator()( issues, checkMissingReglatoryElementsInTrafficLight(map)); lanelet::autoware::validation::appendIssues( issues, checkMissingReglatoryElementsInCrosswalk(map)); + lanelet::autoware::validation::appendIssues(issues, checkMissingReglatoryElementsInStopLine(map)); return issues; } @@ -142,5 +143,52 @@ MissingRegulatoryElementsChecker::checkMissingReglatoryElementsInCrosswalk( 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/test/src/autoware_lanelet2_validation/test_missing_regulatory_elements.cpp b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_missing_regulatory_elements.cpp index 1f9bed25..0c326a27 100644 --- 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 @@ -191,3 +191,25 @@ TEST_F(TestSuite, MissingRegulatoryElementOfCrosswalk) // NOLINT for gtest 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); + } +} From 13bd2f2f3b0563ef43910dd86190604e1c23a4e2 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Mon, 5 Feb 2024 18:34:32 +0900 Subject: [PATCH 28/34] Added regulatory element details checker Signed-off-by: Shigeki Kobayashi --- tmp/lanelet2_extension/CMakeLists.txt | 1 + .../autoware_lanelet2_validation/utils.hpp | 45 +++ .../vals/regulatory_element_details.hpp | 38 +++ .../lib/vals/regulatory_element_details.cpp | 158 +++++++++ .../test_regulatory_element_details.cpp | 310 ++++++++++++++++++ 5 files changed, 552 insertions(+) create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/regulatory_element_details.hpp create mode 100644 tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/regulatory_element_details.cpp create mode 100644 tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_regulatory_element_details.cpp diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index d7e6a2fe..a773d2b8 100644 --- a/tmp/lanelet2_extension/CMakeLists.txt +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -125,6 +125,7 @@ if(BUILD_TESTING) endfunction() add_validation_test(missing_regulatory_elements) + add_validation_test(regulatory_element_details) endif() ament_auto_package() 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 index 4d1a4abd..af705d51 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/utils.hpp @@ -33,6 +33,51 @@ 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 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..96985356 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/autoware_lanelet2_validation/vals/regulatory_element_details.hpp @@ -0,0 +1,38 @@ +#include "lanelet2_extension/autoware_lanelet2_validation/utils.hpp" +#include "lanelet2_extension/regulatory_elements/crosswalk.hpp" + +#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_ + +// TODO: update description +//! This check looks for points within linestrings or polygons that appear two times in succession. +//! These are not allowed because they often confuse geometry algorithms. +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: + lanelet::validation::Issues checkRegulatoryElementOfTrafficLight(const lanelet::LaneletMap & map); + lanelet::validation::Issues checkRegulatoryElementOfCrosswalk(const lanelet::LaneletMap & map); + + std::set tl_elem_with_cw_; +}; +} // 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/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..7847e167 --- /dev/null +++ b/tmp/lanelet2_extension/src/autoware_lanelet2_validation/lib/vals/regulatory_element_details.cpp @@ -0,0 +1,158 @@ +#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; +} + +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_tl, 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: 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 crosswalk + if (ref_lines.empty() && tl_elem_with_cw_.find(elem->id()) == tl_elem_with_cw_.end()) { + 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 and has crosswalk polygon + 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."); + } + // Report warning if regulatory element has two or more crosswalk polygon + else if (crosswalk_polygons.size() > 1) { + 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)."); + } + // Report error if regulatory element has two or more lanelet of crosswalk + else if (refers.size() > 1) { + 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/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..de8d99b9 --- /dev/null +++ b/tmp/lanelet2_extension/test/src/autoware_lanelet2_validation/test_regulatory_element_details.cpp @@ -0,0 +1,310 @@ +#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; + const auto & tl = LineString3d(99998, {}); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({tl}); + tl_reg_elem_no_tl = TrafficLight::make( + 99999, tl_re_attr, {tl}, + {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; + 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}); + tl_reg_elem_no_sl = TrafficLight::make(99999, tl_re_attr, {tl}, {}); + 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 = 1; + static constexpr const char * expected_message = + "Regulatory element of traffic light must have a 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::Error, issue.severity); + EXPECT_EQ(lanelet::validation::Primitive::RegulatoryElement, issue.primitive); + } +} + +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); + + 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 + + RegulatoryElementPtr cw_reg_elem_no_sl; + 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); + + 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_no_cw = Polygon3d(getId(), {Point3d(getId(), 3.0, 3.0, 0.1)}, cw_poly_attr); + 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)})); + + RegulatoryElementPtr reg_elem = Crosswalk::make( + 99999, cw_re_attr, ll, poly_no_cw, + {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_no_cw}); + 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; + } + } +} From 1d0d25fb98252902e92c659baa09447f6a25fa5f Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Mon, 5 Feb 2024 18:39:08 +0900 Subject: [PATCH 29/34] Added copyright and license notice Signed-off-by: Shigeki Kobayashi --- .../vals/regulatory_element_details.hpp | 14 ++++++++++++++ .../lib/vals/regulatory_element_details.cpp | 14 ++++++++++++++ .../test_regulatory_element_details.cpp | 14 ++++++++++++++ 3 files changed, 42 insertions(+) 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 index 96985356..0c009f9a 100644 --- 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 @@ -1,3 +1,17 @@ +// 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/crosswalk.hpp" 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 index 7847e167..a2991a7b 100644 --- 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 @@ -1,3 +1,17 @@ +// 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 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 index de8d99b9..90f9ce57 100644 --- 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 @@ -1,3 +1,17 @@ +// 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 From ae7ec07b29639098663715c917003647612e7b9f Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Mon, 5 Feb 2024 18:39:44 +0900 Subject: [PATCH 30/34] Updated comments Signed-off-by: Shigeki Kobayashi --- .../vals/regulatory_element_details.hpp | 3 --- .../lib/vals/regulatory_element_details.cpp | 12 +++++------- 2 files changed, 5 insertions(+), 10 deletions(-) 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 index 0c009f9a..393a3b42 100644 --- 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 @@ -30,9 +30,6 @@ namespace validation #ifndef LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__REGULATORY_ELEMENT_DETAILS_HPP_ #define LANELET2_EXTENSION__AUTOWARE_LANELET2_VALIDATION__VALS__REGULATORY_ELEMENT_DETAILS_HPP_ -// TODO: update description -//! This check looks for points within linestrings or polygons that appear two times in succession. -//! These are not allowed because they often confuse geometry algorithms. class RegulatoryElementDetailsChecker : public lanelet::validation::MapValidator { public: 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 index a2991a7b..ce85e0b0 100644 --- 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 @@ -72,7 +72,7 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, elem->id(), "Regulatory element of traffic light must have a traffic light(refers)."); } - // TODO: Check correct behavior if regulatory element has two or more traffic light + // 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, @@ -138,9 +138,8 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem issues.emplace_back( lanelet::validation::Severity::Warning, lanelet::validation::Primitive::RegulatoryElement, elem->id(), "Regulatory element of cross walk is nice to have crosswalk_polygon."); - } - // Report warning if regulatory element has two or more crosswalk polygon - else if (crosswalk_polygons.size() > 1) { + } else if (crosswalk_polygons.size() > 1) { // Report warning 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."); @@ -156,9 +155,8 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem issues.emplace_back( lanelet::validation::Severity::Error, lanelet::validation::Primitive::RegulatoryElement, elem->id(), "Regulatory element of cross walk must have lanelet of crosswalk(refers)."); - } - // Report error if regulatory element has two or more lanelet of crosswalk - else if (refers.size() > 1) { + } 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(), From 54bee135e280734f1fb6eac4097f2b4de4f2faee Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Tue, 6 Feb 2024 14:10:35 +0900 Subject: [PATCH 31/34] Fixed bug of the issue which is reported when ref line of traffic light regulatory element does not have type of stop line Signed-off-by: Shigeki Kobayashi --- .../lib/vals/regulatory_element_details.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index ce85e0b0..e3ed3223 100644 --- 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 @@ -65,7 +65,7 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem 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_tl, issues); + ref_lines, lanelet::AttributeValueString::StopLine, issue_sl, issues); if (refers.empty()) { issues.emplace_back( @@ -94,7 +94,7 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem const lanelet::LaneletMap & map) { lanelet::validation::Issues issues; - // filter elem whose Subtype is crosswalk and has crosswalk polygon + // 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); @@ -138,7 +138,7 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem 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 warning if regulatory element has two or + } 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, From 530dc9b640ebb9578f4ecaccc962ee24970a7d13 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Tue, 6 Feb 2024 14:13:29 +0900 Subject: [PATCH 32/34] Added more test case Signed-off-by: Shigeki Kobayashi --- .../test_regulatory_element_details.cpp | 49 +++++++++++++------ 1 file changed, 34 insertions(+), 15 deletions(-) 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 index 90f9ce57..d12067ee 100644 --- 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 @@ -150,10 +150,13 @@ TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutTrafficLight) // NOLINT // Check regulatory element of traffic light without traffic light RegulatoryElementPtr tl_reg_elem_no_tl; - const auto & tl = LineString3d(99998, {}); - LaneletMapPtr test_map_ptr = lanelet::utils::createMap({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, {tl}, + 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); @@ -187,24 +190,37 @@ TEST_F(TestSuite, RegulatoryElementofTrafficLightWithoutStopLine) // NOLINT for // 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}); - tl_reg_elem_no_sl = TrafficLight::make(99999, tl_re_attr, {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 = 1; - static constexpr const char * expected_message = + 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) { - EXPECT_EQ(expected_message, issue.message); - EXPECT_EQ(99999, issue.id); - EXPECT_EQ(lanelet::validation::Severity::Error, issue.severity); - EXPECT_EQ(lanelet::validation::Primitive::RegulatoryElement, issue.primitive); + 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; + } } } @@ -219,6 +235,8 @@ TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutPolygon) // NOLINT for gte 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( @@ -254,13 +272,13 @@ TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutStopline) // NOLINT for gt { // Check regulatory element of crosswalk without stop line - RegulatoryElementPtr cw_reg_elem_no_sl; 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), {}); @@ -286,17 +304,18 @@ TEST_F(TestSuite, RegulatoryElementOfCrosswalkWithoutCrosswalk) // NOLINT for g { // Check regulatory element of crosswalk without crosswalk - const auto poly_no_cw = Polygon3d(getId(), {Point3d(getId(), 3.0, 3.0, 0.1)}, cw_poly_attr); + 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_no_cw, + 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_no_cw}); + LaneletMapPtr test_map_ptr = lanelet::utils::createMap({poly}); addTestMap(test_map_ptr); test_map_ptr->add(reg_elem); From f17a8a9c1df1625a9ee9b765cc4f93909955b889 Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Tue, 6 Feb 2024 14:17:51 +0900 Subject: [PATCH 33/34] Added TODO comment. Check if regulatory element of traffic light must have stop line or crosswalk Signed-off-by: Shigeki Kobayashi --- .../lib/vals/regulatory_element_details.cpp | 1 + 1 file changed, 1 insertion(+) 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 index e3ed3223..93b2a187 100644 --- 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 @@ -80,6 +80,7 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem // traffic light must have only one traffic light(refers)."); // } + // TODO(sgk-000): Check if regulatory element of traffic light must have stop line or crosswalk // Report error if regulatory element does not have stop line and crosswalk if (ref_lines.empty() && tl_elem_with_cw_.find(elem->id()) == tl_elem_with_cw_.end()) { issues.emplace_back( From 5e08fad6cfbd050f988d6b75eb146aaa6264d24a Mon Sep 17 00:00:00 2001 From: Shigeki Kobayashi Date: Wed, 7 Feb 2024 15:47:24 +0900 Subject: [PATCH 34/34] Report error if traffic light regulatory element does not have stop line and this is not a pedestrian traffic light Signed-off-by: Shigeki Kobayashi --- .../vals/regulatory_element_details.hpp | 5 +++-- .../lib/vals/regulatory_element_details.cpp | 20 +++++++++++++++---- 2 files changed, 19 insertions(+), 6 deletions(-) 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 index 393a3b42..ae59ed8a 100644 --- 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 @@ -13,6 +13,7 @@ // 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 @@ -21,6 +22,7 @@ #include #include +#include namespace lanelet { @@ -38,10 +40,9 @@ class RegulatoryElementDetailsChecker : public lanelet::validation::MapValidator 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); - - std::set tl_elem_with_cw_; }; } // 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 index 93b2a187..7f1bba9e 100644 --- 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 @@ -35,6 +35,19 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::operator()( 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) { @@ -52,7 +65,6 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem 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(), @@ -80,9 +92,9 @@ lanelet::validation::Issues RegulatoryElementDetailsChecker::checkRegulatoryElem // traffic light must have only one traffic light(refers)."); // } - // TODO(sgk-000): Check if regulatory element of traffic light must have stop line or crosswalk - // Report error if regulatory element does not have stop line and crosswalk - if (ref_lines.empty() && tl_elem_with_cw_.find(elem->id()) == tl_elem_with_cw_.end()) { + // 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).");