test(autoware_behavior_path_planner_common): add tests for calcInterp…

…olatedPoseWithVelocity (#8270)

* test: add interpolated pose calculation function's test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* disabled SpecialCases test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp

@@ -76,6 +76,14 @@ double calcMinimumLongitudinalLength(
   const double front_object_velocity, const double rear_object_velocity,
   const RSSparams & rss_params);
 
+/**
+ * @brief Calculates an interpolated pose with velocity for a given relative time along a path.
+ * @param path A vector of PoseWithVelocityStamped objects representing the path.
+ * @param relative_time The relative time at which to calculate the interpolated pose and velocity.
+ * @return An optional PoseWithVelocityStamped object. If the interpolation is successful,
+ *         it contains the interpolated pose, velocity, and time. If the interpolation fails
+ *         (e.g., empty path, negative time, or time beyond the path), it returns std::nullopt.
+ */
 std::optional<PoseWithVelocityStamped> calcInterpolatedPoseWithVelocity(
   const std::vector<PoseWithVelocityStamped> & path, const double relative_time);
 

planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp

@@ -27,14 +27,36 @@
 
 constexpr double epsilon = 1e-6;
 
+using autoware::behavior_path_planner::utils::path_safety_checker::calcInterpolatedPoseWithVelocity;
 using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
+using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
+using autoware::universe_utils::createPoint;
+using autoware::universe_utils::createQuaternionFromRPY;
 using autoware::universe_utils::Point2d;
 using autoware::universe_utils::Polygon2d;
 using autoware_perception_msgs::msg::Shape;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 
+geometry_msgs::msg::Pose createPose(
+  double x, double y, double z, double roll, double pitch, double yaw)
+{
+  geometry_msgs::msg::Pose p;
+  p.position = createPoint(x, y, z);
+  p.orientation = createQuaternionFromRPY(roll, pitch, yaw);
+  return p;
+}
+
+std::vector<PoseWithVelocityStamped> createTestPath()
+{
+  std::vector<PoseWithVelocityStamped> path;
+  path.emplace_back(0.0, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), 1.0);
+  path.emplace_back(1.0, createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0), 2.0);
+  path.emplace_back(2.0, createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), 3.0);
+  return path;
+}
+
 TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon)
 {
   using autoware::behavior_path_planner::utils::path_safety_checker::createExtendedPolygon;
@@ -207,3 +229,74 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance)
     EXPECT_NEAR(calcRssDistance(front_vel, rear_vel, params), 63.75, epsilon);
   }
 }
+
+// Basic interpolation test
+TEST(CalcInterpolatedPoseWithVelocityTest, BasicInterpolation)
+{
+  auto path = createTestPath();
+  auto result = calcInterpolatedPoseWithVelocity(path, 0.5);
+
+  ASSERT_TRUE(result.has_value());
+  EXPECT_NEAR(result->time, 0.5, 1e-6);
+  EXPECT_NEAR(result->pose.position.x, 0.5, 1e-6);
+  EXPECT_NEAR(result->velocity, 1.5, 1e-6);
+}
+
+// Boundary conditions test
+TEST(CalcInterpolatedPoseWithVelocityTest, BoundaryConditions)
+{
+  auto path = createTestPath();
+
+  // First point of the path
+  auto start_result = calcInterpolatedPoseWithVelocity(path, 0.0);
+  ASSERT_TRUE(start_result.has_value());
+  EXPECT_NEAR(start_result->time, 0.0, 1e-6);
+  EXPECT_NEAR(start_result->pose.position.x, 0.0, 1e-6);
+  EXPECT_NEAR(start_result->velocity, 1.0, 1e-6);
+
+  // Last point of the path
+  auto end_result = calcInterpolatedPoseWithVelocity(path, 2.0);
+  ASSERT_TRUE(end_result.has_value());
+  EXPECT_NEAR(end_result->time, 2.0, 1e-6);
+  EXPECT_NEAR(end_result->pose.position.x, 2.0, 1e-6);
+  EXPECT_NEAR(end_result->velocity, 3.0, 1e-6);
+}
+
+// Invalid input test
+TEST(CalcInterpolatedPoseWithVelocityTest, InvalidInput)
+{
+  auto path = createTestPath();
+
+  // Empty path
+  EXPECT_FALSE(calcInterpolatedPoseWithVelocity({}, 1.0).has_value());
+
+  // Negative relative time
+  EXPECT_FALSE(calcInterpolatedPoseWithVelocity(path, -1.0).has_value());
+
+  // Relative time greater than the last time in the path
+  EXPECT_FALSE(calcInterpolatedPoseWithVelocity(path, 3.0).has_value());
+}
+
+// Special cases test
+TEST(CalcInterpolatedPoseWithVelocityTest, DISABLED_SpecialCases)
+{
+  // Case with consecutive points at the same time
+  std::vector<PoseWithVelocityStamped> same_time_path;
+  same_time_path.emplace_back(0.0, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), 1.0);
+  same_time_path.emplace_back(0.0, createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0), 2.0);
+  same_time_path.emplace_back(1.0, createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), 3.0);
+
+  auto same_time_result = calcInterpolatedPoseWithVelocity(same_time_path, 0.0);
+  ASSERT_TRUE(same_time_result.has_value());
+  EXPECT_NEAR(same_time_result->pose.position.x, 0.0, 1e-6);
+  EXPECT_NEAR(same_time_result->velocity, 1.0, 1e-6);
+
+  // Case with reversed time order
+  std::vector<PoseWithVelocityStamped> reverse_time_path;
+  reverse_time_path.emplace_back(2.0, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), 1.0);
+  reverse_time_path.emplace_back(1.0, createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0), 2.0);
+  reverse_time_path.emplace_back(0.0, createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), 3.0);
+
+  auto reverse_time_result = calcInterpolatedPoseWithVelocity(reverse_time_path, 1.5);
+  ASSERT_FALSE(reverse_time_result.has_value());
+}