validate msg onto /initialpose would result in a stack-buffer-overflow bug in nav2_amcl

[GoesM]

Bug report

Required Info:

• Operating System:
  • Ubuntu22.04
• ROS2 Version:
  • humble
• Version or commit hash:
  • the latest
• DDS implementation:
  • defaulted

Steps to reproduce issue

Launch the navigation2 normally, as following steps:

#!/bin/bash
export ASAN_OPTIONS=halt_on_error=0:new_delete_type_mismatch=0:detect_leaks=0:log_pah=asan
source install/setup.bash
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=True use_rviz:=False use_composition:=False 

Curious about how nav2 face to topic-interception, i sent validate /PoseWithCovarianceStamped msg onto topic /initialpose, which is like this:

ros2 topic pub /initialpose geometry_msgs/PoseWithCovarianceStamped " 
header:
  frame_id: map
  stamp:
    nanosec: 146734875
    sec: 1711029956
pose:
  covariance:
    - 0.25
    - 0.0
    - 0.0
    - 0.0
    - 0.0
    - 0.0
    - 0.0
    - .nan
    - 0.0
    - .nan
    - 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
    - 1.8985442697712696e-305
    - 0.0
    - 0.0
    - 0.0
    - 0.0
    - 0.0
    - 0.000000
    - 0.0
    - 0.0
    - 0.06853891909122467
  pose:
    orientation:
      w: 0.9440542194053062
      x: 0.0
      y: 0.0
      z: -0.32979028309372299
    position:
      x: 0.50010401010515571
      y: 1.7468730211257935
      z: 0.0" -1

[notice] here's some .nan value in such validate message.

Expected behavior

Actual behavior

The ASAN reporting a stack-buffer-overflow bug to me as following, and the nav2_amcl stop its work.

=================================================================
==8261==ERROR: AddressSanitizer: stack-buffer-overflow on address 0x7ffded1003f8 at pc 0x7b4da0922499 bp 0x7ffded100030 sp 0x7ffded100028
READ of size 8 at 0x7ffded1003f8 thread T0
    #0 0x7b4da0922498 in eigen_decomposition (/home/***/nav2/install/nav2_amcl/lib/libpf_lib.so+0x10498) (BuildId: 92f6f6da07c69e4e8fdb674a2fb2b39a58de9a00)
    #1 0x7b4da091fca5 in pf_matrix_unitary (/home/***/nav2/install/nav2_amcl/lib/libpf_lib.so+0xdca5) (BuildId: 92f6f6da07c69e4e8fdb674a2fb2b39a58de9a00)
    #2 0x7b4da091ea8f in pf_pdf_gaussian_alloc (/home/***/nav2/install/nav2_amcl/lib/libpf_lib.so+0xca8f) (BuildId: 92f6f6da07c69e4e8fdb674a2fb2b39a58de9a00)
    #3 0x7b4da0917fa3 in pf_init (/home/***/nav2/install/nav2_amcl/lib/libpf_lib.so+0x5fa3) (BuildId: 92f6f6da07c69e4e8fdb674a2fb2b39a58de9a00)
    #4 0x7b4da0f46df2 in nav2_amcl::AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> >&) (/home/***/nav2/install/nav2_amcl/lib/libamcl_core.so+0x346df2) (BuildId: ef8b35bb0836c058bcbcd5d12c39479c3c6c37be)
    #5 0x7b4da0f446c6 in nav2_amcl::AmclNode::initialPoseReceived(std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > >) (/home/***/nav2/install/nav2_amcl/lib/libamcl_core.so+0x3446c6) (BuildId: ef8b35bb0836c058bcbcd5d12c39479c3c6c37be)
    #6 0x7b4da1126d57 in void std::__invoke_impl<void, void (nav2_amcl::AmclNode::*&)(std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > >), nav2_amcl::AmclNode*&, std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > > >(std::__invoke_memfun_deref, void (nav2_amcl::AmclNode::*&)(std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > >), nav2_amcl::AmclNode*&, std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > >&&) (/home/***/nav2/install/nav2_amcl/lib/libamcl_core.so+0x526d57) (BuildId: ef8b35bb0836c058bcbcd5d12c39479c3c6c37be)
    #7 0x7b4da11561cb in auto rclcpp::AnySubscriptionCallback<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > >, rclcpp::MessageInfo const&)::'lambda'(auto&&)::operator()<std::function<void (std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > >)>&>(auto&&) const (/home/***/nav2/install/nav2_amcl/lib/libamcl_core.so+0x5561cb) (BuildId: ef8b35bb0836c058bcbcd5d12c39479c3c6c37be)
    #8 0x7b4da1153061 in rclcpp::AnySubscriptionCallback<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> > >, rclcpp::MessageInfo const&) (/home/***/nav2/install/nav2_amcl/lib/libamcl_core.so+0x553061) (BuildId: ef8b35bb0836c058bcbcd5d12c39479c3c6c37be)
    #9 0x7b4da112fc34 in rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> >, std::allocator<void>, geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> >, geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> >, rclcpp::message_memory_strategy::MessageMemoryStrategy<geometry_msgs::msg::PoseWithCovarianceStamped_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) (/home/***/nav2/install/nav2_amcl/lib/libamcl_core.so+0x52fc34) (BuildId: ef8b35bb0836c058bcbcd5d12c39479c3c6c37be)
    #10 0x7b4da1f5c7bb in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/humble/lib/librclcpp.so+0xe77bb) (BuildId: 4cca8a387f3c93d38a0567a8efc7cba9106f5d9a)
    #11 0x7b4da1f5cfbe in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/humble/lib/librclcpp.so+0xe7fbe) (BuildId: 4cca8a387f3c93d38a0567a8efc7cba9106f5d9a)
    #12 0x7b4da1f648af in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/humble/lib/librclcpp.so+0xef8af) (BuildId: 4cca8a387f3c93d38a0567a8efc7cba9106f5d9a)
    #13 0x7b4da1f64ac4 in rclcpp::spin(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) (/opt/ros/humble/lib/librclcpp.so+0xefac4) (BuildId: 4cca8a387f3c93d38a0567a8efc7cba9106f5d9a)
    #14 0x5f22ccd704cd in main (/home/***/nav2/install/nav2_amcl/lib/nav2_amcl/amcl+0xe84cd) (BuildId: 6374af6c6a02284720c5116aa2ef067ebdd75367)
    #15 0x7b4da0229d8f in __libc_start_call_main csu/../sysdeps/nptl/libc_start_call_main.h:58:16
    #16 0x7b4da0229e3f in __libc_start_main csu/../csu/libc-start.c:392:3
    #17 0x5f22cccaf5e4 in _start (/home/***/nav2/install/nav2_amcl/lib/nav2_amcl/amcl+0x275e4) (BuildId: 6374af6c6a02284720c5116aa2ef067ebdd75367)

