`ObstacleAvoidancePlanner` sometimes crash in `SplineInterpolation::getSplineInterpolatedDiffValues` because of NaN filled trajectories

VRichardJP commented 1 year ago

Checklist

[X] I've read the contribution guidelines.
[X] I've searched other issues and no duplicate issues were found.
[X] I'm convinced that this is not my fault but a bug.

Description

While playing with the planning simulator, I caught this crash in GDB:

Thread 12 "component_conta" received signal SIGABRT, Aborted.
[Switching to Thread 0x7fffe57fa640 (LWP 452830)]
__pthread_kill_implementation (no_tid=0, signo=6, threadid=140737043736128) at ./nptl/pthread_kill.c:44
44  ./nptl/pthread_kill.c: No such file or directory.
(gdb) bt
#0  __pthread_kill_implementation (no_tid=0, signo=6, threadid=140737043736128) at ./nptl/pthread_kill.c:44
#1  __pthread_kill_internal (signo=6, threadid=140737043736128) at ./nptl/pthread_kill.c:78
#2  __GI___pthread_kill (threadid=140737043736128, signo=signo@entry=6) at ./nptl/pthread_kill.c:89
#3  0x00007ffff7925476 in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#4  0x00007ffff790b7f3 in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007ffff7bcdbbe in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff7bd924c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff7bd92b7 in std::terminate() () from /lib/x86_64-linux-gnu/libstdc++.so.6
#8  0x00007ffff7bd9518 in __cxa_throw () from /lib/x86_64-linux-gnu/libstdc++.so.6
#9  0x00007fffe47980fe in interpolation_utils::validateKeys (base_keys=..., query_keys=...) at /home/sig/autoware/install/interpolation/include/interpolation/interpolation_utils.hpp:79
#10 0x00007ffff402e1e5 in SplineInterpolation::getSplineInterpolatedDiffValues (this=this@entry=0x7fffe57f7340, query_keys=std::vector of length 0, capacity 0) at /home/sig/autoware/src/universe/autoware.universe/common/interpolation/src/spline_interpolation.cpp:248
#11 0x00007ffff40312ef in interpolation::slerp2dFromXY (base_keys=std::vector of length 99, capacity 128 = {...}, base_x_values=std::vector of length 99, capacity 99 = {...}, base_y_values=std::vector of length 99, capacity 99 = {...}, query_keys=std::vector of length 0, capacity 0) at /home/sig/autoware/src/universe/autoware.universe/common/interpolation/src/spline_interpolation_points_2d.cpp:79
#12 0x00007fffe43ae36e in interpolation_utils::interpolate2DTrajectoryPoints (base_x=std::vector of length 99, capacity 99 = {...}, base_y=std::vector of length 99, capacity 99 = {...}, resolution=0.10000000000000001) at /home/sig/autoware/src/universe/autoware.universe/planning/obstacle_avoidance_planner/src/utils/utils.cpp:393
#13 0x00007fffe43af034 in interpolation_utils::getInterpolatedTrajectoryPoints (points=std::vector of length 99, capacity 99 = {...}, delta_arc_length=0.10000000000000001) at /usr/include/c++/11/array:56
#14 0x00007fffe43d608f in ObstacleAvoidancePlanner::generateFineTrajectoryPoints (this=0x7fffbc010fc0, path_points=std::vector of length 352, capacity 352 = {...}, traj_points=std::vector of length 99, capacity 99 = {...}) at /home/sig/autoware/src/universe/autoware.universe/planning/obstacle_avoidance_planner/src/node.cpp:1539
#15 0x00007fffe43dfa75 in ObstacleAvoidancePlanner::generatePostProcessedTrajectory (this=0x7fffbc010fc0, path_points=std::vector of length 352, capacity 352 = {...}, optimized_traj_points=..., planner_data=...) at /home/sig/autoware/src/universe/autoware.universe/planning/obstacle_avoidance_planner/src/node.cpp:1437
#16 0x00007fffe43e0c27 in ObstacleAvoidancePlanner::generateTrajectory (this=0x7fffbc010fc0, planner_data=...) at /home/sig/autoware/src/universe/autoware.universe/planning/obstacle_avoidance_planner/src/node.cpp:972
#17 0x00007fffe43e25fc in ObstacleAvoidancePlanner::onPath (this=0x7fffbc010fc0, path_ptr=std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >> (use count 4, weak count 0) = {...}) at /home/sig/autoware/src/universe/autoware.universe/planning/obstacle_avoidance_planner/src/node.cpp:931
#18 0x00007fffe44000f7 in std::__invoke_impl<void, void (ObstacleAvoidancePlanner::*&)(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >), ObstacleAvoidancePlanner*&, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > > (__f=<optimized out>, __t=<optimized out>, __f=<optimized out>, __t=<optimized out>) at /usr/include/c++/11/bits/invoke.h:74
#19 std::__invoke<void (ObstacleAvoidancePlanner::*&)(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >), ObstacleAvoidancePlanner*&, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > > (__fn=<optimized out>) at /usr/include/c++/11/bits/invoke.h:96
#20 std::_Bind<void (ObstacleAvoidancePlanner::*(ObstacleAvoidancePlanner*, std::_Placeholder<1>))(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>::__call<void, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul>) (__args=..., this=<optimized out>) at /usr/include/c++/11/functional:420
#21 std::_Bind<void (ObstacleAvoidancePlanner::*(ObstacleAvoidancePlanner*, std::_Placeholder<1>))(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>::operator()<std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, void>(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >&&) (this=<optimized out>) at /usr/include/c++/11/functional:503
#22 std::__invoke_impl<void, std::_Bind<void (ObstacleAvoidancePlanner::*(ObstacleAvoidancePlanner*, std::_Placeholder<1>))(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >(std::__invoke_other, std::_Bind<void (ObstacleAvoidancePlanner::*(ObstacleAvoidancePlanner*, std::_Placeholder<1>))(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >&&) (__f=...) at /usr/include/c++/11/bits/invoke.h:61
#23 std::__invoke_r<void, std::_Bind<void (ObstacleAvoidancePlanner::*(ObstacleAvoidancePlanner*, std::_Placeholder<1>))(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >(std::_Bind<void (ObstacleAvoidancePlanner::*(ObstacleAvoidancePlanner*, std::_Placeholder<1>))(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >&&) (__fn=...) at /usr/include/c++/11/bits/invoke.h:111
#24 std::_Function_handler<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >), std::_Bind<void (ObstacleAvoidancePlanner::*(ObstacleAvoidancePlanner*, std::_Placeholder<1>))(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >&&) (__functor=..., __args#0=...) at /usr/include/c++/11/bits/std_function.h:290
#25 0x00007fffe43fe9a2 in std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>::operator()(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >) const (__args#0=std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >> (empty) = {...}, this=<optimized out>) at /usr/include/c++/11/bits/std_function.h:590
#26 rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}::operator()<std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&>(std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&) const (__closure=<optimized out>, __closure=<optimized out>, callback=...) at /opt/ros/humble/include/rclcpp/rclcpp/any_subscription_callback.hpp:556
#27 std::__invoke_impl<void, rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&>(std::__invoke_other, rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&) (__f=...) at /usr/include/c++/11/bits/invoke.h:61
#28 std::__invoke<rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&>(rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>&) (__fn=...) at /usr/include/c++/11/bits/invoke.h:96
#29 std::__detail::__variant::__gen_vtable_impl<std::__detail::__variant::_Multi_array<std::__detail::__variant::__deduce_visit_result<void> (*)(rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::variant<std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&)>, std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&)>, std::integer_sequence<unsigned long, 16ul> >::__visit_invoke(rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::variant<std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&)>, std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&) (__visitor=..., __vars#0=...) at /usr/include/c++/11/variant:1035
#30 0x00007fffe443a86f in std::__do_visit<std::__detail::__variant::__deduce_visit_result<void>, rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}, std::variant<std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&)>, std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&>(rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::variant<std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&)>, std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&) (__visitor=...) at /usr/include/c++/11/variant:863
#31 std::visit<rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}, std::variant<std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&)>, std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&>(rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::variant<std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&)>, std::function<void (autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::default_delete<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&) (__visitor=...) at /usr/include/c++/11/variant:1795
#32 rclcpp::AnySubscriptionCallback<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> >::dispatch (message_info=..., message=std::shared_ptr<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >> (use count 4, weak count 0) = {...}, this=0x7fffbc04ad78) at /opt/ros/humble/include/rclcpp/rclcpp/any_subscription_callback.hpp:504
#33 rclcpp::Subscription<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void>, autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, rclcpp::message_memory_strategy::MessageMemoryStrategy<autoware_auto_planning_msgs::msg::Path_<std::allocator<void> >, std::allocator<void> > >::handle_message (this=0x7fffbc04ac10, message=..., message_info=...) at /opt/ros/humble/include/rclcpp/rclcpp/subscription.hpp:343
#34 0x00007ffff7e70eac in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) () from /opt/ros/humble/lib/librclcpp.so
#35 0x00007ffff7e716af in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () from /opt/ros/humble/lib/librclcpp.so
#36 0x00007ffff7e7890a in rclcpp::executors::MultiThreadedExecutor::run(unsigned long) () from /opt/ros/humble/lib/librclcpp.so
#37 0x00007ffff7c072b3 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#38 0x00007ffff7977b43 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:442
#39 0x00007ffff7a09a00 in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:81
(gdb)

