AMCL change map / first_map_only = false

[mrmara]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Foxy

Steps to reproduce issue

set first_map_only to false
load a newmap with map server
Ensure that amcl accepted and loaded the new map (log to appear: Received a %d X %d map @ %.3f m/pix) and that it is receiving laserscan data

Expected behavior

AMCL should load the new map and start localizing in it

Actual behavior

AMCL crashes

Additional information

Backtrace:

Thread 13 "amcl" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7ffff0920700 (LWP 28300)]
0x00007ffff7413d8c in map_calc_range ()
   from /home/mrmara/ws//install/nav2_amcl/lib/libmap_lib.so
(gdb) backtrace
#0  0x00007ffff7413d8c in map_calc_range ()
   from /home/mrmara/ws//install/nav2_amcl/lib/libmap_lib.so
#1  0x00007ffff74d9ff7 in nav2_amcl::BeamModel::sensorFunctionWithRejection(nav2_amcl::LaserData*, _pf_sample_set_t*, bool*) ()
   from /home/mrmara/ws//install/nav2_amcl/lib/libsensors_lib.so
#2  0x00007ffff740a95d in pf_update_sensor ()
   from /home/mrmara/ws//install/nav2_amcl/lib/libpf_lib.so
#3  0x00007ffff74da465 in nav2_amcl::BeamModel::sensorUpdate(_pf_t*, nav2_amcl::LaserData*, bool, bool*) ()
   from /home/mrmara/ws//install/nav2_amcl/lib/libsensors_lib.so
#4  0x00007ffff7d16889 in nav2_amcl::AmclNode::updateFilter(int const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, pf_vector_t const&, pf_vector_t const&) ()
   from /home/mrmara/ws//install/nav2_amcl/lib/libamcl_core.so
#5  0x00007ffff7d18a47 in nav2_amcl::AmclNode::laserReceived(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) ()
--Type <RET> for more, q to quit, c to continue without paging--ibamcl_core.so
#6  0x00007ffff7d32b8c in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), std::_Bind<void (nav2_amcl::AmclNode::*(nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) () from /home/mrmara/ws//install/nav2_amcl/lib/libamcl_core.so
#7  0x00007ffff7d6cb66 in message_filters::CallbackHelper1T<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, bool) () from /home/mrmara/ws//install/nav2_amcl/lib/libamcl_core.so
#8  0x00007ffff7d692e3 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::transformReadyCallback(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long) () from /home/mrmara/ws//install/nav2_amcl/lib/libamcl_core.so
#9  0x00007ffff754a368 in std::function<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&)>::operator()(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) const (__args#0=..., this=0x7fffc4003728) at /usr/include/c++/9/bits/std_function.h:683
#10 tf2_ros::Buffer::<lambda(tf2::TransformableRequestHandle, const string&, const string&, tf2::TimePoint, tf2::TransformableResult)>::operator() (result=<optimized out>, time=..., source_frame=..., 
    target_frame=..., request_handle=<optimized out>, __closure=0x7fffc4003710) at ./src/buffer.cpp:263
#11 std::_Function_handler<void(long unsigned int, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long int, std::ratio<1, 1000000000> > >, tf2::TransformableResult), tf2_ros::Buffer::waitForTransform(const string&, const string&, const TimePoint&, const Duration&, tf2_ros::TransformReadyCallback)::<lambda(tf2::TransformableRequestHandle, const string&, const string&, tf2::TimePoint, tf2::TransformableResult)> >::_M_invoke(const std::_Any_data &, unsigned long &&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > &, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > &, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1, 1000000000> > > &&, tf2::TransformableResult &&) (
    __functor=..., __args#0=<optimized out>, __args#1=..., __args#2=..., __args#3=..., __args#4=<optimized out>) at /usr/include/c++/9/bits/std_function.h:300
#12 0x00007ffff7363331 in std::function<void (unsigned long, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, tf2::TransformableResult)>::operator()(unsigned long, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, tf2::TransformableResult) const (__args#4=<optimized out>, __args#3=..., __args#2="camera_link_right", 
    __args#1=..., __args#0=<optimized out>, this=0x7fffc40035d0) at /usr/include/c++/9/bits/std_function.h:683
