slipServoActionTest.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * \author Joe Romano
30  */
31 
32 // @author: Joe Romano
33 // @email: joeromano@gmail.com
34 // @brief: slipServoActionTest.cpp - example for using the slipServo
35 // action server
36 
37 #include <ros/ros.h>
38 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
39 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactAction.h>
40 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoAction.h>
42 
44 // Our Action interface type, provided as a typedef for convenience
46 // Our Action interface type, provided as a typedef for convenience
48 // Our Action interface type, provided as a typedef for convenience
49 
50 
51 class Gripper{
52 private:
56 
57 public:
58  //Action client initialization
60 
61  //Initialize the client for the Action interface to the gripper controller
62  //and tell the action client that we want to spin a thread by default
63  gripper_client_ = new GripperClient("r_gripper_sensor_controller/gripper_action", true);
64  contact_client_ = new ContactClient("r_gripper_sensor_controller/find_contact",true);
65  slip_client_ = new SlipClient("r_gripper_sensor_controller/slip_servo",true);
66 
67 
68  //wait for the gripper action server to come up
69  while(!gripper_client_->waitForServer(ros::Duration(5.0))){
70  ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
71  }
72 
73  while(!contact_client_->waitForServer(ros::Duration(5.0))){
74  ROS_INFO("Waiting for the r_gripper_sensor_controller/find_contact action server to come up");
75  }
76 
77  while(!slip_client_->waitForServer(ros::Duration(5.0))){
78  ROS_INFO("Waiting for the r_gripper_sensor_controller/slip_servo action server to come up");
79  }
80  }
81 
83  delete gripper_client_;
84  delete contact_client_;
85  delete slip_client_;
86 
87  }
88 
89  //Open the gripper
90  void open(){
91  pr2_controllers_msgs::Pr2GripperCommandGoal open;
92  open.command.position = 0.09; // position open (9 cm)
93 
94  ROS_INFO("Sending open goal");
95  gripper_client_->sendGoal(open);
96  gripper_client_->waitForResult();
98  ROS_INFO("The gripper opened!");
99  else
100  ROS_INFO("The gripper failed to open.");
101  }
102 
103 
104 
105 
106  //Find two contacts and go into force control mode
108  pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
109  findTwo.command.contact_conditions = findTwo.command.BOTH; // close until both fingers contact
110  findTwo.command.zero_fingertip_sensors = true; // zero fingertip sensor values before moving
111 
112  ROS_INFO("Sending find 2 contact goal");
113  contact_client_->sendGoal(findTwo);
114  contact_client_->waitForResult(ros::Duration(5.0));
115  if(contact_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
116  {
117  ROS_INFO("Contact found. Left: %d, Right: %d", contact_client_->getResult()->data.left_fingertip_pad_contact,
118  contact_client_->getResult()->data.right_fingertip_pad_contact);
119  ROS_INFO("Contact force. Left: %f, Right: %f", contact_client_->getResult()->data.left_fingertip_pad_force,
120  contact_client_->getResult()->data.right_fingertip_pad_force);
121  }
122  else
123  ROS_INFO("The gripper did not find a contact.");
124  }
125 
126 
127  //Slip servo the robot
128  void slipServo(){
129  pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal slip_goal;
130 
131  ROS_INFO("Slip Servoing");
132  slip_client_->sendGoal(slip_goal);
133  //slip_client_->waitForResult(); //thre is no reason to wait since this action never returns a result
135  {}
136  else
137  ROS_INFO("The gripper did not find a contact or could not maintain contact force.");
138  }
139 };
140 
141 int main(int argc, char** argv){
142  ros::init(argc, argv, "simple_gripper");
143 
144  Gripper gripper;
145 
146 
147  gripper.open();
148  sleep(3.0);
149  gripper.findTwoContacts();
150  gripper.slipServo();
151 
152  return 0;
153 }
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
void findTwoContacts()
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
actionlib::SimpleActionClient< pr2_gripper_sensor_msgs::PR2GripperFindContactAction > ContactClient
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
actionlib::SimpleActionClient< pr2_gripper_sensor_msgs::PR2GripperSlipServoAction > SlipClient
#define ROS_INFO(...)
ContactClient * contact_client_
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > GripperClient
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
GripperClient * gripper_client_
SlipClient * slip_client_
SimpleClientGoalState getState() const
ResultConstPtr getResult() const


pr2_gripper_sensor_action
Author(s): Joe Romano
autogenerated on Wed Apr 1 2020 03:58:26