$search
00001 /* 00002 * Copyright (c) 2009, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 * 00029 * \author Joe Romano 00030 */ 00031 00032 // @author: Joe Romano 00033 // @email: joeromano@gmail.com 00034 // @brief: pr2_gripper_grab_action.cpp - action server for the 00035 // pr2_gripper_grab action command 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 // Our Action interface type, provided as a typedef for convenience 00050 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> ContactClient; 00051 // Our Action interface type, provided as a typedef for convenience 00052 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient; 00053 // Our Action interface type, provided as a typedef for convenience 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 //Action client initialization 00067 Gripper(){ 00068 00069 //Initialize the client for the Action interface to the gripper controller 00070 //and tell the action client that we want to spin a thread by default 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 //wait for the gripper action server to come up 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 //Open the gripper 00105 void open(double position_open){ 00106 00107 pr2_controllers_msgs::Pr2GripperCommandGoal open; 00108 open.command.position = position_open; // position open (9 cm) 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 //Hold somethign with a constant force in the gripper 00123 int hold( double holdForce){ 00124 pr2_gripper_sensor_msgs::PR2GripperForceServoGoal squeeze; 00125 squeeze.command.fingertip_force = holdForce; // hold with X N of force 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 //Find two contacts and go into force control mode 00138 int findTwoContacts(double *contact_force){ 00139 pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo; 00140 findTwo.command.contact_conditions = findTwo.command.BOTH; // close until both fingers contact 00141 findTwo.command.zero_fingertip_sensors = true; // zero fingertip sensor values before moving 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 //Slip servo the robot 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 // create messages that are used to published feedback/result 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 // helper variables 00214 bool prempted = false; 00215 double contact_force, servo_force; 00216 00217 // check that preempt has not been requested by the client 00218 if (as_.isPreemptRequested() || !ros::ok()) 00219 { 00220 ROS_INFO("%s: Preempted", action_name_.c_str()); 00221 // set the action state to preempted 00222 as_.setPreempted(); 00223 prempted = true; 00224 } 00225 if(!prempted) 00226 gripper.open(position_open); 00227 00228 // check that preempt has not been requested by the client 00229 if (as_.isPreemptRequested() || !ros::ok()) 00230 { 00231 ROS_INFO("%s: Preempted", action_name_.c_str()); 00232 // set the action state to preempted 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 // check that preempt has not been requested by the client 00244 if (as_.isPreemptRequested() || !ros::ok()) 00245 { 00246 ROS_INFO("%s: Preempted", action_name_.c_str()); 00247 // set the action state to preempted 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 // check that preempt has not been requested by the client 00261 if (as_.isPreemptRequested() || !ros::ok()) 00262 { 00263 ROS_INFO("%s: Preempted", action_name_.c_str()); 00264 // set the action state to preempted 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 // set the action state to succeeded 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