#13 tf2::BufferCore::testTransformableRequests (this=0x5555559e4a80) at ./src/buffer_core.cpp:1510
#14 0x00007ffff73645c4 in tf2::BufferCore::setTransformImpl (this=0x5555559e4a80, transform_in=..., frame_id=..., child_frame_id=..., stamp=..., authority="Authority undetectable", is_static=false)
    at ./src/buffer_core.cpp:337

#15 0x00007ffff73663fd in tf2::BufferCore::setTransform (this=0x5555559e4a80, transform=..., authority="Authority undetectable", is_static=is_static@entry=false)
    at /usr/include/c++/9/bits/basic_string.h:936
#16 0x00007ffff7551716 in tf2_ros::TransformListener::subscription_callback (this=0x555555a41cc0, msg=..., is_static=<optimized out>) at /usr/include/c++/9/bits/stl_vector.h:1058
#17 0x00007ffff755926d in std::__invoke_impl<void, void (tf2_ros::TransformListener::*&)(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool), tf2_ros::TransformListener*&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool&> (__t=<optimized out>, __f=<optimized out>) at /usr/include/c++/9/bits/shared_ptr_base.h:756
#18 std::__invoke<void (tf2_ros::TransformListener::*&)(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool), tf2_ros::TransformListener*&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool&> (__fn=<optimized out>) at /usr/include/c++/9/bits/invoke.h:95
#19 std::_Bind<void (tf2_ros::TransformListener::*(tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)>::__call<void, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&, 0ul, 1ul, 2ul>(std::tuple<std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul, 2ul>) (
    __args=..., this=<optimized out>) at /usr/include/c++/9/functional:400
#20 std::_Bind<void (tf2_ros::TransformListener::*(tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)>::operator()<std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, void>(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&) (this=<optimized out>) at /usr/include/c++/9/functional:484
#21 std::_Function_handler<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >), std::_Bind<void (tf2_ros::TransformListener::*(tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&) (__functor=..., 
    __args#0=...) at /usr/include/c++/9/bits/std_function.h:300
#22 0x00007ffff7565ca1 in std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >)>::operator()(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >) const (
    __args#0=std::shared_ptr<struct tf2_msgs::msg::TFMessage_<std::allocator<void> >> (empty) = {...}, this=0x555555abee90) at /usr/include/c++/9/bits/std_function.h:683
#23 rclcpp::AnySubscriptionCallback<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> >::dispatch (this=0x555555abee90, 
    message=std::shared_ptr<struct tf2_msgs::msg::TFMessage_<std::allocator<void> >> (use count 4, weak count 0) = {...}, message_info=...)
    at /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:163
#24 0x00007ffff756650f in rclcpp::Subscription<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> > >::handle_message (this=0x555555abedb0, message=..., message_info=...) at /usr/include/c++/9/ext/atomicity.h:96
#25 0x00007ffff7ed066c in ?? () from /opt/ros/foxy/lib/librclcpp.so
#26 0x00007ffff7ed0f2b in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) () from /opt/ros/foxy/lib/librclcpp.so
#27 0x00007ffff7ed16e5 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () from /opt/ros/foxy/lib/librclcpp.so
#28 0x00007ffff7ed696c in rclcpp::executors::SingleThreadedExecutor::spin() () from /opt/ros/foxy/lib/librclcpp.so
#29 0x00007ffff7551962 in tf2_ros::TransformListener::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>::operator() (node_base_interface=..., __closure=0x555555abed58)
    at /usr/include/c++/9/bits/shared_ptr_base.h:1020
