Function LongitudinalSpeedPlanner::getQuadraticAccelerationDuration can return negative duration.

[gmajrobotec]

Description

Location: longitudinal_speed_planning.cpp:186, 200

Function getQuadraticAccelerationDuration can return a negative duration. It is possible because getQuadraticAccelerationDurationWithConstantJerk is being invoked if-and-only-if v and target_speed are almost equal, which does not seem correct. This part of the code also seems incongruent with the comments in 185, 189, 199 and 203.

Example In the example attached below, both usages at first and third invocation (in these examples std::abs(v - target_speed) >= 0.01 evaluates to false) result in a negative duration of -5.64109 and -5.6312 respectively, while the second call returns 0.01.

TEST_F(LongitudinalSpeedPlannerTest, example)
{
  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);

  const double epsilon = 1e-5;
  double target_speed, result_duration;

  {
    target_speed = current_twist.linear.x + epsilon;
    result_duration =
      planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel);
    EXPECT_GE(result_duration, 0.0);
    EXPECT_LE(result_duration, epsilon);
  }
  {
    target_speed = current_twist.linear.x + 0.0100;
    result_duration =
      planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel);
    EXPECT_GE(result_duration, 0.0);
    EXPECT_LE(result_duration, 0.0100 + epsilon);
  }
  {
    target_speed = current_twist.linear.x + 0.0099;
    result_duration =
      planner.getAccelerationDuration(target_speed, constraints, current_twist, current_accel);
    EXPECT_GE(result_duration, 0.0);
    EXPECT_LE(result_duration, 0.0099 + epsilon);
  }
}

Solution We want to make a suggestion based on our understanding of the code. Further review or any suggestions are welcome as we are not sure if the solution provided below is correct and in accordance with the author’s intentions.

auto LongitudinalSpeedPlanner::getQuadraticAccelerationDuration(
  double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints,
  const geometry_msgs::msg::Twist & current_twist,
  const geometry_msgs::msg::Accel & current_accel) const -> double
{
  if (isAccelerating(target_speed, current_twist)) {
    double duration =
      (constraints.max_acceleration - current_accel.linear.x) / constraints.max_acceleration_rate;
    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.
    else {
      return getQuadraticAccelerationDurationWithConstantJerk(
        target_speed, constraints, current_twist, current_accel);
    }
  } else {
    double duration =
      (current_accel.linear.x - constraints.max_deceleration) / constraints.max_deceleration_rate;
    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.
    else {
      return getQuadraticAccelerationDurationWithConstantJerk(
        target_speed, constraints, current_twist, current_accel);
    }
  }
}

[hakuturu583]

This is a small problem, but thank you for reporting.