It is cause in ObstacleAvoidancePlanner::generateFineTrajectoryPoints, because the trajectory is filled with nan values:

#14 0x00007fffe43d608f in ObstacleAvoidancePlanner::generateFineTrajectoryPoints (this=0x7fffbc010fc0, path_points=std::vector of length 352, capacity 352 = {...}, traj_points=std::vector of length 99, capacity 99 = {...}) at /home/sig/autoware/src/universe/autoware.universe/planning/obstacle_avoidance_planner/src/node.cpp:1539
1539      auto interpolated_traj_points = interpolation_utils::getInterpolatedTrajectoryPoints(
(gdb) p traj_points
$1 = std::vector of length 99, capacity 99 = {{time_from_start = {sec = 0, nanosec = 0}, pose = {position = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = 0}, orientation = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = -nan(0x8000000000000), w = -nan(0x8000000000000)}}, longitudinal_velocity_mps = 8.33333302, lateral_velocity_mps = 0, acceleration_mps2 = 0, heading_rate_rps = 0, front_wheel_angle_rad = 0, rear_wheel_angle_rad = 0}, {time_from_start = {sec = 0, nanosec = 0}, pose = {position = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = 0}, orientation = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = -nan(0x8000000000000), w = -nan(0x8000000000000)}}, longitudinal_velocity_mps = 8.33333302, lateral_velocity_mps = 0, acceleration_mps2 = 0, heading_rate_rps = 0, front_wheel_angle_rad = 0, rear_wheel_angle_rad = 0}, {time_from_start = {sec = 0, nanosec = 0}, pose = {position = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = 0}, orientation = {x = -nan(0x8000000000000), y = -nan(0x8000000000000)

This is the full_traj_points from here:

https://github.com/autowarefoundation/autoware.universe/blob/e50dc494f5f3093adc574ffbea48b67ba97b06e5/planning/obstacle_avoidance_planner/src/node.cpp#L1433-L1437

extended_traj_points is empty so points_utils::concatTrajectory should just copy optimized_traj_points into full_traj_points:

1437      const auto fine_traj_points = generateFineTrajectoryPoints(path_points, full_traj_points);
(gdb) p optimized_traj_points
$4 = <optimized out>
(gdb) p extended_traj_points
$5 = std::vector of length 0, capacity 0

Going up 1 frame, I can see optimized_traj_points is indeed filled with NaN values:

https://github.com/autowarefoundation/autoware.universe/blob/e50dc494f5f3093adc574ffbea48b67ba97b06e5/planning/obstacle_avoidance_planner/src/node.cpp#L970-L973

#16 0x00007fffe43e0c27 in ObstacleAvoidancePlanner::generateTrajectory (this=0x7fffbc010fc0, planner_data=...) at /home/sig/autoware/src/universe/autoware.universe/planning/obstacle_avoidance_planner/src/node.cpp:972
972   const auto post_processed_traj_points =
(gdb) p optimized_traj_points
$8 = std::vector of length 99, capacity 99 = {{time_from_start = {sec = 0, nanosec = 0}, pose = {position = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = 0}, orientation = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = -nan(0x8000000000000), w = -nan(0x8000000000000)}}, longitudinal_velocity_mps = 8.33333302, lateral_velocity_mps = 0, acceleration_mps2 = 0, heading_rate_rps = 0, front_wheel_angle_rad = 0, rear_wheel_angle_rad = 0}, {time_from_start = {sec = 0, nanosec = 0}, pose = {position = {x = -nan(0x8000000000000), y = -nan(0x8000000000000), z = 0}, orientation = {x = -nan(0x8000000000000), y = -nan(0x8000000000000),

So generateOptimizedTrajectory(planner_data) must be where the NaNs come from. But I cannot find the cause.

It seems to be the exact same issue I had long time ago: https://github.com/autowarefoundation/autoware.universe/issues/1956

Expected behavior

No crash

Actual behavior

Crash

Steps to reproduce

Run a simulation for quite a long time (>10 minutes?) See an example here: https://github.com/autowarefoundation/autoware.universe/issues/2925#issuecomment-1445520537

Versions

No response

Possible causes

No response

Additional context

No response

VRichardJP commented 1 year ago

Managed reproduce and track further.

in ObstacleAvoidancePlanner::generateOptimizedTrajectory: auto optimal_trajs = optimizeTrajectory(planner_data) is filled with NaNs
in ObstacleAvoidancePlanner::optimizeTrajectory: eb_traj data looks fine, but mpt_trajs is plagued with NaNs past the few first points. debug_data_ also contains quite a lot of NaNs. mpt_trajs.txt debug_data.txt

in MPTOptimizer::getModelPredictiveTrajectory: full_ref_points has NaNs already from index 10: full_ref_points.txt

(gdb) p full_ref_points[10]
$62 = {p = {x = 68340.349355984203, y = 87556.120593821382, z = 0}, k = -0.013618963751321597, v = 0, yaw = 0.13447614778344213, s = 103.00034775595475, alpha = -0.018055671208563676, bounds = {lower_bound = -1.334256644760081, upper_bound = 1.2359930104880295, lower_collision_type = CollisionType::NO_COLLISION, upper_collision_type = CollisionType::NO_COLLISION}, near_objects = false, fix_kinematic_state = {<boost::optional_detail::optional_base<Eigen::Matrix<double, 2, 1, 0, 2, 1> >> = {<boost::optional_detail::optional_tag> = {<No data fields>}, m_initialized = true, m_storage = {dummy_ = {data = "`\367\377\377\377\377\377\377`\367\377\377\377\377\377\377", aligner_ = {<No data fields>}}}}, <No data fields>}, plan_from_ego = true, optimized_kinematic_state = {<Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >> = {<Eigen::MatrixBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >> = {<Eigen::DenseBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >> = {<Eigen::DenseCoeffsBase<Eigen::Matrix<double, 2, 1, 0, 2, 1>, 3>> = {<Eigen::DenseCoeffsBase<Eigen::Matrix<double, 2, 1, 0, 2, 1>, 1>> = {<Eigen::DenseCoeffsBase<Eigen::Matrix<double, 2, 1, 0, 2, 1>, 0>> = {<Eigen::EigenBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >> = {<No data fields>}, <No data fields>}, <No data fields>}, <No data fields>}, <No data fields>}, <No data fields>}, m_storage = {m_data = {array = {-nan(0xffffffffff760), -nan(0xffffffffff760)}}}}, <No data fields>}, optimized_input = -nan(0xfffffffffffff), beta = std::vector of length 2, capacity 2 = {{<boost::optional_detail::tc_optional_base<double>> = {<boost::optional_detail::optional_tag> = {<No data fields>}, m_initialized = true, m_storage = -0.033652800855534099}, <No data fields>}, {<boost::optional_detail::tc_optional_base<double>> = {<boost::optional_detail::optional_tag> = {<No data fields>}, m_initialized = true, m_storage = 0.027222244326961437}, <No data fields>}}, vehicle_bounds = std::vector of length 2, capacity 2 = {{lower_bound = -1.2823146600490181, upper_bound = 1.2887599624182762, lower_collision_type = CollisionType::NO_COLLISION, upper_collision_type = CollisionType::NO_COLLISION}, {lower_bound = -1.286394608775006, upper_bound = 1.2832801305609018, lower_collision_type = CollisionType::NO_COLLISION, upper_collision_type = CollisionType::NO_COLLISION}}, vehicle_bounds_poses = std::vector of length 6, capacity 6 = {{position = {x = 68338.377956195429, y = 87555.785982500194, z = 0}, orientation = {x = 0, y = 0, z = 0.083966274399585844, w = 0.99646859697807511}}, {position = {x = 68344.988822598709, y = 87556.621077250617, z = 0}, orientation = {x = 0, y = 0, z = 0.053703979785262194, w = 0.9985569000088198}}, {position = {x = 68338.37795561926, y = 87555.785985564318, z = 0}, orientation = {x = 0, y = 0, z = 0.083965497661904434, w = 0.99646866242867305}}, {position = {x = 68344.988903609308, y = 87556.620268475846, z = 0}, orientation = {x = 0, y = 0, z = 0.053617014340098697, w = 0.99856157335101459}}, {position = {x = 68338.37795561926, y = 87555.785985564333, z = 0}, orientation = {x = 0, y = 0, z = 0.083965497658517296, w = 0.99646866242895848}}, {position = {x = 68344.988918233212, y = 87556.620121877102, z = 0}, orientation = {x = 0, y = 0, z = 0.053601251579584688, w = 0.9985624195958418}}}}

At this moment I realized prev_optimal_trajs_ptr_ in ObstacleAvoidancePlanner::optimizeTrajectory has NaNs too. But I don't know if its the cause or a consequence. Indeed, by the time the program crashes and is taken over by gdb, the prev_optimal_trajs_ptr_ has been updated already. So as I explore the program state, it may not be exactly the same as how things were before the crash.

So maybe it's wrong, but I tried reset the prev_optimal_trajs_ptr_ and tried to call ObstacleAvoidancePlanner::optimizeTrajectory again

(gdb) call prev_optimal_trajs_ptr_->model_predictive_trajectory.clear()
(gdb) call prev_optimal_trajs_ptr_->mpt_ref_points.clear()

But then, back in MPTOptimizer::getModelPredictiveTrajectory does not produce any more NaNs. Anyway, at this point I totally lost the state of the program. I guess I have no choice but to add some random assert in the code so I can catch the NaNs as early as possible. That's for a next time.

In the meantime, if anyone is versed in the ObstacleAvoidancePlanner and dare to look at the log data, it would be much appreciated!

VRichardJP commented 1 year ago

Down the rabbit hole again. This time I have added a few asserts before prev_optimal_trajs_ptr_ is updated so I can keep the program state unaltered when it crashes.

This time, full_ref_points is totally fine. All data looks fine down to here:

https://github.com/autowarefoundation/autoware.universe/blob/bd0058142ee9266a4c31b73b24172a7097fbc58a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp#L296-L297

Where I suddenly get NaNs only:

(gdb) p optimized_control_variables.get().size()
$59 = 90
(gdb) p *optimized_control_variables.get().data()@90
$60 = {-nan(0xfffffffffffff) <repeats 90 times>}

I also get this error message when the function executeOptimization is called:

ERROR in osqp_update_lower_bound: upper bound must be greater than or equal to lower bound

However I get this one fairly often so it might not be directly related.

Although it is difficult to check everything, the input of the function seemed ok to me (at least no NaN). A few examples:

(gdb) p mpt_matrix.Bex.size()
$50 = 16020
(gdb) p *mpt_matrix.Bex.data()@100
$51 = {1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0}
(gdb) p *mpt_matrix.Bex.data()@1000
$52 = {1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0.50000257173695672, 1, 1.0000034642413596, 1, 1.5000054081307184, 1, 2.0000139428925507, 1, 2.5000152804727662, 1, 3.000021309151208, 1, 3.5000259654839567, 1, 4.0000315638297081, 1, 4.500037724050685, 1, 5.0000446928801665, 1...}
(gdb) p *mpt_matrix.Bex.data()@16020
value requires 128160 bytes, which is more than max-value-size
(gdb) p mpt_matrix.Wex.size()
$53 = 178
(gdb) p *mpt_matrix.Wex.data()@178
$54 = {0, 0, 0, 0.017548233583924384, 0.0087741324538379278, 0.035903063067845728, 0.026725733779343036, 0.055067608044679181, 0.054260007790601972, 0.075040988772028819, 0.091780602549958318, 0.09589394460780834, 0.13972815296761903, 0.11765775156279717, 0.19855757660265938, 0.1404204166941391, 0.26876857107177216, 0.16424070835378637, 0.3508899370077222, 0.18920289957917824, 0.44549270532005592, 0.21538972215560287, 0.55318915218737752, 0.24291996108287903, 0.67465108557201403, 0.27190726913607727, 0.81060706878537769, 0.30248662901855194, 0.96185324737589217, 0.33479013784391437, 1.1292518170858838, 0.36894692815579155, 1.3137297497878737, 0.40509928695989733, 1.5162837929255615, 0.44333602692113677, 1.7379593755244682, 0.4835547148715918, 1.9797411354941548, 0.52546844937934822, 2.2424844164912274, 0.56861228217958348, 2.5267977738685468, 0.61229045931620818, 2.832950336936408, 0.65570661007791509, 3.1608113398848343, 0.69825540524751817, 3.5099465272802162, 0.7394892807304897, 3.8796988629297782, 0.77912851210627332, 4.2692707210784304, 0.81708769152567973, 4.677822014137897, 0.85341319617485789, 5.1045357776541511, 0.88814734227874215, 5.5486163246597267, 0.92134609056122829, 6.0092962938121861, 0.9530662129502725, 6.4858358254733606, 0.98336317382274374, 6.9775239715607213, 1.0122903871298308, 7.4836751692104198, 1.0398938924863852, 8.0036277029285099, 1.0662209094789727, 8.5367433683804954, 1.0913290207769299, 9.0824128175062704, 1.1152822938663745, 9.6400586429758874, 1.1381500161447098, 10.209138092764125, 1.1600033000185128, 10.789143821623517, 1.1809067188044726, 11.379600899674656, 1.20091030116916, 11.980059468917315, 1.2200480935989122, 12.590086684050082, 1.238336540168915, 13.209257924112977, 1.2557808800082013, 13.837151103248322, 1.272379647703032, 14.473343387839726, 1.2881285449416702, 15.117409801600719, 1.3030243811340845, 15.768923884916632, 1.3170678912062559, 16.427459537629545, 1.3302624978163577, 17.09259232660084, 1.3426137760640984, 17.763900574848495, 1.3541289932545102, 18.440966220964121, 1.3648165792880123, 19.123375438357407, 1.3746857033306092, 19.810719056608026, 1.3837458798886353, 20.502592645172847, 1.39200666069882, 21.198596518208618, 1.3994773205680355, 21.898335609782649, 1.4061666411658453, 22.601419240042823, 1.412082730645791, 23.307460805605231, 1.41723284433078, 24.016077361848154, 1.4216233488462382, 24.726889126839065, 1.4252595639050101, 25.439518961858088, 1.4281455684737199, 26.153591767738487, 1.4302858183761729, 26.868734676206092, 1.4316781317527152, 27.584573729445655, 1.4323367318909694, 28.300742192065478, 1.4323138825330586, 29.016899065712028, 1.431693381951247, 29.732745875588929, 1.4305892768948338, 30.448040532774677, 1.4291600460088645, 31.162620595789136, 1.4275625325240342, 31.876401897472192, 1.4259093274243579, 32.589356588591926, 1.4242825564481849, 33.301497894602491, 1.4227319896460016, 34.012863913246356, 1.4212683433782012, 34.723498106324925, 1.4198865061633856, 35.433441377466032, 1.4185867156396756, 36.142734750533542, 1.4173676357660683, 36.851418581692009, 1.4162281040265308, 37.559532645361941, 1.4151669269470841, 38.267116118956245, 1.4141828313666043, 38.974207543090408, 1.4132745481089375, 39.680844823835287, 1.412440800629297, 40.387065229352793, 1.4116803313260449, 41.092905399221223, 1.4109919152988799, 41.798401360316177, 1.4103743744531358, 42.503588550233665, 1.4098265893409514, 43.208501846844236, 1.4093475089949405, 43.91317560256558, 1.4089361584865954, 44.617643683032391, 1.4085916441728985}

Unfortunately, I broke the gdb stack after that. So that's for a next time...

VRichardJP commented 1 year ago

The first NaNs appear here:

https://github.com/autowarefoundation/autoware.universe/blob/bd0058142ee9266a4c31b73b24172a7097fbc58a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp#L842

(gdb) p result
$18 = std::tuple containing = {[1] = std::vector of length 179, capacity 179 = {-nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff)}, [2] = std::vector of length 628, capacity 628 = {-nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff), -nan(0xfffffffffffff)...}, [3] = -1, [4] = 1, [5] = 25}

The result reports status_polish of -1. I don't know if it unusual or not. @satoshi-ota ?

Since the enable_warm_start is true, the the OSQP solver is initialized with:

https://github.com/autowarefoundation/autoware.universe/blob/bd0058142ee9266a4c31b73b24172a7097fbc58a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp#L816-L825

(gdb) p mpt_param_.enable_warm_start && prev_mat_n == H.rows() && prev_mat_m == A.rows()
$21 = true

I checked all the input data, I don't see any NaN:

(gdb) p P_csc
$22 = {m_vals = std::vector of length 4095, capacity 4095 = {8844.8573278948734, 223679.12554210974, 7112705.0079846624, 28250.334253672951, 903023.6795740769, 114690.74854535289, 27729.579774967053, 890989.29043587775, 113141.86347875866, 111706.64825871879, 27222.62688385712, 879225.90475084784, 111667.04849576758, 110210.44612973722, 108836.80878267472, 26724.969229357415, 867593.9739895669, 110198.59205709063, 108780.04733563663, 107384.09226009792, 106053.32694241795, 26239.304403438047, 856186.29040130309, 108758.29066053593, 107367.10708203627, 106008.1439534235, 104654.23502986548, 103376.22033165891, 25771.230830259447, 845192.44160932302, 107370.24060823284, 106005.42735684403, 104672.4787645693, 103354.45375494292, 102052.06454184778, 100847.58885423384, 25331.533362180711, 834976.89573328744, 106080.76381244497, 104740.76282876113, 103432.29800456118, 102138.63766335334, 100870.3756738677, 99639.541952294283, 98548.868095484475, 24915.356373609171, 825393.34852934873, 104871.29975399264, 103554.8148244341, 102269.55950435452, 100998.99869564953, 99753.61556310879, 98555.208157461646, 97436.059107578447, 96438.932951354291, 24513.518060156184, 816148.07349276461, 103704.54901622112, 102410.77287071499, 101147.9307601766, 99899.677238934513, 98676.388705861464, 97499.639589180253, 96411.279513248897, 95384.284099555036, 94444.502776100737, 24126.512037194872, 807265.12521659071, 102583.58287968465, 101311.68496241757, 100070.43749393834, 98843.677281581215, 97641.678222020608, 96485.800038090485, 95417.476622259099, 94419.886541653192, 93449.250267959738, 92568.071417546947, 23736.929354379532, 798167.89341231936, 101435.13967934214, 100185.2093342601, 98965.636024960331, 97760.442963706082, 96579.798485567066, 95444.841297801991, 94396.577296569143, 93418.391897670735, 92476.885603769959, 91564.553080186379, 90675.712770895683, 23310.428919499165, 787695.67474303616, 100111.70002981287, 98885.632756046936, 97689.575054385015, 96507.76392112131, 95350.244268027076, 94237.896706814296, 93211.227183075767, 92253.855999363266, 91332.821247434127, 90450.603693755285, 89532.339822381153, 88507.221427398457, 22850.72885758456, 775956.35749089147, 98626.942039023867, 97426.431327387108, 96255.532551739307, 95098.721693487518, 93965.903709625316, 92877.666808403053, 91873.950404793039, 90938.635405538997, 90039.250537762637, 89178.257975073342, 88291.718215122572, 87240.635553118511, 86095.981841028624, 22356.633243273343, 762892.91362788423, 96973.586047854769, 95800.382259765902, 94656.340209246177, 93526.201187323561, 92419.712894813012, 91357.135234594549, 90377.771415747702, 89465.789780304651, 88589.26512607855, 87750.636079736098, 86886.998008914845, 85871.393442669549, 84704.580262588803, 83439.353880238792, 21826.188152581268, 748421.42282965244, 95140.932097090685, 93996.881869241799, 92881.487187116683, 91779.782636086442, 90701.34024324239, 89666.053390989036, 88712.514916618384, 87825.209095856524, 86972.815747131957, 86157.744645117054, 85318.243995540644, 84329.63487615998, 83202.442656714164, 81919.550451994568, 80530.897817049874, 21264.61871464107, 732703.96785799623, 93149.552351657228, 92036.222235220746, 90950.991464925028, 89879.212646173357, 88830.265744507851, 87823.643631614017, 86897.162800176593, 86035.64852465874, 85208.437813834447, 84417.90672564898, 83603.561088031754, 82643.214448918108, 81547.143537430602, 80308.333418931739, 78906.877459818992, 77418.783717283921, 20686.579766887317, 716232.56557906582, 91061.973299547855, 89980.157880683226, 88925.854303720844, 87884.748070350397, 86866.016962871436, 85888.732378815752, 84989.893763339758, 84154.682209590668, 83353.123497994995, 82587.556639130722, 81798.809387565372, 80867.320091663147, 79803.107712484692, 78599.268975571395, 77246.064265900815, 75749.11527181034, 74218.33361948005, 20090.643312371962, 698949.14024292899, 88870.795091280161, 87821.360499604329, 86798.816599974467, 85789.197818289787, 84801.46886608495, 83854.257115176006, 82983.70117768184, 82175.353950350327...}, m_row_idxs = std::vector of length 4095, capacity 4095 = {0, 0, 1, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 1, 2, 3, 4, 5, 0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 8, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9...}, m_col_idxs = std::vector of length 180, capacity 180 = {0, 1, 3, 6, 10, 15, 21, 28, 36, 45, 55, 66, 78, 91, 105, 120, 136, 153, 171, 190, 210, 231, 253, 276, 300, 325, 351, 378, 406, 435, 465, 496, 528, 561, 595, 630, 666, 703, 741, 780, 820, 861, 903, 946, 990, 1035, 1081, 1128, 1176, 1225, 1275, 1326, 1378, 1431, 1485, 1540, 1596, 1653, 1711, 1770, 1830, 1891, 1953, 2016, 2080, 2145, 2211, 2278, 2346, 2415, 2485, 2556, 2628, 2701, 2775, 2850, 2926, 3003, 3081, 3160, 3240, 3321, 3403, 3486, 3570, 3655, 3741, 3828, 3916, 4005, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095}}
(gdb) p f
$23 = std::vector of length 179, capacity 179 = {57.044268201842286, 1128.3183213377647, 140.12988628746132, 136.68268942641242, 133.35402586313003, 129.98125970551854, 126.60345945899397, 123.17915521325899, 119.84818815667367, 116.63075266428936, 113.46881094066478, 110.28617593499484, 107.28765555687555, 104.43750565407049, 101.46381808111744, 98.226204736574388, 94.484403887820235, 90.327020205454573, 86.133753909248995, 81.944824283200148, 77.755387402203496, 73.582572924833357, 69.424467792524851, 65.293697140528806, 61.202057208812107, 57.14356991339217, 53.117122176961665, 49.122676494114359, 45.167854108332484, 41.278401200952302, 37.471467991697295, 33.756233602802482, 30.147641345709083, 26.659935761372374, 23.307060498519103, 20.098165842263789, 17.036309018052094, 14.12513128381757, 11.372344970738684, 8.7868176841804875, 6.3727333438958809, 4.1329098683046226, 2.0813697304627254, 0.23119402084057938, -1.4120127811199019, -2.8439040796269808, -4.0639643215122838, -5.0753835187639069, -5.8838358959959649, -6.496634946712053, -6.9223982828810797, -7.1705365357500312, -7.2514133523944793, -7.1760026301402711, -6.9544229819649352, -6.6091903824229874, -6.1696814374330611, -5.6593430293489213, -5.0993521273280145, -4.5122251613405044, -3.9159203230559658, -3.3588072373093762, -2.8284481378205157, -2.2448801237600478, -1.6308493799644683, -1.0359469553063576, -0.47561202818133275, 0.031712886240454097, 0.47885903506284855, 0.86225297064313189, 1.1794909662825006, 1.4300306758726986, 1.6147398016296481, 1.7358109288611345, 1.7966003780493427, 1.8014746717736845, 1.7556697459708275, 1.6651514392912627, 1.5364818232619299, 1.3766901689473405, 1.1931474477362034, 0.99344440793600519, 0.78527426349186769, 0.60183643268374309, 0.33078609236194323, 0.19945891597763454, 0.083365887283917361, 0.00666105892431057, -0.033061254832142062, -0.039145333560314466, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}
(gdb) p A_csc
$24 = {m_vals = std::vector of length 16994, capacity 16994 = {0.99670378972464124, 0.9963223310057755, 0.9959867180569939, 0.99568964028580953, 0.99523264246981225, 0.9948605324727966, 0.99431984980398203, 0.99377888109796919, 0.9931747876250181, 0.99245325264865825, 0.99137506679278498, 0.99004352399323681, 0.9889197077818227, 0.98785128764522612, 0.98737835453600165, 0.98733018414549567, 0.98714334918925351, 0.98714750681226515, 0.9874233664685903, 0.9879077458958857, 0.98863278416471123, 0.98942805817131285, 0.99017188749929752, 0.99089634997202991, 0.99158131757749068, 0.99222419478190693, 0.9928187254786851, 0.99336619812759674, 0.9938888403740338, 0.99439234573462654, 0.99487993186270063, 0.99535225680482986, 0.99579379254083056, 0.99620275697782057, 0.99658296603460028, 0.99693487222852306, 0.99726036741810753, 0.99755907916721664, 0.99783396086491949, 0.99808542442873505, 0.99830700833615971, 0.99850076952622779, 0.99866999475300589, 0.99881747132840604, 0.99895254131206968, 0.99907870458371351, 0.99919613222538994, 0.9993063800479427, 0.99940948777965222, 0.99950499017758021, 0.99959253949328408, 0.99967192373114111, 0.99974294597164182, 0.99980546439512985, 0.99985941331527628, 0.99990465776489545, 0.99994123014301739, 0.99996906234452154, 0.99998785148210423, 0.99999832198788441, 0.99999955154435394, 0.99999603816919935, 0.99999213908479889, 0.9999911808695795, 0.99999211971694635, 0.99999247146034043, 0.99999287228811695, 0.99999330605351855, 0.999993698049924, 0.99999409671174744, 0.99999448176095573, 0.99999485968420054, 0.99999522852523437, 0.99999558803044808, 0.99999593761740468, 0.99999627655094447, 0.99999660410202007, 0.99999691953995373, 0.99999722216756493, 0.99999751132077375, 0.99999778640938441, 0.9999980469211065, 0.99999829241811145, 0.99999852253218435, 0.99999873694928387, 0.99999893540020224, 0.99999911765262961, 0.99999928349800682, 0.99999943273051772, -0.99670378972464124, -0.9963223310057755, -0.9959867180569939, -0.99568964028580953, -0.99523264246981225, -0.9948605324727966, -0.99431984980398203, -0.99377888109796919, -0.9931747876250181, -0.99245325264865825, -0.99137506679278498, -0.99004352399323681, -0.9889197077818227, -0.98785128764522612, -0.98737835453600165, -0.98733018414549567, -0.98714334918925351, -0.98714750681226515, -0.9874233664685903, -0.9879077458958857, -0.98863278416471123, -0.98942805817131285, -0.99017188749929752, -0.99089634997202991, -0.99158131757749068, -0.99222419478190693, -0.9928187254786851, -0.99336619812759674, -0.9938888403740338, -0.99439234573462654, -0.99487993186270063, -0.99535225680482986, -0.99579379254083056, -0.99620275697782057, -0.99658296603460028, -0.99693487222852306, -0.99726036741810753, -0.99755907916721664, -0.99783396086491949, -0.99808542442873505, -0.99830700833615971, -0.99850076952622779, -0.99866999475300589, -0.99881747132840604, -0.99895254131206968, -0.99907870458371351, -0.99919613222538994, -0.9993063800479427, -0.99940948777965222, -0.99950499017758021, -0.99959253949328408, -0.99967192373114111, -0.99974294597164182, -0.99980546439512985, -0.99985941331527628, -0.99990465776489545, -0.99994123014301739, -0.99996906234452154, -0.99998785148210423, -0.99999832198788441, -0.99999955154435394, -0.99999603816919935, -0.99999213908479889, -0.9999911808695795, -0.99999211971694635, -0.99999247146034043, -0.99999287228811695, -0.99999330605351855, -0.999993698049924, -0.99999409671174744, -0.99999448176095573, -0.99999485968420054, -0.99999522852523437, -0.99999558803044808, -0.99999593761740468, -0.99999627655094447, -0.99999660410202007, -0.99999691953995373, -0.99999722216756493, -0.99999751132077375, -0.99999778640938441, -0.9999980469211065, -0.99999829241811145, -0.99999852253218435, -0.99999873694928387, -0.99999893540020224, -0.99999911765262961, -0.99999928349800682, -0.99999943273051772, 0.9666768130798683, 0.96244232753665582, 0.95803713377671407, 0.95373577796196018, 0.94952828257148192, 0.94526807873133523, 0.94137853046252318, 0.93780763197236405, 0.93511306482221235, 0.9332805127887559, 0.9334241301864793, 0.93505546385199279, 0.93693793330707686, 0.93941631504885881, 0.94235402794512879, 0.94565979281482282, 0.94934860708073021, 0.95302733253262972, 0.95646827680341873, 0.95978492294901174, 0.96293575061251124, 0.96592044599244942...}, m_row_idxs = std::vector of length 16994, capacity 16994 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 267, 268, 269, 270, 271, 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, 286, 287, 288...}, m_col_idxs = std::vector of length 180, capacity 180 = {0, 359, 720, 1076, 1426, 1771, 2112, 2449, 2782, 3111, 3436, 3757, 4074, 4387, 4696, 5001, 5302, 5599, 5892, 6181, 6466, 6747, 7024, 7297, 7566, 7831, 8092, 8349, 8602, 8851, 9096, 9337, 9574, 9807, 10036, 10261, 10482, 10699, 10912, 11121, 11326, 11527, 11724, 11917, 12106, 12291, 12472, 12649, 12822, 12991, 13156, 13317, 13474, 13627, 13776, 13921, 14062, 14199, 14332, 14461, 14586, 14707, 14824, 14937, 15046, 15151, 15252, 15349, 15442, 15531, 15616, 15697, 15774, 15847, 15916, 15981, 16042, 16099, 16152, 16199, 16242, 16281, 16316, 16347, 16374, 16397, 16416, 16433, 16446, 16455, 16460, 16466, 16472, 16478, 16484, 16490, 16496, 16502, 16508, 16514, 16520, 16526, 16532, 16538, 16544, 16550, 16556, 16562, 16568, 16574, 16580, 16586, 16592, 16598, 16604, 16610, 16616, 16622, 16628, 16634, 16640, 16646, 16652, 16658, 16664, 16670, 16676, 16682, 16688, 16694, 16700, 16706, 16712, 16718, 16724, 16730, 16736, 16742, 16748, 16754, 16760, 16766, 16772, 16778, 16784, 16790, 16796, 16802, 16808, 16814, 16820, 16826, 16832, 16838, 16844, 16850, 16856, 16862, 16868, 16874, 16880, 16886, 16892, 16898, 16904, 16910, 16916, 16922, 16928, 16934, 16940, 16946, 16952, 16958, 16964, 16970, 16976, 16982, 16988, 16994}}
(gdb) p lower_bound
$19 = std::vector of length 628, capacity 628 = {-1.0876527711970603, -1.054262245294036, -1.0121502558614712, -1.0270082615776137, -1.0606913617274827, -1.0828553425516152, -1.0951748587883308, -1.0958096045904862, -1.0882452971984555, -1.1118380651141464, -1.1257811390271832, -1.125557134137241, -1.1055640802138094, -1.0692292332921891, -1.0182203764143818, -0.95423025696367214, -0.925327687877606, -0.96129671317600884, -0.98257188854043886, -0.98991674260357887, -0.98340499995087738, -0.96309823946719852, -0.92885108451032217, -0.8803270369765186, -0.89194413180231669, -0.90666473819467353, -0.90860871410630528, -0.89795078172855369, -0.87452604935118838, -0.86485566927421775, -0.89978430029676382, -0.92329382454262188, -0.93568101925898706, -0.93716780218526985, -0.93442004559949243, -0.98238234103232802, -1.0207235209651611, -1.0498384223779986, -1.0699867013157807, -1.0814839970646428, -1.0848180604932889, -1.0802264170448534, -1.0932070817098793, -1.1198991071741169, -1.1396525375019717, -1.1527776377251016, -1.1596091270446074, -1.1604929466989038, -1.1597149884481723, -1.1826679292005746, -1.2008549791578673, -1.2146367983146042, -1.2243789939256615, -1.230466344436735, -1.2333240262119105, -1.233427975195486, -1.2312184466975467, -1.2437051074807606, -1.2559524879149393, -1.2664835241546193, -1.2764032335654854, -1.286423403216812, -1.2956376286946067, -1.3038666319561258, -1.311349785737054, -1.3183543863514409, -1.317062843828019, -1.3134854022307714, -1.3100738270642296, -1.3069093057596355, -1.3040466011559957, -1.3015243531423479, -1.2993688900353391, -1.2975965478290945, -1.2962152995626006, -1.2952261056769174, -1.2946241282203843, -1.2943998611170073, -1.2945401428945118, -1.2950287861583121, -1.2958458010876654, -1.2969624764618359, -1.2983271217486276, -1.3004738101908657, -1.3026048997234341, -1.3047201940037638, -1.3068195152694031, -1.3089027015466215, -1.3109695972889668, -1.5225203276398642, -1.5624148386176056, -1.6143554897860626, -1.5899985784434638, -1.5478516451847031, -1.519596334614759, -1.5038222694695114, -1.5026865449608897, -1.5179395077974833, -1.4911867323596424, -1.4716165090179802, -1.4705544048270334, -1.4942731321423179, -1.5403364531646546, -1.6097466492127617, -1.7002202188689592, -1.6946797733354551, -1.6398467637784615, -1.6077698210228268, -1.5976760590227272, -1.6095538473931492, -1.6428108124249141, -1.6969273567434466, -1.7245262506249031, -1.6899162287392355, -1.6744355330563059, -1.6771135382203743, -1.6971771392064396, -1.7342626254807327, -1.7184987437486328, -1.6777132028675927, -1.6521782614987721, -1.6411228731964949, -1.6438705946516938, -1.6538898884061277, -1.59837340755083, -1.5550653595135344, -1.5232107296969124, -1.5022236409327743, -1.4914942973920802, -1.4902698081374854, -1.4980757604996011, -1.480583846862773, -1.4519475903616677, -1.4312048242574562, -1.417903229055085, -1.4115756127339623, -1.4117564621985448, -1.4130092756468109, -1.3887242934658328, -1.3696918952233084, -1.3554588732076862, -1.345574276771778, -1.3395754665784061, -1.3369671095321678, -1.337210943565232, -1.3398123337184735, -1.3263885384374881, -1.3139819432279083, -1.3033556913186928, -1.2933833805064978, -1.283338899241393, -1.274124234445793, -1.2659239439448982, -1.2584752551233791, -1.251507255719865, -1.2541276402573054, -1.2609078067074009, -1.2675280502092505, -1.2739070155656123, -1.2799895714966922, -1.2857367332453151, -1.2911218459076075, -1.2961282452700722, -1.3007476386624433, -1.3049787511698696, -1.308826112249065, -1.312298925829495, -1.3154100586255666, -1.3181754115098627, -1.3206146968175929, -1.3227563582494568, -1.3246518296825371, -1.3257667928838544, -1.326898657334425, -1.3280473996918758, -1.3292129914318451, -1.3303954020860544, -1.3315946089323916, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0...}
(gdb) p upper_bound
$20 = std::vector of length 628, capacity 628 = {1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30, 1e+30...}

I could not get further down in the osqp code yet.

satoshi-ota commented 1 year ago

@VRichardJP Thanks for your report, but I'm not well versed in obstacle avoidance module :pray: So, @takayuki5168 could you review this issue ?

VRichardJP commented 1 year ago

Unfortunately, I cannot get deeper than:

https://github.com/autowarefoundation/autoware.universe/blob/bd0058142ee9266a4c31b73b24172a7097fbc58a/common/osqp_interface/src/osqp_interface.cpp#L367

Before the data does not seem to contain NaNs. After it does. What happens in between is a black box. Others people have had issues with NaNs in OSQP but I can't see any that fit the current use case.

The OSQP solver maths is beyond my understanding, maybe the input data is absolutely wrong but I can't see.

Anyway, I have wandered around a bit with GDB. Trying to call osqp_solver_ptr_->optimize() manually and see what is going on in OSQP m_work input/output. If any one is interested: gdb.txt

Regarding the original crash itself, I guess it should be safe to assume osqp solver may return NaNs, and always check the solution does not contain any. I don't know if status_polish == -1 is a sign there is a problem, or if we should just simply check the output values.

VRichardJP commented 1 year ago

If anyone is interested into reproducing the bug. Here is my current setup:

add exception when a NaN is found in OSQPInterface::solve, and start obstacle avoidance planner in gdb:


diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp
index 972a586b16..9b08de25e9 100644
--- a/common/osqp_interface/src/osqp_interface.cpp
+++ b/common/osqp_interface/src/osqp_interface.cpp
@@ -24,6 +24,7 @@
#include <string>
#include <tuple>
#include <vector>
+#include <stdexcept>

namespace autoware
{
@@ -385,6 +386,12 @@ OSQPInterface::solve()

m_latest_work_info = *(m_work->info);

if ((!sol_primal.empty() && isnan(sol_primal.front()))
|| (!sol_lagrange_multiplier.empty() && isnan(sol_lagrange_multiplier.front())))
{
throw std::runtime_error("NaN!");
}
return result; }

diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index de8104af44..f847d92d1f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -212,6 +212,7 @@ def launch_setup(context, *args, **kwargs): obstacle_avoidance_planner_component, obstacle_velocity_limiter_component, ],

