diff --git a/common/autoware_lanelet2_utility/CMakeLists.txt b/common/autoware_lanelet2_utility/CMakeLists.txt new file mode 100644 index 000000000..fb51b5792 --- /dev/null +++ b/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} +) diff --git a/common/autoware_lanelet2_utility/README.md b/common/autoware_lanelet2_utility/README.md new file mode 100644 index 000000000..45e8d6b0e --- /dev/null +++ b/common/autoware_lanelet2_utility/README.md @@ -0,0 +1,184 @@ +# autoware_lanelet2_utility + +## Nomenclature + +This package aims to strictly define the meaning of several words to clarify the documentation and API's scope. In the table below, `codespace` words are given specific meanings when used in the API and API description. _italic_ words are emphasized to indicate that it refers to social common sense which often comes with ambiguity. To help disambiguate the meaning, illustration is provided. "Lanelet" refers to the entity of a`lanelet::ConstLanelet` object in order to distinguish with the word "lane" used in social customs. `A` and `B` stands for some Lanelets objects. + +| Word | Meaning | Illustration | +| --------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `driving` | The vehicle position belongs to the designated Lanelet. | In each map, green Lanelet are the `driving` lanes of the vehicle.
![driving](./media/nomenclature/driving.drawio.svg) | +| `boundary`,
`entry`,
`exit` | The `boundary` of a Lanelet refers to the left or right Linestring. | ![boundary_entry_exit](./media/nomenclature/boundary_entry_exit.drawio.svg) | +| `adjacent` | If A is `adjacent` to B, A and B share a common `boundary` with same direction either on the left or right side. | In each map, orange Lanelet is `adjacent` to green Lanelet.
![adjacent](./media/nomenclature/adjacent.drawio.svg) | +| `same_direction` | Lanelet A and Lanelet B are `same_direction` if A and B are directly or indirectly `adjacent` to each other. | In each map, orange Lanelets are `same_direction` as green Lanelet.
![same_direction](./media/nomenclature/same_direction.drawio.svg) | +| `bundle` | A `bundle` refers to a transitive closure set of Lanelets which are `same_direction` to each other. | The enclosed sets of Lanelets are `bundle`s.
![bundle](./media/nomenclature/bundle.drawio.svg) | +| `opposite` | If A is `opposite` to B, A and B share a common `boundary` with opposite direction. | In the first map, green Lanelet and orange Lanelet are `opposite` to each other.
In the second map, two red Lanelets are not `opposite` relation because they do not share a common LineString.
![opposite](./media/nomenclature/opposite.drawio.svg) | +| `opposite_direction` | If A and B are `opposite_direction`, the `bundle` of A and B are directly `opposite` to each other. | In the each map, green Lanelet and orange Lanelet are `opposite_direction` because their `bundle`s(enclosed in dotted line) are `opposite` relation.
![opposite_direction](./media/nomenclature/opposite_direction.drawio.svg) | +| `connected` | A is `connected` to(from) B if and only if the `exit`(`entry`) of A is identical to the `entry`(`exit`) of B. | A is connected to B, and B is connected from A.
![connected](./media/nomenclature/connected.drawio.svg) | +| `following` | The `following` Lanelets of A is the list of Lanelets to which A is `connected`. | In each map, orange Lanelets are the `following` of green Lanelet.
![following](./media/nomenclature/following.drawio.svg) | +| `previous` | The `previous` Lanelets of A is the list of Lanelets from which A is `connected`. | In each map, orange Lanelets are the `previous` of green Lanelet.
![previous](./media/nomenclature/previous.drawio.svg) | +| `conflicting` | A is `conflicting` with B if A and B are geometrically intersecting. | | +| `merging` | A is said to be `merging` Lanelet of B if and only if A is `conflicting` with B and both A and B are connected to a common Lanelet. | In each map, one of the orange Lanelet is a `merging` Lanelet of the other orange Lanelet.
![merging](./media/nomenclature/merging.drawio.svg) | +| `sibling` | The designated Lanelets are referred to as `sibling` if all of them are `connected` from a common Lanelet. | In each map, orange Lanelets are `sibling`s.
![sibling](./media/nomenclature/sibling.drawio.svg) | +| `oncoming` | TBD | TBD | +| `upcoming` | TBD | TBD | +| `sequence` | `sequence` is a list of Lanelets whose each element is `connected from` or `adjacent to` the previous element. | ![sequence](./media/nomenclature/sequence.drawio.svg) | +| `similar` | A and B are called `similar` if and only if both of them have same valid `turn_direction` value. | | + +## API description + +| Header | function | description | average computational complexity | illustration | +| ------------------------------------------ | ----------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `` | `is_road_lane` | | $O(1)$ | | +| | `is_shoulder_lane` | | $O(1)$ | | +| | `is_bicycle_lane` | | $O(1)$ | | +| `` | `instantiate_routing_graph` | This function creates a `RoutingGraph` object only from "road" lanes, which means "road_shoulder" and "bicycle_lane" Lanelets are inaccessible from left/right adjacency. | | | +| | `left_lanelet` | This function ignores the permission of lane change. Also it ignores `shoulder` and `bicycle` Lanelet. | $O(1)$ | In the first map, the green Lanelet is the `left_lanelet` of the orange Lanelet.
In the second and third map, the `left_lanelet` of the orange Lanelet is `null`.
![left_lanelet](./media/api/left_lanelet.drawio.svg) | +| | `right_lanelet` | same as above `left_lanelet` | $O(1)$ | | +| | `left_similar_lanelet`(TODO) | same as above `left_lanelet` | $O(1)$ | | +| | `right_similar_lanelet`(TODO) | same as above `left_lanelet` | $O(1)$ | | +| | `left_opposite_lanelet` | same as below `right_opposite_lanelet` | $O(1)$
see [`findUsage`](./#complexity-of-findusage) for detail | | +| | `right_opposite_lanelet` | | $O(1)$
see [`findUsage`](./#complexity-of-findusage) for detail | In the first and second map, the green Lanelet is the `right_opposite_lanelet` of the orange Lanelet.
In the third map, the `right_opposite_lanelet` of the orange Lanelet is `null`.
![right_opposite_lanelet](./media/api/right_opposite_lanelet.drawio.svg) | +| | `leftmost_lanelet` | | $O(W)$ where $W$ is the size of the `bundle`. | In the first and second map, the green Lanelet is the `leftmost_lanelet` of the orange Lanelet.
In the third map, the `leftmost_lanelet` of the orange Lanelet is `null`.
![leftmost_lanelet](./media/api/leftmost_lanelet.drawio.svg) | +| | `rightmost_lanelet` | | $O(W)$ where $W$ is the size of the `bundle`. | In the first map, the green Lanelet is the `rightmost_lanelet` of the orange Lanelet.
In the second and third map, the `rightmost_lanelet` of the orange Lanelet is `null`.
![rightmost_lanelet](./media/api/rightmost_lanelet.drawio.svg) | +| | `left_lanelets` | The input Lanelet is not included in the output. | $O(W)$ where $W$ is the size of the `bundle`. | In the first map, the green Lanelets are the `left_lanelets` of the orange Lanelet.
In the second and third map, `left_lanelets` of the orange Lanelet is empty.
If the flag `include_opposite = true`, the left opposite Lanelet and all of its `same_direction` Lanelets area also retrieved as illustrated in the fourth and fifth maps.
![left_lanelets](./media/api/left_lanelets.drawio.svg) | +| | `right_lanelets` | same as above `left_lanelets`. | $O(W)$ where $W$ is the size of the `bundle.` | In the first map, the green Lanelets are the `right_lanelets` of the orange Lanelet.
In the second and third map, `right_lanelets` of the orange Lanelet is empty.
If the flag `include_opposite = true`, the right opposite Lanelet and all of its `same_direction` Lanelets area also retrieved as illustrated in the fourth and fifth maps.
![right_lanelets](./media/api/right_lanelets.drawio.svg) | +| | `following_lanelets` | | $O(E)$ where $E$ is the number of Lanelets to which the input is connected to. | | +| | `previous_lanelets` | | $O(E)$ where $E$ is the number of Lanelets from which the input is connected from. | | +| | `sibling_lanelets` | | $O(E)$ where $E$ is the number of sibling Lanelets | | +| | `from_ids` | | $O(n)$ | | + +### complexity of `findUsage` + +The readers should be noted that following description is implementation dependent. + +- [LaneletMap.h](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/d9320cf66698004cd5e57988ac001e02e73e2e40/lanelet2_core/include/lanelet2_core/LaneletMap.h) +- [LaneletMap.cpp](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/d9320cf66698004cd5e57988ac001e02e73e2e40/lanelet2_core/src/LaneletMap.cpp) + +Lanelet map primitives(like `Lanelet`, `Area`, `RegulatoryElement`) are stored in several `PrimitiveLayer` objects according to their types as shown below. + +```cpp title="lanelet2_core/LaneletMap.h#L375-L438" +class LaneletMap : public LaneletMapLayers { + public: + using LaneletMapLayers::LaneletMapLayers; + <...> +}; +``` + +```cpp title="lanelet2_core/LaneletMap.h#L313-L359" +class LaneletMapLayers { + <...> + LaneletLayer laneletLayer; //!< access to the lanelets within this map + AreaLayer areaLayer; //!< access to areas + RegulatoryElementLayer regulatoryElementLayer; //!< access to regElems + PolygonLayer polygonLayer; //!< access to the polygons + LineStringLayer lineStringLayer; //!< access to the lineStrings + PointLayer pointLayer; //!< access to the points +}; +``` + +```cpp title="lanelet2_core/LaneletMap.h#L285-L303" +class LaneletLayer : public PrimitiveLayer { + public: + using PrimitiveLayer::findUsages; + LaneletLayer() = default; + ~LaneletLayer() = default; + LaneletLayer(const LaneletLayer&) = delete; + LaneletLayer operator=(LaneletLayer&) = delete; + Lanelets findUsages(const RegulatoryElementConstPtr& regElem); + ConstLanelets findUsages(const RegulatoryElementConstPtr& regElem) const; + <...> +}; +``` + +Each `PrimitiveLayer` owns a field named `tree_` that contains a lookup table named `usage` of type `UsageLookup`, + +```cpp title="lanelet2_core/LaneletMap.h#L38-L253" +template +class PrimitiveLayer { + public: + <...> + /** + * @brief finds usages of an owned type within this layer + * + * This is the non-const version to find usages of a primitive in a layer. + */ + std::vector findUsages(const traits::ConstPrimitiveType>& primitive); + <...> + struct Tree; + // NOLINTNEXTLINE + std::unique_ptr tree_; //!< Hides boost trees from you/the compiler +``` + +```cpp title="lanelet2_core/src/LaneletMap.cpp#L277-L308" +template +struct PrimitiveLayer::Tree { + using TreeNode = std::pair; + using RTree = bgi::rtree>; + static TreeNode treeNode(const T& elem) { return {geometry::boundingBox2d(to2D(elem)), elem}; } + <...> + RTree rTree; + UsageLookup usage; +}; +``` + +and `UsageLookup` contains reference relation between different types as `std::unordered_multimap`. + +```cpp title="lanelet2_core/src/LaneletMap.cpp#L259-L270" +template <> +struct UsageLookup { + void add(Lanelet ll) { + ownedLookup.insert(std::make_pair(ll.leftBound(), ll)); + ownedLookup.insert(std::make_pair(ll.rightBound(), ll)); + for (const auto& elem : ll.regulatoryElements()) { + regElemLookup.insert(std::make_pair(elem, ll)); + } + } + std::unordered_multimap ownedLookup; + std::unordered_multimap regElemLookup; +}; +``` + +Thus the complexity of `findUsage` function is equal to that of `std::unordered_multimap::equal_range` which is $O(1)$. + +```cpp title="lanelet2_core/src/LaneletMap.cpp#L419-L424" +template +std::vector::ConstPrimitiveT> PrimitiveLayer::findUsages( + const traits::ConstPrimitiveType::PrimitiveT>>& primitive) const { + return forEachMatchInMultiMap::PrimitiveT>>( + tree_->usage.ownedLookup, primitive, [](const auto& elem) { return traits::toConst(elem.second); }); +} +``` + +```cpp title="lanelet2_core/src/LaneletMap.cpp#L165-L169" +template +std::vector forEachMatchInMultiMap(const MapT& map, const KeyT& key, Func&& f) { + auto range = map.equal_range(key); + return utils::transform(range.first, range.second, f); +} +``` + +## Test maps + +All of the maps are in `MGRS` coordinate. In each map, an anchor point is set to an origin point $(100.0, 100.0)$ for simplicity. + +| Map name | Origin point id | Image | +| --------------------------- | --------------- | --------------------------------------------------- | +| `road_shoulder/highway.osm` | `1` | ![highway](./media/maps/road_shoulder/highway.png) | +| `road_shoulder/pudo.osm` | `140` | ![pudo](./media/maps/road_shoulder/pudo.png) | +| `intersection/crossing.osm` | `1791` | ![crossing](./media/maps/intersection/crossing.png) | + +### How to craft test map + +On the VMB, create the map in MGRS system and save the file as ``. Next, select the point to set as origin, get its `` and run + +```bash +ros2 run autoware_lanelet2_utility lanelet_anonymizer.py +``` + +Then the coordinate of the specified point is (100, 100) on the loaded map(NOTE: not exactly (0, 0) because MGRS does not any point to have negative coordinate value). + +By applying `lanelet_id_aligner.py`, the primitive ids are aligned to start from 1 and increase one-by-one. + +```bash +ros2 run autoware_lanelet2_utility lanelet_id_aligner.py +``` diff --git a/common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/kind.hpp b/common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/kind.hpp new file mode 100644 index 000000000..974a5623d --- /dev/null +++ b/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 + +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_ diff --git a/common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/topology.hpp b/common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/topology.hpp new file mode 100644 index 000000000..1fab4540e --- /dev/null +++ b/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 +#include +#include + +#include +#include + +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 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 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 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 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 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 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 leftmost_lanelet( + const lanelet::ConstLanelet & lanelet, + const lanelet::routing::RoutingGraphConstPtr routing_graph); + +std::optional 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 & ids); +} // namespace autoware::lanelet2_utility + +#endif // AUTOWARE_LANELET2_UTILITY__TOPOLOGY_HPP_ diff --git a/common/autoware_lanelet2_utility/media/api/left_lanelet.drawio.svg b/common/autoware_lanelet2_utility/media/api/left_lanelet.drawio.svg new file mode 100644 index 000000000..4d61a038c --- /dev/null +++ b/common/autoware_lanelet2_utility/media/api/left_lanelet.drawio.svg @@ -0,0 +1,648 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + shoulder lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + bicycle lane + + + + + + +
+
+
+ + (1) + +
+
+
+ + (1) + + + + + + +
+
+
+ + (2) + +
+
+
+ + (2) + + + + + + +
+
+
+ + (3) + +
+
+
+ + (3) + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/api/left_lanelets.drawio.svg b/common/autoware_lanelet2_utility/media/api/left_lanelets.drawio.svg new file mode 100644 index 000000000..5eac8f02d --- /dev/null +++ b/common/autoware_lanelet2_utility/media/api/left_lanelets.drawio.svg @@ -0,0 +1,1213 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + shoulder lane + + + + + + + bicycle lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ + (3) + +
+
+
+ + (3) + + + + + + +
+
+
+ + (1) + +
+
+
+ + (1) + + + + + + +
+
+
+ + (2) + +
+
+
+ + (2) + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ + include_opposite = false (default) + +
+
+
+ + include_opposite = false (default) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + shoulder lane + + + + + + + bicycle lane + + + + + +
+
+
+ + (5) + +
+
+
+ + (5) + + + + + + +
+
+
+ + (4) + +
+
+
+ + (4) + + + + + + + +
+
+
+ + include_opposite = true + +
+
+
+ + include_opposite = true + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/api/leftmost_lanelet.drawio.svg b/common/autoware_lanelet2_utility/media/api/leftmost_lanelet.drawio.svg new file mode 100644 index 000000000..3b046687e --- /dev/null +++ b/common/autoware_lanelet2_utility/media/api/leftmost_lanelet.drawio.svg @@ -0,0 +1,629 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + shoulder lane + + + + + + + bicycle lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + (1) + +
+
+
+ + (1) + + + + + + +
+
+
+ + (2) + +
+
+
+ + (2) + + + + + + +
+
+
+ + (3) + +
+
+
+ + (3) + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/api/right_lanelets.drawio.svg b/common/autoware_lanelet2_utility/media/api/right_lanelets.drawio.svg new file mode 100644 index 000000000..318269020 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/api/right_lanelets.drawio.svg @@ -0,0 +1,803 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + bicycle lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ + (1) + +
+
+
+ + (1) + + + + + + +
+
+
+ + (2) + +
+
+
+ + (2) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + (3) + +
+
+
+ + (3) + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + shoulder lane + + + + + +
+
+
+ + (4) + +
+
+
+ + (4) + + + + + + + +
+
+
+ + include_opposite = false (default) + +
+
+
+ + include_opposite = false (default) + + + + + + +
+
+
+ + include_opposite = true + +
+
+
+ + include_opposite = true + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/api/right_opposite_lanelet.drawio.svg b/common/autoware_lanelet2_utility/media/api/right_opposite_lanelet.drawio.svg new file mode 100644 index 000000000..98c63bf58 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/api/right_opposite_lanelet.drawio.svg @@ -0,0 +1,640 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + shoulder lane + + + + + + + bicycle lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + (1) + +
+
+
+ + (1) + + + + + + +
+
+
+ + (2) + +
+
+
+ + (2) + + + + + + +
+
+
+ + (3) + +
+
+
+ + (3) + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/api/rightmost_lanelet.drawio.svg b/common/autoware_lanelet2_utility/media/api/rightmost_lanelet.drawio.svg new file mode 100644 index 000000000..02bece1c1 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/api/rightmost_lanelet.drawio.svg @@ -0,0 +1,435 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + bicycle lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + (1) + +
+
+
+ + (1) + + + + + + +
+
+
+ + (2) + +
+
+
+ + (2) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + (3) + +
+
+
+ + (3) + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/maps/intersection/crossing.png b/common/autoware_lanelet2_utility/media/maps/intersection/crossing.png new file mode 100644 index 000000000..def5a0d22 Binary files /dev/null and b/common/autoware_lanelet2_utility/media/maps/intersection/crossing.png differ diff --git a/common/autoware_lanelet2_utility/media/maps/road_shoulder/highway.png b/common/autoware_lanelet2_utility/media/maps/road_shoulder/highway.png new file mode 100644 index 000000000..04c6a4f39 Binary files /dev/null and b/common/autoware_lanelet2_utility/media/maps/road_shoulder/highway.png differ diff --git a/common/autoware_lanelet2_utility/media/maps/road_shoulder/pudo.png b/common/autoware_lanelet2_utility/media/maps/road_shoulder/pudo.png new file mode 100644 index 000000000..4f800f7e9 Binary files /dev/null and b/common/autoware_lanelet2_utility/media/maps/road_shoulder/pudo.png differ diff --git a/common/autoware_lanelet2_utility/media/nomenclature/adjacent.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/adjacent.drawio.svg new file mode 100644 index 000000000..82f2bd174 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/adjacent.drawio.svg @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/boundary_entry_exit.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/boundary_entry_exit.drawio.svg new file mode 100644 index 000000000..9ef3ceff7 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/boundary_entry_exit.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + +
+
+
+ boundary +
+
+
+ + boundary + + + + + + + +
+
+
+ exit +
+
+
+ + exit + + + + + + + +
+
+
+ entry +
+
+
+ + entry + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/bundle.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/bundle.drawio.svg new file mode 100644 index 000000000..dfcb29f47 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/bundle.drawio.svg @@ -0,0 +1,506 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/connected.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/connected.drawio.svg new file mode 100644 index 000000000..8d6bfd6b1 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/connected.drawio.svg @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ B +
+
+
+ + B + + + + + + +
+
+
+ A +
+
+
+ + A + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/driving.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/driving.drawio.svg new file mode 100644 index 000000000..3bc1bbac7 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/driving.drawio.svg @@ -0,0 +1,424 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/following.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/following.drawio.svg new file mode 100644 index 000000000..603257410 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/following.drawio.svg @@ -0,0 +1,604 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/merging.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/merging.drawio.svg new file mode 100644 index 000000000..1dbee5f82 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/merging.drawio.svg @@ -0,0 +1,289 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/nomenclature.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/nomenclature.drawio.svg new file mode 100644 index 000000000..dca28ee2d --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/nomenclature.drawio.svg @@ -0,0 +1,693 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/opposite.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/opposite.drawio.svg new file mode 100644 index 000000000..d5fb01c5e --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/opposite.drawio.svg @@ -0,0 +1,411 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/opposite_direction.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/opposite_direction.drawio.svg new file mode 100644 index 000000000..b304541b5 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/opposite_direction.drawio.svg @@ -0,0 +1,616 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/previous.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/previous.drawio.svg new file mode 100644 index 000000000..555128ece --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/previous.drawio.svg @@ -0,0 +1,481 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/same_direction.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/same_direction.drawio.svg new file mode 100644 index 000000000..552358977 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/same_direction.drawio.svg @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/sequence.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/sequence.drawio.svg new file mode 100644 index 000000000..dfca466e7 --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/sequence.drawio.svg @@ -0,0 +1,562 @@ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + Text is not SVG - cannot display + + + diff --git a/common/autoware_lanelet2_utility/media/nomenclature/sibling.drawio.svg b/common/autoware_lanelet2_utility/media/nomenclature/sibling.drawio.svg new file mode 100644 index 000000000..88794ff6a --- /dev/null +++ b/common/autoware_lanelet2_utility/media/nomenclature/sibling.drawio.svg @@ -0,0 +1,603 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/package.xml b/common/autoware_lanelet2_utility/package.xml new file mode 100644 index 000000000..38d3e7b2e --- /dev/null +++ b/common/autoware_lanelet2_utility/package.xml @@ -0,0 +1,27 @@ + + + + autoware_lanelet2_utility + 0.0.0 + The autoware_lanelet2_utility package + Mamoru Sobue + Kosuke Takeuchi + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + autoware_lanelet2_extension + range-v3 + rclcpp + + ament_cmake_ros + ament_index_cpp + autoware_pyplot + + + ament_cmake + + diff --git a/common/autoware_lanelet2_utility/sample_map/intersection/crossing.osm b/common/autoware_lanelet2_utility/sample_map/intersection/crossing.osm new file mode 100644 index 000000000..383c4e489 --- /dev/null +++ b/common/autoware_lanelet2_utility/sample_map/intersection/crossing.osm @@ -0,0 +1,14523 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/sample_map/intersection/crossing_inverse.osm b/common/autoware_lanelet2_utility/sample_map/intersection/crossing_inverse.osm new file mode 100644 index 000000000..17eca88dc --- /dev/null +++ b/common/autoware_lanelet2_utility/sample_map/intersection/crossing_inverse.osm @@ -0,0 +1,12470 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/sample_map/road_shoulder/highway.osm b/common/autoware_lanelet2_utility/sample_map/road_shoulder/highway.osm new file mode 100644 index 000000000..c65d925bb --- /dev/null +++ b/common/autoware_lanelet2_utility/sample_map/road_shoulder/highway.osm @@ -0,0 +1,362 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/sample_map/road_shoulder/pudo.osm b/common/autoware_lanelet2_utility/sample_map/road_shoulder/pudo.osm new file mode 100644 index 000000000..65a63f400 --- /dev/null +++ b/common/autoware_lanelet2_utility/sample_map/road_shoulder/pudo.osm @@ -0,0 +1,3106 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_lanelet2_utility/scripts/lanelet_anonymizer.py b/common/autoware_lanelet2_utility/scripts/lanelet_anonymizer.py new file mode 100755 index 000000000..06854c935 --- /dev/null +++ b/common/autoware_lanelet2_utility/scripts/lanelet_anonymizer.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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. + +import argparse +import xml.etree.ElementTree as ET + +origin_x = 100.0 +origin_y = 100.0 + + +def update_osm_latlon(osm_file, output_file, origin_id): + tree = ET.parse(osm_file) + root = tree.getroot() + + old_origin_local_xy = None + + for node in root.findall("node"): + local_x_tag = node.find(".//tag[@k='local_x']") + local_y_tag = node.find(".//tag[@k='local_y']") + + if node.attrib["id"] == str(origin_id): + old_origin_local_xy = (float(local_x_tag.attrib["v"]), float(local_y_tag.attrib["v"])) + local_x_tag.set("v", str(origin_x)) + local_y_tag.set("v", str(origin_y)) + break + + if old_origin_local_xy is None: + print(f"could not find point of id {origin_id}") + return + + (old_origin_x, old_origin_y) = old_origin_local_xy + for node in root.findall("node"): + # make origin value exactly (0.0, 0.0) + if node.attrib["id"] == str(origin_id): + continue + + local_x_tag = node.find(".//tag[@k='local_x']") + local_y_tag = node.find(".//tag[@k='local_y']") + + local_x = float(local_x_tag.attrib["v"]) + local_y = float(local_y_tag.attrib["v"]) + adj_local_x = local_x - old_origin_x + adj_local_y = local_y - old_origin_y + + local_x_tag.set("v", str(origin_x + adj_local_x)) + local_y_tag.set("v", str(origin_y + adj_local_y)) + + tree.write(output_file, encoding="UTF-8", xml_declaration=True) + print(f"Updated OSM file created: {output_file}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Update OSM file with new origin and adjusted coordinates." + ) + parser.add_argument("input_osm", help="Path to the input OSM file") + parser.add_argument("output_osm", help="Path to the output OSM file") + parser.add_argument("origin_id", help="id of the point to reset as origin", type=int) + args = parser.parse_args() + + update_osm_latlon(args.input_osm, args.output_osm, args.origin_id) diff --git a/common/autoware_lanelet2_utility/scripts/lanelet_id_aligner.py b/common/autoware_lanelet2_utility/scripts/lanelet_id_aligner.py new file mode 100755 index 000000000..ca19aaacb --- /dev/null +++ b/common/autoware_lanelet2_utility/scripts/lanelet_id_aligner.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +# 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. + +import argparse +import xml.etree.ElementTree as ET + + +def renumber_osm_ids(input_file): + tree = ET.parse(input_file) + root = tree.getroot() + + id_map = {} + new_id = 1 + + # Collect nodes, ways, and relations with new ids + for element in root.findall("node") + root.findall("way") + root.findall("relation"): + old_id = element.attrib["id"] + id_map[old_id] = str(new_id) + element.set("id", str(new_id)) + new_id += 1 + + # Update references in (inside ) and (inside ) + for way in root.findall("way"): + for nd in way.findall("nd"): + old_ref = nd.attrib["ref"] + if old_ref in id_map: + nd.set("ref", id_map[old_ref]) + else: + print(f"reference to ref={old_ref} was invalid") + + for relation in root.findall("relation"): + for member in relation.findall("member"): + old_ref = member.attrib["ref"] + if old_ref in id_map: + member.set("ref", id_map[old_ref]) + else: + print(f"reference to ref={old_ref} was invalid") + + for tag in relation.findall("tag"): + if tag.attrib["k"] != "intersection_area": + continue + + old_ref = tag.attrib["v"] + if old_ref in id_map: + tag.set("k", id_map[old_ref]) + else: + print(f"reference to ref={old_ref} was invalid") + + tree.write(input_file, encoding="utf-8", xml_declaration=True) + print(f"Updated OSM file {input_file}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Update OSM file with new origin and adjusted coordinates." + ) + parser.add_argument("input_osm", help="Path to the input OSM file") + args = parser.parse_args() + renumber_osm_ids(args.input_osm) diff --git a/common/autoware_lanelet2_utility/src/kind.cpp b/common/autoware_lanelet2_utility/src/kind.cpp new file mode 100644 index 000000000..d369a22ef --- /dev/null +++ b/common/autoware_lanelet2_utility/src/kind.cpp @@ -0,0 +1,38 @@ +// 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 + +#include + +namespace autoware::lanelet2_utility +{ +bool is_road_lane(const lanelet::ConstLanelet & lanelet) +{ + return strcmp(lanelet.attributeOr(lanelet::AttributeName::Subtype, "none"), k_road_lane_type) == + 0; +} + +bool is_shoulder_lane(const lanelet::ConstLanelet & lanelet) +{ + return strcmp( + lanelet.attributeOr(lanelet::AttributeName::Subtype, "none"), k_shoulder_lane_type) == 0; +} + +bool is_bicycle_lane(const lanelet::ConstLanelet & lanelet) +{ + return strcmp( + lanelet.attributeOr(lanelet::AttributeName::Subtype, "none"), k_bicycle_lane_type) == 0; +} +} // namespace autoware::lanelet2_utility diff --git a/common/autoware_lanelet2_utility/src/topology.cpp b/common/autoware_lanelet2_utility/src/topology.cpp new file mode 100644 index 000000000..164b1e118 --- /dev/null +++ b/common/autoware_lanelet2_utility/src/topology.cpp @@ -0,0 +1,265 @@ +// 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 +#include +#include + +#include +#include + +#include + +namespace autoware::lanelet2_utility +{ + +static constexpr size_t k_normal_bundle_max_size = 10; + +lanelet::routing::RoutingGraphConstPtr instantiate_routing_graph( + lanelet::LaneletMapConstPtr lanelet_map, const char * location, const char * participant) +{ + const auto traffic_rules = + lanelet::traffic_rules::TrafficRulesFactory::create(location, participant); + return lanelet::routing::RoutingGraph::build(*lanelet_map, *traffic_rules); +} + +std::optional left_lanelet( + const lanelet::ConstLanelet & lanelet, const lanelet::routing::RoutingGraphConstPtr routing_graph) +{ + if (const auto left_lane = routing_graph->left(lanelet)) { + // lane changeable + return *left_lane; + } + if (const auto adjacent_left_lane = routing_graph->adjacentLeft(lanelet)) { + return *adjacent_left_lane; + } + return std::nullopt; +} + +std::optional right_lanelet( + const lanelet::ConstLanelet & lanelet, const lanelet::routing::RoutingGraphConstPtr routing_graph) +{ + if (const auto right_lane = routing_graph->right(lanelet)) { + // lane changeable + return *right_lane; + } + if (const auto adjacent_right_lane = routing_graph->adjacentRight(lanelet)) { + return *adjacent_right_lane; + } + return std::nullopt; +} + +/* +std::optional left_similar_lanelet( +const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, +const lanelet::routing::RoutingGraphConstPtr routing_graph); +*/ + +/* +std::optional right_similar_lanelet( +const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, +const lanelet::routing::RoutingGraphConstPtr routing_graph); +*/ + +std::optional left_opposite_lanelet( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map) +{ + for (const auto & opposite_candidate : + lanelet_map->laneletLayer.findUsages(lanelet.leftBound().invert())) { + return opposite_candidate; + } + return std::nullopt; +} + +std::optional right_opposite_lanelet( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map) +{ + for (const auto & opposite_candidate : + lanelet_map->laneletLayer.findUsages(lanelet.rightBound().invert())) { + return opposite_candidate; + } + return std::nullopt; +} + +std::optional leftmost_lanelet( + const lanelet::ConstLanelet & lanelet, const lanelet::routing::RoutingGraphConstPtr routing_graph) +{ + auto left_lane = left_lanelet(lanelet, routing_graph); + if (!left_lane) { + return std::nullopt; + } + size_t bundle_size_diagnosis = 0; + while (bundle_size_diagnosis < k_normal_bundle_max_size) { + const auto next_left_lane = left_lanelet(left_lane.value(), routing_graph); + if (!next_left_lane) { + // reached + return left_lane; + } + left_lane = next_left_lane.value(); + bundle_size_diagnosis++; + } + + // LCOV_EXCL_START + RCLCPP_ERROR( + rclcpp::get_logger("autoware_lanelet2_utility"), + "You have passed an unrealistic map with a bundle of size>=10"); + return std::nullopt; + // LCOV_EXCL_STOP +} + +std::optional rightmost_lanelet( + const lanelet::ConstLanelet & lanelet, const lanelet::routing::RoutingGraphConstPtr routing_graph) +{ + auto right_lane = right_lanelet(lanelet, routing_graph); + if (!right_lane) { + return std::nullopt; + } + size_t bundle_size_diagnosis = 0; + while (bundle_size_diagnosis < k_normal_bundle_max_size) { + const auto next_right_lane = right_lanelet(right_lane.value(), routing_graph); + if (!next_right_lane) { + // reached + return right_lane; + } + right_lane = next_right_lane.value(); + bundle_size_diagnosis++; + } + + // LCOV_EXCL_START + RCLCPP_ERROR( + rclcpp::get_logger("autoware_lanelet2_utility"), + "You have passed an unrealistic map with a bundle of size>=10"); + return std::nullopt; + // LCOV_EXCL_STOP +} + +lanelet::ConstLanelets left_lanelets( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, + const lanelet::routing::RoutingGraphConstPtr routing_graph, const bool include_opposite, + const bool invert_opposite_lane) +{ + lanelet::ConstLanelets lefts{}; + auto left_lane = left_lanelet(lanelet, routing_graph); + size_t bundle_size_diagnosis = 0; + while (bundle_size_diagnosis < k_normal_bundle_max_size) { + if (!left_lane) { + break; + } + lefts.push_back(left_lane.value()); + left_lane = left_lanelet(left_lane.value(), routing_graph); + bundle_size_diagnosis++; + } + // LCOV_EXCL_START + if (bundle_size_diagnosis >= k_normal_bundle_max_size) { + RCLCPP_ERROR( + rclcpp::get_logger("autoware_lanelet2_utility"), + "You have passed an unrealistic map with a bundle of size>=10"); + return {}; + } + // LCOV_EXCL_STOP + if (lefts.empty()) { + return {}; + } + const auto & leftmost = lefts.back(); + if (include_opposite) { + const auto direct_opposite = left_opposite_lanelet(leftmost, lanelet_map); + if (direct_opposite) { + const auto opposites = + right_lanelets(direct_opposite.value(), lanelet_map, routing_graph, false, false); + lefts.push_back(direct_opposite.value()); + for (const auto & opposite : opposites) { + lefts.push_back(invert_opposite_lane ? opposite.invert() : opposite); + } + } + } + return lefts; +} + +lanelet::ConstLanelets right_lanelets( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, + const lanelet::routing::RoutingGraphConstPtr routing_graph, const bool include_opposite, + const bool invert_opposite_lane) +{ + lanelet::ConstLanelets rights{}; + auto right_lane = right_lanelet(lanelet, routing_graph); + size_t bundle_size_diagnosis = 0; + while (bundle_size_diagnosis < k_normal_bundle_max_size) { + if (!right_lane) { + break; + } + rights.push_back(right_lane.value()); + right_lane = right_lanelet(right_lane.value(), routing_graph); + bundle_size_diagnosis++; + } + // LCOV_EXCL_START + if (bundle_size_diagnosis >= k_normal_bundle_max_size) { + RCLCPP_ERROR( + rclcpp::get_logger("autoware_lanelet2_utility"), + "You have passed an unrealistic map with a bundle of size>=10"); + return {}; + } + // LCOV_EXCL_STOP + if (rights.empty()) { + return {}; + } + const auto & rightmost = rights.back(); + if (include_opposite) { + const auto direct_opposite = right_opposite_lanelet(rightmost, lanelet_map); + if (direct_opposite) { + const auto opposites = + left_lanelets(direct_opposite.value(), lanelet_map, routing_graph, false, false); + rights.push_back(direct_opposite.value()); + for (const auto & opposite : opposites) { + rights.push_back(invert_opposite_lane ? opposite.invert() : opposite); + } + } + } + return rights; +} + +lanelet::ConstLanelets following_lanelets( + const lanelet::ConstLanelet & lanelet, const lanelet::routing::RoutingGraphConstPtr routing_graph) +{ + return routing_graph->following(lanelet); +} + +lanelet::ConstLanelets previous_lanelets( + const lanelet::ConstLanelet & lanelet, const lanelet::routing::RoutingGraphConstPtr routing_graph) +{ + return routing_graph->previous(lanelet); +} + +lanelet::ConstLanelets sibling_lanelets( + const lanelet::ConstLanelet & lanelet, const lanelet::routing::RoutingGraphConstPtr routing_graph) +{ + lanelet::ConstLanelets siblings; + for (const auto & previous : previous_lanelets(lanelet, routing_graph)) { + for (const auto & following : following_lanelets(previous, routing_graph)) { + if (following.id() != lanelet.id()) { + siblings.push_back(following); + } + } + } + return siblings; +} + +lanelet::ConstLanelets from_ids( + const lanelet::LaneletMapConstPtr lanelet_map, const std::vector & ids) +{ + return ids | ranges::views::transform([&](const auto id) { + return lanelet_map->laneletLayer.get(id); + }) | + ranges::to(); +} +} // namespace autoware::lanelet2_utility diff --git a/common/autoware_lanelet2_utility/test/kind.cpp b/common/autoware_lanelet2_utility/test/kind.cpp new file mode 100644 index 000000000..d9c66898a --- /dev/null +++ b/common/autoware_lanelet2_utility/test/kind.cpp @@ -0,0 +1,97 @@ +// 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 +#include + +#include + +#include +#include + +namespace fs = std::filesystem; + +namespace autoware +{ +class TestWithRoadShoulderHighwayMap : 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 road_shoulder_highway_map_path = sample_map_dir / "road_shoulder" / "highway.osm"; + + lanelet_map_ptr_ = load_mgrs_coordinate_map(road_shoulder_highway_map_path.string()); + } +}; + +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 road_shoulder_highway_map_path = sample_map_dir / "intersection" / "crossing.osm"; + + lanelet_map_ptr_ = load_mgrs_coordinate_map(road_shoulder_highway_map_path.string()); + } +}; + +TEST_F(TestWithRoadShoulderHighwayMap, LoadCheck) +{ + const auto point = lanelet_map_ptr_->pointLayer.get(1); + EXPECT_NEAR(point.x(), 100.0, 0.05); + EXPECT_NEAR(point.y(), 100.0, 0.05); +} + +TEST_F(TestWithRoadShoulderHighwayMap, is_road_lane) +{ + const auto ll = lanelet_map_ptr_->laneletLayer.get(46); + EXPECT_EQ(lanelet2_utility::is_road_lane(ll), true); + EXPECT_EQ(lanelet2_utility::is_shoulder_lane(ll), false); + EXPECT_EQ(lanelet2_utility::is_bicycle_lane(ll), false); +} + +TEST_F(TestWithRoadShoulderHighwayMap, is_shoulder_lane) +{ + const auto ll = lanelet_map_ptr_->laneletLayer.get(47); + EXPECT_EQ(lanelet2_utility::is_road_lane(ll), false); + EXPECT_EQ(lanelet2_utility::is_shoulder_lane(ll), true); + EXPECT_EQ(lanelet2_utility::is_bicycle_lane(ll), false); +} + +TEST_F(TestWithIntersectionCrossingMap, is_shoulder_lane) +{ + const auto ll = lanelet_map_ptr_->laneletLayer.get(2303); + EXPECT_EQ(lanelet2_utility::is_road_lane(ll), false); + EXPECT_EQ(lanelet2_utility::is_shoulder_lane(ll), false); + EXPECT_EQ(lanelet2_utility::is_bicycle_lane(ll), true); +} +} // namespace autoware + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/autoware_lanelet2_utility/test/map_loader.hpp b/common/autoware_lanelet2_utility/test/map_loader.hpp new file mode 100644 index 000000000..d173b583b --- /dev/null +++ b/common/autoware_lanelet2_utility/test/map_loader.hpp @@ -0,0 +1,35 @@ +// 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 MAP_LOADER_HPP_ +#define MAP_LOADER_HPP_ + +#include + +#include +#include +#include + +#include +#include + +inline lanelet::LaneletMapConstPtr load_mgrs_coordinate_map(const std::string & path) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector; + auto lanelet_map_ptr_mut = lanelet::load(path, projector, &errors); + return lanelet::LaneletMapConstPtr{std::move(lanelet_map_ptr_mut)}; +} + +#endif // MAP_LOADER_HPP_ diff --git a/common/autoware_lanelet2_utility/test/topology.cpp b/common/autoware_lanelet2_utility/test/topology.cpp new file mode 100644 index 000000000..0f57bb876 --- /dev/null +++ b/common/autoware_lanelet2_utility/test/topology.cpp @@ -0,0 +1,298 @@ +// 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 +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace fs = std::filesystem; + +namespace autoware +{ + +class TestWithIntersectionCrossingMap : public ::testing::Test +{ +protected: + lanelet::LaneletMapConstPtr lanelet_map_ptr_{nullptr}; + lanelet::routing::RoutingGraphConstPtr routing_graph_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()); + routing_graph_ptr_ = lanelet2_utility::instantiate_routing_graph(lanelet_map_ptr_); + } +}; + +TEST_F(TestWithIntersectionCrossingMap, LoadCheck) +{ + const auto point = lanelet_map_ptr_->pointLayer.get(1791); + EXPECT_NEAR(point.x(), 100.0, 0.05); + EXPECT_NEAR(point.y(), 100.0, 0.05); +} + +TEST_F(TestWithIntersectionCrossingMap, shoulder_lane_is_inaccessible_on_routing_graph) +{ + const auto lane = + lanelet2_utility::left_lanelet(lanelet_map_ptr_->laneletLayer.get(2257), routing_graph_ptr_); + EXPECT_EQ(lane.has_value(), false); +} + +TEST_F(TestWithIntersectionCrossingMap, bicycle_lane_is_inaccessible_on_routing_graph) +{ + const auto lane = + lanelet2_utility::left_lanelet(lanelet_map_ptr_->laneletLayer.get(2286), routing_graph_ptr_); + EXPECT_EQ(lane.has_value(), false); +} + +TEST_F(TestWithIntersectionCrossingMap, left_lanelet_without_lc_permission) +{ + const auto lane = + lanelet2_utility::left_lanelet(lanelet_map_ptr_->laneletLayer.get(2246), routing_graph_ptr_); + EXPECT_EQ(lane.value().id(), 2245); +} + +TEST_F(TestWithIntersectionCrossingMap, left_lanelet_with_lc_permission) +{ + const auto lane = + lanelet2_utility::left_lanelet(lanelet_map_ptr_->laneletLayer.get(2245), routing_graph_ptr_); + EXPECT_EQ(lane.value().id(), 2244); +} + +TEST_F(TestWithIntersectionCrossingMap, right_lanelet_without_lc_permission) +{ + const auto lane = + lanelet2_utility::right_lanelet(lanelet_map_ptr_->laneletLayer.get(2245), routing_graph_ptr_); + EXPECT_EQ(lane.value().id(), 2246); +} + +TEST_F(TestWithIntersectionCrossingMap, right_lanelet_with_lc_permission) +{ + const auto lane = + lanelet2_utility::right_lanelet(lanelet_map_ptr_->laneletLayer.get(2244), routing_graph_ptr_); + EXPECT_EQ(lane.value().id(), 2245); +} + +TEST_F(TestWithIntersectionCrossingMap, right_opposite_lanelet_valid) +{ + const auto lane = lanelet2_utility::right_opposite_lanelet( + lanelet_map_ptr_->laneletLayer.get(2288), lanelet_map_ptr_); + EXPECT_EQ(lane.value().id(), 2311); +} + +TEST_F(TestWithIntersectionCrossingMap, right_opposite_lanelet_null) +{ + const auto lane = lanelet2_utility::right_opposite_lanelet( + lanelet_map_ptr_->laneletLayer.get(2260), lanelet_map_ptr_); + EXPECT_EQ(lane.has_value(), false); +} + +TEST_F(TestWithIntersectionCrossingMap, leftmost_lanelet_valid) +{ + const auto lane = lanelet2_utility::leftmost_lanelet( + lanelet_map_ptr_->laneletLayer.get(2288), routing_graph_ptr_); + EXPECT_EQ(lane.value().id(), 2286); +} + +TEST_F(TestWithIntersectionCrossingMap, leftmost_lanelet_null) +{ + const auto lane = lanelet2_utility::leftmost_lanelet( + lanelet_map_ptr_->laneletLayer.get(2286), routing_graph_ptr_); + EXPECT_EQ(lane.has_value(), false); +} + +TEST_F(TestWithIntersectionCrossingMap, rightmost_lanelet_valid) +{ + const auto lane = lanelet2_utility::rightmost_lanelet( + lanelet_map_ptr_->laneletLayer.get(2286), routing_graph_ptr_); + EXPECT_EQ(lane.value().id(), 2288); +} + +TEST_F(TestWithIntersectionCrossingMap, rightmost_lanelet_null) +{ + const auto lane = lanelet2_utility::rightmost_lanelet( + lanelet_map_ptr_->laneletLayer.get(2288), routing_graph_ptr_); + EXPECT_EQ(lane.has_value(), false); +} + +TEST_F(TestWithIntersectionCrossingMap, left_lanelets_without_opposite) +{ + const auto lefts = lanelet2_utility::left_lanelets( + lanelet_map_ptr_->laneletLayer.get(2288), lanelet_map_ptr_, routing_graph_ptr_); + EXPECT_EQ(lefts.size(), 2); + EXPECT_EQ(lefts[0].id(), 2287); + EXPECT_EQ(lefts[1].id(), 2286); +} + +TEST_F(TestWithIntersectionCrossingMap, left_lanelets_without_opposite_empty) +{ + const auto lefts = lanelet2_utility::left_lanelets( + lanelet_map_ptr_->laneletLayer.get(2286), lanelet_map_ptr_, routing_graph_ptr_); + EXPECT_EQ(lefts.size(), 0); +} + +TEST_F(TestWithIntersectionCrossingMap, right_lanelets_without_opposite) +{ + const auto lefts = lanelet2_utility::right_lanelets( + lanelet_map_ptr_->laneletLayer.get(2286), lanelet_map_ptr_, routing_graph_ptr_); + EXPECT_EQ(lefts.size(), 2); + EXPECT_EQ(lefts[0].id(), 2287); + EXPECT_EQ(lefts[1].id(), 2288); +} + +TEST_F(TestWithIntersectionCrossingMap, right_lanelets_without_opposite_empty) +{ + const auto lefts = lanelet2_utility::right_lanelets( + lanelet_map_ptr_->laneletLayer.get(2288), lanelet_map_ptr_, routing_graph_ptr_); + EXPECT_EQ(lefts.size(), 0); +} + +TEST_F(TestWithIntersectionCrossingMap, right_lanelets_with_opposite) +{ + const auto rights = lanelet2_utility::right_lanelets( + lanelet_map_ptr_->laneletLayer.get(2286), lanelet_map_ptr_, routing_graph_ptr_, + true /* include opposite */); + EXPECT_EQ(rights.size(), 4); + EXPECT_EQ(rights[0].id(), 2287); + EXPECT_EQ(rights[1].id(), 2288); + EXPECT_EQ(rights[2].id(), 2311); + EXPECT_EQ(rights[3].id(), 2312); +} + +TEST_F(TestWithIntersectionCrossingMap, right_lanelets_with_opposite_without_actual_opposites) +{ + const auto rights = lanelet2_utility::right_lanelets( + lanelet_map_ptr_->laneletLayer.get(2259), lanelet_map_ptr_, routing_graph_ptr_, + true /* include opposite */); + EXPECT_EQ(rights.size(), 1); + EXPECT_EQ(rights[0].id(), 2260); +} + +TEST_F(TestWithIntersectionCrossingMap, following_lanelets) +{ + const auto following = lanelet2_utility::following_lanelets( + lanelet_map_ptr_->laneletLayer.get(2244), routing_graph_ptr_); + EXPECT_EQ(following.size(), 2); + const auto ids = following | ranges::views::transform([](const auto & l) { return l.id(); }) | + ranges::to(); + EXPECT_EQ(ids.find(2271) != ids.end(), true); + EXPECT_EQ(ids.find(2265) != ids.end(), true); +} + +TEST_F(TestWithIntersectionCrossingMap, previous_lanelets) +{ + const auto previous = lanelet2_utility::previous_lanelets( + lanelet_map_ptr_->laneletLayer.get(2249), routing_graph_ptr_); + EXPECT_EQ(previous.size(), 3); + const auto ids = previous | ranges::views::transform([](const auto & l) { return l.id(); }) | + ranges::to(); + EXPECT_EQ(ids.find(2283) != ids.end(), true); + EXPECT_EQ(ids.find(2265) != ids.end(), true); + EXPECT_EQ(ids.find(2270) != ids.end(), true); +} + +TEST_F(TestWithIntersectionCrossingMap, sibling_lanelets) +{ + const auto siblings = lanelet2_utility::sibling_lanelets( + lanelet_map_ptr_->laneletLayer.get(2273), routing_graph_ptr_); + const auto ids = siblings | ranges::views::transform([](const auto & l) { return l.id(); }) | + ranges::to(); + EXPECT_EQ(ids.find(2273) != ids.end(), false); + EXPECT_EQ(ids.find(2280) != ids.end(), true); + EXPECT_EQ(ids.find(2281) != ids.end(), true); +} + +TEST_F(TestWithIntersectionCrossingMap, from_ids) +{ + const auto lanelets = + lanelet2_utility::from_ids(lanelet_map_ptr_, std::vector({2296, 2286, 2270})); + EXPECT_EQ(lanelets.size(), 3); + EXPECT_EQ(lanelets[0].id(), 2296); + EXPECT_EQ(lanelets[1].id(), 2286); + EXPECT_EQ(lanelets[2].id(), 2270); +} + +class TestWithIntersectionCrossingInverseMap : public ::testing::Test +{ +protected: + lanelet::LaneletMapConstPtr lanelet_map_ptr_{nullptr}; + lanelet::routing::RoutingGraphConstPtr routing_graph_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_inverse.osm"; + + lanelet_map_ptr_ = load_mgrs_coordinate_map(intersection_crossing_map_path.string()); + routing_graph_ptr_ = lanelet2_utility::instantiate_routing_graph(lanelet_map_ptr_); + } +}; + +TEST_F(TestWithIntersectionCrossingInverseMap, left_opposite_lanelet_valid) +{ + const auto lane = lanelet2_utility::left_opposite_lanelet( + lanelet_map_ptr_->laneletLayer.get(2311), lanelet_map_ptr_); + EXPECT_EQ(lane.value().id(), 2288); +} + +TEST_F(TestWithIntersectionCrossingInverseMap, left_opposite_lanelet_null) +{ + const auto lane = lanelet2_utility::left_opposite_lanelet( + lanelet_map_ptr_->laneletLayer.get(2252), lanelet_map_ptr_); + EXPECT_EQ(lane.has_value(), false); +} + +TEST_F(TestWithIntersectionCrossingInverseMap, left_lanelets_with_opposite) +{ + const auto lefts = lanelet2_utility::left_lanelets( + lanelet_map_ptr_->laneletLayer.get(2312), lanelet_map_ptr_, routing_graph_ptr_, true); + EXPECT_EQ(lefts.size(), 4); + EXPECT_EQ(lefts[0].id(), 2311); + EXPECT_EQ(lefts[1].id(), 2288); + EXPECT_EQ(lefts[2].id(), 2287); + EXPECT_EQ(lefts[3].id(), 2286); +} + +TEST_F(TestWithIntersectionCrossingInverseMap, left_lanelets_with_opposite_without_actual_opposites) +{ + const auto lefts = lanelet2_utility::left_lanelets( + lanelet_map_ptr_->laneletLayer.get(2251), lanelet_map_ptr_, routing_graph_ptr_, true); + EXPECT_EQ(lefts.size(), 1); + EXPECT_EQ(lefts[0].id(), 2252); +} + +} // namespace autoware + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}