//Action client initialization
Gripper(){
//Initialize the client for the Action interface to the gripper controller
//and tell the action client that we want to spin a thread by default
gripper_client_l_ = new GripperClient("l_gripper_sensor_controller/gripper_action",true);
place_client_l_ = new PlaceClient("l_gripper_sensor_controller/event_detector",true);
gripper_client_r_ = new GripperClient("r_gripper_sensor_controller/gripper_action",true);
place_client_r_ = new PlaceClient("r_gripper_sensor_controller/event_detector",true);
//wait for the gripper action server to come up
while(!gripper_client_r_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
}
while(!place_client_r_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the r_gripper_sensor_controller/event_detector action server to come up");
}
//wait for the gripper action server to come up
while(!gripper_client_l_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the l_gripper_sensor_controller/gripper_action action server to come up");
}
while(!place_client_l_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the l_gripper_sensor_controller/event_detector action server to come up");
}
}
//Open the gripper
void open(bool left, bool right){
pr2_controllers_msgs::Pr2GripperCommandGoal open;
open.command.position = 0.09; // position open (9 cm)
open.command.max_effort = -1.0; // Do not limit effort (negative)
if(left)
gripper_client_l_->sendGoal(open);
if(right)
gripper_client_r_->sendGoal(open);
}
//Close the gripper
void close(){
pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
squeeze.command.position = 0.0;
squeeze.command.max_effort = -1.0;
ROS_INFO("Sending squeeze goal");
gripper_client_->sendGoal(squeeze);
gripper_client_->waitForResult();
if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("The gripper closed!");
else
ROS_INFO("The gripper failed to close.");
}
//Open the gripper
void open(){
pr2_controllers_msgs::Pr2GripperCommandGoal open;
open.command.position = 0.08;
open.command.max_effort = -1.0;
ROS_INFO("Sending open goal");
gripper_client_->sendGoal(open);
gripper_client_->waitForResult();
if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("The gripper opened!");
else
ROS_INFO("The gripper failed to open.");
}
//Open the gripper
bool open(){
pr2_controllers_msgs::Pr2GripperCommandGoal open;
open.command.position = 0.08;
open.command.max_effort = -1.0; // Do not limit effort (negative)
ROS_INFO("Sending open goal");
gripper_client_->sendGoal(open);
gripper_client_->waitForResult();
if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
ROS_INFO("The gripper opened!");
return true;
}
else {
ROS_INFO("The gripper failed to open.");
return false;
}
}
//Close the gripper
void close(bool left, bool right,bool wait_for_result){
pr2_controllers_msgs::Pr2GripperCommandGoal close;
close.command.position = 0.002; // position open (9 cm)
close.command.max_effort = -1.0; // Do not limit effort (negative)
if(left)
{
gripper_client_l_->sendGoal(close);
if(wait_for_result)
gripper_client_l_->waitForResult();
}
if(right)
{
gripper_client_r_->sendGoal(close);
if(wait_for_result)
gripper_client_r_->waitForResult();
}
}
