simplify lambda

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -1547,27 +1547,19 @@ auto HdMapUtils::getLongitudinalDistance(
   if (route.empty()) {
     return std::nullopt;
   }
-  double distance = 0.0;
-
-  auto with_lane_change = [this](
-                            const bool allow_lane_change, const lanelet::Id current_lanelet,
-                            const lanelet::Id next_lanelet) -> bool {
-    if (allow_lane_change) {
-      auto next_lanelet_ids = getNextLaneletIds(current_lanelet);
-      auto next_lanelet_itr = std::find_if(
-        next_lanelet_ids.begin(), next_lanelet_ids.end(),
-        [next_lanelet](const lanelet::Id & id) { return id == next_lanelet; });
-      return next_lanelet_itr == next_lanelet_ids.end();
-    } else {
-      return false;
-    }
+
+  const auto with_lane_change =
+    [this](const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool {
+    const auto next_lanelet_ids = getNextLaneletIds(current_lanelet);
+    return std::none_of(
+      next_lanelet_ids.begin(), next_lanelet_ids.end(),
+      [next_lanelet](const lanelet::Id id) { return id == next_lanelet; });
   };
 
-  /// @note in this for loop, some cases are marked by @note command. each case is explained in the document.
   /// @sa https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/DistanceCalculation/
+  double accumulated_distance = 0.0;
   for (std::size_t i = 0UL; i < route.size() - 1UL; ++i) {
-    if (with_lane_change(allow_lane_change, route[i], route[i + 1UL])) {
-      /// @note "the lanelet before the lane change" case
+    if (with_lane_change(route[i], route[i + 1UL])) {
       const auto curr_lanelet_spline = getCenterPointsSpline(route[i]);
       const auto next_lanelet_spline = getCenterPointsSpline(route[i + 1UL]);
       const double mid_length =
@@ -1583,21 +1575,21 @@ auto HdMapUtils::getLongitudinalDistance(
         next_lanelet_spline->getSValue(curr_lanelet_spline->getPose(mid_length), 10.0);
 
       if (curr_start_matching_s.has_value()) {
-        distance += curr_start_matching_s.value();
+        accumulated_distance += curr_start_matching_s.value();
       } else if (next_start_matching_s.has_value()) {
-        distance -= next_start_matching_s.value();
+        accumulated_distance -= next_start_matching_s.value();
       } else if (curr_middle_matching_s.has_value()) {
-        distance += curr_middle_matching_s.value() - mid_length;
+        accumulated_distance += curr_middle_matching_s.value() - mid_length;
       } else if (next_middle_matching_s.has_value()) {
-        distance -= next_middle_matching_s.value() - mid_length;
+        accumulated_distance -= next_middle_matching_s.value() - mid_length;
       } else {
         return std::nullopt;
       }
     } else {
-      distance += getLaneletLength(route[i]);
+      accumulated_distance += getLaneletLength(route[i]);
     }
   }
-  return distance - from.s + to.s;
+  return accumulated_distance - from.s + to.s;
 }
 
 auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin

simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp

@@ -125,6 +125,22 @@ class HdMapUtilsTest_KashiwanohaMap : public testing::Test
 
   hdmap_utils::HdMapUtils hdmap_utils;
 };
+class HdMapUtilsTest_IntersectionMap : public testing::Test
+{
+protected:
+  HdMapUtilsTest_IntersectionMap()
+  : hdmap_utils(
+      ament_index_cpp::get_package_share_directory("traffic_simulator") +
+        "/map/intersection/lanelet2_map.osm",
+      geographic_msgs::build<geographic_msgs::msg::GeoPoint>()
+        .latitude(35.64200728302)
+        .longitude(139.74821144562)
+        .altitude(0.0))
+  {
+  }
+
+  hdmap_utils::HdMapUtils hdmap_utils;
+};
 
 /**
  * @note Test basic functionality.
@@ -2055,6 +2071,48 @@ TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348)
     54.18867466433655977198213804513216018676757812500000));
 }
 
+/**
+ * @note Test for the corner case described in https://github.com/tier4/scenario_simulator_v2/issues/1364
+ */
+TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange)
+{
+  {
+    const auto pose_from = traffic_simulator::helper::constructLaneletPose(563UL, 5.0);
+    const auto pose_to = traffic_simulator::helper::constructLaneletPose(659UL, 5.0);
+
+    const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 157.0, 1.0));
+  }
+  {
+    const auto pose_from = traffic_simulator::helper::constructLaneletPose(563UL, 5.0);
+    const auto pose_to = traffic_simulator::helper::constructLaneletPose(658UL, 5.0);
+
+    const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 161.0, 1.0));
+  }
+  {
+    const auto pose_from = traffic_simulator::helper::constructLaneletPose(563UL, 5.0);
+    const auto pose_to = traffic_simulator::helper::constructLaneletPose(657UL, 5.0);
+
+    const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 161.0, 1.0));
+  }
+  {
+    const auto pose_from = traffic_simulator::helper::constructLaneletPose(643UL, 5.0);
+    const auto pose_to = traffic_simulator::helper::constructLaneletPose(666UL, 5.0);
+
+    const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 250.0, 1.0));
+  }
+  {
+    const auto pose_from = traffic_simulator::helper::constructLaneletPose(643UL, 5.0);
+    const auto pose_to = traffic_simulator::helper::constructLaneletPose(665UL, 5.0);
+
+    const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 253.0, 1.0));
+  }
+}
+
 /**
  * @note Test basic functionality.
  * Test obtaining stop line ids correctness with a route that has no stop lines.