diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 341fff96ba..e1377ae27b 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -5,6 +5,7 @@ common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4 common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners +common/autoware_object_recognition_utils/** takayuki.murooka@tier4.jp yoshi.ri@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_point_types/** cynthia.liu@autocore.ai david.wong@tier4.jp max.schmeller@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml index b56718c00b..178bf29d17 100644 --- a/.github/workflows/build-and-test-daily.yaml +++ b/.github/workflows/build-and-test-daily.yaml @@ -8,7 +8,7 @@ on: jobs: build-and-test-daily: runs-on: ubuntu-24.04 - container: ghcr.io/autowarefoundation/autoware:core-devel + container: ghcr.io/autowarefoundation/autoware:core-common-devel steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 1615c218ff..62734cc0f4 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -26,7 +26,7 @@ jobs: needs: require-label if: ${{ needs.require-label.outputs.result == 'true' }} runs-on: ubuntu-24.04 - container: ghcr.io/autowarefoundation/autoware:core-devel + container: ghcr.io/autowarefoundation/autoware:core-common-devel steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" @@ -47,7 +47,7 @@ jobs: clang-tidy-differential: needs: build-and-test-differential runs-on: ubuntu-24.04 - container: ghcr.io/autowarefoundation/autoware:core-devel + container: ghcr.io/autowarefoundation/autoware:core-common-devel steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 7dab86f775..43bf8cbaaa 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -17,7 +17,7 @@ env: jobs: build-and-test: runs-on: ubuntu-24.04 - container: ghcr.io/autowarefoundation/autoware:core-devel + container: ghcr.io/autowarefoundation/autoware:core-common-devel steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/common/autoware_lanelet2_utility/CMakeLists.txt b/common/autoware_lanelet2_utility/CMakeLists.txt new file mode 100644 index 0000000000..fb51b57923 --- /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 0000000000..c8e344dae0 --- /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.[Open]({{ drawio("/common/autoware_lanelet2_utility/media/nomenclature/driving.drawio.svg") }}) | +| `boundary`,`entry`,`exit` | The `boundary` of a Lanelet refers to the left or right Linestring. | [Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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. [Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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. | [Open]({{ drawio("/common/autoware_lanelet2_utility/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`.[Open]({{ drawio("/common/autoware_lanelet2_utility/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`.[Open]({{ drawio("/common/autoware_lanelet2_utility/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`.[Open]({{ drawio("/common/autoware_lanelet2_utility/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`.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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.[Open]({{ drawio("/common/autoware_lanelet2_utility/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` |  | +| `road_shoulder/pudo.osm` | `140` |  | +| `intersection/crossing.osm` | `1791` |  | + +### 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 0000000000..974a5623d9 --- /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 0000000000..1fab4540e1 --- /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 0000000000..4d61a038c0 --- /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 0000000000..5eac8f02dd --- /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 0000000000..3b046687e3 --- /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 0000000000..318269020f --- /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 0000000000..98c63bf588 --- /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 0000000000..02bece1c1e --- /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 0000000000..def5a0d22d 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 0000000000..04c6a4f392 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 0000000000..4f800f7e98 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 0000000000..82f2bd1745 --- /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 0000000000..9ef3ceff7d --- /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 0000000000..dfcb29f47f --- /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 0000000000..8d6bfd6b1f --- /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 0000000000..3bc1bbac7e --- /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 0000000000..6032574100 --- /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 0000000000..1dbee5f827 --- /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 0000000000..dca28ee2dc --- /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 0000000000..d5fb01c5ed --- /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 0000000000..b304541b51 --- /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 0000000000..555128ecee --- /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 0000000000..5523589777 --- /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 0000000000..dfca466e74 --- /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 0000000000..88794ff6a0 --- /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 0000000000..38d3e7b2e6 --- /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 0000000000..383c4e489e --- /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 0000000000..17eca88dc1 --- /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 0000000000..c65d925bbc --- /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 0000000000..65a63f400a --- /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 0000000000..06854c9352 --- /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 0000000000..ca19aaacb9 --- /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 0000000000..d369a22ef2 --- /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 0000000000..164b1e118b --- /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 0000000000..d9c66898a3 --- /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 0000000000..d173b583bc --- /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 0000000000..0f57bb8767 --- /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(); +} diff --git a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_path_point.cpp similarity index 100% rename from common/autoware_trajectory/examples/example_trajectory_path_point.cpp rename to common/autoware_trajectory/examples/example_path_point.cpp diff --git a/common/autoware_trajectory/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_point.cpp similarity index 100% rename from common/autoware_trajectory/examples/example_trajectory_point.cpp rename to common/autoware_trajectory/examples/example_point.cpp diff --git a/common/autoware_trajectory/examples/example_trajectory_pose.cpp b/common/autoware_trajectory/examples/example_pose.cpp similarity index 100% rename from common/autoware_trajectory/examples/example_trajectory_pose.cpp rename to common/autoware_trajectory/examples/example_pose.cpp diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp index f4150b7e37..dd64a7d3eb 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp @@ -20,7 +20,9 @@ #include #include +#include #include +#include #include #include @@ -63,12 +65,14 @@ struct ImmutablePoint3d : ImmutablePoint2d MutablePoint3d to_point(geometry_msgs::msg::Point & p); MutablePoint3d to_point(geometry_msgs::msg::Pose & p); MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p); +MutablePoint3d to_point(autoware_planning_msgs::msg::TrajectoryPoint & p); MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); MutablePoint2d to_point(lanelet::BasicPoint2d & p); ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p); ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p); ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p); +ImmutablePoint3d to_point(const autoware_planning_msgs::msg::TrajectoryPoint & p); ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p); diff --git a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp new file mode 100644 index 0000000000..e507c933b8 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp @@ -0,0 +1,221 @@ +// Copyright 2024 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__TRAJECTORY__TRAJECTORY_POINT_HPP_ +#define AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_ + +#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/pose.hpp" + +#include + +#include +#include +#include + +namespace autoware::trajectory +{ +template <> +class Trajectory +: public Trajectory +{ + using BaseClass = Trajectory; + using PointType = autoware_planning_msgs::msg::TrajectoryPoint; + + std::shared_ptr> + longitudinal_velocity_mps_; //!< Longitudinal velocity in m/s + std::shared_ptr> + lateral_velocity_mps_; //!< Lateral velocity in m/s + std::shared_ptr> + heading_rate_rps_; //!< Heading rate in rad/s}; + std::shared_ptr> + acceleration_mps2_; //!< Longitudinal acceleration in m/s^2} Warning, this is not used + std::shared_ptr> + front_wheel_angle_rad_; //!< Front wheel angle in rad} Warning, this is not used + std::shared_ptr> + rear_wheel_angle_rad_; //!< Rear wheel angle in rad} Warning, this is not used + +public: + Trajectory(); + ~Trajectory() override = default; + Trajectory(const Trajectory & rhs); + Trajectory(Trajectory && rhs) = default; + Trajectory & operator=(const Trajectory & rhs); + Trajectory & operator=(Trajectory && rhs) = default; + + [[nodiscard]] std::vector get_internal_bases() const override; + + detail::InterpolatedArray & longitudinal_velocity_mps() + { + return *longitudinal_velocity_mps_; + } + + detail::InterpolatedArray & lateral_velocity_mps() { return *lateral_velocity_mps_; } + + detail::InterpolatedArray & heading_rate_rps() { return *heading_rate_rps_; } + + detail::InterpolatedArray & acceleration_mps2() { return *acceleration_mps2_; } + + detail::InterpolatedArray & front_wheel_angle_rad() { return *front_wheel_angle_rad_; } + + detail::InterpolatedArray & rear_wheel_angle_rad() { return *rear_wheel_angle_rad_; } + + [[nodiscard]] const detail::InterpolatedArray & longitudinal_velocity_mps() const + { + return *longitudinal_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & lateral_velocity_mps() const + { + return *lateral_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & heading_rate_rps() const + { + return *heading_rate_rps_; + } + + [[nodiscard]] const detail::InterpolatedArray & acceleration_mps2() const + { + return *acceleration_mps2_; + } + + [[nodiscard]] const detail::InterpolatedArray & front_wheel_angle_rad() const + { + return *front_wheel_angle_rad_; + } + + [[nodiscard]] const detail::InterpolatedArray & rear_wheel_angle_rad() const + { + return *rear_wheel_angle_rad_; + } + + /** + * @brief Build the trajectory from the points + * @param points Vector of points + * @return True if the build is successful + */ + bool build(const std::vector & points); + + /** + * @brief Compute the point on the trajectory at a given s value + * @param s Arc length + * @return Point on the trajectory + */ + [[nodiscard]] PointType compute(double s) const; + + /** + * @brief Restore the trajectory points + * @param min_points Minimum number of points + * @return Vector of points + */ + [[nodiscard]] std::vector restore(const size_t & min_points = 4) const; + + class Builder + { + private: + std::unique_ptr trajectory_; + + public: + Builder() : trajectory_(std::make_unique()) {} + + template + Builder & set_xy_interpolator(Args &&... args) + { + trajectory_->x_interpolator_ = + std::make_shared(std::forward(args)...); + trajectory_->y_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_z_interpolator(Args &&... args) + { + trajectory_->z_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_orientation_interpolator(Args &&... args) + { + trajectory_->orientation_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_longitudinal_velocity_interpolator(Args &&... args) + { + trajectory_->longitudinal_velocity_mps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_lateral_velocity_interpolator(Args &&... args) + { + trajectory_->lateral_velocity_mps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_heading_rate_interpolator(Args &&... args) + { + trajectory_->heading_rate_rps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_acceleration_interpolator(Args &&... args) + { + trajectory_->acceleration_mps2_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_front_wheel_angle_interpolator(Args &&... args) + { + trajectory_->front_wheel_angle_rad_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_rear_wheel_angle_interpolator(Args &&... args) + { + trajectory_->rear_wheel_angle_rad_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + std::optional build(const std::vector & points) + { + if (trajectory_->build(points)) { + auto result = std::make_optional(std::move(*trajectory_)); + trajectory_.reset(); + return result; + } + return std::nullopt; + } + }; +}; + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_ diff --git a/common/autoware_trajectory/src/detail/types.cpp b/common/autoware_trajectory/src/detail/types.cpp index 9c3174f608..dec4205f25 100644 --- a/common/autoware_trajectory/src/detail/types.cpp +++ b/common/autoware_trajectory/src/detail/types.cpp @@ -14,6 +14,8 @@ #include "autoware/trajectory/detail/types.hpp" +#include + namespace autoware::trajectory::detail { MutablePoint3d to_point(geometry_msgs::msg::Point & p) @@ -31,6 +33,11 @@ MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p) return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; } +MutablePoint3d to_point(autoware_planning_msgs::msg::TrajectoryPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; @@ -56,6 +63,11 @@ ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p) return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; } +ImmutablePoint3d to_point(const autoware_planning_msgs::msg::TrajectoryPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; diff --git a/common/autoware_trajectory/src/trajectory_point.cpp b/common/autoware_trajectory/src/trajectory_point.cpp new file mode 100644 index 0000000000..6cb49735db --- /dev/null +++ b/common/autoware_trajectory/src/trajectory_point.cpp @@ -0,0 +1,157 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/trajectory_point.hpp" + +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/forward.hpp" +#include "autoware/trajectory/interpolator/stairstep.hpp" +#include "autoware/trajectory/pose.hpp" + +#include + +#include +#include + +namespace autoware::trajectory +{ + +using PointType = autoware_planning_msgs::msg::TrajectoryPoint; + +Trajectory::Trajectory() +: longitudinal_velocity_mps_(std::make_shared>( + std::make_shared>())), + lateral_velocity_mps_(std::make_shared>( + std::make_shared>())), + heading_rate_rps_(std::make_shared>( + std::make_shared>())), + acceleration_mps2_(std::make_shared>( + std::make_shared>())), + front_wheel_angle_rad_(std::make_shared>( + std::make_shared>())), + rear_wheel_angle_rad_(std::make_shared>( + std::make_shared>())) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: BaseClass(rhs), + longitudinal_velocity_mps_( + std::make_shared>(*rhs.longitudinal_velocity_mps_)), + lateral_velocity_mps_( + std::make_shared>(*rhs.lateral_velocity_mps_)), + heading_rate_rps_(std::make_shared>(*rhs.heading_rate_rps_)), + acceleration_mps2_(std::make_shared>(*rhs.acceleration_mps2_)), + front_wheel_angle_rad_( + std::make_shared>(*rhs.front_wheel_angle_rad_)), + rear_wheel_angle_rad_( + std::make_shared>(*rhs.rear_wheel_angle_rad_)) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + *longitudinal_velocity_mps_ = *rhs.longitudinal_velocity_mps_; + *lateral_velocity_mps_ = *rhs.lateral_velocity_mps_; + *heading_rate_rps_ = *rhs.heading_rate_rps_; + *acceleration_mps2_ = *rhs.acceleration_mps2_; + *front_wheel_angle_rad_ = *rhs.front_wheel_angle_rad_; + *rear_wheel_angle_rad_ = *rhs.rear_wheel_angle_rad_; + } + return *this; +} + +bool Trajectory::build(const std::vector & points) +{ + std::vector poses; + std::vector longitudinal_velocity_mps_values; + std::vector lateral_velocity_mps_values; + std::vector heading_rate_rps_values; + std::vector acceleration_mps2_values; + std::vector front_wheel_angle_rad_values; + std::vector rear_wheel_angle_rad_values; + + for (const auto & point : points) { + poses.emplace_back(point.pose); + longitudinal_velocity_mps_values.emplace_back(point.longitudinal_velocity_mps); + lateral_velocity_mps_values.emplace_back(point.lateral_velocity_mps); + heading_rate_rps_values.emplace_back(point.heading_rate_rps); + acceleration_mps2_values.emplace_back(point.acceleration_mps2); + front_wheel_angle_rad_values.emplace_back(point.front_wheel_angle_rad); + rear_wheel_angle_rad_values.emplace_back(point.rear_wheel_angle_rad); + } + + bool is_valid = true; + + is_valid &= Trajectory::build(poses); + is_valid &= this->longitudinal_velocity_mps().build(bases_, longitudinal_velocity_mps_values); + is_valid &= this->lateral_velocity_mps().build(bases_, lateral_velocity_mps_values); + is_valid &= this->heading_rate_rps().build(bases_, heading_rate_rps_values); + is_valid &= this->acceleration_mps2().build(bases_, acceleration_mps2_values); + is_valid &= this->front_wheel_angle_rad().build(bases_, front_wheel_angle_rad_values); + is_valid &= this->rear_wheel_angle_rad().build(bases_, rear_wheel_angle_rad_values); + + return is_valid; +} + +std::vector Trajectory::get_internal_bases() const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()), + get_bases(this->heading_rate_rps()), get_bases(this->acceleration_mps2()), + get_bases(this->front_wheel_angle_rad()), get_bases(this->rear_wheel_angle_rad())); + + bases = detail::crop_bases(bases, start_, end_); + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + +PointType Trajectory::compute(double s) const +{ + PointType result; + result.pose = Trajectory::compute(s); + s = clamp(s); + result.longitudinal_velocity_mps = + static_cast(this->longitudinal_velocity_mps().compute(s)); + result.lateral_velocity_mps = static_cast(this->lateral_velocity_mps().compute(s)); + result.heading_rate_rps = static_cast(this->heading_rate_rps().compute(s)); + result.acceleration_mps2 = static_cast(this->acceleration_mps2().compute(s)); + result.front_wheel_angle_rad = static_cast(this->front_wheel_angle_rad().compute(s)); + result.rear_wheel_angle_rad = static_cast(this->rear_wheel_angle_rad().compute(s)); + return result; +} + +std::vector Trajectory::restore(const size_t & min_points) const +{ + std::vector bases = get_internal_bases(); + bases = detail::fill_bases(bases, min_points); + + std::vector points; + points.reserve(bases.size()); + for (const auto & s : bases) { + points.emplace_back(compute(s)); + } + return points; +} + +} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp b/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp new file mode 100644 index 0000000000..d4eacd4833 --- /dev/null +++ b/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp @@ -0,0 +1,173 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "autoware/trajectory/trajectory_point.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "autoware/trajectory/utils/curvature_utils.hpp" +#include "lanelet2_core/primitives/LineString.h" + +#include + +#include + +using Trajectory = autoware::trajectory::Trajectory; + +autoware_planning_msgs::msg::TrajectoryPoint trajectory_point(double x, double y) +{ + autoware_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + return point; +} + +TEST(TrajectoryCreatorTestForTrajectoryPoint, create) +{ + { + std::vector points{trajectory_point(0.00, 0.00)}; + auto trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(!trajectory); + } + { + std::vector points{ + trajectory_point(0.00, 0.00), trajectory_point(0.81, 1.68), trajectory_point(1.65, 2.98), + trajectory_point(3.30, 4.01)}; + auto trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(trajectory); + } +} + +class TrajectoryTestForTrajectoryPoint : public ::testing::Test +{ +public: + std::optional trajectory; + + void SetUp() override + { + std::vector points{ + trajectory_point(0.00, 0.00), trajectory_point(0.81, 1.68), trajectory_point(1.65, 2.98), + trajectory_point(3.30, 4.01), trajectory_point(4.70, 4.52), trajectory_point(6.49, 5.20), + trajectory_point(8.11, 6.07), trajectory_point(8.76, 7.23), trajectory_point(9.36, 8.74), + trajectory_point(10.0, 10.0)}; + + trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(trajectory); + } +}; + +TEST_F(TrajectoryTestForTrajectoryPoint, compute) +{ + double length = trajectory->length(); + + trajectory->longitudinal_velocity_mps() + .range(trajectory->length() / 3.0, trajectory->length()) + .set(10.0); + auto point = trajectory->compute(length / 2.0); + + EXPECT_LT(0, point.pose.position.x); + EXPECT_LT(point.pose.position.x, 10); + + EXPECT_LT(0, point.pose.position.y); + EXPECT_LT(point.pose.position.y, 10); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, manipulate_velocity) +{ + trajectory->longitudinal_velocity_mps() = 10.0; + trajectory->longitudinal_velocity_mps() + .range(trajectory->length() / 3, 2.0 * trajectory->length() / 3) + .set(5.0); + auto point1 = trajectory->compute(0.0); + auto point2 = trajectory->compute(trajectory->length() / 2.0); + auto point3 = trajectory->compute(trajectory->length()); + + EXPECT_EQ(10.0, point1.longitudinal_velocity_mps); + EXPECT_EQ(5.0, point2.longitudinal_velocity_mps); + EXPECT_EQ(10.0, point3.longitudinal_velocity_mps); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, direction) +{ + double dir = trajectory->azimuth(0.0); + EXPECT_LT(0, dir); + EXPECT_LT(dir, M_PI / 2); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, curvature) +{ + double curvature_val = trajectory->curvature(0.0); + EXPECT_LT(-1.0, curvature_val); + EXPECT_LT(curvature_val, 1.0); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, restore) +{ + using autoware::trajectory::Trajectory; + trajectory->longitudinal_velocity_mps().range(4.0, trajectory->length()).set(5.0); + auto points = trajectory->restore(0); + EXPECT_EQ(11, points.size()); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, crossed) +{ + lanelet::LineString2d line_string; + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 10.0, 0.0)); + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, 0.0, 0.0)); + + auto crossed_point = autoware::trajectory::crossed(*trajectory, line_string); + ASSERT_EQ(crossed_point.size(), 1); + + EXPECT_LT(0.0, crossed_point.at(0)); + EXPECT_LT(crossed_point.at(0), trajectory->length()); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, closest) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 5.0; + pose.position.y = 5.0; + + auto closest_pose = trajectory->compute(autoware::trajectory::closest(*trajectory, pose)); + + double distance = std::hypot( + closest_pose.pose.position.x - pose.position.x, closest_pose.pose.position.y - pose.position.y); + + EXPECT_LT(distance, 3.0); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, crop) +{ + double length = trajectory->length(); + + auto start_point_expect = trajectory->compute(length / 3.0); + auto end_point_expect = trajectory->compute(length / 3.0 + 1.0); + + trajectory->crop(length / 3.0, 1.0); + + EXPECT_EQ(trajectory->length(), 1.0); + + auto start_point_actual = trajectory->compute(0.0); + auto end_point_actual = trajectory->compute(trajectory->length()); + + EXPECT_EQ(start_point_expect.pose.position.x, start_point_actual.pose.position.x); + EXPECT_EQ(start_point_expect.pose.position.y, start_point_actual.pose.position.y); + + EXPECT_EQ(end_point_expect.pose.position.x, end_point_actual.pose.position.x); + EXPECT_EQ(end_point_expect.pose.position.y, end_point_actual.pose.position.y); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, max_curvature) +{ + double max_curvature = autoware::trajectory::max_curvature(*trajectory); + EXPECT_LT(0, max_curvature); +} diff --git a/mkdocs_macros.py b/mkdocs_macros.py index 97f76442be..1df423af5d 100644 --- a/mkdocs_macros.py +++ b/mkdocs_macros.py @@ -1,4 +1,5 @@ import json +import urllib from tabulate import tabulate @@ -70,3 +71,8 @@ def json_to_markdown(json_schema_file_path): with open(json_schema_file_path) as f: data = json.load(f) return format_json(data) + + @env.macro + def drawio(image_path): + image_url = urllib.parse.quote(f"{env.conf['site_url']}{image_path}", "") + return f"https://app.diagrams.net/?lightbox=1#U{image_url}" diff --git a/testing/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/testing/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index a15fb1fc77..12f31b7ac2 100644 --- a/testing/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/testing/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -374,6 +374,7 @@ T generateTrajectory( using Point = typename T::_points_type::value_type; T traj; + traj.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); for (size_t i = 0; i < num_points; ++i) { const double theta = init_theta + static_cast(i) * delta_theta; const double x = static_cast(i) * point_interval * std::cos(theta); @@ -399,6 +400,7 @@ inline PathWithLaneId generateTrajectory( const double init_theta, const double delta_theta, const size_t overlapping_point_index) { PathWithLaneId traj; + traj.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); for (size_t i = 0; i < num_points; i++) { const double theta = init_theta + static_cast(i) * delta_theta;