#30 std::__invoke_impl<void, tf2_ros::TransformListener::initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>, std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> > (__f=...) at /usr/include/c++/9/bits/invoke.h:60
--Type <RET> for more, q to quit, c to continue without paging--
#31 std::__invoke<tf2_ros::TransformListener::initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>, std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> > (__fn=...) at /usr/include/c++/9/bits/invoke.h:95
#32 std::thread::_Invoker<std::tuple<tf2_ros::TransformListener::initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>, std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> > >::_M_invoke<0, 1> (this=0x555555abed48) at /usr/include/c++/9/thread:244
#33 std::thread::_Invoker<std::tuple<tf2_ros::TransformListener::initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>, std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> > >::operator() (this=0x555555abed48) at /usr/include/c++/9/thread:251
#34 std::thread::_State_impl<std::thread::_Invoker<std::tuple<tf2_ros::TransformListener::initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>, std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> > > >::_M_run(void) (this=0x555555abed40) at /usr/include/c++/9/thread:195
#35 0x00007ffff7b22de4 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#36 0x00007ffff7824609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#37 0x00007ffff795e163 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

[SteveMacenski]

Does this happen deterministically or randomly? Does this behavior continue with either Galactic or Rolling?

For context:
Broadly, we have stopped support for Foxy since so much has changed since Galactic that it became essentially impossible to backport any software without breaking ABI/API due to the speed of development in the project. But if this is something you see in other more current distributions, then that's something we can more easily look into, but it might have been an issue long ago solved not current in any actively maintained branch.

If its the case that its been fixed, then we can find where it was fixed and see if we can't backport to Foxy. If its not been fixed, then it lets us know we can test this on more recent distributions to help you debug this issue and fix it across the board.

@padhupradheep if you're interested in a project to work on, this would be helpful!

[mrmara]

Thank you Sir for the support. I just installed Galactic and the corresponding branch of amcl. At first sight, the problem seems to be not present anymore. What is interesting is that the rest of the system and all the other nodes are still running in Foxy, this makes me think that the problem is specifically in the amcl package. I can see that lot has changed between the two versions of amcl, starting from dependencies.
I will update the issue with any further finding I discover.

[padhupradheep]

@padhupradheep if you're interested in a project to work on, this would be helpful!

Sure, I would take a look at it. I suppose, there are still a lot of companies using Foxy.

We (Neobotix) were planning to completely stop supporting Foxy, because we(Nav2 community) are at least like 100's of kilometers forward for the feature upgrades that were / is being made in Nav2.

I just installed Galactic and the corresponding branch of amcl. At first sight, the problem seems to be not present anymore. What is interesting is that the rest of the system and all the other nodes are still running in Foxy, this makes me think that the problem is specifically in the amcl package.

@mrmara Thanks for the update, I'll also give it a check...

[mrmara]

After more testing I was able to reproduce the same issue also in galactic. It is not clear to me why this happens less frequently, roughly after changing map 6 or 7 times. Still getting segmentation fault in the method map_calc_range ().
Thank you @padhupradheep and @SteveMacenski for checking this out

[padhupradheep]

On initial thoughts, it sounds like a race condition.. let me get back..

[mrmara]

If this can be useful here there is another backtrace taken on galactic:

