From 126ed159b85ee19510df44f15f417e89c7272cf4 Mon Sep 17 00:00:00 2001
From: Lennart Nachtigall <firesurfer127@gmail.com>
Date: Thu, 22 Aug 2024 16:02:58 +0200
Subject: [PATCH] Fix segfault at reconfigure of AdmittanceController (#1248)

(cherry picked from commit 31f7fbebf023de7cd8bfc2be5117f39bfa77af42)
---
 .../include/admittance_controller/admittance_rule_impl.hpp  | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
index 9b03924882..fa1dcfcc63 100644
--- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
+++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
@@ -45,11 +45,17 @@ controller_interface::return_type AdmittanceRule::configure(
   {
     try
     {
+      // Make sure we destroy the interface first. Otherwise we might run into a segfault
+      if (kinematics_loader_)
+      {
+        kinematics_.reset();
+      }
       kinematics_loader_ =
         std::make_shared<pluginlib::ClassLoader<kinematics_interface::KinematicsInterface>>(
           parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface");
       kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
         kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));
+
       if (!kinematics_->initialize(
             node->get_node_parameters_interface(), parameters_.kinematics.tip))
       {