Handle NaNs when using OSQP

[maxime-clem]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I've agreed with the maintainers that I can plan this task.

Description

OSQP is used to solve optimization problems in several Autoware nodes. In some cases, OSQP can return a result containing NaN values, which is not currently handled correctly and can lead to crashes.

See https://github.com/autowarefoundation/autoware.universe/issues/2925 for the original reported issue.

Purpose

We want Autoware to remain functional even in the case where OSQP returns NaN values.

Possible approaches

Special cases need to be implemented when OSQP returns NaN values.

• Use a previous solution (e.g., previously computed trajectory).
• Use an emergency solution (e.g., stopping trajectory).
• Do not publish anything.

Definition of done

Handling of NaNs is implemented in the following packages:

• [ ] motion_velocity_smoother.
• [ ] mpc_lateral_controller.
• [ ] obstacle_avoidance_planner.
• [ ] obstacle_cruise_planner.

[soblin]

I think returning std::optional is an option. Like returning nullopt if not has_solution.