placeActionTest.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: placeActionTest.cpp - example for using the place
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 <pr2_gripper_sensor_msgs/PR2GripperEventDetectorAction.h>
00042 #include <actionlib/client/simple_action_client.h>
00043 
00044 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperSlipServoAction> SlipClient;
00045 // Our Action interface type, provided as a typedef for convenience
00046 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> ContactClient;
00047 // Our Action interface type, provided as a typedef for convenience                   
00048 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperEventDetectorAction> EventDetectorClient;
00049 // Our Action interface type, provided as a typedef for convenience                   
00050 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00051 // Our Action interface type, provided as a typedef for convenience                   
00052 
00053 
00054 
00055 class Gripper{
00056 private:
00057   GripperClient* gripper_client_;  
00058   ContactClient* contact_client_;
00059   SlipClient* slip_client_;
00060   EventDetectorClient* event_detector_client_;
00061 
00062 public:
00063   //Action client initialization
00064   Gripper(){
00065 
00066     //Initialize the client for the Action interface to the gripper controller
00067     //and tell the action client that we want to spin a thread by default
00068     gripper_client_ = new GripperClient("r_gripper_sensor_controller/gripper_action", true);
00069     contact_client_  = new ContactClient("r_gripper_sensor_controller/find_contact",true);
00070     slip_client_  = new SlipClient("r_gripper_sensor_controller/slip_servo",true);
00071     event_detector_client_  = new EventDetectorClient("r_gripper_sensor_controller/event_detector",true);
00072 
00073 
00074     //wait for the gripper action server to come up 
00075     while(!gripper_client_->waitForServer(ros::Duration(5.0))){
00076       ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
00077     }
00078 
00079     while(!contact_client_->waitForServer(ros::Duration(5.0))){
00080       ROS_INFO("Waiting for the r_gripper_sensor_controller/find_contact action server to come up");
00081     }
00082     
00083     while(!slip_client_->waitForServer(ros::Duration(5.0))){
00084       ROS_INFO("Waiting for the r_gripper_sensor_controller/slip_servo action server to come up");
00085     }    
00086 
00087     while(!event_detector_client_->waitForServer(ros::Duration(5.0))){
00088       ROS_INFO("Waiting for the r_gripper_sensor_controller/event_detector action server to come up");
00089     }    
00090   }
00091 
00092   ~Gripper(){
00093     delete gripper_client_;
00094     delete contact_client_;
00095     delete slip_client_;
00096     delete event_detector_client_;
00097 
00098   }
00099 
00100   //Open the gripper
00101   void open(){
00102     pr2_controllers_msgs::Pr2GripperCommandGoal open;
00103     open.command.position = 0.09;    // position open (9 cm)
00104     open.command.max_effort = -1.0;  // unlimited motor effort
00105 
00106     ROS_INFO("Sending open goal");
00107     gripper_client_->sendGoal(open);
00108     gripper_client_->waitForResult();
00109     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00110       ROS_INFO("The gripper opened!");
00111     else
00112       ROS_INFO("The gripper failed to open.");
00113   }
00114 
00115 
00116   //Find two contacts and go into force control mode
00117   void findTwoContacts(){
00118     pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
00119     findTwo.command.contact_conditions = findTwo.command.BOTH;  // close until both fingers contact
00120     findTwo.command.zero_fingertip_sensors = true;   // zero fingertip sensor values before moving
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.");
00134   }
00135   
00136   
00137   //Slip servo the robot
00138   void slipServo(){
00139     pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal slip_goal;
00140 
00141     ROS_INFO("Slip Servoing");
00142     slip_client_->sendGoal(slip_goal);
00143     //slip_client_->waitForResult();  // thre is no reason to wait since this action never returns a result
00144     if(slip_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00145       ROS_INFO("You Should Never See This Message!");
00146     else
00147       ROS_INFO("SlipServo Action returned without success.");
00148   }  
00149 
00150 
00151   //move into event_detector mode to drop an object
00152   void place(){
00153     pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal place_goal;
00154     place_goal.command.trigger_conditions = place_goal.command.FINGER_SIDE_IMPACT_OR_SLIP_OR_ACC;  
00155     place_goal.command.acceleration_trigger_magnitude = 4.0;  // set the contact acceleration to n m/s^2
00156     place_goal.command.slip_trigger_magnitude = .005;
00157 
00158     ROS_INFO("Waiting for object placement contact...");
00159     event_detector_client_->sendGoal(place_goal);
00160     event_detector_client_->waitForResult();
00161     if(event_detector_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00162     {
00163       ROS_INFO("Place Success");
00164       ROS_INFO("cond met: %d, acc_met: %d, slip_met: %d", event_detector_client_->getResult()->data.trigger_conditions_met, event_detector_client_->getResult()->data.acceleration_event, event_detector_client_->getResult()->data.slip_event);
00165     }
00166     else
00167       ROS_INFO("Place Failure");
00168   }  
00169 };
00170 
00171 int main(int argc, char** argv){
00172   ros::init(argc, argv, "simple_gripper");
00173 
00174   Gripper gripper;
00175 
00176   // do this until we get ctrl-c
00177     // open the hand to prepare for a grasp
00178     gripper.open();
00179   
00180     // wait (you don't have to do this in your code)
00181     // in this demo here is a good time to put an object inside the gripper
00182     // in your code the robot would move its arm around an object
00183     sleep(7.0);
00184 
00185     // close the gripper until we have a contact on each finger and move into force control mode
00186     gripper.findTwoContacts();
00187 
00188     // now hold the object but don't drop it
00189     gripper.slipServo();
00190 
00191     // now we wait some amount of time until we want to put it down
00192     // in your code you probably wouldn't do this but you would move the arm around instead
00193     sleep(8.0);
00194 
00195     // now we decided we are ready to put the  object down, so tell the gripper that
00196     gripper.place();
00197     
00198     // open the gripper once placement has been detected
00199     gripper.open();
00200 
00201   return 0;
00202 }


pr2_gripper_sensor_action
Author(s): Joe Romano
autogenerated on Fri Jan 3 2014 11:54:43