[INFO] [1651498782.441765234] [amcl]: Setting pose (1651498782.441765): 46.614 17.433 -2.971
[INFO] [1651498782.639429163] [amcl]: Received a 1183 X 599 map @ 0.100 m/pix
[INFO] [1651498782.745273944] [amcl]: createLaserObject
[INFO] [1651498782.793789386] [amcl]: createLaserObject
[INFO] [1651498786.519259567] [amcl]: initialPoseReceived
[INFO] [1651498786.519360497] [amcl]: Setting pose (1651498786.519361): 21.588 13.716 0.198
[INFO] [1651498786.622554197] [amcl]: Received a 512 X 599 map @ 0.100 m/pix
[INFO] [1651498786.743473921] [amcl]: createLaserObject
[INFO] [1651498786.772597306] [amcl]: createLaserObject
[INFO] [1651498788.695803205] [amcl]: initialPoseReceived
[INFO] [1651498788.695895015] [amcl]: Setting pose (1651498788.695895): 48.944 20.461 0.084
[INFO] [1651498788.935513773] [amcl]: Received a 1183 X 599 map @ 0.100 m/pix
[INFO] [1651498789.094987664] [amcl]: createLaserObject
[INFO] [1651498789.126608402] [amcl]: createLaserObject
[INFO] [1651498790.649475553] [amcl]: initialPoseReceived
[INFO] [1651498790.649615632] [amcl]: Setting pose (1651498790.649616): 77.061 15.419 -3.068
[INFO] [1651498790.846807156] [amcl]: Received a 762 X 599 map @ 0.100 m/pix
[INFO] [1651498790.944276848] [amcl]: createLaserObject
[INFO] [1651498790.974446523] [amcl]: createLaserObject
[INFO] [1651498792.251159811] [amcl]: initialPoseReceived
[INFO] [1651498792.251272775] [amcl]: Setting pose (1651498792.251273): 43.997 15.349 3.029
[INFO] [1651498792.544881107] [amcl]: Received a 1183 X 599 map @ 0.100 m/pix
[INFO] [1651498792.745980116] [amcl]: createLaserObject
[INFO] [1651498792.774151316] [amcl]: createLaserObject
[INFO] [1651498794.143101541] [amcl]: initialPoseReceived
[INFO] [1651498794.143194481] [amcl]: Setting pose (1651498794.143194): 20.691 14.318 0.034
[INFO] [1651498794.334629925] [amcl]: Received a 512 X 599 map @ 0.100 m/pix
[INFO] [1651498794.345443762] [amcl]: createLaserObject
[INFO] [1651498794.545522028] [amcl]: createLaserObject
[INFO] [1651498796.053537689] [amcl]: initialPoseReceived
[INFO] [1651498796.053639789] [amcl]: Setting pose (1651498796.053640): 49.170 21.212 -0.048
[INFO] [1651498796.245550154] [amcl]: Received a 1183 X 599 map @ 0.100 m/pix
[INFO] [1651498796.246363967] [amcl]: createLaserObject

Thread 12 "amcl" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffe55f8700 (LWP 52893)]
0x00007ffff7331439 in map_calc_range ()
   from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libmap_lib.so
(gdb) backtrace
#0  0x00007ffff7331439 in map_calc_range ()
   from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libmap_lib.so
#1  0x00007ffff73e87ed in nav2_amcl::BeamModel::sensorFunction(nav2_amcl::LaserData*, _pf_sample_set_t*) () from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libsensors_lib.so
#2  0x00007ffff732691a in pf_update_sensor ()
   from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libpf_lib.so
#3  0x00007ffff73e895d in nav2_amcl::BeamModel::sensorUpdate(_pf_t*, nav2_amcl::LaserData*) ()
   from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libsensors_lib.so
#4  0x00007ffff7cb334f in nav2_amcl::AmclNode::updateFilter(int const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, pf_vector_t const&) ()
   from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libamcl_core.so
#5  0x00007ffff7cc0d1b in nav2_amcl::AmclNode::laserReceived(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) ()
   from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libamcl_core.so
#6  0x00007ffff7ce0bcc in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), std::_Bind<void (nav2_amcl::AmclNode::*(nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) () from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libamcl_core.so
#7  0x00007ffff7d16456 in message_filters::CallbackHelper1T<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, bool) () from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libamcl_core.so
#8  0x00007ffff7d12b3b in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::transformReadyCallback(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long) ()
   from /home/mrmara/nav2_ws/install/nav2_amcl/lib/libamcl_core.so
#9  0x00007ffff74652f0 in ?? () from /opt/ros/galactic/lib/libtf2_ros.so
#10 0x00007ffff71e7101 in tf2::BufferCore::testTransformableRequests() ()
   from /opt/ros/galactic/lib/libtf2.so
