Skip to content

Commit

Permalink
add pull requests from pal-robotics/gazebo_ros_link_attacher
Browse files Browse the repository at this point in the history
  • Loading branch information
yizhenzhang committed Aug 21, 2021
1 parent be9ed40 commit 1447797
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 9 deletions.
11 changes: 5 additions & 6 deletions include/gazebo_ros_link_attacher.h
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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;
Expand All @@ -84,4 +84,3 @@ namespace gazebo
}

#endif

29 changes: 26 additions & 3 deletions src/gazebo_ros_link_attacher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}


Expand Down Expand Up @@ -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)
Expand All @@ -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;
}

Expand Down Expand Up @@ -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();
}
}

}

0 comments on commit 1447797

Please sign in to comment.