add pull requests from pal-robotics/gazebo_ros_link_attacher

pal-robotics#11

Author: yizhenzhang

include/gazebo_ros_link_attacher.h

@@ -1,9 +1,5 @@
-/*
- * Desc: Gazebo link attacher plugin.
- * Author: Sammy Pfeiffer ([email protected])
- * Date: 05/04/2016
- */
 
+/*...
 #ifndef GAZEBO_ROS_LINK_ATTACHER_HH
 #define GAZEBO_ROS_LINK_ATTACHER_HH
 
@@ -70,8 +66,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;
@@ -84,4 +84,3 @@ namespace gazebo
 }
 
 #endif
-

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));
   }
 
 
@@ -126,7 +129,6 @@ namespace gazebo
      failed in void gazebo::physics::Entity::PublishPose():
      /tmp/buildd/gazebo2-2.2.3/gazebo/physics/Entity.cc(225):
      An entity without a parent model should not happen
-
      * If SetModel is given the same model than CreateJoint given
      * Gazebo crashes with
      * ***** Internal Program Error - assertion (self->inertial != __null)
@@ -151,7 +153,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 +213,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();
+    }
+  }
+
+}