From bbff5c6015e8736cdeae29b583e6d2794e91d572 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Thu, 6 Mar 2025 20:57:33 +0900
Subject: [PATCH] added more tests

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../src/topology.cpp                          |  9 +++--
 .../test/topology.cpp                         | 35 +++++++++++++++++--
 2 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/common/autoware_lanelet2_utility/src/topology.cpp b/common/autoware_lanelet2_utility/src/topology.cpp
index 49f8c8216..ce50e2832 100644
--- a/common/autoware_lanelet2_utility/src/topology.cpp
+++ b/common/autoware_lanelet2_utility/src/topology.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <autoware_lanelet2_utility/topology.hpp>
+#include <range/v3/all.hpp>
 #include <rclcpp/logging.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
@@ -256,10 +257,8 @@ lanelet::ConstLanelets sibling_lanelets(
 lanelet::ConstLanelets from_ids(
   const lanelet::LaneletMapConstPtr lanelet_map, const std::vector<lanelet::Id> & ids)
 {
-  lanelet::ConstLanelets lanelets;
-  for (const auto id : ids) {
-    lanelets.push_back(lanelet_map->laneletLayer.get(id));
-  }
-  return lanelets;
+  return ids |
+         ranges::view::transform([&](const auto id) { return lanelet_map->laneletLayer.get(id); }) |
+         ranges::to<std::vector>();
 }
 }  // namespace autoware::lanelet2_utility
diff --git a/common/autoware_lanelet2_utility/test/topology.cpp b/common/autoware_lanelet2_utility/test/topology.cpp
index 92486a5e4..574444f41 100644
--- a/common/autoware_lanelet2_utility/test/topology.cpp
+++ b/common/autoware_lanelet2_utility/test/topology.cpp
@@ -98,13 +98,20 @@ TEST_F(TestWithIntersectionCrossingMap, right_lanelet_with_lc_permission)
   EXPECT_EQ(lane.value().id(), 2245);
 }
 
-TEST_F(TestWithIntersectionCrossingMap, right_opposite_lanelet)
+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(
@@ -177,6 +184,15 @@ TEST_F(TestWithIntersectionCrossingMap, right_lanelets_with_opposite)
   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(
@@ -240,13 +256,20 @@ class TestWithIntersectionCrossingInverseMap : public ::testing::Test
   }
 };
 
-TEST_F(TestWithIntersectionCrossingInverseMap, left_opposite_lanelet)
+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(
@@ -258,6 +281,14 @@ TEST_F(TestWithIntersectionCrossingInverseMap, left_lanelets_with_opposite)
   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)