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
00089 ROS_INFO("Sending open goal");
00090 gripper_client_->sendGoal(open);
00091
00092 }
00093
00094
00095 void place(int trigger_conditions, double acc_trigger, double slip_trigger){
00096 pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal place_goal;
00097 place_goal.command.trigger_conditions = trigger_conditions;
00098 place_goal.command.acceleration_trigger_magnitude = acc_trigger;
00099 place_goal.command.slip_trigger_magnitude = slip_trigger;
00100
00101 ROS_INFO("Waiting for object placement contact...");
00102 event_client_->sendGoal(place_goal);
00103 event_client_->waitForResult();
00104 if(event_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00105 {
00106 ROS_INFO("Place Success");
00107 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);
00108 }
00109 else
00110 ROS_INFO("Place Failure");
00111 }
00112
00113
00114 };
00115
00116
00117
00118 class PR2GripperReleaseAction
00119 {
00120 protected:
00121
00122 ros::NodeHandle nh_;
00123 actionlib::SimpleActionServer<pr2_gripper_sensor_msgs::PR2GripperReleaseAction> as_;
00124 std::string action_name_;
00125
00126 pr2_gripper_sensor_msgs::PR2GripperReleaseFeedback feedback_;
00127 pr2_gripper_sensor_msgs::PR2GripperReleaseResult result_;
00128 Gripper gripper;
00129
00130 public:
00131
00132 PR2GripperReleaseAction(std::string name) :
00133 as_(nh_, name, boost::bind(&PR2GripperReleaseAction::executeCB, this, _1)),
00134 action_name_(name)
00135 {
00136 }
00137
00138 ~PR2GripperReleaseAction(void)
00139 {
00140 }
00141
00142 void executeCB(const pr2_gripper_sensor_msgs::PR2GripperReleaseGoalConstPtr &goal)
00143 {
00144 double position_open;
00145 if(!nh_.getParam("position_open", position_open))
00146 ROS_ERROR("No position_open given in namespace: '%s')", nh_.getNamespace().c_str());
00147
00148
00149 bool prempted = false;
00150
00151 gripper.place(goal->command.event.trigger_conditions, goal->command.event.acceleration_trigger_magnitude, goal->command.event.slip_trigger_magnitude);
00152
00153
00154 if (as_.isPreemptRequested() || !ros::ok())
00155 {
00156 ROS_INFO("%s: Preempted", action_name_.c_str());
00157
00158 as_.setPreempted();
00159 prempted = true;
00160 }
00161 if(!prempted)
00162 gripper.open(position_open);
00163
00164
00165 result_.data.rtstate.realtime_controller_state = feedback_.data.rtstate.realtime_controller_state;
00166 ROS_INFO("%s: Succeeded", action_name_.c_str());
00167
00168 if(!prempted)
00169 as_.setSucceeded(result_);
00170 }
00171 };
00172
00173
00174 int main(int argc, char** argv)
00175 {
00176 ros::init(argc, argv, "release");
00177
00178 PR2GripperReleaseAction Release("release");
00179 ros::spin();
00180
00181 return 0;
00182 }
00183