Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00046 GazeboRosGraspHack::GazeboRosGraspHack(Entity *parent )
00047 : Controller(parent)
00048 {
00049 ROS_ERROR("starting grasp hack");
00050
00051
00052
00053
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
00068 this->l_grasp = false;
00069 this->r_grasp = false;
00070 }
00071
00073
00074 GazeboRosGraspHack::~GazeboRosGraspHack()
00075 {
00076 delete this->robotNamespaceP;
00077 delete this->topicNameP;
00078 delete this->rosnode_;
00079 }
00080
00082
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
00113 void GazeboRosGraspHack::GraspHackConnect()
00114 {
00115 this->graspHackConnectCount++;
00116 }
00118
00119 void GazeboRosGraspHack::GraspHackDisconnect()
00120 {
00121 this->graspHackConnectCount--;
00122 }
00123
00125
00126 void GazeboRosGraspHack::InitChild()
00127 {
00128 this->last_time = Simulator::Instance()->GetSimTime();
00129
00130
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
00162 void GazeboRosGraspHack::UpdateChild()
00163 {
00164
00165
00166 if (l_grasp)
00167 {
00168
00169 gazebo::Pose3d pose = this->l_grasp_relative_pose + this->l_gripper_palm_body->GetWorldPose();
00170 this->myParentModel->SetWorldPose(pose);
00171 }
00172
00173
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
00206 gazebo::Pose3d pose = this->r_grasp_relative_pose + this->r_gripper_palm_body->GetWorldPose();
00207 this->myParentModel->SetWorldPose(pose);
00208
00209
00210
00211
00212
00213
00214
00215 }
00216
00217
00218
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
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
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
00259
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
00269 this->pub_.publish(pubStr);
00270 }
00271 this->lock.unlock();
00272
00273
00274 this->last_time = cur_time;
00275 }
00276 }
00277 }
00278
00280
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
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