Possible square root of a negative number in LongitudinalSpeedPlanner::planConstraintsFromJerkAndTimeConstraint

[gmajrobotec]

Description

Location: longitudinal_speed_planning.cpp:65, 74
Possible square root of a negative number. target_speed value can overwhelm the equation’s value.

Example
In the example attached below there are two planConstraintsFromJerkAndTimeConstraint invocations. In the first case, there is deceleration and the statement at line 69 provided to std::sqrt evaluates to -7. In the second case, acceleration occurs, and the statement at line 60 evaluates to -5.

TEST_F(LongitudinalSpeedPlannerTest, example)
{
  geometry_msgs::msg::Twist current_twist{};
  geometry_msgs::msg::Accel current_accel{};
  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 target_speed = 5.0;
  const double acceleration_duration = 1.0;
  const double plausible_lower_bound = 0.0;
  const double plausible_upper_bound = 1e2;

  traffic_simulator_msgs::msg::DynamicConstraints new_constraints;
  {
    current_twist.linear.x = 10.0;
    const auto problematic_value =
      (constraints.max_deceleration_rate * acceleration_duration *
       constraints.max_deceleration_rate * acceleration_duration) -
      2 * constraints.max_deceleration_rate *
        (current_twist.linear.x - current_accel.linear.x * acceleration_duration - target_speed);

    EXPECT_TRUE(problematic_value >= 0);

    new_constraints = planner.planConstraintsFromJerkAndTimeConstraint(
      target_speed, current_twist, current_accel, acceleration_duration, constraints);

    EXPECT_CONSTRAINTS_BOUNDED(new_constraints, plausible_lower_bound, plausible_upper_bound);
  }
  {
    current_twist.linear.x = 1.0;

    const auto problematic_value =
      (constraints.max_acceleration_rate * acceleration_duration *
       constraints.max_acceleration_rate * acceleration_duration) +
      2 * constraints.max_acceleration_rate *
        (current_twist.linear.x + current_accel.linear.x * acceleration_duration - target_speed);

    EXPECT_TRUE(problematic_value >= 0);

    new_constraints = planner.planConstraintsFromJerkAndTimeConstraint(
      target_speed, current_twist, current_accel, acceleration_duration, constraints);
    EXPECT_CONSTRAINTS_BOUNDED(new_constraints, plausible_lower_bound, plausible_upper_bound);
  }
}

Solution
There is no proposed solution as we are not certain what exactly the equations in this undocumented code calculate, but probably some conditions should be added before the calculations to make sure such situations never take place.