Revert "refactor(motion_utils): stop using CAPACITY from Trajectory m…

…essage (autowarefoundation#5755)" This reverts commit f582b5a.
tier4 · Dec 20, 2023 · 42066f2 · 42066f2
1 parent bca290d
commit 42066f2
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 1 deletion.
diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/tmp_conversion.cpp
@@ -33,7 +33,12 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory)
 {
   autoware_auto_planning_msgs::msg::Trajectory output{};
-  for (const auto & pt : trajectory) output.points.push_back(pt);
+  for (const auto & pt : trajectory) {
+    output.points.push_back(pt);
+    if (output.points.size() >= output.CAPACITY) {
+      break;
+    }
+  }
   return output;
 }
 

diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp
@@ -803,6 +803,17 @@ TEST(trajectory, convertToTrajectory)
     const auto traj = convertToTrajectory(traj_input);
     EXPECT_EQ(traj.points.size(), traj_input.size());
   }
+
+  // Clipping check
+  {
+    const auto traj_input = generateTestTrajectoryPointArray(10000, 1.0);
+    const auto traj = convertToTrajectory(traj_input);
+    EXPECT_EQ(traj.points.size(), traj.CAPACITY);
+    // Value check
+    for (size_t i = 0; i < traj.points.size(); ++i) {
+      EXPECT_EQ(traj.points.at(i), traj_input.at(i));
+    }
+  }
 }
 
 TEST(trajectory, convertToTrajectoryPointArray)