prefix="gnome-terminal -- gdb -ex=r --args" )

obstacle_stop_planner_loader = LoadComposableNodes(

compile packages you want to investigate with -DCMAKE_BUILD_TYPE=Debug. For example:

$ colcon build --symlink-install --cmake-args ' -DCMAKE_BUILD_TYPE=Debug' --packages-select obstacle_avoidance_planner osqp_interface

I am using a bus-like vehicle, based on this one. I am not sure whether the exact dimensions matter. I have used these parameters:

sample_bus_description/config/vehicle_info.param.yaml

/**:
ros__parameters:
# approx. bus size
wheel_radius: 0.487 # The radius of the wheel, primarily used for dead reckoning.
wheel_width: 0.226 #0.235 # The lateral width of a wheel tire, primarily used for dead reckoning.
wheel_base: 4.0 # between front wheel center and rear wheel center
wheel_tread: 2.10 #1.64 # between left wheel center and right wheel center
front_overhang: 2.0 #1.0 # between front wheel center and vehicle front
rear_overhang: 2.0 #1.1 # between rear wheel center and vehicle rear
left_overhang: 0.150 #0.128 # between left wheel center and vehicle left
right_overhang: 0.150 #0.128 # between right wheel center and vehicle right
vehicle_height: 3.2
# 0.5 = 30 degrees
max_steer_angle: 0.50 #0.70 # [rad]

sample_bus_description/urdf/vehicle.xacro (so that the 3D model in Rviz matches the vehicle info)


<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- load parameter -->
<xacro:property name="vehicle_info" value="${xacro.load_yaml('$(find sample_bus_description)/config/vehicle_info.param.yaml')}"/>

<!-- vehicle body -->
<link name="base_link">
<visual>
  <origin xyz="-${vehicle_info['/**']['ros__parameters']['rear_overhang']} 0 ${vehicle_info['/**']['ros__parameters']['vehicle_height']}" rpy="${pi/2.0} 0 ${pi}"/>
  <geometry>
    <mesh filename="package://sample_bus_description/mesh/cbus.dae" scale="1.133 1.188 1.185"/>
    <!-- <mesh filename="package://sample_bus_description/mesh/lexus.dae" scale="1.64 1.31 1.89"/> -->
  </geometry>
</visual>
</link>
</robot>


The map I used is this one: [lanelet2_map.osm.txt](https://github.com/autowarefoundation/autoware.universe/files/10835152/lanelet2_map.osm.txt)
I am confident that the bug can be triggered on many other maps, as I have had it on 2 others already.  I also think the vehicle does not matter, as the last time I had the bug was with an EV car.

Finally I have written a small python script to reset the vehicle pose every 45 seconds while the simulation is running (because the bug may take quite a lot of time to appear):
```python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node

from geometry_msgs.msg import PoseWithCovarianceStamped

class InitialPosePublisher2(Node):
  def __init__(self):
    super().__init__("initial_pose_publisher2")
    self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 10)
    # reset pose every minute
    self.timer_ = self.create_timer(45, self.timer_callback)
    # immediatly trigger initial pose
    self.timer_callback()

  def timer_callback(self):
    msg = PoseWithCovarianceStamped()
    msg.header.frame_id = 'map'
    msg.header.stamp = self.get_clock().now().to_msg()
    msg.pose.pose.position.x = 68328.265625
    msg.pose.pose.position.y = 87469.578125
    msg.pose.pose.orientation.z = 0.7437593644046556
    msg.pose.pose.orientation.w = 0.6684474608077906
    msg.pose.covariance = [0.25,0.0,0.0,0.0,0.0,0.0,0.0,0.25,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.06853892]
    self.publisher_.publish(msg)
    self.get_logger().info('Reset Initial Pose') 

def main(args=None):
  rclpy.init(args=args)
  node = InitialPosePublisher2()
  rclpy.spin(node)
  rclpy.shutdown()

if __name__ == '__main__':
  main()

Launching:

run the planning simulator:

$ source install/setup.bash
$ ros2 launch autoware_launch planning_simulator.launch.xml map_path:=<MAP_PATH> vehicle_model:=sample_bus sensor_model:=sample_bus_sensor_kit rviz:=false

run rviz2:

$ source install/setup.bash
$ ros2 run rviz2 rviz2 -d src/launcher/autoware_launch/autoware_launch/rviz/autoware.rviz

set initial/goal position there:
set velocity limit to 10km/h. I am not sure about why, but the bug seems to occur more often when speed limit is set low.

In a last terminal, run the pose reset script:

$ source install/setup.bash
$ ./initialpose.py

Finally, set operation mode to AUTO to start the simulation

Normally, the vehicle is not fast enough to reach the goal pose within the 45 seconds, so the simulation will keep running endlessly until something crashes. If nothing has crashed after ~20 minutes, I usually change the goal position a bit, as it seems to have an influence.

VRichardJP commented 1 year ago

Similarly to https://github.com/autowarefoundation/autoware.universe/issues/2922, I cannot reproduce the crash anymore. I don't know if it has been fixed or if it is just related to the zigzag issue (like the zigzag are caused by crazy osqp solutions, and NaNs are the peak of craziness). Anyway, I think my comment https://github.com/autowarefoundation/autoware.universe/issues/2925#issuecomment-1445346844 still holds: it would be safer to assume osqp may generate NaNs, and ignore the generated trajectory if any NaN is detected.

maxime-clem commented 1 year ago

There are situations where osqp returns NaNs (https://github.com/osqp/osqp/blob/e9ee6669f44ae989e5a4cc78896ab6d7ff9e0e19/src/auxil.c#L543-L544, https://github.com/osqp/osqp/blob/e9ee6669f44ae989e5a4cc78896ab6d7ff9e0e19/src/auxil.c#L709) so it would be safer to have special cases to handle NaNs everytime we use osqp. As far as I could find, osqp is used in the following nodes:

motion_velocity_smoother.
mpc_lateral_controller.
obstacle_avoidance_planner.
obstacle_cruise_planner. I will open a new issue to add handling of NaNs in these nodes.

mitsudome-r commented 1 year ago

I will close this issue since further discussion should be handled in https://github.com/autowarefoundation/autoware.universe/issues/3040

autowarefoundation / autoware.universe