#11 0x00007ffff71e8462 in tf2::BufferCore::setTransformImpl(tf2::Transform const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) () from /opt/ros/galactic/lib/libtf2.so
#12 0x00007ffff71e9f0d in tf2::BufferCore::setTransform(geometry_msgs::msg::TransformStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) () from /opt/ros/galactic/lib/libtf2.so
#13 0x00007ffff7470e39 in tf2_ros::TransformListener::subscription_callback(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool) () from /opt/ros/galactic/lib/libtf2_ros.so
#14 0x00007ffff74787fd in std::_Function_handler<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >), std::_Bind<void (tf2_ros::TransformListener::*(tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&) () from /opt/ros/galactic/lib/libtf2_ros.so
#15 0x00007ffff7479078 in std::__detail::__variant::__gen_vtable_impl<true, std::__detail::__variant::_Multi_array<void (*)(rclcpp::AnySubscriptionCallback<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}&&, std::variant<std::function<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const&)>, std::variant<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const, rclcpp::MessageInfo const&)>, std::variant<void (std::unique_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > > >)>, std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::variant<void (std::shared_ptr<std::function>)>, std::var--Type <RET> for more, q to quit, c to continue without paging--
iant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, rclcpp::MessageInfo const&)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> const&)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> const, rclcpp::MessageInfo const&)>, std::variant<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >)>, std::variant<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> >&)>, std::tuple<std::variant<std::function<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const&)>, std::variant<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const, rclcpp::MessageInfo const&)>, std::variant<void (std::unique_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > > >)>, std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::variant<void (std::shared_ptr<std::function>)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, rclcpp::MessageInfo const&)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> const&)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> const, rclcpp::MessageInfo const&)>, std::variant<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >)>, std::variant<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> > >, std::integer_sequence<unsigned long, 8ul> >::__visit_invoke(rclcpp::AnySubscriptionCallback<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)::{lambda(auto:1&&)#1}, std::variant<std::function<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const&)>, std::variant<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const, rclcpp::MessageInfo const&)>, std::variant<void (std::unique_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > > >)>, std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::variant<void (std::shared_ptr<std::function>)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, rclcpp::MessageInfo const&)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> const&)>, std::variant<void (std::variant<void (std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> const, rclcpp::MessageInfo const&)>, std::variant<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >)>, std::variant<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)> >) ()
   from /opt/ros/galactic/lib/libtf2_ros.so
#16 0x00007ffff748dd5c in rclcpp::Subscription<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) () from /opt/ros/galactic/lib/libtf2_ros.so
#17 0x00007ffff7e8e014 in ?? () from /opt/ros/galactic/lib/librclcpp.so
#18 0x00007ffff7e8e8e4 in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) () from /opt/ros/galactic/lib/librclcpp.so
#19 0x00007ffff7e8f015 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) ()
   from /opt/ros/galactic/lib/librclcpp.so
#20 0x00007ffff7e9612c in rclcpp::executors::SingleThreadedExecutor::spin() ()
   from /opt/ros/galactic/lib/librclcpp.so
#21 0x00007ffff7470712 in ?? () from /opt/ros/galactic/lib/libtf2_ros.so
#22 0x00007ffff7abbde4 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#23 0x00007ffff77b7609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#24 0x00007ffff78f7163 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

[SteveMacenski]

Maybe something isn't being properly locked or reset on map changes?

First thing I'd do is look how the other threaded-functions are protecting resources and seeing if the map change callback is doing that. The next thing I'd do is check the ROS 1 version and see if there's some resetting of variables not happening on map changes here that was happening there that was lost in porting (under the assumption ROS 1 doesn't have the same issue).

If those 2 low-hanging fruit directions don't pan out, then you need to start actually looking at the code of the function crashing / what the change map callback does and try to find the link of where things are going wrong. Some logging and compiling with -g so that you can get exact line numbers might be of help.

You can safely ignore everything before laserReceived, that's just the stuff for the TF subscriber filter at work (6-24 in your most recent traceback).

[hardesh]

Hi @mrmara,

first_map_only parameter does not work in the foxy and galactic branch. I've fixed it on the main branch in #2870 . It should work with rolling and humble distro

[mrmara]

Hi @hardesh, thanks for your contribution.
I saw your PR but I already did that modification in my code, otherwise the new map would not even load in amcl and the crash could not happen.
The crash is specifically happening because amcl accepts the new map as can be understood from the log:

[INFO] [1651498794.334629925] [amcl]: Received a {px} X {px} map @ {res} m/pix

Thanks @SteveMacenski for the hints, I am going through them.

[mrmara]

