RJD-1334/fix_longitudinal_distance

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -1547,70 +1547,52 @@ auto HdMapUtils::getLongitudinalDistance(
   if (route.empty()) {
     return std::nullopt;
   }
-  double distance = 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/
-  for (unsigned int i = 0; i < route.size(); i++) {
-    if (i < route.size() - 1 && with_lane_change(allow_lane_change, route[i], route[i + 1])) {
-      /// @note "the lanelet before the lane change" case
-      traffic_simulator_msgs::msg::LaneletPose next_lanelet_pose;
-      next_lanelet_pose.lanelet_id = route[i + 1];
-      next_lanelet_pose.s = 0.0;
-      next_lanelet_pose.offset = 0.0;
-
-      if (
-        auto next_lanelet_origin_from_current_lanelet =
-          toLaneletPose(toMapPose(next_lanelet_pose).pose, route[i], 10.0)) {
-        distance += next_lanelet_origin_from_current_lanelet->s;
+  double accumulated_distance = 0.0;
+  for (std::size_t i = 0UL; i < route.size() - 1UL; ++i) {
+    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]);
+
+      constexpr double lanelet_starting_s = 0.0;
+      const double lanelet_half_min_s =
+        std::min(curr_lanelet_spline->getLength(), next_lanelet_spline->getLength()) * 0.5;
+
+      constexpr double matching_distance = 10.0;
+      const auto curr_start_matching_s = curr_lanelet_spline->getSValue(
+        next_lanelet_spline->getPose(lanelet_starting_s), matching_distance);
+      const auto next_start_matching_s = next_lanelet_spline->getSValue(
+        curr_lanelet_spline->getPose(lanelet_starting_s), matching_distance);
+      const auto curr_middle_matching_s = curr_lanelet_spline->getSValue(
+        next_lanelet_spline->getPose(lanelet_half_min_s), matching_distance);
+      const auto next_middle_matching_s = next_lanelet_spline->getSValue(
+        curr_lanelet_spline->getPose(lanelet_half_min_s), matching_distance);
+
+      if (curr_start_matching_s.has_value()) {
+        accumulated_distance += curr_start_matching_s.value();
+      } else if (next_start_matching_s.has_value()) {
+        accumulated_distance -= next_start_matching_s.value();
+      } else if (curr_middle_matching_s.has_value()) {
+        accumulated_distance += curr_middle_matching_s.value() - lanelet_half_min_s;
+      } else if (next_middle_matching_s.has_value()) {
+        accumulated_distance -= next_middle_matching_s.value() - lanelet_half_min_s;
       } else {
-        traffic_simulator_msgs::msg::LaneletPose current_lanelet_pose = next_lanelet_pose;
-        current_lanelet_pose.lanelet_id = route[i];
-        if (
-          auto current_lanelet_origin_from_next_lanelet =
-            toLaneletPose(toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) {
-          distance -= current_lanelet_origin_from_next_lanelet->s;
-        } else {
-          return std::nullopt;
-        }
-      }
-
-      /// @note "first lanelet before the lane change" case
-      if (route[i] == from.lanelet_id) {
-        distance -= from.s;
-        if (route[i + 1] == to.lanelet_id) {
-          distance += to.s;
-          return distance;
-        }
+        return std::nullopt;
       }
     } else {
-      if (route[i] == from.lanelet_id) {
-        /// @note "first lanelet" case
-        distance = getLaneletLength(from.lanelet_id) - from.s;
-      } else if (route[i] == to.lanelet_id) {
-        /// @note "last lanelet" case
-        distance += to.s;
-      } else {
-        ///@note "normal intermediate lanelet" case
-        distance += getLaneletLength(route[i]);
-      }
+      accumulated_distance += getLaneletLength(route[i]);
     }
   }
-  return distance;
+  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,68 @@ 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 without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false);
+    EXPECT_FALSE(without_lane_change.has_value());
+
+    const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    ASSERT_TRUE(with_lane_change.has_value());
+    EXPECT_NEAR(with_lane_change.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 without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false);
+    EXPECT_FALSE(without_lane_change.has_value());
+
+    const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    ASSERT_TRUE(with_lane_change.has_value());
+    EXPECT_NEAR(with_lane_change.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 without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false);
+    EXPECT_FALSE(without_lane_change.has_value());
+
+    const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    ASSERT_TRUE(with_lane_change.has_value());
+    EXPECT_NEAR(with_lane_change.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 without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false);
+    EXPECT_FALSE(without_lane_change.has_value());
+
+    const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    ASSERT_TRUE(with_lane_change.has_value());
+    EXPECT_NEAR(with_lane_change.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 without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false);
+    EXPECT_FALSE(without_lane_change.has_value());
+
+    const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true);
+    ASSERT_TRUE(with_lane_change.has_value());
+    EXPECT_NEAR(with_lane_change.value(), 253.0, 1.0);
+  }
+}
+
 /**
  * @note Test basic functionality.
  * Test obtaining stop line ids correctness with a route that has no stop lines.