$search

gazebo_ros_grasp_hack.cpp

Go to the documentation of this file.

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 * Desc: 3D position interface for ground truth.
 * Author: Sachin Chitta and John Hsu
 * Date: 1 June 2008
 * SVN info: $Id$
 */

#include <gazebo_plugins/gazebo_ros_grasp_hack.h>

#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/HingeJoint.hh>
#include <gazebo/SliderJoint.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <gazebo/World.hh>

using namespace gazebo;

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_grasp_hack", GazeboRosGraspHack);

// Constructor
GazeboRosGraspHack::GazeboRosGraspHack(Entity *parent )
   : Controller(parent)
{
  // check for contact with [lr]_gripper_*_finger_tip_link
  // if contacts exist, grab relative pose between this body and the [lr]_gripper_palm_link
  // fix object to the relative pose until we lose contact

  this->myParentBody = dynamic_cast<Body*>(this->parent);

  if (!this->myParentBody)
    gzthrow("GazeboRosGraspHack controller requires a Body as its parent");

  this->myParentModel = myParentBody->GetModel();

  Param::Begin(&this->parameters);
  this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0);
  this->topicNameP = new ParamT<std::string>("topicName", "", 0);
  this->graspCheckRateP = new ParamT<double>("graspCheckRate", 10, 0);
  Param::End();

  // danger: assume start up without grasp
  this->l_grasp = false;
  this->r_grasp = false;

  this->last_time = Simulator::Instance()->GetSimTime();
  this->last_grasp_check_time = Simulator::Instance()->GetSimTime();
  this->found_gripper_links = false;
  this->topicName.clear();


  // all the link names in the gripper
  this->gripper_bodies.clear();
  this->gripper_bodies.insert(std::make_pair("l_gripper_l_finger_tip_link", GripperPose()));
  this->gripper_bodies.insert(std::make_pair("l_gripper_r_finger_tip_link", GripperPose()));
  this->gripper_bodies.insert(std::make_pair("l_gripper_l_finger_link"    , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("l_gripper_r_finger_link"    , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("l_gripper_motor_slider_link", GripperPose()));
  this->gripper_bodies.insert(std::make_pair("l_gripper_motor_screw_link" , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("l_gripper_l_parallel_link"  , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("l_gripper_r_parallel_link"  , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_l_finger_tip_link", GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_r_finger_tip_link", GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_l_finger_link"    , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_r_finger_link"    , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_motor_slider_link", GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_motor_screw_link" , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_l_parallel_link"  , GripperPose()));
  this->gripper_bodies.insert(std::make_pair("r_gripper_r_parallel_link"  , GripperPose()));
}

// Destructor
GazeboRosGraspHack::~GazeboRosGraspHack()
{
  delete this->robotNamespaceP;
  delete this->topicNameP;
  delete this->graspCheckRateP;
  delete this->rosnode_;
}

// Load the controller
void GazeboRosGraspHack::LoadChild(XMLConfigNode *node)
{
  this->robotNamespaceP->Load(node);
  this->robotNamespace = this->robotNamespaceP->GetValue();
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  this->rosnode_ = new ros::NodeHandle(this->robotNamespace);

  this->topicNameP->Load(node);

  if (this->topicNameP->GetValue() != "")
  {
    this->topicName = this->robotNamespace+"/"+this->topicNameP->GetValue()+"/"+this->GetName();
    ros::AdvertiseOptions grasp_hack_ao = ros::AdvertiseOptions::create<std_msgs::String>(
      this->topicName,1,
      boost::bind( &GazeboRosGraspHack::GraspHackConnect,this),
      boost::bind( &GazeboRosGraspHack::GraspHackDisconnect,this), ros::VoidPtr(), &this->grasp_hack_queue_);
    this->pub_ = this->rosnode_->advertise(grasp_hack_ao);
  }

  this->graspCheckRateP->Load(node);
  this->graspCheckRate = this->graspCheckRateP->GetValue();

}

// Increment count
void GazeboRosGraspHack::GraspHackConnect()
{
  this->graspHackConnectCount++;
}
// Decrement count
void GazeboRosGraspHack::GraspHackDisconnect()
{
  this->graspHackConnectCount--;
}

// Initialize the controller
void GazeboRosGraspHack::InitChild()
{
  // start custom queue for grasp_hack
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosGraspHack::GraspHackQueueThread,this ) );
  this->get_gripper_links_thread_ = boost::thread( boost::bind( &GazeboRosGraspHack::GetGripperLinks,this ) );
}

// Look for Gripper Links
void GazeboRosGraspHack::GetGripperLinks()
{
  this->last_time = Simulator::Instance()->GetSimTime();

  this->l_wrist_roll_body = NULL;
  this->r_wrist_roll_body = NULL;

  // at least one arm is captured
  bool found_gripper_bodies = false;
  while (!found_gripper_bodies)
  {
    std::vector<Model*> all_models = World::Instance()->GetModels();
    for (std::vector<Model*>::iterator iter = all_models.begin(); iter != all_models.end(); iter++)
    {
      if (*iter) // search all models, maybe better to check only for pr2?
      {
        boost::recursive_mutex::scoped_lock lock( *Simulator::Instance()->GetMRMutex());// wait for ProcessEntitiesToLoad() to finish

        // get gripper parent bodies (wrist_roll_link)
        if (!this->l_wrist_roll_body) this->l_wrist_roll_body = dynamic_cast<Body*>((*iter)->GetBody("l_wrist_roll_link"));
        if (!this->r_wrist_roll_body) this->r_wrist_roll_body = dynamic_cast<Body*>((*iter)->GetBody("r_wrist_roll_link"));

        // get all the children links of wrist_roll_body
        for (std::map<std::string, GripperPose>::iterator biter = this->gripper_bodies.begin(); biter != this->gripper_bodies.end(); biter++)
          if (!biter->second.body) biter->second.body = dynamic_cast<Body*>((*iter)->GetBody(biter->first));
      }
    }

    // check if all gripper bodies are found
    found_gripper_bodies = true;
    if (!this->l_wrist_roll_body || !this->r_wrist_roll_body) found_gripper_bodies = false;
    for (std::map<std::string, GripperPose>::iterator biter = this->gripper_bodies.begin(); biter != this->gripper_bodies.end(); biter++)
      if (!biter->second.body) found_gripper_bodies = false;
    usleep(100000);
  }
  this->found_gripper_links = true;
}

// check contacts to determine grasps
void GazeboRosGraspHack::CheckForGrasps()
{
  this->last_grasp_check_time = Simulator::Instance()->GetSimTime();

  //check for contact with left gripper finger tips
  {
    bool l_gripper_l_finger = false;
    bool l_gripper_r_finger = false;

    int jointCount = gazebo::World::Instance()->GetContactJointCount();
    for (int i = 0; i < jointCount ; i++)
    {
      World::ContactJoint j = gazebo::World::Instance()->GetContactJoint(i);
      if ((j.b1 == this->myParentBody) || (j.b2 == this->myParentBody))
      {
        if ((j.b1 == this->gripper_bodies["l_gripper_l_finger_tip_link"].body) || (j.b2 == this->gripper_bodies["l_gripper_l_finger_tip_link"].body))
          l_gripper_l_finger = true;
        if ((j.b1 == this->gripper_bodies["l_gripper_r_finger_tip_link"].body) || (j.b2 == this->gripper_bodies["l_gripper_r_finger_tip_link"].body))
          l_gripper_r_finger = true;
      }
    }

    if (l_gripper_l_finger && l_gripper_r_finger)
    {
      if (!this->l_grasp)
      {
        this->l_grasp = true;
        this->l_grasp_relative_pose = this->myParentModel->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_l_finger_tip_link"].relative_pose = this->gripper_bodies["l_gripper_l_finger_tip_link"].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_r_finger_tip_link"].relative_pose = this->gripper_bodies["l_gripper_r_finger_tip_link"].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_l_finger_link"    ].relative_pose = this->gripper_bodies["l_gripper_l_finger_link"    ].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_r_finger_link"    ].relative_pose = this->gripper_bodies["l_gripper_r_finger_link"    ].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_motor_slider_link"].relative_pose = this->gripper_bodies["l_gripper_motor_slider_link"].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_motor_screw_link" ].relative_pose = this->gripper_bodies["l_gripper_motor_screw_link" ].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_l_parallel_link"  ].relative_pose = this->gripper_bodies["l_gripper_l_parallel_link"  ].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["l_gripper_r_parallel_link"  ].relative_pose = this->gripper_bodies["l_gripper_r_parallel_link"  ].body->GetWorldPose() - this->l_wrist_roll_body->GetWorldPose();

      }
    }
    else
      this->l_grasp = false;
  }
  //check for contact with right gripper finger tips
  {
    bool r_gripper_l_finger = false;
    bool r_gripper_r_finger = false;

    int jointCount = gazebo::World::Instance()->GetContactJointCount();
    for (int i = 0; i < jointCount ; i++)
    {
      World::ContactJoint j = gazebo::World::Instance()->GetContactJoint(i);
      //ROS_ERROR("b %s b1 %s b2 %s",this->myParentBody->GetName().c_str(),j.b1->GetName().c_str(),j.b2->GetName().c_str());
      if ((j.b1 == this->myParentBody) || (j.b2 == this->myParentBody))
      {
        if ((j.b1 == this->gripper_bodies["r_gripper_l_finger_tip_link"].body) || (j.b2 == this->gripper_bodies["r_gripper_l_finger_tip_link"].body))
          r_gripper_l_finger = true;
        if ((j.b1 == this->gripper_bodies["r_gripper_r_finger_tip_link"].body) || (j.b2 == this->gripper_bodies["r_gripper_r_finger_tip_link"].body))
          r_gripper_r_finger = true;
        //ROS_ERROR("%d %d b %ld b1 %ld b2 %ld rtip %ld ltip %ld",
        //   (int)r_gripper_l_finger,(int)r_gripper_r_finger,(long)this->myParentBody,(long)j.b1,(long)j.b2,
        //   (long)this->r_gripper_r_finger_tip_body,(long)this->r_gripper_l_finger_tip_body);
      }
    }

    if (r_gripper_l_finger && r_gripper_r_finger)
    {
      if (!r_grasp)
      {
        this->r_grasp = true;
        this->r_grasp_relative_pose = this->myParentModel->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_l_finger_link"    ].relative_pose = this->gripper_bodies["r_gripper_l_finger_link"    ].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_l_finger_tip_link"].relative_pose = this->gripper_bodies["r_gripper_l_finger_tip_link"].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_r_finger_tip_link"].relative_pose = this->gripper_bodies["r_gripper_r_finger_tip_link"].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_r_finger_link"    ].relative_pose = this->gripper_bodies["r_gripper_r_finger_link"    ].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_motor_slider_link"].relative_pose = this->gripper_bodies["r_gripper_motor_slider_link"].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_motor_screw_link" ].relative_pose = this->gripper_bodies["r_gripper_motor_screw_link" ].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_l_parallel_link"  ].relative_pose = this->gripper_bodies["r_gripper_l_parallel_link"  ].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
        this->gripper_bodies["r_gripper_r_parallel_link"  ].relative_pose = this->gripper_bodies["r_gripper_r_parallel_link"  ].body->GetWorldPose() - this->r_wrist_roll_body->GetWorldPose();
      }
    }
    if (!r_gripper_l_finger || !r_gripper_r_finger)
      r_grasp = false;
  }
}

