gazebo_ros_grasp_hack.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: 3D position interface for ground truth.
00023  * Author: Sachin Chitta and John Hsu
00024  * Date: 1 June 2008
00025  * SVN info: $Id$
00026  */
00027 
00028 #include <pr2_arm_gazebo/gazebo_ros_grasp_hack.h>
00029 
00030 #include <gazebo/Global.hh>
00031 #include <gazebo/XMLConfig.hh>
00032 #include <gazebo/HingeJoint.hh>
00033 #include <gazebo/SliderJoint.hh>
00034 #include <gazebo/Simulator.hh>
00035 #include <gazebo/gazebo.h>
00036 #include <gazebo/GazeboError.hh>
00037 #include <gazebo/ControllerFactory.hh>
00038 #include <gazebo/World.hh>
00039 
00040 using namespace gazebo;
00041 
00042 GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_grasp_hack", GazeboRosGraspHack);
00043 
00045 // Constructor
00046 GazeboRosGraspHack::GazeboRosGraspHack(Entity *parent )
00047    : Controller(parent)
00048 {
00049   ROS_ERROR("starting grasp hack");
00050 
00051   // check for contact with [lr]_gripper_*_finger_tip_link
00052   // if contacts exist, grab relative pose between this body and the [lr]_gripper_palm_link
00053   // fix object to the relative pose until we lose contact
00054 
00055   this->myParentBody = dynamic_cast<Body*>(this->parent);
00056 
00057   if (!this->myParentBody)
00058     gzthrow("GazeboRosGraspHack controller requires a Body as its parent");
00059 
00060   this->myParentModel = myParentBody->GetModel();
00061 
00062   Param::Begin(&this->parameters);
00063   this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0);
00064   this->topicNameP = new ParamT<std::string>("topicName", "grasp_hack", 0);
00065   Param::End();
00066 
00067   // danger: assume start up without grasp
00068   this->l_grasp = false;
00069   this->r_grasp = false;
00070 }
00071 
00073 // Destructor
00074 GazeboRosGraspHack::~GazeboRosGraspHack()
00075 {
00076   delete this->robotNamespaceP;
00077   delete this->topicNameP;
00078   delete this->rosnode_;
00079 }
00080 
00082 // Load the controller
00083 void GazeboRosGraspHack::LoadChild(XMLConfigNode *node)
00084 {
00085   this->robotNamespaceP->Load(node);
00086   this->robotNamespace = this->robotNamespaceP->GetValue();
00087   if (!ros::isInitialized())
00088   {
00089     int argc = 0;
00090     char** argv = NULL;
00091     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00092   }
00093 
00094   this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
00095 
00096   this->topicNameP->Load(node);
00097   this->topicName = this->topicNameP->GetValue();
00098 
00099   if (this->topicName != "")
00100   {
00101   ros::AdvertiseOptions grasp_hack_ao = ros::AdvertiseOptions::create<std_msgs::String>(
00102     this->topicName,1,
00103     boost::bind( &GazeboRosGraspHack::GraspHackConnect,this),
00104     boost::bind( &GazeboRosGraspHack::GraspHackDisconnect,this), ros::VoidPtr(), &this->grasp_hack_queue_);
00105   this->pub_ = this->rosnode_->advertise(grasp_hack_ao);
00106   }
00107 
00108 
00109 }
00110 
00112 // Increment count
00113 void GazeboRosGraspHack::GraspHackConnect()
00114 {
00115   this->graspHackConnectCount++;
00116 }
00118 // Decrement count
00119 void GazeboRosGraspHack::GraspHackDisconnect()
00120 {
00121   this->graspHackConnectCount--;
00122 }
00123 
00125 // Initialize the controller
00126 void GazeboRosGraspHack::InitChild()
00127 {
00128   this->last_time = Simulator::Instance()->GetSimTime();
00129 
00130   // start custom queue for grasp_hack
00131   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosGraspHack::GraspHackQueueThread,this ) );
00132 
00133   std::vector<Model*> all_models = World::Instance()->GetModels();
00134   for (std::vector<Model*>::iterator iter = all_models.begin(); iter != all_models.end(); iter++)
00135   {
00136     if (*iter)
00137     {
00138       this->l_gripper_palm_body          = dynamic_cast<Body*>((*iter)->GetBody("l_wrist_roll_link"));
00139       this->r_gripper_palm_body          = dynamic_cast<Body*>((*iter)->GetBody("r_wrist_roll_link"));
00140       this->l_gripper_l_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("l_gripper_l_finger_tip_link"));
00141       this->l_gripper_r_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("l_gripper_r_finger_tip_link"));
00142       this->r_gripper_l_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("r_gripper_l_finger_tip_link"));
00143       this->r_gripper_r_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("r_gripper_r_finger_tip_link"));
00144 
00145       this->l_gripper_l_finger_joint     = (*iter)->GetJoint("l_gripper_l_finger_joint");
00146       this->l_gripper_r_finger_joint     = (*iter)->GetJoint("l_gripper_r_finger_joint");
00147       this->l_gripper_l_finger_tip_joint = (*iter)->GetJoint("l_gripper_l_finger_tip_joint");
00148       this->l_gripper_r_finger_tip_joint = (*iter)->GetJoint("l_gripper_r_finger_tip_joint");
00149 
00150       this->r_gripper_l_finger_joint     = (*iter)->GetJoint("r_gripper_l_finger_joint");
00151       this->r_gripper_r_finger_joint     = (*iter)->GetJoint("r_gripper_r_finger_joint");
00152       this->r_gripper_l_finger_tip_joint = (*iter)->GetJoint("r_gripper_l_finger_tip_joint");
00153       this->r_gripper_r_finger_tip_joint = (*iter)->GetJoint("r_gripper_r_finger_tip_joint");
00154     }
00155   }
00156   if (!this->r_gripper_palm_body || !this->r_gripper_r_finger_tip_body)
00157     gzthrow("GazeboRosGraspHack controller could not find r_gripper_palm_body or r_gripper_r_finger_tip_body");
00158 }
00159 
00161 // Update the controller
00162 void GazeboRosGraspHack::UpdateChild()
00163 {
00164   //ROS_ERROR("jc %d",gazebo::World::Instance()->GetContactJointCount());
00165 
00166   if (l_grasp)
00167   {
00168     // fix myParentBody relative to l_gripper_palm_body
00169     gazebo::Pose3d pose = this->l_grasp_relative_pose + this->l_gripper_palm_body->GetWorldPose();
00170     this->myParentModel->SetWorldPose(pose);
00171   }
00172 
00173   //check for contact with left gripper finger tips
00174   {
00175     bool l_gripper_l_finger = false;
00176     bool l_gripper_r_finger = false;
00177 
00178     int jointCount = gazebo::World::Instance()->GetContactJointCount();
00179     for (int i = 0; i < jointCount ; i++)
00180     {
00181       World::ContactJoint j = gazebo::World::Instance()->GetContactJoint(i);
00182       if ((j.b1 == this->myParentBody) || (j.b2 == this->myParentBody))
00183       {
00184         if ((j.b1 == this->l_gripper_l_finger_tip_body) || (j.b2 == this->l_gripper_l_finger_tip_body))
00185           l_gripper_l_finger = true;
00186         if ((j.b1 == this->l_gripper_r_finger_tip_body) || (j.b2 == this->l_gripper_r_finger_tip_body))
00187           l_gripper_r_finger = true;
00188       }
00189     }
00190 
00191     if (l_gripper_l_finger && l_gripper_r_finger)
00192     {
00193       if (!l_grasp)
00194       {
00195         l_grasp = true;
00196         l_grasp_relative_pose = this->myParentModel->GetWorldPose() - this->l_gripper_palm_body->GetWorldPose();
00197       }
00198     }
00199     else
00200       l_grasp = false;
00201   }
00202 
00203   if (r_grasp)
00204   {
00205     // fix myParentBody relative to r_gripper_palm_body
00206     gazebo::Pose3d pose = this->r_grasp_relative_pose + this->r_gripper_palm_body->GetWorldPose();
00207     this->myParentModel->SetWorldPose(pose);
00208     //std::cout << "right grasp hack active [" << pose << "]\n";
00209 
00210     // migrate setModelConfiguration from gazeboros into gazebo trunk, and we can do this
00211     //this->r_gripper_l_finger_joint_position     = this->r_gripper_l_finger_joint->GetAngle(0).GetAsRadian();
00212     //this->r_gripper_r_finger_joint_position     = this->r_gripper_r_finger_joint->GetAngle(0).GetAsRadian();
00213     //this->r_gripper_l_finger_tip_joint_position = this->r_gripper_l_finger_tip_joint->GetAngle(0).GetAsRadian();
00214     //this->r_gripper_r_finger_tip_joint_position = this->r_gripper_r_finger_tip_joint->GetAngle(0).GetAsRadian();
00215   }
00216 
00217 
00218   //check for contact with right gripper finger tips
00219   {
00220     bool r_gripper_l_finger = false;
00221     bool r_gripper_r_finger = false;
00222 
00223     int jointCount = gazebo::World::Instance()->GetContactJointCount();
00224     for (int i = 0; i < jointCount ; i++)
00225     {
00226       World::ContactJoint j = gazebo::World::Instance()->GetContactJoint(i);
00227       //ROS_ERROR("b %s b1 %s b2 %s",this->myParentBody->GetName().c_str(),j.b1->GetName().c_str(),j.b2->GetName().c_str());
00228       if ((j.b1 == this->myParentBody) || (j.b2 == this->myParentBody))
00229       {
00230         if ((j.b1 == this->r_gripper_l_finger_tip_body) || (j.b2 == this->r_gripper_l_finger_tip_body))
00231           r_gripper_l_finger = true;
00232         if ((j.b1 == this->r_gripper_r_finger_tip_body) || (j.b2 == this->r_gripper_r_finger_tip_body))
00233           r_gripper_r_finger = true;
00234         //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);
00235       }
00236       if (r_gripper_l_finger && r_gripper_r_finger)
00237       {
00238         if (!r_grasp)
00239         {
00240           this->r_grasp = true;
00241           this->r_grasp_relative_pose = this->myParentModel->GetWorldPose() - this->r_gripper_palm_body->GetWorldPose();
00242           this->r_gripper_l_finger_joint_position = this->r_gripper_l_finger_joint->GetAngle(0).GetAsRadian();
00243           this->r_gripper_r_finger_joint_position = this->r_gripper_r_finger_joint->GetAngle(0).GetAsRadian();
00244           this->r_gripper_l_finger_tip_joint_position = this->r_gripper_l_finger_tip_joint->GetAngle(0).GetAsRadian();
00245           this->r_gripper_r_finger_tip_joint_position = this->r_gripper_r_finger_tip_joint->GetAngle(0).GetAsRadian();
00246 
00247           break;
00248         }
00249       }
00250     }
00251     if (!r_gripper_l_finger || !r_gripper_r_finger)
00252       r_grasp = false;
00253   }
00254 
00255 
00256   {
00257     gazebo::Time cur_time = Simulator::Instance()->GetSimTime();
00258     // get inertial Rates
00259     // differentiate to get accelerations
00260     double tmp_dt = cur_time.Double() - this->last_time.Double();
00261     if (tmp_dt != 0)
00262     {
00263       this->lock.lock();
00264       if (this->topicName != "")
00265       {
00266         std_msgs::String pubStr;
00267         pubStr.data = "ok";
00268         // publish to ros
00269         this->pub_.publish(pubStr);
00270       }
00271       this->lock.unlock();
00272 
00273       // save last time stamp
00274       this->last_time = cur_time;
00275     }
00276   }
00277 }
00278 
00280 // Finalize the controller
00281 void GazeboRosGraspHack::FiniChild()
00282 {
00283   this->rosnode_->shutdown();
00284   this->grasp_hack_queue_.clear();
00285   this->grasp_hack_queue_.disable();
00286   this->callback_queue_thread_.join();
00287 }
00288 
00290 // Put laser data to the interface
00291 void GazeboRosGraspHack::GraspHackQueueThread()
00292 {
00293   static const double timeout = 0.01;
00294 
00295   while (this->rosnode_->ok())
00296   {
00297     this->grasp_hack_queue_.callAvailable(ros::WallDuration(timeout));
00298   }
00299 }
00300 


pr2_arm_gazebo
Author(s): John Hsu
autogenerated on Mon Jan 6 2014 12:02:20