I have been testing amcl map change feature with ROS Noetic and the bug is not present there. There are some differences in the way memory related to map is managed since particle filter and odometry is not resetted, maybe due to this issue.
Anyway, I found something weird happening. When segmentation fault happens in the method map_calc_range() I can see map.size_x and map.size_y not being updated with new sizes.
I added the following code at the beginning of map_calc_range():

printf("ox: %lf,\n\
   oy: %lf,\n\
    oa: %lf,\n\
     max_range: %lf\n", ox,oy,oa,max_range);
  printf("origin_x: %lf\n\
          origin_y: %lf\n\
          size_x: %d\n\
          size_y: %d\n\
          scale: %lf\n", map->origin_x,map->origin_y,map->size_x,map->size_y,map->scale);

Let me attach some logs:

amcl backtrace

ox: 0.248028,
oy: -0.653909,
oa: -0.400617,
max_range: 4.000000
origin_x: 62.800001
origin_y: 52.400001
size_x: 1110
size_y: 1049
scale: 0.100000
ox: 0.248028,
oy: -0.653909,
oa: -0.383169,
max_range: 4.000000
origin_x: 0.000000
origin_y: 0.000000
size_x: 1110
size_y: 1049
scale: 0.100000
--Type for more, q to quit, c to continue without paging--

