forceServoActionTest.cpp
Go to the documentation of this file.
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: findContactActionTest.cpp - example for using the findContact 
00035 //         action server
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/PR2GripperForceServoAction.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 
00043 // Our Action interface type, provided as a typedef for convenience
00044 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> ContactClient;
00045 // Our Action interface type, provided as a typedef for convenience                   
00046 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00047 // Our Action interface type, provided as a typedef for convenience
00048 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperForceServoAction> ForceClient;
00049 
00050 
00051 class Gripper{
00052 private:
00053   GripperClient* gripper_client_;  
00054   ContactClient* contact_client_;
00055   ForceClient* force_client_;
00056 
00057 public:
00058   //Action client initialization
00059   Gripper(){
00060 
00061     //Initialize the client for the Action interface to the gripper controller
00062     //and tell the action client that we want to spin a thread by default
00063     gripper_client_ = new GripperClient("r_gripper_sensor_controller/gripper_action", true);
00064     contact_client_  = new ContactClient("r_gripper_sensor_controller/find_contact",true);
00065     force_client_  = new ForceClient("r_gripper_sensor_controller/force_servo",true);
00066 
00067     //wait for the gripper action server to come up 
00068     while(!gripper_client_->waitForServer(ros::Duration(5.0))){
00069       ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
00070     }
00071 
00072     while(!contact_client_->waitForServer(ros::Duration(5.0))){
00073       ROS_INFO("Waiting for the r_gripper_sensor_controller/find_contact action server to come up");
00074     }
00075 
00076     while(!force_client_->waitForServer(ros::Duration(5.0))){
00077       ROS_INFO("Waiting for the r_gripper_sensor_controller/force_servo action server to come up");
00078     }
00079   }
00080 
00081   ~Gripper(){
00082     delete gripper_client_;
00083     delete contact_client_;
00084     delete force_client_;
00085   }
00086 
00087   //Open the gripper
00088   void open(){
00089     pr2_controllers_msgs::Pr2GripperCommandGoal open;
00090     open.command.position = 0.08;
00091     open.command.max_effort = -1.0;
00092     
00093     ROS_INFO("Sending open goal");
00094     gripper_client_->sendGoal(open);
00095     gripper_client_->waitForResult();
00096     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00097       ROS_INFO("The gripper opened!");
00098     else
00099       ROS_INFO("The gripper failed to open.");
00100   }
00101 
00102   //Close the gripper
00103   void close(){
00104     pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
00105     squeeze.command.position = 0.0;
00106     squeeze.command.max_effort = -1.0;
00107 
00108     ROS_INFO("Sending squeeze goal");
00109     gripper_client_->sendGoal(squeeze);
00110     gripper_client_->waitForResult();
00111     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00112       ROS_INFO("The gripper closed!");
00113     else
00114       ROS_INFO("The gripper failed to close.");
00115   }
00116 
00117 
00118 
00119   //Hold somethign with a constant force in the gripper
00120   void hold( double holdForce){
00121     pr2_gripper_sensor_msgs::PR2GripperForceServoGoal squeeze;
00122     squeeze.command.fingertip_force = holdForce;   // hold with X N of force
00123     
00124     ROS_INFO("Sending hold goal");
00125     force_client_->sendGoal(squeeze);
00126     force_client_->waitForResult();
00127     if(force_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00128       ROS_INFO("Stable force was achieved");
00129     else
00130       ROS_INFO("Stable force was NOT achieved");
00131   }
00132 
00133 
00134   //Find two contacts and go into force control mode
00135   void findTwoContacts(){
00136     pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
00137     findTwo.command.contact_conditions = findTwo.command.BOTH;  // close until both fingers contact
00138     findTwo.command.zero_fingertip_sensors = true;   // zero fingertip sensor values before moving
00139  
00140     
00141     ROS_INFO("Sending find 2 contact goal");
00142     contact_client_->sendGoal(findTwo);
00143     contact_client_->waitForResult(ros::Duration(5.0));
00144     if(contact_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00145     {
00146       ROS_INFO("Contact found. Left: %d, Right: %d", contact_client_->getResult()->data.left_fingertip_pad_contact, 
00147            contact_client_->getResult()->data.right_fingertip_pad_contact);
00148       ROS_INFO("Contact force. Left: %f, Right: %f", contact_client_->getResult()->data.left_fingertip_pad_force, 
00149            contact_client_->getResult()->data.right_fingertip_pad_force);
00150     }
00151     else
00152       ROS_INFO("The gripper did not find a contact or could not maintain contact force.");
00153   }
00154 };
00155 
00156 int main(int argc, char** argv){
00157   ros::init(argc, argv, "simple_gripper");
00158 
00159   Gripper gripper;
00160 
00161   gripper.open();
00162   sleep(2.0);
00163 
00164   gripper.findTwoContacts();
00165   gripper.hold(4.0);   // hold with 10 N of force
00166 
00167 //gripper.close();
00168 
00169   return 0;
00170 }


pr2_gripper_sensor_action
Author(s): Joe Romano
autogenerated on Thu Aug 27 2015 14:25:38