From 5a5a2f113d1dae5fcc341778189dabd8b0821895 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 7 Mar 2025 16:38:37 +0900 Subject: [PATCH] feat(lanelet2_utility): add intersection Signed-off-by: Mamoru Sobue --- .../autoware_lanelet2_utility/CMakeLists.txt | 7 +- .../intersection.hpp | 72 ++++++++++ .../src/intersection.cpp | 56 ++++++++ .../test/intersection.cpp | 124 ++++++++++++++++++ 4 files changed, 258 insertions(+), 1 deletion(-) create mode 100644 common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/intersection.hpp create mode 100644 common/autoware_lanelet2_utility/src/intersection.cpp create mode 100644 common/autoware_lanelet2_utility/test/intersection.cpp diff --git a/common/autoware_lanelet2_utility/CMakeLists.txt b/common/autoware_lanelet2_utility/CMakeLists.txt index fb51b5792..2b1ccf81b 100644 --- a/common/autoware_lanelet2_utility/CMakeLists.txt +++ b/common/autoware_lanelet2_utility/CMakeLists.txt @@ -7,13 +7,18 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/kind.cpp src/topology.cpp + src/intersection.cpp ) if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) ament_auto_find_test_dependencies() - file(GLOB_RECURSE test_files test/*.cpp) + set(test_files + test/kind.cpp + test/topology.cpp + test/intersection.cpp + ) foreach (test_file IN LISTS test_files) get_filename_component(test_file_name ${test_file} NAME) diff --git a/common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/intersection.hpp b/common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/intersection.hpp new file mode 100644 index 000000000..037fbe24e --- /dev/null +++ b/common/autoware_lanelet2_utility/include/autoware_lanelet2_utility/intersection.hpp @@ -0,0 +1,72 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_LANELET2_UTILITY__INTERSECTION_HPP_ +#define AUTOWARE_LANELET2_UTILITY__INTERSECTION_HPP_ + +#include + +#include + +namespace autoware::lanelet2_utility +{ + +static constexpr const char * k_turn_direction = "turn_direction"; +static constexpr const char * k_turn_direction_straight = "straight"; +static constexpr const char * k_turn_direction_left = "left"; +static constexpr const char * k_turn_direction_right = "right"; + +enum TurnDirection : int { Straight = 0, Left, Right }; + +/** + * @brief check if given lanelet has "turn_direction" attribute + * @param [in] input lanelet + * @return true if and only if the given lanelet has "turn_direction" attribute + */ +bool is_intersection_lanelet(const lanelet::ConstLanelet & lanelet); + +/** + * @brief check if given lanelet has "turn_direction" attribute and the value is "straight" + * @param [in] input lanelet + * @return true if and only if the given lanelet has "turn_direction" attribute and the value is + * "straight" + */ +bool is_straight_direction(const lanelet::ConstLanelet & lanelet); + +/** + * @brief check if given lanelet has "turn_direction" attribute and the value is "left" + * @param [in] input lanelet + * @return true if and only if the given lanelet has "turn_direction" attribute and the value is + * "left" + */ +bool is_left_direction(const lanelet::ConstLanelet & lanelet); + +/** + * @brief check if given lanelet has "turn_direction" attribute and the value is "right" + * @param [in] input lanelet + * @return true if and only if the given lanelet has "turn_direction" attribute and the value is + * "right" + */ +bool is_right_direction(const lanelet::ConstLanelet & lanelet); + +/** + * @brief get the turn_direction value + * @param [in] input lanelet + * @return valid TurnDirection value if `lanelet` has valid "turn_direction" value, otherwise null + * "right" + */ +std::optional get_turn_direction(const lanelet::ConstLanelet & lanelet); + +} // namespace autoware::lanelet2_utility +#endif // AUTOWARE_LANELET2_UTILITY__INTERSECTION_HPP_ diff --git a/common/autoware_lanelet2_utility/src/intersection.cpp b/common/autoware_lanelet2_utility/src/intersection.cpp new file mode 100644 index 000000000..6f270ece0 --- /dev/null +++ b/common/autoware_lanelet2_utility/src/intersection.cpp @@ -0,0 +1,56 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace autoware::lanelet2_utility +{ + +bool is_intersection_lanelet(const lanelet::ConstLanelet & lanelet) +{ + return lanelet.hasAttribute(k_turn_direction); +} + +bool is_straight_direction(const lanelet::ConstLanelet & lanelet) +{ + return strcmp(lanelet.attributeOr(k_turn_direction, "else"), k_turn_direction_straight) == 0; +} + +bool is_left_direction(const lanelet::ConstLanelet & lanelet) +{ + return strcmp(lanelet.attributeOr(k_turn_direction, "else"), k_turn_direction_left) == 0; +} + +bool is_right_direction(const lanelet::ConstLanelet & lanelet) +{ + return strcmp(lanelet.attributeOr(k_turn_direction, "else"), k_turn_direction_right) == 0; +} + +std::optional get_turn_direction(const lanelet::ConstLanelet & lanelet) +{ + if (is_straight_direction(lanelet)) { + return TurnDirection::Straight; + } + if (is_left_direction(lanelet)) { + return TurnDirection::Left; + } + if (is_right_direction(lanelet)) { + return TurnDirection::Right; + } + return std::nullopt; +} + +} // namespace autoware::lanelet2_utility diff --git a/common/autoware_lanelet2_utility/test/intersection.cpp b/common/autoware_lanelet2_utility/test/intersection.cpp new file mode 100644 index 000000000..517f633ae --- /dev/null +++ b/common/autoware_lanelet2_utility/test/intersection.cpp @@ -0,0 +1,124 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "map_loader.hpp" + +#include +#include + +#include + +#include +#include + +namespace fs = std::filesystem; + +namespace autoware +{ +class TestWithIntersectionCrossingMap : public ::testing::Test +{ +protected: + lanelet::LaneletMapConstPtr lanelet_map_ptr_{nullptr}; + + void SetUp() override + { + const auto sample_map_dir = + fs::path(ament_index_cpp::get_package_share_directory("autoware_lanelet2_utility")) / + "sample_map"; + const auto intersection_crossing_map_path = sample_map_dir / "intersection" / "crossing.osm"; + + lanelet_map_ptr_ = load_mgrs_coordinate_map(intersection_crossing_map_path.string()); + } +}; + +TEST_F(TestWithIntersectionCrossingMap, is_intersection_lanelet_false) +{ + EXPECT_EQ( + lanelet2_utility::is_intersection_lanelet(lanelet_map_ptr_->laneletLayer.get(2257)), false); +} + +TEST_F(TestWithIntersectionCrossingMap, is_intersection_lanelet_true) +{ + EXPECT_EQ( + lanelet2_utility::is_intersection_lanelet(lanelet_map_ptr_->laneletLayer.get(2274)), true); +} + +TEST_F(TestWithIntersectionCrossingMap, is_straight_direction_false) +{ + EXPECT_EQ( + lanelet2_utility::is_straight_direction(lanelet_map_ptr_->laneletLayer.get(2274)), false); +} + +TEST_F(TestWithIntersectionCrossingMap, is_straight_direction_true) +{ + EXPECT_EQ( + lanelet2_utility::is_straight_direction(lanelet_map_ptr_->laneletLayer.get(2278)), true); +} + +TEST_F(TestWithIntersectionCrossingMap, is_left_direction_false) +{ + EXPECT_EQ(lanelet2_utility::is_left_direction(lanelet_map_ptr_->laneletLayer.get(2278)), false); +} + +TEST_F(TestWithIntersectionCrossingMap, is_left_direction_true) +{ + EXPECT_EQ(lanelet2_utility::is_left_direction(lanelet_map_ptr_->laneletLayer.get(2274)), true); +} + +TEST_F(TestWithIntersectionCrossingMap, is_right_direction_false) +{ + EXPECT_EQ(lanelet2_utility::is_right_direction(lanelet_map_ptr_->laneletLayer.get(2274)), false); +} + +TEST_F(TestWithIntersectionCrossingMap, is_right_direction_true) +{ + EXPECT_EQ(lanelet2_utility::is_right_direction(lanelet_map_ptr_->laneletLayer.get(2277)), true); +} + +TEST_F(TestWithIntersectionCrossingMap, get_turn_direction) +{ + // not intersection + EXPECT_EQ( + lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2257)).has_value(), + false); + + // straight + { + const auto lane = + lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2278)); + EXPECT_EQ(lane.has_value() && lane.value() == lanelet2_utility::TurnDirection::Straight, true); + } + + // left + { + const auto lane = + lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2274)); + EXPECT_EQ(lane.has_value() && lane.value() == lanelet2_utility::TurnDirection::Left, true); + } + + // right + { + const auto lane = + lanelet2_utility::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2277)); + EXPECT_EQ(lane.has_value() && lane.value() == lanelet2_utility::TurnDirection::Right, true); + } +} + +} // namespace autoware + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}