Skip to content

Commit

Permalink
+ Fix for crash during detach
Browse files Browse the repository at this point in the history
  • Loading branch information
guptavaibhav0 committed Dec 17, 2020
1 parent 88e6f06 commit 3c352cd
Show file tree
Hide file tree
Showing 2 changed files with 30 additions and 1 deletion.
4 changes: 4 additions & 0 deletions include/gazebo_ros_link_attacher.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
27 changes: 26 additions & 1 deletion src/gazebo_ros_link_attacher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,10 @@ 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));
}


Expand Down Expand Up @@ -172,7 +176,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;
}

Expand Down Expand Up @@ -231,4 +236,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();
}
}

}

0 comments on commit 3c352cd

Please sign in to comment.