Thread 13 "amcl" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7ffff0a5e700 (LWP 16272)]
0x00007ffff7428d8c in map_calc_range (map=0x7fffc4005180, ox=ox@entry=0.24802753394038771, oy=oy@entry=-0.65390922210997937, oa=, max_range=)
at /home/mrmara/nav2_ws/src/navigation2/nav2_amcl/src/map/map_range.c:104
104 if (!MAP_VALID(map, x, y) || map->cells[MAP_INDEX(map, x, y)].occ_state > -1) {
(gdb) backtrace
#0 0x00007ffff7428d8c in map_calc_range (map=0x7fffc4005180, ox=ox@entry=0.24802753394038771, oy=oy@entry=-0.65390922210997937, oa=, max_range=)
at /home/mrmara/nav2_ws/src/navigation2/nav2_amcl/src/map/map_range.c:104
#1 0x00007ffff74e27ed in nav2_amcl::BeamModel::sensorFunction (data=0x7ffff0a59ea0, set=set@entry=0x555555b6d730)
at /home/mrmara/nav2_ws/src/navigation2/nav2_amcl/src/sensors/laser/beam_model.cpp:78
#2 0x00007ffff741f93a in pf_update_sensor (pf=0x555555b6d680, sensor_fn=sensor_fn@entry=0x7ffff74e2610 <nav2_amcl::BeamModel::sensorFunction(nav2_amcl::LaserData*, pf_sample_set_t*)>,
sensor_data=) at /home/mrmara/nav2_ws/src/navigation2/nav2_amcl/src/pf/pf.c:261
#3 0x00007ffff74e295d in nav2_amcl::BeamModel::sensorUpdate (this=, pf=, data=)
at /home/mrmara/nav2_ws/src/navigation2/nav2_amcl/src/sensors/laser/beam_model.cpp:125
#4 0x00007ffff7d19c5f in nav2_amcl::AmclNode::updateFilter (this=0x5555555dd080, laser_index=@0x7ffff0a5a808: 1,
laser_scan=std::shared_ptr<const struct sensor_msgs::msg::LaserScan<std::allocator >> (use count 5, weak count 0) = {...}, pose=...) at /usr/include/c++/10/bits/stl_vector.h:1043
#5 0x00007ffff7d253bd in nav2_amcl::AmclNode::laserReceived (this=, laser_scan=...) at /home/mrmara/nav2_ws/src/navigation2/nav2_amcl/src/amcl_node.cpp:705
#6 0x00007ffff7d3f15c in std::invoke_impl<void, void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&> (__f=, __t=, __f=, __t=) at /usr/include/c++/10/ext/atomicity.h:100
#7 std::invoke<void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&> (__fn=) at /usr/include/c++/10/bits/invoke.h:95
#8 std::_Bind<void (nav2_amcl::AmclNode::(nav2_amcl::AmclNode, std::Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)>::call<void, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&, 0ul, 1ul>(std::tuple<std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&>&&, std::_Index_tuple<0ul, 1ul>) (
_args=..., this=) at /usr/include/c++/10/functional:416
#9 std::Bind<void (nav2_amcl::AmclNode::(nav2_amcl::AmclNode, std::Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)>::operator()<std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&, void>(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&) (this=)
at /usr/include/c++/10/functional:499
#10 std::__invoke_impl<void, std::Bind<void (nav2_amcl::AmclNode::(nav2_amcl::AmclNode, std::Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)>&, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&>(std::__invoke_other, std::Bind<void (nav2_amcl::AmclNode::(nav2_amcl::AmclNode, std::Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)>&, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&) (__f=...) at /usr/include/c++/10/bits/invoke.h:60
#11 std::_invoke_r<void, std::Bind<void (nav2_amcl::AmclNode::(nav2_amcl::AmclNode, std::Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)>&, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&>(std::Bind<void (nav2_amcl::AmclNode::(nav2_amcl::AmclNode, std::Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)>&, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&) (fn=...) at /usr/include/c++/10/bits/invoke.h:153
#12 std::Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&), std::Bind<void (nav2_amcl::AmclNode::(nav2_amcl::AmclNode, std::Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)> >::M_invoke(std::Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&) (
functor=..., args#0=...) at /usr/include/c++/10/bits/std_function.h:291
#13 0x00007ffff7d719e6 in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const>) const (args#0=std::shared_ptr<const struct sensor_msgs::msg::LaserScan<std::allocator >> (use count 5, weak count 0) = {...}, this=0x555555a125b8)
at /usr/include/c++/10/bits/std_function.h:617
#14 message_filters::CallbackHelper1T<std::shared_ptr<sensor_msgs::msg::LaserScan<std::allocator > const> const&, sensor_msgs::msg::LaserScan<std::allocator > >::call (
this=0x555555a125b0, event=..., nonconst_force_copy=) at /opt/ros/foxy/include/message_filters/signal1.h:74
#15 0x00007ffff7d6e163 in message_filters::Signal1<sensor_msgs::msg::LaserScan<std::allocator > >::call (event=..., this=0x555555a122c8) at /usr/include/c++/10/bits/shared_ptr_base.h:1324
#16 message_filters::SimpleFilter<sensor_msgs::msg::LaserScan<std::allocator > >::signalMessage (event=..., this=0x555555a122c8) at /opt/ros/foxy/include/message_filters/simple_filter.h:133
#17 tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan<std::allocator >, tf2_ros::Buffer>::messageReady (evt=..., this=0x555555a122c0) at /opt/ros/foxy/include/tf2_ros/message_filter.h:643
#18 tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan<std::allocator >, tf2_ros::Buffer>::transformReadyCallback (this=, future=..., handle=)
at /opt/ros/foxy/include/tf2_ros/message_filter.h:537
#19 0x00007ffff7547368 in std::function<void (std::shared_future<geometry_msgs::msg::TransformStamped<std::allocator > > const&)>::operator()(std::shared_future<geometry_msgs::msg::TransformStamped<std::allocator > > const&) const (__args#0=..., this=0x7fffd800b118) at /usr/include/c++/9/bits/std_function.h:683
#20 tf2_ros::Buffer::<lambda(tf2::TransformableRequestHandle, const string&, const string&, tf2::TimePoint, tf2::TransformableResult)>::operator() (result=, time=..., source_frame=...,
target_frame=..., request_handle=, __closure=0x7fffd800b100) at ./src/buffer.cpp:263
#21 std::_Function_handler<void(long unsigned int, const std::__cxx11::basic_string<char, std::char_traits, std::allocator >&, const std::__cxx11::basic_string<char, std::char_traits, std::allocator >&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long int, std::ratio<1, 1000000000> > >, tf2::TransformableResult), tf2_ros::Buffer::waitForTransform(const string&, const string&, const TimePoint&, const Duration&, tf2_ros::TransformReadyCallback)::<lambda(tf2::TransformableRequestHandle, const string&, const string&, tf2::TimePoint, tf2::TransformableResult)> >::_M_invoke(const std::_Any_data &, unsigned long &&, const std::__cxx11::basic_string<char, std::char_traits, std::allocator > &, const std::__cxx11::basic_string<char, std::char_traits, std::allocator > &, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1, 1000000000> > > &&, tf2::TransformableResult &&) (
__functor=..., __args#0=, __args#1=..., __args#2=..., __args#3=..., __args#4=) at /usr/include/c++/9/bits/std_function.h:300
#22 0x00007ffff7372331 in std::function<void (unsigned long, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, tf2::TransformableResult)>::operator()(unsigned long, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, tf2::TransformableResult) const (__args#4=, __args#3=..., __args#2="camera_link_left",
__args#1=..., __args#0=, this=0x7fffd800a5a0) at /usr/include/c++/9/bits/std_function.h:683

