feat(lanelet2_utility): add intersection

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

common/autoware_lanelet2_utility/CMakeLists.txt

@@ -7,13 +7,18 @@ autoware_package()
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/kind.cpp
   src/topology.cpp
+  src/intersection.cpp
 )
 
 if(BUILD_TESTING)
   find_package(ament_cmake_ros REQUIRED)
   ament_auto_find_test_dependencies()
 
-  file(GLOB_RECURSE test_files test/*.cpp)
+  set(test_files
+    test/kind.cpp
+    test/topology.cpp
+    test/intersection.cpp
+  )
 
   foreach (test_file IN LISTS test_files)
     get_filename_component(test_file_name ${test_file} NAME)

common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/intersection.hpp

@@ -0,0 +1,72 @@
+// Copyright 2025 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 AUTOWARE_LANELET2_UTILITY__INTERSECTION_HPP_
+#define AUTOWARE_LANELET2_UTILITY__INTERSECTION_HPP_
+
+#include <lanelet2_core/Forward.h>
+
+#include <optional>
+
+namespace autoware::lanelet2_utility
+{
+
+static constexpr const char * k_turn_direction = "turn_direction";
+static constexpr const char * k_turn_direction_straight = "straight";
+static constexpr const char * k_turn_direction_left = "left";
+static constexpr const char * k_turn_direction_right = "right";
+
+enum TurnDirection : int { Straight = 0, Left, Right };
+
+/**
+ * @brief check if given lanelet has "turn_direction" attribute
+ * @param [in] input lanelet
+ * @return true if and only if the given lanelet has "turn_direction" attribute
+ */
+bool is_intersection_lanelet(const lanelet::ConstLanelet & lanelet);
+
+/**
+ * @brief check if given lanelet has "turn_direction" attribute and the value is "straight"
+ * @param [in] input lanelet
+ * @return true if and only if the given lanelet has "turn_direction" attribute and the value is
+ * "straight"
+ */
+bool is_straight_direction(const lanelet::ConstLanelet & lanelet);
+
+/**
+ * @brief check if given lanelet has "turn_direction" attribute and the value is "left"
+ * @param [in] input lanelet
+ * @return true if and only if the given lanelet has "turn_direction" attribute and the value is
+ * "left"
+ */
+bool is_left_direction(const lanelet::ConstLanelet & lanelet);
+
+/**
+ * @brief check if given lanelet has "turn_direction" attribute and the value is "right"
+ * @param [in] input lanelet
+ * @return true if and only if the given lanelet has "turn_direction" attribute and the value is
+ * "right"
+ */
+bool is_right_direction(const lanelet::ConstLanelet & lanelet);
+
+/**
+ * @brief get the turn_direction value
+ * @param [in] input lanelet
+ * @return valid TurnDirection value if `lanelet` has valid "turn_direction" value, otherwise null
+ * "right"
+ */
+std::optional<TurnDirection> get_turn_direction(const lanelet::ConstLanelet & lanelet);
+
+}  // namespace autoware::lanelet2_utility
+#endif  // AUTOWARE_LANELET2_UTILITY__INTERSECTION_HPP_

common/autoware_lanelet2_utility/src/intersection.cpp

@@ -0,0 +1,56 @@
+// Copyright 2025 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 <autoware_lanelet2_utility/intersection.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+namespace autoware::lanelet2_utility
+{
+
+bool is_intersection_lanelet(const lanelet::ConstLanelet & lanelet)
+{
+  return lanelet.hasAttribute(k_turn_direction);
+}
+
+bool is_straight_direction(const lanelet::ConstLanelet & lanelet)
+{
+  return strcmp(lanelet.attributeOr(k_turn_direction, "else"), k_turn_direction_straight) == 0;
+}
+
+bool is_left_direction(const lanelet::ConstLanelet & lanelet)
+{
+  return strcmp(lanelet.attributeOr(k_turn_direction, "else"), k_turn_direction_left) == 0;
+}
+
+bool is_right_direction(const lanelet::ConstLanelet & lanelet)
+{
+  return strcmp(lanelet.attributeOr(k_turn_direction, "else"), k_turn_direction_right) == 0;
+}
+
+std::optional<TurnDirection> get_turn_direction(const lanelet::ConstLanelet & lanelet)
+{
+  if (is_straight_direction(lanelet)) {
+    return TurnDirection::Straight;
+  }
+  if (is_left_direction(lanelet)) {
+    return TurnDirection::Left;
+  }
+  if (is_right_direction(lanelet)) {
+    return TurnDirection::Right;
+  }
+  return std::nullopt;
+}
+
+}  // namespace autoware::lanelet2_utility

