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 joints; + std::vector 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 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::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(); + } + } + }