map_server log

[INFO] [1651581252.913153218] [map_server]: Handling LoadMap request

[INFO] [map_io]: Loading yaml file: /home/mrmara/alba_dev_ws/install/ros_pkg/share/ros_pkg/maps/map1.yaml

[DEBUG] [map_io]: resolution: 0.1

[DEBUG] [map_io]: origin[0]: 7.3

[DEBUG] [map_io]: origin[1]: 0

[DEBUG] [map_io]: origin[2]: 0

[DEBUG] [map_io]: free_thresh: 0.1

[DEBUG] [map_io]: occupied_thresh: 0.65

[DEBUG] [map_io]: mode: trinary

[DEBUG] [map_io]: negate: 0

[INFO] [map_io]: Loading image_file: /home/mrmara/alba_dev_ws/install/ros_pkg/share/ros_pkg/maps/map1.png

[DEBUG] [map_io]: Read map /home/mrmara/alba_dev_ws/install/ros_pkg/share/ros_pkg/maps/map1.png: 1110 X 1049 map @ 0.1 m/cell

[INFO] [1651581257.312541929] [map_server]: Handling LoadMap request

[INFO] [map_io]: Loading yaml file: /home/mrmara/alba_dev_ws/install/ros_pkg/share/ros_pkg/maps/map2.yaml

[DEBUG] [map_io]: resolution: 0.1

[DEBUG] [map_io]: origin[0]: 42.1

[DEBUG] [map_io]: origin[1]: 0

[DEBUG] [map_io]: origin[2]: 0

[DEBUG] [map_io]: free_thresh: 0.1

[DEBUG] [map_io]: occupied_thresh: 0.35

[DEBUG] [map_io]: mode: trinary

[DEBUG] [map_io]: negate: 0

[INFO] [map_io]: Loading image_file: /home/mrmara/alba_dev_ws/install/ros_pkg/share/ros_pkg/maps/map2.png

[DEBUG] [map_io]: Read map /home/mrmara/alba_dev_ws/install/ros_pkg/share/ros_pkg/maps/map2.png: 762 X 599 map @ 0.1 m/cell

As you can see when segmentation fault happened sizes are 110x1049, the ones of map1, but the map was changed to map2 762x599, hence triggering map_cal_range(). I am still trying to figure out how to sync this behaviour so that map change transition is completed before accepting new laserscans.

[mrmara]

Maybe, as @padhupradheep pointed out, this can be a race problem. When a new map message arrives the following actions are performed:

if (map_ != NULL) {
    map_free(map_);
    map_ = NULL;
  }

But if a new laserscan arrives in the meantime the new map message is being managed the following lines of code inside map_range.c provoke a segmentation fault:

map->cells[MAP_INDEX(map, y, x)].occ_state

This can be a possible reason. In order to confirm it, I am now trying to block new laserscans processing while processing new map message is completed.

[mrmara]

By adding the following line of code:

  std::lock_guard<std::recursive_mutex> cfl(configuration_mutex_);

at the beginning of the callback void AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) the problem seems to be solved. I still need to evaluate performance on localization anyway.

[SteveMacenski]

Seems rational, I did find actually that MANY places where the configuration_mutex_ should be locked are NOT locked at all.

Adding that lock is the right answer, it should be fine. But actually there should be many places that mutex is added, so if you look at the ROS 1 version, add it to the start of all of the functions there that had it. I'm not sure why people removed them here