Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ros/ros.h>
00038 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00039 #include <pr2_gripper_sensor_msgs/PR2GripperReleaseAction.h>
00040 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorAction.h>
00041 #include <std_srvs/Empty.h>
00042 #include <actionlib/server/simple_action_server.h>
00043 #include <actionlib/client/simple_action_client.h>
00044
00045
00046 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperEventDetectorAction> EventClient;
00047
00048 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00049
00050
00051
00052 class Gripper{
00053 private:
00054 GripperClient* gripper_client_;
00055 EventClient* event_client_;
00056
00057
00058 public:
00059
00060 Gripper(){
00061
00062
00063
00064 gripper_client_ = new GripperClient("gripper_action", true);
00065 event_client_ = new EventClient("event_detector",true);
00066
00067
00068
00069 while(!gripper_client_->waitForServer(ros::Duration(5.0))){
00070 ROS_INFO("Waiting for the gripper_action action server to come up");
00071 }
00072
00073 while(!event_client_->waitForServer(ros::Duration(5.0))){
00074 ROS_INFO("Waiting for the event_detector action server to come up");
00075 }
00076 }
00077
00078 ~Gripper(){
00079 delete gripper_client_;
00080 delete event_client_;
00081 }
00082
00083
00084 void open(double position_open){
00085
00086 pr2_controllers_msgs::Pr2GripperCommandGoal open;
00087 open.command.position = position_open;
00088 open.command.max_effort = -1.0;
00089
00090 ROS_INFO("Sending open goal");
00091 gripper_client_->sendGoal(open);
00092
00093 }
00094
00095
00096 void place(int trigger_conditions, double acc_trigger, double slip_trigger){
00097 pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal place_goal;
00098 place_goal.command.trigger_conditions = trigger_conditions;
00099 place_goal.command.acceleration_trigger_magnitude = acc_trigger;
00100 place_goal.command.slip_trigger_magnitude = slip_trigger;
00101
00102 ROS_INFO("Waiting for object placement contact...");
00103 event_client_->sendGoal(place_goal);
00104 event_client_->waitForResult();
00105 if(event_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00106 {
00107 ROS_INFO("Place Success");
00108 ROS_INFO("cond met: %d, acc_met: %d, slip_met: %d", event_client_->getResult()->data.trigger_conditions_met, event_client_->getResult()->data.acceleration_event, event_client_->getResult()->data.slip_event);
00109 }
00110 else
00111 ROS_INFO("Place Failure");
00112 }
00113
00114
00115 };
00116
00117
00118
00119 class PR2GripperReleaseAction
00120 {
00121 protected:
00122
00123 ros::NodeHandle nh_;
00124 actionlib::SimpleActionServer<pr2_gripper_sensor_msgs::PR2GripperReleaseAction> as_;
00125 std::string action_name_;
00126
00127 pr2_gripper_sensor_msgs::PR2GripperReleaseFeedback feedback_;
00128 pr2_gripper_sensor_msgs::PR2GripperReleaseResult result_;
00129 Gripper gripper;
00130
00131 public:
00132
00133 PR2GripperReleaseAction(std::string name) :
00134 as_(nh_, name, boost::bind(&PR2GripperReleaseAction::executeCB, this, _1)),
00135 action_name_(name)
00136 {
00137 }
00138
00139 ~PR2GripperReleaseAction(void)
00140 {
00141 }
00142
00143 void executeCB(const pr2_gripper_sensor_msgs::PR2GripperReleaseGoalConstPtr &goal)
00144 {
00145 double position_open;
00146 if(!nh_.getParam("position_open", position_open))
00147 ROS_ERROR("No position_open given in namespace: '%s')", nh_.getNamespace().c_str());
00148
00149
00150 bool prempted = false;
00151
00152 gripper.place(goal->command.event.trigger_conditions, goal->command.event.acceleration_trigger_magnitude, goal->command.event.slip_trigger_magnitude);
00153
00154
00155 if (as_.isPreemptRequested() || !ros::ok())
00156 {
00157 ROS_INFO("%s: Preempted", action_name_.c_str());
00158
00159 as_.setPreempted();
00160 prempted = true;
00161 }
00162 if(!prempted)
00163 gripper.open(position_open);
00164
00165
00166 result_.data.rtstate.realtime_controller_state = feedback_.data.rtstate.realtime_controller_state;
00167 ROS_INFO("%s: Succeeded", action_name_.c_str());
00168
00169 if(!prempted)
00170 as_.setSucceeded(result_);
00171 }
00172 };
00173
00174
00175 int main(int argc, char** argv)
00176 {
00177 ros::init(argc, argv, "release");
00178
00179 PR2GripperReleaseAction Release("release");
00180 ros::spin();
00181
00182 return 0;
00183 }
00184