38 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h> 39 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactAction.h> 40 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoAction.h> 41 #include <pr2_gripper_sensor_msgs/PR2GripperGrabAction.h> 42 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoAction.h> 43 #include <std_srvs/Empty.h> 73 slip_client_ =
new SlipClient(
"slip_servo",
true);
74 force_client_ =
new ForceClient(
"force_servo",
true);
79 ROS_INFO(
"Waiting for the gripper_action action server to come up");
83 ROS_INFO(
"Waiting for the find_contact action server to come up");
87 ROS_INFO(
"Waiting for the slip_servo action server to come up");
92 ROS_INFO(
"Waiting for the force_servo action server to come up");
105 void open(
double position_open){
107 pr2_controllers_msgs::Pr2GripperCommandGoal
open;
108 open.command.position = position_open;
109 open.command.max_effort = -1.0;
117 ROS_INFO(
"The gripper failed to open.");
124 pr2_gripper_sensor_msgs::PR2GripperForceServoGoal squeeze;
125 squeeze.command.fingertip_force = holdForce;
131 ROS_INFO(
"Stable force was achieved");
133 ROS_INFO(
"Stable force was NOT achieved");
134 return force_client_->
getResult()->data.rtstate.realtime_controller_state;
139 pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
140 findTwo.command.contact_conditions = findTwo.command.BOTH;
141 findTwo.command.zero_fingertip_sensors =
true;
143 ROS_INFO(
"Sending find 2 contact goal");
148 ROS_INFO(
"Contact found. Left: %d, Right: %d", contact_client_->
getResult()->data.left_fingertip_pad_contact,
149 contact_client_->
getResult()->data.right_fingertip_pad_contact);
150 ROS_INFO(
"Contact force. Left: %f, Right: %f", contact_client_->
getResult()->data.left_fingertip_pad_force,
151 contact_client_->
getResult()->data.right_fingertip_pad_force);
154 ROS_INFO(
"The gripper did not find a contact.");
155 *contact_force = (contact_client_->
getResult()->data.right_fingertip_pad_force + contact_client_->
getResult()->data.left_fingertip_pad_force)/2.0;
157 return contact_client_->
getResult()->data.rtstate.realtime_controller_state;
164 pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal slip_goal;
182 pr2_gripper_sensor_msgs::PR2GripperGrabFeedback
feedback_;
183 pr2_gripper_sensor_msgs::PR2GripperGrabResult
result_;
198 void executeCB(
const pr2_gripper_sensor_msgs::PR2GripperGrabGoalConstPtr &goal)
201 if(!nh_.
getParam(
"close_speed", close_speed))
204 double fingertip_force_limit;
205 if(!nh_.
getParam(
"fingertip_force_limit", fingertip_force_limit))
208 double position_open;
209 if(!nh_.
getParam(
"position_open", position_open))
214 bool prempted =
false;
215 double contact_force, servo_force;
220 ROS_INFO(
"%s: Preempted", action_name_.c_str());
226 gripper.
open(position_open);
231 ROS_INFO(
"%s: Preempted", action_name_.c_str());
238 feedback_.data.rtstate.realtime_controller_state = gripper.
findTwoContacts(&contact_force);
242 ROS_INFO(
"Find contact found force: %f",contact_force);
246 ROS_INFO(
"%s: Preempted", action_name_.c_str());
253 servo_force = (contact_force/close_speed)*goal->command.hardness_gain;
254 if(servo_force > fingertip_force_limit && fingertip_force_limit > 0)
255 servo_force = fingertip_force_limit;
256 feedback_.data.rtstate.realtime_controller_state = gripper.
hold(servo_force);
263 ROS_INFO(
"%s: Preempted", action_name_.c_str());
271 result_.data.rtstate.realtime_controller_state = feedback_.data.rtstate.realtime_controller_state;
272 ROS_INFO(
"%s: Succeeded", action_name_.c_str());
280 int main(
int argc,
char** argv)
void publishFeedback(const FeedbackConstPtr &feedback)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
void open(double position_open)
actionlib::SimpleActionClient< pr2_gripper_sensor_msgs::PR2GripperSlipServoAction > SlipClient
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > GripperClient
ROSCPP_DECL void spin(Spinner &spinner)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
int main(int argc, char **argv)
const std::string & getNamespace() const
ContactClient * contact_client_
bool isPreemptRequested()
int findTwoContacts(double *contact_force)
PR2GripperGrabAction(std::string name)
ForceClient * force_client_
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
pr2_gripper_sensor_msgs::PR2GripperGrabResult result_
int hold(double holdForce)
bool getParam(const std::string &key, std::string &s) const
GripperClient * gripper_client_
SlipClient * slip_client_
void hold(double holdForce)
actionlib::SimpleActionServer< pr2_gripper_sensor_msgs::PR2GripperGrabAction > as_
SimpleClientGoalState getState() const
ResultConstPtr getResult() const
pr2_gripper_sensor_msgs::PR2GripperGrabFeedback feedback_
void executeCB(const pr2_gripper_sensor_msgs::PR2GripperGrabGoalConstPtr &goal)
actionlib::SimpleActionClient< pr2_gripper_sensor_msgs::PR2GripperForceServoAction > ForceClient
~PR2GripperGrabAction(void)
actionlib::SimpleActionClient< pr2_gripper_sensor_msgs::PR2GripperFindContactAction > ContactClient