Address 0x7ffded1003f8 is located in stack of thread T0 at offset 280 in frame
    #0 0x7b4da091facf in pf_matrix_unitary (/home/***/nav2/install/nav2_amcl/lib/libpf_lib.so+0xdacf) (BuildId: 92f6f6da07c69e4e8fdb674a2fb2b39a58de9a00)

  This frame has 4 object(s):
    [32, 104) 'a.byval'
    [144, 216) 'aa' (line 237)
    [256, 280) 'eval' (line 238) <== Memory access at offset 280 overflows this variable
    [320, 392) 'evec' (line 239)
HINT: this may be a false positive if your program uses some custom stack unwind mechanism, swapcontext or vfork
      (longjmp and C++ exceptions *are* supported)
SUMMARY: AddressSanitizer: stack-buffer-overflow (/home/***/nav2/install/nav2_amcl/lib/libpf_lib.so+0x10498) (BuildId: 92f6f6da07c69e4e8fdb674a2fb2b39a58de9a00) in eigen_decomposition
Shadow bytes around the buggy address:
  0x10003da18020: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
  0x10003da18030: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
  0x10003da18040: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
  0x10003da18050: 00 00 00 00 00 00 00 00 00 00 00 00 f1 f1 f1 f1
  0x10003da18060: 00 00 00 00 00 00 00 00 00 f2 f2 f2 f2 f2 00 00
=>0x10003da18070: 00 00 00 00 00 00 00 f2 f2 f2 f2 f2 00 00 00[f2]
  0x10003da18080: f2 f2 f2 f2 00 00 00 00 00 00 00 00 00 f3 f3 f3
  0x10003da18090: f3 f3 f3 f3 00 00 00 00 00 00 00 00 00 00 00 00
  0x10003da180a0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
  0x10003da180b0: 00 00 00 00 00 00 00 00 f1 f1 f1 f1 00 00 00 f2
  0x10003da180c0: f2 f2 f2 f2 00 00 00 00 00 00 00 00 00 f2 f2 f2
Shadow byte legend (one shadow byte represents 8 application bytes):
  Addressable:           00
  Partially addressable: 01 02 03 04 05 06 07 
  Heap left redzone:       fa
  Freed heap region:       fd
  Stack left redzone:      f1
  Stack mid redzone:       f2
  Stack right redzone:     f3
  Stack after return:      f5
  Stack use after scope:   f8
  Global redzone:          f9
  Global init order:       f6
  Poisoned by user:        f7
  Container overflow:      fc
  Array cookie:            ac
  Intra object redzone:    bb
  ASan internal:           fe
  Left alloca redzone:     ca
  Right alloca redzone:    cb
==8261==ABORTING

Additional information

---

It could confirmed that the bug was caused by the value .nan of such validate message

similar issue and PR: #4177 #4276

[SteveMacenski]

I think that sounds like a great next message validation: check if there are NaNs!

For future things like this that you run into, you've proven yourself here enough - you're welcome to just submit a PR for things of this nature (i.e. input message invalid -> make a validation util -> use it in that area) to speed up your work.