simple_robot_control: simple_robot_control: gripper

00001 /*
00002  * gripper_control.cpp
00003  *
00004  *  Created on: Jul 27, 2010
00005  *      Author: christian
00006  */
00007 
00008 #include <simple_robot_control/gripper_control.h>
00009 #include <simple_robot_control/ReturnJointStates.h>
00010 
00011 namespace simple_robot_control{
00012 
00013 Gripper::Gripper(char gripper_side): gripperside_str (&gripper_side, 1),
00014                 gripper_client_("/" + gripperside_str + "_gripper_controller/gripper_action", true){
00015 
00016          if (gripper_side !='l' && gripper_side != 'r' ) ROS_ERROR("Gripper:specify l or r for arm side");
00017 
00018 
00019     //wait for the gripper action server to come up
00020     while(!gripper_client_.waitForServer(ros::Duration(5.0))){
00021       ROS_INFO("Waiting for the %s_gripper_controller/gripper_action action server to come up", gripperside_str.c_str());
00022     }
00023   }
00024 
00025 Gripper::~Gripper(){
00026 
00027   }
00028 
00029   //Open the gripper
00030   void Gripper::open(double effort){
00031     pr2_controllers_msgs::Pr2GripperCommandGoal open;
00032     open.command.position = 0.08;
00033     open.command.max_effort = effort;  // Do not limit effort (negative)
00034 
00035     ROS_INFO("Sending open goal");
00036     gripper_client_.sendGoal(open);
00037     gripper_client_.waitForResult(ros::Duration(5.0));
00038     if(gripper_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00039       ROS_INFO("The gripper opened!");
00040     else
00041       ROS_INFO("The gripper failed to open.");
00042   }
00043 
00044   //Close the gripper
00045   void Gripper::close(double effort){
00046     pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
00047     squeeze.command.position = -100.0;
00048     squeeze.command.max_effort = effort;  // Close gently
00049 
00050     ROS_INFO("Sending squeeze goal");
00051     gripper_client_.sendGoal(squeeze);
00052     gripper_client_.waitForResult(ros::Duration(5.0));
00053     if(gripper_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00054       ROS_INFO("The gripper closed!");
00055     else
00056       ROS_INFO("The gripper failed to close.");
00057   }
00058 
00059 
00060 
00061 
00062   void Gripper::gripperToPos(double pos, double effort){
00063     pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
00064     squeeze.command.position = pos;
00065     squeeze.command.max_effort = effort;  // Close gently
00066 
00067     ROS_INFO("Sending squeeze goal");
00068     gripper_client_.sendGoal(squeeze);
00069     gripper_client_.waitForResult(ros::Duration(1.5));
00070     if(gripper_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00071       ROS_INFO("The gripper was moved to pos %f!", pos);
00072     else
00073       ROS_INFO("The gripper failed to move to pos %f.", effort);
00074   }
00075 
00076 
00077   bool Gripper::getOpeningWidth(double& opening_width){
00078         simple_robot_control::ReturnJointStates req;
00079         req.request.name.push_back( gripperside_str + "_gripper_joint");
00080 
00081         if(!ros::service::waitForService("return_joint_states", ros::Duration(5.0))){
00082                 ROS_ERROR("Gripper:Could not contact return_joint_states service. Did you run joint_state_listner?");
00083                 return false;
00084         }
00085         if (!ros::service::call("return_joint_states",req)){
00086                 ROS_ERROR("Gripper service call failed");
00087         }
00088         opening_width =  req.response.position[0];
00089         return true;
00090 
00091   }
00092 
00093 }