RJD-1333/previous following lanelets

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -138,8 +138,8 @@ class HdMapUtils
     const lanelet::Id traffic_light_id) const -> std::optional<double>;
 
   auto getFollowingLanelets(
-    const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids,
-    const double distance = 100, const bool include_self = true) const -> lanelet::Ids;
+    const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon = 100,
+    const bool include_current_lanelet_id = true) const -> lanelet::Ids;
 
   auto getFollowingLanelets(
     const lanelet::Id, const double distance = 100, const bool include_self = true) const
@@ -219,7 +219,8 @@ class HdMapUtils
   auto getPreviousLaneletIds(const lanelet::Id, const std::string & turn_direction) const
     -> lanelet::Ids;
 
-  auto getPreviousLanelets(const lanelet::Id, const double distance = 100) const -> lanelet::Ids;
+  auto getPreviousLanelets(const lanelet::Id, const double backward_horizon = 100) const
+    -> lanelet::Ids;
 
   auto getRightBound(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
 

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -790,75 +790,83 @@ auto HdMapUtils::getLaneChangeableLaneletId(
   return target;
 }
 
-auto HdMapUtils::getPreviousLanelets(const lanelet::Id lanelet_id, const double distance) const
-  -> lanelet::Ids
+auto HdMapUtils::getPreviousLanelets(
+  const lanelet::Id current_lanelet_id, const double backward_horizon) const -> lanelet::Ids
 {
-  lanelet::Ids ret;
+  lanelet::Ids previous_lanelets_ids;
   double total_distance = 0.0;
-  ret.push_back(lanelet_id);
-  while (total_distance < distance) {
-    auto ids = getPreviousLaneletIds(lanelet_id, "straight");
-    if (ids.size() != 0) {
-      total_distance = total_distance + getLaneletLength(ids[0]);
-      ret.push_back(ids[0]);
-      continue;
+  previous_lanelets_ids.push_back(current_lanelet_id);
+  while (total_distance < backward_horizon) {
+    const auto & reference_lanelet_id = previous_lanelets_ids.back();
+    if (const auto straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id, "straight");
+        not straight_lanelet_ids.empty()) {
+      total_distance = total_distance + getLaneletLength(straight_lanelet_ids[0]);
+      previous_lanelets_ids.push_back(straight_lanelet_ids[0]);
+    } else if (auto non_straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id);
+               not non_straight_lanelet_ids.empty()) {
+      total_distance = total_distance + getLaneletLength(non_straight_lanelet_ids[0]);
+      previous_lanelets_ids.push_back(non_straight_lanelet_ids[0]);
     } else {
-      auto else_ids = getPreviousLaneletIds(lanelet_id);
-      if (else_ids.size() != 0) {
-        total_distance = total_distance + getLaneletLength(else_ids[0]);
-        ret.push_back(else_ids[0]);
-        continue;
-      } else {
-        break;
-      }
+      break;
     }
   }
-  return ret;
+  return previous_lanelets_ids;
 }
 
 auto HdMapUtils::isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & route) const -> bool
 {
-  return std::find_if(route.begin(), route.end(), [lanelet_id](const auto id) {
-           return lanelet_id == id;
-         }) != route.end();
+  return std::find(route.cbegin(), route.cend(), lanelet_id) != route.cend();
 }
 
 auto HdMapUtils::getFollowingLanelets(
-  const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, const double distance,
-  const bool include_self) const -> lanelet::Ids
+  const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon,
+  const bool include_current_lanelet_id) const -> lanelet::Ids
 {
-  if (candidate_lanelet_ids.empty()) {
-    return {};
+  const auto is_following_lanelet =
+    [this](const auto & current_lanelet, const auto & candidate_lanelet) {
+      const auto next_ids = getNextLaneletIds(current_lanelet);
+      return std::find(next_ids.cbegin(), next_ids.cend(), candidate_lanelet) != next_ids.cend();
+    };
+
+  lanelet::Ids following_lanelets_ids;
+
+  if (route.empty()) {
+    return following_lanelets_ids;
   }
-  lanelet::Ids ids;
+
   double total_distance = 0.0;
-  bool found = false;
-  for (const auto id : candidate_lanelet_ids) {
-    if (found) {
-      ids.emplace_back(id);
-      total_distance = total_distance + getLaneletLength(id);
-      if (total_distance > distance) {
-        return ids;
+  bool found_starting_lanelet = false;
+
+  for (const auto & candidate_lanelet_id : route) {
+    if (found_starting_lanelet) {
+      if (const auto previous_lanelet =
+            following_lanelets_ids.empty() ? current_lanelet_id : following_lanelets_ids.back();
+          not is_following_lanelet(previous_lanelet, candidate_lanelet_id)) {
+        THROW_SEMANTIC_ERROR(
+          candidate_lanelet_id + " is not the follower of lanelet " + previous_lanelet);
       }
-    }
-    if (id == lanelet_id) {
-      found = true;
-      if (include_self) {
-        ids.push_back(id);
+      following_lanelets_ids.push_back(candidate_lanelet_id);
+      total_distance += getLaneletLength(candidate_lanelet_id);
+      if (total_distance > horizon) {
+        break;
+      }
+    } else if (candidate_lanelet_id == current_lanelet_id) {
+      found_starting_lanelet = true;
+      if (include_current_lanelet_id) {
+        following_lanelets_ids.push_back(candidate_lanelet_id);
       }
     }
   }
-  if (!found) {
+  if (following_lanelets_ids.empty()) {
     THROW_SEMANTIC_ERROR("lanelet id does not match");
+  } else if (total_distance <= horizon) {
+    const auto remaining_lanelets =
+      getFollowingLanelets(route.back(), horizon - total_distance, false);
+    following_lanelets_ids.insert(
+      following_lanelets_ids.end(), remaining_lanelets.begin(), remaining_lanelets.end());
   }
-  if (total_distance > distance) {
-    return ids;
-  }
-  // clang-format off
-  return ids + getFollowingLanelets(
-    candidate_lanelet_ids[candidate_lanelet_ids.size() - 1],
-    distance - total_distance, false);
-  // clang-format on
+
+  return following_lanelets_ids;
 }
 
 auto HdMapUtils::getFollowingLanelets(

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

@@ -1613,6 +1613,19 @@ TEST_F(HdMapUtilsTest_StandardMap, getFollowingLanelets_candidateTrajectory)
     (lanelet::Ids{id, 34495, 34507}));
 }
 
+/**
+ * @note Test basic functionality.
+ * Test following lanelets obtaining
+ * with a candidate trajectory longer than the given distance without starting lanelet.
+ */
+TEST_F(HdMapUtilsTest_StandardMap, getFollowingLanelets_candidateTrajectoryFalse)
+{
+  const lanelet::Id id = 34564;
+  EXPECT_EQ(
+    hdmap_utils.getFollowingLanelets(id, lanelet::Ids{id, 34495, 34507, 34795, 34606}, 40.0, false),
+    (lanelet::Ids{34495, 34507}));
+}
+
 /**
  * @note Test basic functionality.
  * Test following lanelets obtaining with
@@ -1646,6 +1659,18 @@ TEST_F(HdMapUtilsTest_StandardMap, getFollowingLanelets_candidateTrajectoryEmpty
     hdmap_utils.getFollowingLanelets(120660, {}, 1.0e3, true).size(), static_cast<std::size_t>(0));
 }
 
+/**
+ * @note Test function behavior when called with a candidate trajectory
+ * that contains wrong candidates
+ */
+TEST_F(HdMapUtilsTest_StandardMap, getFollowingLanelets_candidatesDoNotMatchRealTrajectory)
+{
+  EXPECT_THROW(
+    hdmap_utils.getFollowingLanelets(
+      34564, lanelet::Ids{34564, 34495, 34507, 34399, 34399}, 100.0, true),
+    common::Error);
+}
+
 /**
  * @note Test basic functionality.
  * Test lane change possibility checking
@@ -2698,3 +2723,15 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_emptyVec
           makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})
       .has_value());
 }
+
+/**
+ * @note Test basic functionality.
+ */
+TEST_F(HdMapUtilsTest_StandardMap, getPreviousLanelets)
+{
+  const lanelet::Id id = 34600;
+  const auto result_previous = hdmap_utils.getPreviousLanelets(id, 100.0);
+  const lanelet::Ids actual_previous{id, 34783, 34606, 34795, 34507};
+
+  EXPECT_EQ(result_previous, actual_previous);
+}