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


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