//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(); } }