Merge pull request #1366 from tier4/RJD-1197/distance

RJD-1197/distance
tier4 · Sep 19, 2024 · 54cd077 · 54cd077
2 parents 47a6675 + b0d1d15
commit 54cd077
Show file tree

Hide file tree

Showing 10 changed files with 16,203 additions and 23 deletions.
diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp
@@ -214,6 +214,9 @@ auto distanceToLeftLaneBound(
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
 {
+  if (lanelet_ids.empty()) {
+    THROW_SEMANTIC_ERROR("Failing to calculate distanceToLeftLaneBound given an empty vector.");
+  }
   std::vector<double> distances;
   std::transform(
     lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) {
@@ -245,10 +248,13 @@ auto distanceToRightLaneBound(
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
 {
+  if (lanelet_ids.empty()) {
+    THROW_SEMANTIC_ERROR("Failing to calculate distanceToRightLaneBound for given empty vector.");
+  }
   std::vector<double> distances;
   std::transform(
     lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) {
-      return distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr);
+      return distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr);
     });
   return *std::min_element(distances.begin(), distances.end());
 }

diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt
@@ -1,9 +1,9 @@
-add_subdirectory(src/traffic_lights)
-add_subdirectory(src/helper)
-add_subdirectory(src/entity)
 add_subdirectory(src/behavior)
+add_subdirectory(src/data_type)
+add_subdirectory(src/entity)
+add_subdirectory(src/hdmap_utils)
+add_subdirectory(src/helper)
 add_subdirectory(src/job)
 add_subdirectory(src/simulation_clock)
-add_subdirectory(src/hdmap_utils)
+add_subdirectory(src/traffic_lights)
 add_subdirectory(src/utils)
-add_subdirectory(src/data_type)