// Update the controller
void GazeboRosGraspHack::UpdateChild()
{
  if (!this->found_gripper_links) return;

  gazebo::Time cur_time = Simulator::Instance()->GetSimTime();
  // throttled check for grasp (computationally expensive)
  if ((this->graspCheckRate > 0) && (cur_time - this->last_grasp_check_time) > 1.0/this->graspCheckRate)
    this->CheckForGrasps();

  // apply pose update if grasp is made
  if (this->l_grasp)
  {
    // fix myParentBody relative to l_wrist_roll_body
    gazebo::Pose3d pose = this->l_grasp_relative_pose + this->l_wrist_roll_body->GetWorldPose();
    this->myParentModel->SetWorldPose(pose);
    /*  this does not allow gripper to open once a grasp is made :(
    this->gripper_bodies["l_gripper_l_finger_tip_link"].body->SetWorldPose( this->gripper_bodies["l_gripper_l_finger_tip_link"].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_r_finger_tip_link"].body->SetWorldPose( this->gripper_bodies["l_gripper_r_finger_tip_link"].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_l_finger_link"    ].body->SetWorldPose( this->gripper_bodies["l_gripper_l_finger_link"    ].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_r_finger_link"    ].body->SetWorldPose( this->gripper_bodies["l_gripper_r_finger_link"    ].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_motor_slider_link"].body->SetWorldPose( this->gripper_bodies["l_gripper_motor_slider_link"].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_motor_screw_link" ].body->SetWorldPose( this->gripper_bodies["l_gripper_motor_screw_link" ].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_l_parallel_link"  ].body->SetWorldPose( this->gripper_bodies["l_gripper_l_parallel_link"  ].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_r_parallel_link"  ].body->SetWorldPose( this->gripper_bodies["l_gripper_r_parallel_link"  ].relative_pose + this->l_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["l_gripper_l_finger_tip_link"].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_r_finger_tip_link"].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_l_finger_link"    ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_r_finger_link"    ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_motor_slider_link"].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_motor_screw_link" ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_l_parallel_link"  ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_r_parallel_link"  ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_l_finger_tip_link"].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_r_finger_tip_link"].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_l_finger_link"    ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_r_finger_link"    ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_motor_slider_link"].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_motor_screw_link" ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_l_parallel_link"  ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["l_gripper_r_parallel_link"  ].body->SetLinearVel( Vector3(0,0,0) );*/
  }


  // apply pose update if grasp is made
  if (r_grasp)
  {
    // fix myParentBody relative to r_wrist_roll_body
    gazebo::Pose3d pose = this->r_grasp_relative_pose + this->r_wrist_roll_body->GetWorldPose();
    this->myParentModel->SetWorldPose(pose);
    /*  this does not allow gripper to open once a grasp is made :(
    this->gripper_bodies["r_gripper_l_finger_tip_link"].body->SetWorldPose( this->gripper_bodies["r_gripper_l_finger_tip_link"].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_r_finger_tip_link"].body->SetWorldPose( this->gripper_bodies["r_gripper_r_finger_tip_link"].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_l_finger_link"    ].body->SetWorldPose( this->gripper_bodies["r_gripper_l_finger_link"    ].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_r_finger_link"    ].body->SetWorldPose( this->gripper_bodies["r_gripper_r_finger_link"    ].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_motor_slider_link"].body->SetWorldPose( this->gripper_bodies["r_gripper_motor_slider_link"].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_motor_screw_link" ].body->SetWorldPose( this->gripper_bodies["r_gripper_motor_screw_link" ].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_l_parallel_link"  ].body->SetWorldPose( this->gripper_bodies["r_gripper_l_parallel_link"  ].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_r_parallel_link"  ].body->SetWorldPose( this->gripper_bodies["r_gripper_r_parallel_link"  ].relative_pose + this->r_wrist_roll_body->GetWorldPose() );
    this->gripper_bodies["r_gripper_l_finger_tip_link"].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_r_finger_tip_link"].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_l_finger_link"    ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_r_finger_link"    ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_motor_slider_link"].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_motor_screw_link" ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_l_parallel_link"  ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_r_parallel_link"  ].body->SetAngularVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_l_finger_tip_link"].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_r_finger_tip_link"].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_l_finger_link"    ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_r_finger_link"    ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_motor_slider_link"].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_motor_screw_link" ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_l_parallel_link"  ].body->SetLinearVel( Vector3(0,0,0) );
    this->gripper_bodies["r_gripper_r_parallel_link"  ].body->SetLinearVel( Vector3(0,0,0) );*/
  }




  // misc debugging
  if (!this->topicName.empty())
  {
    // get inertial Rates
    // differentiate to get accelerations
    if(cur_time.Double() > this->last_time.Double())
    {
      this->lock.lock();
      std_msgs::String pubStr;

      std::ostringstream stream;
      stream    << "grasp hack [" << this->GetName()
                << "] links_found [" << this->found_gripper_links
                << "] l_touch [" << this->l_grasp
                << "] r_touch [" << this->r_grasp
                << "] time[" << cur_time
                << "]\n";
      pubStr.data = stream.str();
      // publish to ros
      this->pub_.publish(pubStr);
      this->lock.unlock();

      // save last time stamp
      this->last_time = cur_time;
    }
  }

}

// Finalize the controller
void GazeboRosGraspHack::FiniChild()
{
  this->rosnode_->shutdown();
  this->grasp_hack_queue_.clear();
  this->grasp_hack_queue_.disable();
  this->callback_queue_thread_.join();
  this->get_gripper_links_thread_.join();
}

// Put laser data to the interface
void GazeboRosGraspHack::GraspHackQueueThread()
{
  static const double timeout = 0.01;

  while (this->rosnode_->ok())
  {
    this->grasp_hack_queue_.callAvailable(ros::WallDuration(timeout));
  }
}

---

gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Sat Mar 2 13:39:46 2013