feat(perception_utils): add maximum size boundary (#1437)

tier4 · Oct 7, 2022 · 4ebc5c3 · 4ebc5c3
1 parent cc37846
commit 4ebc5c3
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 5 deletions.
diff --git a/common/perception_utils/include/perception_utils/predicted_path_utils.hpp b/common/perception_utils/include/perception_utils/predicted_path_utils.hpp
@@ -103,27 +103,27 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath(
   const auto interpolated_z = use_spline_for_z ? slerp(z) : lerp(z);
 
   autoware_auto_perception_msgs::msg::PredictedPath resampled_path;
+  const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size());
   resampled_path.confidence = path.confidence;
-  resampled_path.path.resize(resampled_time.size());
+  resampled_path.path.resize(resampled_size);
 
   // Set Position
-  for (size_t i = 0; i < resampled_path.path.size(); ++i) {
+  for (size_t i = 0; i < resampled_size; ++i) {
     const auto p = tier4_autoware_utils::createPoint(
       interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i));
     resampled_path.path.at(i).position = p;
   }
 
   // Set Quaternion
-  for (size_t i = 0; i < resampled_path.path.size() - 1; ++i) {
+  for (size_t i = 0; i < resampled_size - 1; ++i) {
     const auto & src_point = resampled_path.path.at(i).position;
     const auto & dst_point = resampled_path.path.at(i + 1).position;
     const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point);
     const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
     resampled_path.path.at(i).orientation =
       tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
   }
-  resampled_path.path.back().orientation =
-    resampled_path.path.at(resampled_path.path.size() - 2).orientation;
+  resampled_path.path.back().orientation = resampled_path.path.at(resampled_size - 2).orientation;
 
   return resampled_path;
 }

diff --git a/common/perception_utils/test/src/test_predicted_path_utils.cpp b/common/perception_utils/test/src/test_predicted_path_utils.cpp
@@ -174,6 +174,29 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector)
     }
   }
 
+  // Resample which exceeds the maximum size
+  {
+    std::vector<double> resampling_vec(101);
+    for (size_t i = 0; i < 101; ++i) {
+      resampling_vec.at(i) = i * 0.05;
+    }
+
+    const auto resampled_path = resamplePredictedPath(path, resampling_vec);
+
+    EXPECT_EQ(resampled_path.path.size(), resampled_path.path.max_size());
+    EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon);
+
+    for (size_t i = 0; i < resampled_path.path.max_size(); ++i) {
+      EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i), epsilon);
+      EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon);
+      EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon);
+      EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon);
+    }
+  }
+
   // Some points are out of range
   {
     const std::vector<double> resampling_vec = {-1.0, 0.0, 5.0, 9.0, 9.1};