From 61a5995187628a53eb12b48196a9ff603d0d1490 Mon Sep 17 00:00:00 2001
From: Pitrified <nobilipietro@gmail.com>
Date: Tue, 28 Apr 2020 20:22:21 +0200
Subject: [PATCH] Fix random segmentation fault during Detach

Thanks to https://answers.gazebosim.org/question/12118/intermittent-segmentation-fault-possibly-by-custom-worldplugin-attaching-and-detaching-child/?answer=24271#post-id-24271

If the call to Detach is done during an update of the physics engine, a
joint that was present in the beginning of the iteration of the physics
engine is suddenly erased by the call to the Detach function, resulting
in a segmentation fault.

The solution is to save the joints in a vector during the detach_callback,
and execute the actual Detach in another callback, linked to the event
ConnectBeforePhysicsUpdate.
---
 include/gazebo_ros_link_attacher.h |  4 ++++
 src/gazebo_ros_link_attacher.cpp   | 26 +++++++++++++++++++++++++-
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/include/gazebo_ros_link_attacher.h b/include/gazebo_ros_link_attacher.h
index 939c869..993b112 100644
--- a/include/gazebo_ros_link_attacher.h
+++ b/include/gazebo_ros_link_attacher.h
@@ -70,8 +70,12 @@ namespace gazebo
                               gazebo_ros_link_attacher::Attach::Response &res);
         bool detach_callback(gazebo_ros_link_attacher::Attach::Request &req,
                              gazebo_ros_link_attacher::Attach::Response &res);
+        void OnUpdate();
 
         std::vector<fixedJoint> joints;
+        std::vector<fixedJoint> detach_vector;
+
+        event::ConnectionPtr beforePhysicsUpdateConnection;
 
         /// \brief The physics engine.
         physics::PhysicsEnginePtr physics;
diff --git a/src/gazebo_ros_link_attacher.cpp b/src/gazebo_ros_link_attacher.cpp
index f9b7dbf..5d1c578 100644
--- a/src/gazebo_ros_link_attacher.cpp
+++ b/src/gazebo_ros_link_attacher.cpp
@@ -15,6 +15,9 @@ namespace gazebo
   GazeboRosLinkAttacher::GazeboRosLinkAttacher() :
     nh_("link_attacher_node")
   {
+    std::vector<fixedJoint> vect;
+    this->detach_vector = vect;
+    this->beforePhysicsUpdateConnection = event::Events::ConnectBeforePhysicsUpdate(std::bind(&GazeboRosLinkAttacher::OnUpdate, this));
   }
 
 
@@ -151,7 +154,8 @@ namespace gazebo
       // search for the instance of joint and do detach
       fixedJoint j;
       if(this->getJoint(model1, link1, model2, link2, j)){
-          j.joint->Detach();
+          this->detach_vector.push_back(j);
+          ROS_INFO_STREAM("Detach joint request pushed in the detach vector");
           return true;
       }
 
@@ -210,4 +214,24 @@ namespace gazebo
       return true;
   }
 
+  // thanks to https://answers.gazebosim.org/question/12118/intermittent-segmentation-fault-possibly-by-custom-worldplugin-attaching-and-detaching-child/?answer=24271#post-id-24271
+  void GazeboRosLinkAttacher::OnUpdate()
+  {
+    if(!this->detach_vector.empty())
+    {
+      ROS_INFO_STREAM("Received before physics update callback... Detaching joints");
+      std::vector<fixedJoint>::iterator it;
+      it = this->detach_vector.begin();
+      fixedJoint j;
+      while (it != this->detach_vector.end())
+      {
+        j = *it;
+        j.joint->Detach();
+        ROS_INFO_STREAM("Joint detached !");
+        ++it;
+      }
+      detach_vector.clear();
+    }
+  }
+
 }