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
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ros/ros.h>
00038 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00039 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactAction.h>
00040 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoAction.h>
00041 #include <pr2_gripper_sensor_msgs/PR2GripperGrabAction.h>
00042 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoAction.h>
00043 #include <std_srvs/Empty.h>
00044 #include <actionlib/server/simple_action_server.h>
00045 #include <actionlib/client/simple_action_client.h>
00046
00047
00048 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperSlipServoAction> SlipClient;
00049
00050 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> ContactClient;
00051
00052 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00053
00054 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperForceServoAction> ForceClient;
00055
00056
00057 class Gripper{
00058 private:
00059 GripperClient* gripper_client_;
00060 ContactClient* contact_client_;
00061 SlipClient* slip_client_;
00062 ForceClient* force_client_;
00063
00064
00065 public:
00066
00067 Gripper(){
00068
00069
00070
00071 gripper_client_ = new GripperClient("gripper_action", true);
00072 contact_client_ = new ContactClient("find_contact",true);
00073 slip_client_ = new SlipClient("slip_servo",true);
00074 force_client_ = new ForceClient("force_servo",true);
00075
00076
00077
00078 while(!gripper_client_->waitForServer(ros::Duration(5.0))){
00079 ROS_INFO("Waiting for the gripper_action action server to come up");
00080 }
00081
00082 while(!contact_client_->waitForServer(ros::Duration(5.0))){
00083 ROS_INFO("Waiting for the find_contact action server to come up");
00084 }
00085
00086 while(!slip_client_->waitForServer(ros::Duration(5.0))){
00087 ROS_INFO("Waiting for the slip_servo action server to come up");
00088 }
00089
00090
00091 while(!force_client_->waitForServer(ros::Duration(5.0))){
00092 ROS_INFO("Waiting for the force_servo action server to come up");
00093 }
00094
00095 }
00096
00097 ~Gripper(){
00098 delete gripper_client_;
00099 delete contact_client_;
00100 delete slip_client_;
00101 delete force_client_;
00102 }
00103
00104
00105 void open(double position_open){
00106
00107 pr2_controllers_msgs::Pr2GripperCommandGoal open;
00108 open.command.position = position_open;
00109 open.command.max_effort = -1.0;
00110
00111 ROS_INFO("Sending open goal");
00112 gripper_client_->sendGoal(open);
00113 gripper_client_->waitForResult(ros::Duration(4.0));
00114 if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00115 ROS_INFO("The gripper opened!");
00116 else
00117 ROS_INFO("The gripper failed to open.");
00118 }
00119
00120
00121
00122
00123 int hold( double holdForce){
00124 pr2_gripper_sensor_msgs::PR2GripperForceServoGoal squeeze;
00125 squeeze.command.fingertip_force = holdForce;
00126
00127 ROS_INFO("Sending hold goal");
00128 force_client_->sendGoal(squeeze);
00129 force_client_->waitForResult(ros::Duration(6.0));
00130 if(force_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00131 ROS_INFO("Stable force was achieved");
00132 else
00133 ROS_INFO("Stable force was NOT achieved");
00134 return force_client_->getResult()->data.rtstate.realtime_controller_state;
00135 }
00136
00137
00138 int findTwoContacts(double *contact_force){
00139 pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
00140 findTwo.command.contact_conditions = findTwo.command.BOTH;
00141 findTwo.command.zero_fingertip_sensors = true;
00142
00143 ROS_INFO("Sending find 2 contact goal");
00144 contact_client_->sendGoal(findTwo);
00145 contact_client_->waitForResult(ros::Duration(5.0));
00146 if(contact_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00147 {
00148 ROS_INFO("Contact found. Left: %d, Right: %d", contact_client_->getResult()->data.left_fingertip_pad_contact,
00149 contact_client_->getResult()->data.right_fingertip_pad_contact);
00150 ROS_INFO("Contact force. Left: %f, Right: %f", contact_client_->getResult()->data.left_fingertip_pad_force,
00151 contact_client_->getResult()->data.right_fingertip_pad_force);
00152 }
00153 else
00154 ROS_INFO("The gripper did not find a contact.");
00155 *contact_force = (contact_client_->getResult()->data.right_fingertip_pad_force + contact_client_->getResult()->data.left_fingertip_pad_force)/2.0;
00156
00157 return contact_client_->getResult()->data.rtstate.realtime_controller_state;
00158
00159 }
00160
00161
00162
00163 void slipServo(){
00164 pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal slip_goal;
00165
00166 ROS_INFO("Slip Servoing");
00167 slip_client_->sendGoal(slip_goal);
00168 }
00169
00170 };
00171
00172
00173
00174 class PR2GripperGrabAction
00175 {
00176 protected:
00177
00178 ros::NodeHandle nh_;
00179 actionlib::SimpleActionServer<pr2_gripper_sensor_msgs::PR2GripperGrabAction> as_;
00180 std::string action_name_;
00181
00182 pr2_gripper_sensor_msgs::PR2GripperGrabFeedback feedback_;
00183 pr2_gripper_sensor_msgs::PR2GripperGrabResult result_;
00184 Gripper gripper;
00185
00186 public:
00187
00188 PR2GripperGrabAction(std::string name) :
00189 as_(nh_, name, boost::bind(&PR2GripperGrabAction::executeCB, this, _1)),
00190 action_name_(name)
00191 {
00192 }
00193
00194 ~PR2GripperGrabAction(void)
00195 {
00196 }
00197
00198 void executeCB(const pr2_gripper_sensor_msgs::PR2GripperGrabGoalConstPtr &goal)
00199 {
00200 double close_speed;
00201 if(!nh_.getParam("close_speed", close_speed))
00202 ROS_ERROR("No close_speed given in namespace: '%s')", nh_.getNamespace().c_str());
00203
00204 double fingertip_force_limit;
00205 if(!nh_.getParam("fingertip_force_limit", fingertip_force_limit))
00206 ROS_ERROR("No fingertip_force_limit given in namespace: '%s')", nh_.getNamespace().c_str());
00207
00208 double position_open;
00209 if(!nh_.getParam("position_open", position_open))
00210 ROS_ERROR("No position_open given in namespace: '%s')", nh_.getNamespace().c_str());
00211
00212
00213
00214 bool prempted = false;
00215 double contact_force, servo_force;
00216
00217
00218 if (as_.isPreemptRequested() || !ros::ok())
00219 {
00220 ROS_INFO("%s: Preempted", action_name_.c_str());
00221
00222 as_.setPreempted();
00223 prempted = true;
00224 }
00225 if(!prempted)
00226 gripper.open(position_open);
00227
00228
00229 if (as_.isPreemptRequested() || !ros::ok())
00230 {
00231 ROS_INFO("%s: Preempted", action_name_.c_str());
00232
00233 as_.setPreempted();
00234 prempted = true;
00235 }
00236 if(!prempted)
00237 {
00238 feedback_.data.rtstate.realtime_controller_state = gripper.findTwoContacts(&contact_force);
00239 as_.publishFeedback(feedback_);
00240 }
00241
00242 ROS_INFO("Find contact found force: %f",contact_force);
00243
00244 if (as_.isPreemptRequested() || !ros::ok())
00245 {
00246 ROS_INFO("%s: Preempted", action_name_.c_str());
00247
00248 as_.setPreempted();
00249 prempted = true;
00250 }
00251 if(!prempted)
00252 {
00253 servo_force = (contact_force/close_speed)*goal->command.hardness_gain;
00254 if(servo_force > fingertip_force_limit && fingertip_force_limit > 0)
00255 servo_force = fingertip_force_limit;
00256 feedback_.data.rtstate.realtime_controller_state = gripper.hold(servo_force);
00257 as_.publishFeedback(feedback_);
00258 }
00259
00260
00261 if (as_.isPreemptRequested() || !ros::ok())
00262 {
00263 ROS_INFO("%s: Preempted", action_name_.c_str());
00264
00265 as_.setPreempted();
00266 prempted = true;
00267 }
00268 if(!prempted)
00269 gripper.slipServo();
00270
00271 result_.data.rtstate.realtime_controller_state = feedback_.data.rtstate.realtime_controller_state;
00272 ROS_INFO("%s: Succeeded", action_name_.c_str());
00273
00274 if(!prempted)
00275 as_.setSucceeded(result_);
00276 }
00277 };
00278
00279
00280 int main(int argc, char** argv)
00281 {
00282 ros::init(argc, argv, "grab");
00283
00284 PR2GripperGrabAction Grab("grab");
00285 ros::spin();
00286
00287 return 0;
00288 }
00289