slipServoActionTest.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: slipServoActionTest.cpp - example for using the slipServo
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/PR2GripperSlipServoAction.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 
00043 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperSlipServoAction> SlipClient;
00044 // Our Action interface type, provided as a typedef for convenience
00045 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> ContactClient;
00046 // Our Action interface type, provided as a typedef for convenience                   
00047 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00048 // Our Action interface type, provided as a typedef for convenience                   
00049 
00050 
00051 class Gripper{
00052 private:
00053   GripperClient* gripper_client_;  
00054   ContactClient* contact_client_;
00055   SlipClient* slip_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     slip_client_  = new SlipClient("r_gripper_sensor_controller/slip_servo",true);
00066 
00067 
00068     //wait for the gripper action server to come up 
00069     while(!gripper_client_->waitForServer(ros::Duration(5.0))){
00070       ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
00071     }
00072 
00073     while(!contact_client_->waitForServer(ros::Duration(5.0))){
00074       ROS_INFO("Waiting for the r_gripper_sensor_controller/find_contact action server to come up");
00075     }
00076     
00077     while(!slip_client_->waitForServer(ros::Duration(5.0))){
00078       ROS_INFO("Waiting for the r_gripper_sensor_controller/slip_servo action server to come up");
00079     }    
00080   }
00081 
00082   ~Gripper(){
00083     delete gripper_client_;
00084     delete contact_client_;
00085     delete slip_client_;
00086 
00087   }
00088 
00089   //Open the gripper
00090   void open(){
00091     pr2_controllers_msgs::Pr2GripperCommandGoal open;
00092     open.command.position = 0.09;    // position open (9 cm)
00093     
00094     ROS_INFO("Sending open goal");
00095     gripper_client_->sendGoal(open);
00096     gripper_client_->waitForResult();
00097     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00098       ROS_INFO("The gripper opened!");
00099     else
00100       ROS_INFO("The gripper failed to open.");
00101   }
00102 
00103 
00104 
00105 
00106   //Find two contacts and go into force control mode
00107   void findTwoContacts(){
00108     pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
00109     findTwo.command.contact_conditions = findTwo.command.BOTH;  // close until both fingers contact
00110     findTwo.command.zero_fingertip_sensors = true;   // zero fingertip sensor values before moving
00111  
00112     ROS_INFO("Sending find 2 contact goal");
00113     contact_client_->sendGoal(findTwo);
00114     contact_client_->waitForResult(ros::Duration(5.0));
00115     if(contact_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00116     {
00117       ROS_INFO("Contact found. Left: %d, Right: %d", contact_client_->getResult()->data.left_fingertip_pad_contact, 
00118            contact_client_->getResult()->data.right_fingertip_pad_contact);
00119       ROS_INFO("Contact force. Left: %f, Right: %f", contact_client_->getResult()->data.left_fingertip_pad_force, 
00120            contact_client_->getResult()->data.right_fingertip_pad_force);
00121     }
00122     else
00123       ROS_INFO("The gripper did not find a contact.");
00124   }
00125   
00126   
00127   //Slip servo the robot
00128   void slipServo(){
00129     pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal slip_goal;
00130 
00131     ROS_INFO("Slip Servoing");
00132     slip_client_->sendGoal(slip_goal);
00133     //slip_client_->waitForResult();   //thre is no reason to wait since this action never returns a result
00134     if(slip_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00135     {}
00136     else
00137       ROS_INFO("The gripper did not find a contact or could not maintain contact force.");
00138   }  
00139 };
00140 
00141 int main(int argc, char** argv){
00142   ros::init(argc, argv, "simple_gripper");
00143 
00144   Gripper gripper;
00145 
00146 
00147   gripper.open();
00148   sleep(3.0);
00149   gripper.findTwoContacts();
00150   gripper.slipServo();
00151 
00152   return 0;
00153 }


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