Merge pull request #1411 from tier4/RJD-1337/getQuadraticAccelerationDuration

RJD-1337 Fix getQuadraticAccelerationDuration returning negative duration

Author: hakuturu583

simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp

@@ -194,7 +194,7 @@ auto LongitudinalSpeedPlanner::getQuadraticAccelerationDuration(
     double v = getVelocityWithConstantJerk(
       current_twist, current_accel, constraints.max_acceleration_rate, duration);
     // While quadratic acceleration, the entity does not reached the target speed.
-    if (std::abs(v - target_speed) >= 0.01) {
+    if (v < target_speed) {
       return duration;
     }
     // While quadratic acceleration, the entity reached the target speed.
@@ -208,7 +208,7 @@ auto LongitudinalSpeedPlanner::getQuadraticAccelerationDuration(
     double v = getVelocityWithConstantJerk(
       current_twist, current_accel, -constraints.max_deceleration_rate, duration);
     // While quadratic acceleration, the entity does not reached the target speed.
-    if (std::abs(v - target_speed) >= 0.01) {
+    if (v < target_speed) {
       return duration;
     }
     // While quadratic acceleration, the entity reached the target speed.

simulation/traffic_simulator/test/src/behavior/test_longitudinal_speed_planner.cpp

@@ -121,6 +121,49 @@ TEST_F(LongitudinalSpeedPlannerTest, getAccelerationDuration_acceleration)
       8.5, constraints, makeTwistWithLinearX(1.0), makeAccelWithLinearX(1.0)),
     4.0, 1e-5);
 }
+/**
+ * @note Test calculations correctness when difference between getVelocityWithConstantJerk 
+ * and target_speed is more than 0.01, https://github.com/tier4/scenario_simulator_v2/issues/1395
+ */
+
+TEST_F(LongitudinalSpeedPlannerTest, getAccelerationDuration_targetSpeed_difference)
+{
+  geometry_msgs::msg::Twist current_twist{};
+  geometry_msgs::msg::Accel current_accel{};
+  current_twist.linear.x = 1.0;
+  current_accel.linear.x = 1.0;
+
+  const auto constraints =
+    traffic_simulator_msgs::build<traffic_simulator_msgs::msg::DynamicConstraints>()
+      .max_acceleration(1.0)
+      .max_acceleration_rate(1.0)
+      .max_deceleration(1.0)
+      .max_deceleration_rate(1.0)
+      .max_speed(10.0);
+
+  constexpr double epsilon = 1e-5;
+  {
+    const double target_speed = current_twist.linear.x + epsilon;
+    const double result_duration =
+      planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel);
+    EXPECT_GE(result_duration, 0.0);
+    EXPECT_LE(result_duration, epsilon);
+  }
+  {
+    const double target_speed = current_twist.linear.x + 0.0100;
+    const double result_duration =
+      planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel);
+    EXPECT_GE(result_duration, 0.0);
+    EXPECT_LE(result_duration, 0.0100 + epsilon);
+  }
+  {
+    const double target_speed = current_twist.linear.x + 0.0099;
+    const double result_duration =
+      planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel);
+    EXPECT_GE(result_duration, 0.0);
+    EXPECT_LE(result_duration, 0.0099 + epsilon);
+  }
+}
 
 /**
  * @note Test functionality aggregation used in other classes.