feat(lanelet2_extension): add route checker (tier4#1149)

* feat(lanelet2_extension): add route checker

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* chore: move pkg from tier4_autoware_utils to lanelet2_extension

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* fix: test

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* ci(pre-commit): autofix

* chore: add comment

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* ci(pre-commit): autofix

* chore: rm unused pkg

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* 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 <shumpei.wakabayashi@tier4.jp>

* ci(pre-commit): autofix

* fix: change interface

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* ci(pre-commit): autofix

* fix: change style

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* 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

@@ -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()

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 <lanelet2_extension/utility/query.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+
+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_

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 <lanelet2_extension/utility/message_conversion.hpp>
+
+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

map/lanelet2_extension/package.xml

@@ -12,6 +12,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
   <depend>geographiclib</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_core</depend>

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 <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+
+#include <gtest/gtest.h>
+
+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<autoware_auto_planning_msgs::msg::HADMapRoute>(sample_route1);
+  const auto route_ptr2 =
+    std::make_shared<autoware_auto_planning_msgs::msg::HADMapRoute>(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();
+}