feat(autoware_lanelet2_utility)!: add autoware_lanelet2_utility packa…

…ge (#231)

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

common/autoware_lanelet2_utility/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(autoware_lanelet2_utility)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/kind.cpp
+  src/topology.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+  ament_auto_find_test_dependencies()
+
+  file(GLOB_RECURSE test_files test/*.cpp)
+
+  foreach (test_file IN LISTS test_files)
+    get_filename_component(test_file_name ${test_file} NAME)
+    ament_auto_add_gtest(${test_file_name}_${PROJECT_NAME} ${test_file})
+    target_link_libraries(${test_file_name}_${PROJECT_NAME}
+      ${PROJECT_NAME}
+    )
+  endforeach()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  sample_map
+)
+
+install(PROGRAMS
+  scripts/lanelet_anonymizer.py
+  scripts/lanelet_id_aligner.py
+  DESTINATION lib/${PROJECT_NAME}
+)

common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/kind.hpp

@@ -0,0 +1,62 @@
+// 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__KIND_HPP_
+#define AUTOWARE_LANELET2_UTILITY__KIND_HPP_
+
+#include <lanelet2_core/Forward.h>
+
+namespace autoware::lanelet2_utility
+{
+static constexpr const char * k_road_lane_type = "road";
+static constexpr const char * k_shoulder_lane_type = "road_shoulder";
+static constexpr const char * k_bicycle_lane_type = "bicycle_lane";
+
+/*
+ * TODO(soblin): distinguish road lane type
+class RoadLane : public lanelet::ConstLanelet
+{
+};
+
+class RoadShoulderLane : public lanelet::ConstLanelet
+{
+};
+
+class BicycleLane : public lanelet::ConstLanelet
+{
+};
+*/
+
+/**
+ * @brief check if the given lanelet type has "road" subtype
+ * @param [in] lanelet input lanelet
+ * @return if the lanelet is road or not
+ */
+bool is_road_lane(const lanelet::ConstLanelet & lanelet);
+
+/**
+ * @brief check if the given lanelet type has "road_shoulder" subtype
+ * @param [in] lanelet input lanelet
+ * @return if the lanelet is road_shoulder or not
+ */
+bool is_shoulder_lane(const lanelet::ConstLanelet & lanelet);
+
+/**
+ * @brief check if the given lanelet type has "bicycle_lane" subtype
+ * @param [in] lanelet input lanelet
+ * @return if the lanelet is bicycle_lane or not
+ */
+bool is_bicycle_lane(const lanelet::ConstLanelet & lanelet);
+}  // namespace autoware::lanelet2_utility
+#endif  // AUTOWARE_LANELET2_UTILITY__KIND_HPP_

common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/topology.hpp

@@ -0,0 +1,186 @@
+// 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__TOPOLOGY_HPP_
+#define AUTOWARE_LANELET2_UTILITY__TOPOLOGY_HPP_
+
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_routing/Forward.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <optional>
+#include <vector>
+
+namespace autoware::lanelet2_utility
+{
+/**
+ * @brief instantiate RoutingGraph from given LaneletMap only from "road" lanes
+ * @param location [in, opt, lanelet::Locations::Germany] location value
+ * @param participant [in, opt, lanelet::Participants::Vehicle] participant value
+ * @return RoutingGraph object without road_shoulder and bicycle_lane
+ */
+lanelet::routing::RoutingGraphConstPtr instantiate_routing_graph(
+  lanelet::LaneletMapConstPtr lanelet_map, const char * location = lanelet::Locations::Germany,
+  const char * participant = lanelet::Participants::Vehicle);
+
+/**
+ * @brief get the left adjacent and same_direction lanelet on the routing graph if exists regardless
+ * of lane change permission
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return optional of left adjacent lanelet(nullopt if there is no such adjacent lanelet)
+ */
+std::optional<lanelet::ConstLanelet> left_lanelet(
+  const lanelet::ConstLanelet & lanelet,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+/**
+ * @brief get the right adjacent and same_direction lanelet on the routing graph if exists
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return optional of right adjacent lanelet(nullopt if there is no such adjacent lanelet)
+ */
+std::optional<lanelet::ConstLanelet> right_lanelet(
+  const lanelet::ConstLanelet & lanelet,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+/**
+ * @brief get left_lanelet() or sibling lanelet. If `lanelet` has turn_direction, search for sibling
+ * lanelet is limited to the one with same turn_direction value
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return optional of aforementioned lanelet(nullopt if there is no such lanelet)
+ */
+/*
+std::optional<lanelet::ConstLanelet> left_similar_lanelet(
+const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map,
+const lanelet::routing::RoutingGraphConstPtr routing_graph);
+*/
+
+/**
+ * @brief get right_lanelet() or sibling lanelet. If `lanelet` has turn_direction, search for
+ * sibling lanelet is limited to the one with same turn_direction value
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return optional of aforementioned lanelet(nullopt if there is no such lanelet)
+ */
+/*
+std::optional<lanelet::ConstLanelet> right_similar_lanelet(
+const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map,
+const lanelet::routing::RoutingGraphConstPtr routing_graph);
+*/
+
+/**
+ * @brief get the left adjacent and opposite_direction lanelet on the routing graph if exists
+ * @param [in] lanelet input lanelet
+ * @param [in] lanelet_map lanelet_map containing `lanelet`
+ * @return optional of the left opposite lanelet(nullopt if there is not such opposite lanelet)
+ */
+std::optional<lanelet::ConstLanelet> left_opposite_lanelet(
+  const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map);
+
+/**
+ * @brief get the right adjacent and opposite_direction lanelet on the routing graph if exists
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return optional of the right opposite lanelet(nullopt if there is no such opposite lanelet)
+ */
+std::optional<lanelet::ConstLanelet> right_opposite_lanelet(
+  const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map);
+
+/**
+ * @brief get the leftmost same_direction lanelet if exists
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return optional of such lanelet(nullopt if there is no such adjacent lanelet)
+ */
+std::optional<lanelet::ConstLanelet> leftmost_lanelet(
+  const lanelet::ConstLanelet & lanelet,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+std::optional<lanelet::ConstLanelet> rightmost_lanelet(
+  const lanelet::ConstLanelet & lanelet,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+/**
+ * @brief get the left lanelets which are adjacent to `lanelet`
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @param [in] include_opposite flag if opposite_direction lanelet is included
+ * @param [in] invert_opposite_lanelet flag if the opposite lanelet in the output is `.inverted()`
+ * or not
+ * @return the list of lanelets excluding `lanelet` which is ordered in the *hopping* number from
+ * `lanelet`
+ */
+lanelet::ConstLanelets left_lanelets(
+  const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph, const bool include_opposite = false,
+  const bool invert_opposite_lane = false);
+
+/**
+ * @brief get the right lanelets which are adjacent to `lanelet`
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @param [in] include_opposite flag if opposite_direction lanelet is included
+ * @param [in] invert_opposite_lanelet flag if the opposite lanelet in the output is `.inverted()`
+ * or not
+ * @return the list of lanelets excluding `lanelet` which is ordered in the *hopping* number from
+ * `lanelet`
+ */
+lanelet::ConstLanelets right_lanelets(
+  const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph, const bool include_opposite = false,
+  const bool invert_opposite_lane = false);
+
+/**
+ * @brief get the following lanelets
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return the following lanelets
+ */
+lanelet::ConstLanelets following_lanelets(
+  const lanelet::ConstLanelet & lanelet,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+/**
+ * @brief get the previous lanelets
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return the previous lanelets
+ */
+lanelet::ConstLanelets previous_lanelets(
+  const lanelet::ConstLanelet & lanelet,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+/**
+ * @brief get the sibling lanelets
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return the sibling lanelets excluding `lanelet`
+ */
+lanelet::ConstLanelets sibling_lanelets(
+  const lanelet::ConstLanelet & lanelet,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+/**
+ * @brief get Lanelet instances of the designated ids
+ * @param [in] lanelet input lanelet
+ * @param [in] routing_graph routing_graph containing `lanelet`
+ * @return the list of Lanelets in the same order as `ids`
+ */
+lanelet::ConstLanelets from_ids(
+  const lanelet::LaneletMapConstPtr lanelet_map, const std::vector<lanelet::Id> & ids);
+}  // namespace autoware::lanelet2_utility
+
+#endif  // AUTOWARE_LANELET2_UTILITY__TOPOLOGY_HPP_