[Iron] - Admittance controller regulary crashes with segfault on activation / configuration

[firesurfer]

Describe the bug

In order to change the kinematic tip and there for the admittant frame I regulary: deactivate -> set new params -> reconfigure -> activate the controller. Sometimes the controller crashes with a segfault.

See #1175 for the why I need to reconfigure the controller

Output:

[ros2_control_node-14] [INFO] [1719385013.551660859] [controller_manager]: Configuring controller 'admittance_controller_top'
[ros2_control_node-14] [INFO] [1719385013.551826348] [admittance_controller_top]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-14] [INFO] [1719385013.551938667] [admittance_controller_top]: state int types are: position
[ros2_control_node-14]
[ros2_control_node-14] [INFO] [1719385013.552066065] [admittance_controller_top]: command int types are: position
[ros2_control_node-14]
[ros2_control_node-14] [INFO] [1719385013.552158598] [admittance_controller_top]: chainable int types are: position
[ros2_control_node-14]
[ros2_control_node-14] [INFO] [1719385013.552234329] [admittance_controller_top]: chainable int types are: velocity
[ros2_control_node-14]
[ros2_control_node-14] [INFO] [1719385013.552316803] [admittance_controller_top]: Command interfaces are [position] and and state interfaces are [position].
[ros2_control_node-14] Stack trace (most recent call last) in thread 3073738:
[ros2_control_node-14] #19 Object "", at 0xffffffffffffffff, in
[ros2_control_node-14] #18 Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 81, in clone3 [0x72f6ae92684f]
[ros2_control_node-14] #17 Source "./nptl/pthread_create.c", line 442, in start_thread [0x72f6ae894ac2]
[ros2_control_node-14] #16 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x72f6aecdc252, in
[ros2_control_node-14] #15 Object "/install/rclcpp/lib/librclcpp.so", at 0x72f6af19f501, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)
[ros2_control_node-14] #14 Object "/install/rclcpp/lib/librclcpp.so", at 0x72f6af189253, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[ros2_control_node-14] #13 Object "/install/rclcpp/lib/librclcpp.so", at 0x72f6af187fcc, in rclcpp::Executor::execute_service(std::shared_ptrrclcpp::ServiceBase)
[ros2_control_node-14] #12 Object "/install/rclcpp/lib/librclcpp.so", at 0x72f6af185e23, in void take_and_do_error_handling<rclcpp::Executor::execute_service(std::shared_ptrrclcpp::ServiceBase)::{lambda()#1}, rclcpp::Executor::execute_service(std::shared_ptrrclcpp::ServiceBase)::{lambda()#2}>(char const*, char const*, rclcpp::Executor::execute_service(std::shared_ptrrclcpp::ServiceBase)::{lambda()#1}, rclcpp::Executor::execute_service(std::shared_ptrrclcpp::ServiceBase)::{lambda()#2}) [clone .constprop.0]
[ros2_control_node-14] #11 Object "/opt/ros/iron/lib/libcontroller_manager.so", at 0x72f6af4679ba, in
[ros2_control_node-14] #10 Object "/opt/ros/iron/lib/libcontroller_manager.so", at 0x72f6af43d40b, in std::Function_handler<void (std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Response<std::allocator > >), std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Response<std::allocator > >)> >::M_invoke(std::Any_data const&, std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Request<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Response<std::allocator > >&&)
[ros2_control_node-14] #9 Object "/opt/ros/iron/lib/libcontroller_manager.so", at 0x72f6af411d96, in controller_manager::ControllerManager::configure_controller_service_cb(std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::ConfigureController_Response<std::allocator > >)
[ros2_control_node-14] #8 Object "/opt/ros/iron/lib/libcontroller_manager.so", at 0x72f6af405e2f, in controller_manager::ControllerManager::configure_controller(std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&)
[ros2_control_node-14] #7 Object "/opt/ros/iron/lib/libcontroller_interface.so", at 0x72f6aef61165, in controller_interface::ControllerInterfaceBase::configure()
[ros2_control_node-14] #6 Object "/install/rclcpp_lifecycle/lib/librclcpp_lifecycle.so", at 0x72f6aebe27d9, in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(unsigned char)
[ros2_control_node-14] #5 Object "/install/rclcpp_lifecycle/lib/librclcpp_lifecycle.so", at 0x72f6aebe1cf1, in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::change_state(unsigned char, rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn&)
[ros2_control_node-14] #4 Object "/install/rclcpp_lifecycle/lib/librclcpp_lifecycle.so", at 0x72f6aebe18f6, in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) const
[ros2_control_node-14] #3 Object "/opt/ros/iron/lib/libadmittance_controller.so", at 0x72f6ac0c54dd, in admittance_controller::AdmittanceController::on_configure(rclcpp_lifecycle::State const&)
[ros2_control_node-14] #2 Object "/opt/ros/iron/lib/libadmittance_controller.so", at 0x72f6ac0b62a9, in admittance_controller::AdmittanceRule::configure(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> const&, unsigned long)
[ros2_control_node-14] #1 Object "/opt/ros/iron/lib/libkinematics_interface_kdl.so", at 0x72f6ac044a49, in
[ros2_control_node-14] #0 Object "/opt/ros/iron/lib/libkinematics_interface_kdl.so", at 0x72f6ac03a0bf, in
[ros2_control_node-14] Segmentation fault (Address not mapped to object [0x72f6800fc3c8])

To Reproduce

Not sure. I have a rather complex setup. But basically:

1. Setup admittance controller with chained jtc
2. Run the deactivate -> set new params -> reconfigure -> activate steps. (I adapt the stiffness + the kinematic tip frame + the admittance frame)
3. Controller should crash in some cases

Expected behavior

Controller should not crash

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version Iron - binary install
• But RCLCPP with race condition fix: https://github.com/cellumation/rclcpp/tree/action_iron_backport (Issue happens also with default rclcpp)

EDIT: This behavior also occurs with the normal JTC instead of the scaled JTC

[firesurfer]

It looks like this issue is related to reconfiguring the controller and does not occur always but only in like 2-3 of 10 tries.

Edit:

Issue seems origin somewhere in the AdmittanceRule implementation:

0x00007ffff44ab0bf in ?? () from /opt/ros/iron/lib/libkinematics_interface_kdl.so
(gdb) backtrace 
#0  0x00007ffff44ab0bf in ?? ()
   from /opt/ros/iron/lib/libkinematics_interface_kdl.so
#1  0x00007ffff44b5a4a in ?? ()
   from /opt/ros/iron/lib/libkinematics_interface_kdl.so
#2  0x00007ffff45272aa in admittance_controller::AdmittanceRule::configure(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> const&, unsigned long) ()
   from /opt/ros/iron/lib/libadmittance_controller.so