common/autoware_lanelet2_utility/test/intersection.cpp

@@ -0,0 +1,124 @@
+// Copyright 2025 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 "map_loader.hpp"
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <autoware_lanelet2_utility/intersection.hpp>
+
+#include <gtest/gtest.h>
+
+#include <filesystem>
+#include <string>
+
+namespace fs = std::filesystem;
+
+namespace autoware
+{
+class TestWithIntersectionCrossingMap : public ::testing::Test
+{
+protected:
+  lanelet::LaneletMapConstPtr lanelet_map_ptr_{nullptr};
+
+  void SetUp() override
+  {
+    const auto sample_map_dir =
+      fs::path(ament_index_cpp::get_package_share_directory("autoware_lanelet2_utility")) /
+      "sample_map";
+    const auto intersection_crossing_map_path = sample_map_dir / "intersection" / "crossing.osm";
+
+    lanelet_map_ptr_ = load_mgrs_coordinate_map(intersection_crossing_map_path.string());
+  }
+};
+
+TEST_F(TestWithIntersectionCrossingMap, is_intersection_lanelet_false)
+{
+  EXPECT_EQ(
+    lanelet2_utility::is_intersection_lanelet(lanelet_map_ptr_->laneletLayer.get(2257)), false);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, is_intersection_lanelet_true)
+{
+  EXPECT_EQ(
+    lanelet2_utility::is_intersection_lanelet(lanelet_map_ptr_->laneletLayer.get(2274)), true);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, is_straight_direction_false)
+{
+  EXPECT_EQ(
+    lanelet2_utility::is_straight_direction(lanelet_map_ptr_->laneletLayer.get(2274)), false);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, is_straight_direction_true)
+{
+  EXPECT_EQ(
+    lanelet2_utility::is_straight_direction(lanelet_map_ptr_->laneletLayer.get(2278)), true);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, is_left_direction_false)
+{
+  EXPECT_EQ(lanelet2_utility::is_left_direction(lanelet_map_ptr_->laneletLayer.get(2278)), false);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, is_left_direction_true)
+{
+  EXPECT_EQ(lanelet2_utility::is_left_direction(lanelet_map_ptr_->laneletLayer.get(2274)), true);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, is_right_direction_false)
+{
+  EXPECT_EQ(lanelet2_utility::is_right_direction(lanelet_map_ptr_->laneletLayer.get(2274)), false);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, is_right_direction_true)
+{
+  EXPECT_EQ(lanelet2_utility::is_right_direction(lanelet_map_ptr_->laneletLayer.get(2277)), true);
+}
+
+TEST_F(TestWithIntersectionCrossingMap, get_turn_direction)
+{
+  // not intersection
+  EXPECT_EQ(
+    lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2257)).has_value(),
+    false);
+
+  // straight
+  {
+    const auto lane =
+      lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2278));
+    EXPECT_EQ(lane.has_value() && lane.value() == lanelet2_utility::TurnDirection::Straight, true);
+  }
+
+  // left
+  {
+    const auto lane =
+      lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2274));
+    EXPECT_EQ(lane.has_value() && lane.value() == lanelet2_utility::TurnDirection::Left, true);
+  }
+
+  // right
+  {
+    const auto lane =
+      lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2277));
+    EXPECT_EQ(lane.has_value() && lane.value() == lanelet2_utility::TurnDirection::Right, true);
+  }
+}
+
+}  // namespace autoware
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}