fix(pose_instability_detector): empty check order

[yn-mrse]

Description

Thread 1 "pose_instabilit" received signal SIGSEGV, Segmentation fault.
0x00007ffff7f16acd in rclcpp::Time::Time(builtin_interfaces::msg::Time_<std::allocator<void> > const&, rcl_clock_type_e) () from /opt/ros/humble/lib/librclcpp.so
(gdb) bt
#0  0x00007ffff7f16acd in rclcpp::Time::Time(builtin_interfaces::msg::Time_<std::allocator<void> > const&, rcl_clock_type_e) () from /opt/ros/humble/lib/librclcpp.so
#1  0x00007ffff6599242 in PoseInstabilityDetector::callback_odometry(std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > const>) ()
   from /home/autoware/pilot-auto/install/pose_instability_detector/lib/libpose_instability_detector.so
#2  0x00007ffff65b1dd8 in std::_Function_handler<void (std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > const>), std::_Bind<void (PoseInstabilityDetector::*(PoseInstabilityDetector*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > const>)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > const>&&) ()
--Type <RET> for more, q to quit, c to continue without paging--
   from /home/autoware/pilot-auto/install/pose_instability_detector/lib/libpose_instability_detector.so
#3  0x00007ffff65b1257 in std::__detail::__variant::__gen_vtable_impl<std::__detail::__variant::_Multi_array<std::__detail::__variant::__deduce_visit_result<void> (*)(rclcpp::AnySubscriptionCallback<nav_msgs::msg::Odometry_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::variant<std::function<void (nav_msgs::msg::Odometry_<std::allocator<void> > const&)>, std::function<void (nav_msgs::msg::Odometry_<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<nav_msgs::msg::Odo--Type <RET> for more, q to quit, c to continue without paging--
metry_<std::allocator<void> >, std::default_delete<nav_msgs::msg::Odometry_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<nav_msgs::msg::Odometry_<std::allocator<void> >, std::default_delete<nav_msgs::msg::Odometry_<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<nav_msgs::msg::Odometry_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<nav_msgs::msg::Odometry_<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::sha--Type <RET> for more, q to quit, c to continue without paging--
red_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<nav_msgs::msg::Odometry_<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<nav_msgs::msg::Odometry_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<nav_msgs::msg::Odometry_<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, 8ul> >::__visit_invoke(rclcpp::AnySubscriptionCallback<nav_msgs::msg::Odometry_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<nav_msgs::msg::Odom--Type <RET> for more, q to quit, c to continue without paging--
etry_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::variant<std::function<void (nav_msgs::msg::Odometry_<std::allocator<void> > const&)>, std::function<void (nav_msgs::msg::Odometry_<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<nav_msgs::msg::Odometry_<std::allocator<void> >, std::default_delete<nav_msgs::msg::Odometry_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<nav_msgs::msg::Odometry_<std::allocator<void> >, std::default_delete<nav_msgs::msg::Odometry_<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<--Type <RET> for more, q to quit, c to continue without paging--
rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<nav_msgs::msg::Odometry_<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<nav_msgs::msg::Odometry_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<nav_msgs::msg::Odometry_<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<nav_msgs::msg::Odometry_<std::allocator<void> > >)>, std::function<v--Type <RET> for more, q to quit, c to continue without paging--
oid (std::shared_ptr<nav_msgs::msg::Odometry_<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&)> >&) ()
   from /home/autoware/pilot-auto/install/pose_instability_detector/lib/libpose_instability_detector.so
#4  0x00007ffff65cf985 in rclcpp::Subscription<nav_msgs::msg::Odometry_<std::allocator<void> >, std::allocator<void>, nav_msgs::msg::Odometry_<std::allocator<void> >, nav_msgs::msg::Odometry_<std::allocator<void> >, rclcpp::message_memory_strategy::MessageMemoryStrategy<nav_msgs::msg::Odometry_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) ()
--Type <RET> for more, q to quit, c to continue without paging--
   from /home/autoware/pilot-auto/install/pose_instability_detector/lib/libpose_instability_detector.so
#5  0x00007ffff7ea188c in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) () from /opt/ros/humble/lib/librclcpp.so
#6  0x00007ffff7ea208f in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () from /opt/ros/humble/lib/librclcpp.so
#7  0x00007ffff7ea9980 in rclcpp::executors::SingleThreadedExecutor::spin() ()
   from /opt/ros/humble/lib/librclcpp.so

#8  0x000055555555843e in main ()

twist_buffer_がemptyのままtwist_buffer_.back()へアクセスを試みてsegmentation faultが発生するバグを修正しました。

empty checkの順序が逆になっているため、empty check後にtwist_buffer_.back()へアクセスするように変更します。

Related links

• 管理チケット

How was this PR tested?

PSimにて高確率で発生していたsegmentation faultの発生が、
対策後になくなっていることを確認しました。

本対策は処理順序のみの変更であり、懸念されるリグレッションはありません。

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[sfukuta]

@yn-mrse

segmentation fault

が発生した時のstack traceがあれば転記をお願いします。
traceがなければ、修正ないようとdescriptionは一致しているので、不要です。

[yn-mrse]

@yn-mrse

segmentation fault

が発生した時のstack traceがあれば転記をお願いします。 traceがなければ、修正ないようとdescriptionは一致しているので、不要です。

stuck trace を転記しました！

[sonarqubecloud]

Quality Gate passed

Issues
0 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarCloud