diff --git a/map/autoware_lanelet2_map_validator/src/common/map_loader.cpp b/map/autoware_lanelet2_map_validator/src/common/map_loader.cpp new file mode 100644 index 00000000..418a1e51 --- /dev/null +++ b/map/autoware_lanelet2_map_validator/src/common/map_loader.cpp @@ -0,0 +1,82 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lanelet2_map_validator/map_loader.hpp" + +#include +#include + +namespace lanelet::autoware::validation +{ + +namespace +{ +namespace projector_names +{ +constexpr const char * mgrs = "mgrs"; +constexpr const char * transverse_mercator = "transverse_mercator"; +constexpr const char * utm = "utm"; +} // namespace projector_names +} // namespace + +std::unique_ptr getProjector( + const std::string & projector_type, const lanelet::GPSPoint & origin) + +{ + if (projector_type == projector_names::mgrs) { + return std::make_unique(); + } + if (projector_type == projector_names::transverse_mercator) { + return std::make_unique( + lanelet::Origin{origin}); + } + if (projector_type == projector_names::utm) { + return std::make_unique(lanelet::Origin{origin}); + } + return nullptr; +} + +std::pair> +loadAndValidateMap( + const std::string & projector_type, const std::string & map_file, + const lanelet::validation::ValidationConfig & val_config) +{ + std::vector issues; + lanelet::LaneletMapPtr map{nullptr}; + lanelet::validation::Strings errors; + try { + const auto projector = getProjector(projector_type, val_config.origin); + if (!projector) { + errors.push_back("No valid map projection type specified!"); + } else { + map = lanelet::load(map_file, *projector, &errors); + } + if (!map) { + errors.push_back("Failed to load map!"); + } + 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); + })); + } + + return {map, issues}; +} + +} // namespace lanelet::autoware::validation diff --git a/map/autoware_lanelet2_map_validator/src/common/validation.cpp b/map/autoware_lanelet2_map_validator/src/common/validation.cpp index b3e33b8f..baceaac8 100644 --- a/map/autoware_lanelet2_map_validator/src/common/validation.cpp +++ b/map/autoware_lanelet2_map_validator/src/common/validation.cpp @@ -14,8 +14,6 @@ #include "lanelet2_map_validator/validation.hpp" -#include -#include #include #include @@ -42,56 +40,11 @@ namespace lanelet::autoware::validation { -std::unique_ptr getProjector( - const std::string & projector_type, const lanelet::GPSPoint & origin) - +std::vector apply_validation( + const lanelet::LaneletMap & lanelet_map, const lanelet::validation::ValidationConfig & val_config) { - if (projector_type == projector_names::mgrs) { - return std::make_unique(); - } - if (projector_type == projector_names::transverse_mercator) { - return std::make_unique( - lanelet::Origin{origin}); - } - if (projector_type == projector_names::utm) { - return std::make_unique(lanelet::Origin{origin}); - } - return nullptr; -} - -std::vector validateMap( - const std::string & projector_type, const std::string & map_file, - const lanelet::validation::ValidationConfig & val_config) -{ - std::vector issues; - lanelet::LaneletMapPtr map{nullptr}; - lanelet::validation::Strings errors; - try { - const auto projector = getProjector(projector_type, val_config.origin); - if (!projector) { - errors.push_back("No valid map projection type specified!"); - } else { - map = lanelet::load(map_file, *projector, &errors); - } - if (map) { - appendIssues(issues, lanelet::validation::validateMap(*map, val_config)); - } else { - errors.push_back("Failed to load map!"); - } - 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); - })); - } - - return issues; + return lanelet::validation::validateMap( + const_cast(lanelet_map), val_config); } Validators parse_validators(const json & json_data) @@ -326,9 +279,10 @@ lanelet::validation::ValidationConfig replace_validator( return temp; } -void process_requirements(json json_data, const MetaConfig & validator_config) +void process_requirements( + json json_data, const MetaConfig & validator_config, const lanelet::LaneletMap & lanelet_map) { - std::vector issues; + std::vector total_issues; // List up validators in order Validators validators = parse_validators(json_data); @@ -338,44 +292,41 @@ void process_requirements(json json_data, const MetaConfig & validator_config) if (auto unused_validator_issues = describe_unused_validators_to_json(json_data, remaining_validators); !unused_validator_issues.empty()) { - appendIssues(issues, std::move(unused_validator_issues)); + appendIssues(total_issues, std::move(unused_validator_issues)); } // Main validation process while (!validation_queue.empty()) { - std::string validator_name = validation_queue.front(); + const std::string validator_name = validation_queue.front(); validation_queue.pop(); - std::vector temp_issues; - // Check prerequisites are OK - appendIssues(temp_issues, check_prerequisite_completion(validators, validator_name)); - - if (temp_issues.size() == 0) { - // Validate map - appendIssues( - temp_issues, - validateMap( - validator_config.projector_type, validator_config.command_line_config.mapFile, - replace_validator( - validator_config.command_line_config.validationConfig, validator_name))); - } + const auto prerequisite_issues = check_prerequisite_completion(validators, validator_name); + appendIssues(total_issues, prerequisite_issues); + + // NOTE: if prerequisite_issues is not empty, skip the content validation process + const auto issues = + prerequisite_issues.empty() + ? apply_validation( + lanelet_map, replace_validator( + validator_config.command_line_config.validationConfig, validator_name)) + : std::vector(); // Add validation results to the json data json & validator_json = find_validator_block(json_data, validator_name); - if (temp_issues.size() == 0) { + if (issues.empty()) { validator_json["passed"] = true; continue; } - if (temp_issues[0].warnings().size() + temp_issues[0].errors().size() == 0) { + if (issues[0].warnings().size() + issues[0].errors().size() == 0) { validator_json["passed"] = true; } else { validator_json["passed"] = false; } - if (temp_issues[0].issues.size() > 0) { + if (!issues[0].issues.empty()) { json issues_json; - for (const auto & issue : temp_issues[0].issues) { + for (const auto & issue : issues[0].issues) { json issue_json; issue_json["severity"] = lanelet::validation::toString(issue.severity); issue_json["primitive"] = lanelet::validation::toString(issue.primitive); @@ -392,12 +343,12 @@ void process_requirements(json json_data, const MetaConfig & validator_config) } validator_json["issues"] = issues_json; } - appendIssues(issues, std::move(temp_issues)); + appendIssues(total_issues, issues); } // Show results summarize_validator_results(json_data); - lanelet::validation::printAllIssues(issues); + lanelet::validation::printAllIssues(total_issues); // Save results if (!validator_config.output_file_path.empty()) { diff --git a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/map_loader.hpp b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/map_loader.hpp new file mode 100644 index 00000000..0ae5e7e5 --- /dev/null +++ b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/map_loader.hpp @@ -0,0 +1,37 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELET2_MAP_VALIDATOR__MAP_LOADER_HPP_ // NOLINT +#define LANELET2_MAP_VALIDATOR__MAP_LOADER_HPP_ // NOLINT + +#include +#include +#include + +#include +#include +#include +#include + +namespace lanelet::autoware::validation +{ + +std::pair> +loadAndValidateMap( + const std::string & projector_type, const std::string & map_file, + const lanelet::validation::ValidationConfig & val_config); + +} // namespace lanelet::autoware::validation + +#endif // LANELET2_MAP_VALIDATOR__MAP_LOADER_HPP_ diff --git a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/utils.hpp b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/utils.hpp index db93c2a4..7989d4ba 100644 --- a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/utils.hpp +++ b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/utils.hpp @@ -19,14 +19,21 @@ #include #include +#include #include namespace lanelet::autoware::validation { -template -void appendIssues(std::vector & to, std::vector && from) +template +auto appendIssues(std::vector & to, Container && from) -> + typename std::enable_if_t::value_type>, void> { - to.insert(to.end(), std::make_move_iterator(from.begin()), std::make_move_iterator(from.end())); + if constexpr (std::is_rvalue_reference::value) { + to.insert(to.end(), std::make_move_iterator(from.begin()), std::make_move_iterator(from.end())); + } + if constexpr (std::is_lvalue_reference::value) { + to.insert(to.end(), from.begin(), from.end()); + } } template diff --git a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validation.hpp b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validation.hpp index fb90dffd..273ea639 100644 --- a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validation.hpp +++ b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validation.hpp @@ -20,12 +20,9 @@ #include -#include -#include #include #include -#include #include #include #include @@ -36,16 +33,6 @@ using json = nlohmann::json; -namespace -{ -namespace projector_names -{ -constexpr const char * mgrs = "mgrs"; -constexpr const char * transverse_mercator = "transverse_mercator"; -constexpr const char * utm = "utm"; -} // namespace projector_names -} // namespace - namespace lanelet::autoware::validation { struct ValidatorInfo @@ -58,11 +45,8 @@ struct ValidatorInfo using Validators = std::unordered_map; -std::unique_ptr getProjector( - const std::string & projector_type, const lanelet::GPSPoint & origin); - -std::vector validateMap( - const std::string & projector_type, const std::string & map_file, +std::vector apply_validation( + const lanelet::LaneletMap & lanelet_map, const lanelet::validation::ValidationConfig & val_config); Validators parse_validators(const json & json_data); @@ -85,7 +69,8 @@ lanelet::validation::ValidationConfig replace_validator( const lanelet::validation::ValidationConfig & input, const std::string & validator_name); void process_requirements( - json json_data, const lanelet::autoware::validation::MetaConfig & validator_config); + json json_data, const lanelet::autoware::validation::MetaConfig & validator_config, + const lanelet::LaneletMap & lanelet_map); } // namespace lanelet::autoware::validation #endif // LANELET2_MAP_VALIDATOR__VALIDATION_HPP_ diff --git a/map/autoware_lanelet2_map_validator/src/main.cpp b/map/autoware_lanelet2_map_validator/src/main.cpp index 9c97bf06..66918b0d 100644 --- a/map/autoware_lanelet2_map_validator/src/main.cpp +++ b/map/autoware_lanelet2_map_validator/src/main.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "lanelet2_map_validator/cli.hpp" +#include "lanelet2_map_validator/map_loader.hpp" #include "lanelet2_map_validator/utils.hpp" #include "lanelet2_map_validator/validation.hpp" @@ -53,22 +54,27 @@ int main(int argc, char * argv[]) // Validation start if (meta_config.command_line_config.mapFile.empty()) { throw std::invalid_argument("No map file specified!"); - } else if (!std::filesystem::is_regular_file(meta_config.command_line_config.mapFile)) { + } + if (!std::filesystem::is_regular_file(meta_config.command_line_config.mapFile)) { throw std::invalid_argument("Map file doesn't exist or is not a file!"); } + const auto [lanelet_map_ptr, map_issue] = lanelet::autoware::validation::loadAndValidateMap( + meta_config.projector_type, meta_config.command_line_config.mapFile, + meta_config.command_line_config.validationConfig); - if (!meta_config.requirements_file.empty()) { + if (!lanelet_map_ptr) { + lanelet::validation::printAllIssues(map_issue); + } else if (!meta_config.requirements_file.empty()) { if (!std::filesystem::is_regular_file(meta_config.requirements_file)) { throw std::invalid_argument("Input file doesn't exist or is not a file!"); } std::ifstream input_file(meta_config.requirements_file); json json_data; input_file >> json_data; - lanelet::autoware::validation::process_requirements(json_data, meta_config); + lanelet::autoware::validation::process_requirements(json_data, meta_config, *lanelet_map_ptr); } else { - auto issues = lanelet::autoware::validation::validateMap( - meta_config.projector_type, meta_config.command_line_config.mapFile, - meta_config.command_line_config.validationConfig); + const auto issues = lanelet::autoware::validation::apply_validation( + *lanelet_map_ptr, meta_config.command_line_config.validationConfig); lanelet::validation::printAllIssues(issues); }