From 87df1c4ed59bf92d60df75f9a5aab85565e8252c Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Thu, 23 Jun 2022 19:39:08 +0900 Subject: [PATCH] feat(lanelet2_extension): add route checker (#1149) * feat(lanelet2_extension): add route checker Signed-off-by: Shumpei Wakabayashi * chore: move pkg from tier4_autoware_utils to lanelet2_extension Signed-off-by: Shumpei Wakabayashi * fix: test Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix * chore: add comment Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix * chore: rm unused pkg Signed-off-by: Shumpei Wakabayashi * Update map/lanelet2_extension/test/src/test_route_checker.cpp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Update map/lanelet2_extension/test/src/test_route_checker.cpp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * chore: update code style Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix * fix: change interface Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix * fix: change style Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- map/lanelet2_extension/CMakeLists.txt | 3 + .../utility/route_checker.hpp | 32 +++++++ map/lanelet2_extension/lib/route_checker.cpp | 44 +++++++++ map/lanelet2_extension/package.xml | 1 + .../test/src/test_route_checker.cpp | 94 +++++++++++++++++++ 5 files changed, 174 insertions(+) create mode 100644 map/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp create mode 100644 map/lanelet2_extension/lib/route_checker.cpp create mode 100644 map/lanelet2_extension/test/src/test_route_checker.cpp diff --git a/map/lanelet2_extension/CMakeLists.txt b/map/lanelet2_extension/CMakeLists.txt index 709c7bdaa6858..a0db7fe78bdb2 100644 --- a/map/lanelet2_extension/CMakeLists.txt +++ b/map/lanelet2_extension/CMakeLists.txt @@ -42,6 +42,7 @@ ament_auto_add_library(lanelet2_extension_lib SHARED lib/utilities.cpp lib/virtual_traffic_light.cpp lib/visualization.cpp + lib/route_checker.cpp ) target_link_libraries(lanelet2_extension_lib ${GeographicLib_LIBRARIES} @@ -72,6 +73,8 @@ if(BUILD_TESTING) target_link_libraries(regulatory_elements-test lanelet2_extension_lib) ament_add_ros_isolated_gtest(utilities-test test/src/test_utilities.cpp) target_link_libraries(utilities-test lanelet2_extension_lib) + ament_add_ros_isolated_gtest(route-test test/src/test_route_checker.cpp) + target_link_libraries(route-test lanelet2_extension_lib) endif() ament_auto_package() diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp new file mode 100644 index 0000000000000..8ab73e8e973ef --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp @@ -0,0 +1,32 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_ +#define LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_ + +#include +#include + +#include +#include + +namespace lanelet::utils::route +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_planning_msgs::msg::HADMapRoute; + +bool isRouteValid(const HADMapRoute route, const lanelet::LaneletMapPtr lanelet_map_ptr_); +} // namespace lanelet::utils::route + +#endif // LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_ diff --git a/map/lanelet2_extension/lib/route_checker.cpp b/map/lanelet2_extension/lib/route_checker.cpp new file mode 100644 index 0000000000000..c86fae96833f5 --- /dev/null +++ b/map/lanelet2_extension/lib/route_checker.cpp @@ -0,0 +1,44 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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/utility/route_checker.hpp" + +#include + +namespace lanelet +{ +namespace utils +{ +bool route::isRouteValid(const HADMapRoute route_msg, const lanelet::LaneletMapPtr lanelet_map_ptr_) +{ + for (const auto & route_section : route_msg.segments) { + for (const auto & primitive : route_section.primitives) { + const auto id = primitive.id; + try { + lanelet_map_ptr_->laneletLayer.get(id); + } catch (const std::exception & e) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; + return false; + } + } + } + return true; +} + +} // namespace utils +} // namespace lanelet diff --git a/map/lanelet2_extension/package.xml b/map/lanelet2_extension/package.xml index d46d05a7f3064..e85e9243eb304 100644 --- a/map/lanelet2_extension/package.xml +++ b/map/lanelet2_extension/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_auto_mapping_msgs + autoware_auto_planning_msgs geographiclib geometry_msgs lanelet2_core diff --git a/map/lanelet2_extension/test/src/test_route_checker.cpp b/map/lanelet2_extension/test/src/test_route_checker.cpp new file mode 100644 index 0000000000000..e3dc6bdd30594 --- /dev/null +++ b/map/lanelet2_extension/test/src/test_route_checker.cpp @@ -0,0 +1,94 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/route_checker.hpp" + +#include +#include + +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::utils::getId; + +class TestSuite : public ::testing::Test +{ +public: + TestSuite() : sample_map_ptr(new lanelet::LaneletMap()) + { + // create sample lanelets + const Point3d p1(getId(), 0.0, 0.0, 0.0); + const Point3d p2(getId(), 0.0, 1.0, 0.0); + + const LineString3d ls_left(getId(), {p1, p2}); + + const Point3d p3(getId(), 1.0, 0.0, 0.0); + const Point3d p4(getId(), 1.0, 0.0, 0.0); + + const LineString3d ls_right(getId(), {p3, p4}); + + const Lanelet lanelet(getId(), ls_left, ls_right); + + sample_map_ptr->add(lanelet); + + // create sample routes + autoware_auto_mapping_msgs::msg::MapPrimitive map_primitive; + autoware_auto_mapping_msgs::msg::HADMapSegment map_segment1; + autoware_auto_mapping_msgs::msg::HADMapSegment map_segment2; + + for (size_t i = 0; i < 2; i++) { + map_primitive.id = lanelet.id(); + map_segment1.primitives.push_back(map_primitive); + map_primitive.id = ls_left.id(); + map_segment2.primitives.push_back(map_primitive); + } + sample_route1.segments.push_back(map_segment1); + sample_route2.segments.push_back(map_segment2); + } + ~TestSuite() {} + + lanelet::LaneletMapPtr sample_map_ptr; + autoware_auto_planning_msgs::msg::HADMapRoute sample_route1; // valid route + autoware_auto_planning_msgs::msg::HADMapRoute sample_route2; // invalid route + +private: +}; + +TEST_F(TestSuite, isRouteValid) +{ + autoware_auto_mapping_msgs::msg::HADMapBin bin_msg; + + const auto route_ptr1 = + std::make_shared(sample_route1); + const auto route_ptr2 = + std::make_shared(sample_route2); + + // toBinMsg is tested at test_message_conversion.cpp + lanelet::utils::conversion::toBinMsg(sample_map_ptr, &bin_msg); + + ASSERT_TRUE(lanelet::utils::route::isRouteValid(*route_ptr1, sample_map_ptr)) + << "The route should be valid, which should be created on the same map as the current one"; + ASSERT_FALSE(lanelet::utils::route::isRouteValid(*route_ptr2, sample_map_ptr)) + << "The route should be invalid, which should be created on the different map from the current " + "one"; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}