How to deal with the failure of speed smoothing in the planning module in autoware?

[hzkkk1997]

While reading the smoother code for motion_velocity_smoother, I've seen that when the smoothVelocity function fails it outputs the result of the previous frame, but what I've seen in the smoothVelocity function is that it looks like when smoother_->apply() fails, It does not return false, but continues to compute, and finally returns true, and finally outputs the results of the current frame. I do not understand why smoother_->apply() fails, and does not directly return false and output the results of the previous frame?
if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) { return prev_output_; }
if (!smoother_->apply( initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) { RCLCPP_WARN(get_logger(), "Fail to solve optimization."); }
https://github.com/autowarefoundation/autoware.universe/blob/main/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

[rej55]

@hzkkk1997

Sorry for late reply.
Generally, I agree that it should return false and output the previous trajectory if smoother_->apply() fails.
I have not estimated the negative effects fully,　so there may be issues in edge cases.

[hzkkk1997]

Never mind, I am always online, thank you for your reply! Are you responsible for the development of this node? What you're saying above is that the strategy of returning false if the solution fails hasn't been tested enough yet, so it's not written in the code?

[rej55]

It is difficult to cover all the cases in which optimization fails, and it is difficult to fully examine the side effects of changing to return the previous planned value.
One possible side effect is that if the optimization fails despite stop planning in the previous module of velocity smoother, the planned stop points will be ignored.
However, it is difficult to theoretically ascertain whether such cases of optimization failure occur, and it may not be necessary to consider the above example.
Since it is difficult to cover all such cases of optimization failure, it may be sufficient to consider that many cases are not a problem, and making the change to return the previous planned value may not be a problem.

[hzkkk1997]

Hello, has this problem been fixed?