$search
00001 /* 00002 * Copyright (c) 2009, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 * 00029 * \author Joe Romano 00030 */ 00031 00032 // @author: Joe Romano 00033 // @email: joeromano@gmail.com 00034 // @brief: pr2_gripper_release_action.cpp - action server for the 00035 // pr2_gripper_release action command 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 // Our Action interface type, provided as a typedef for convenience 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 //Action client initialization 00060 Gripper(){ 00061 00062 //Initialize the client for the Action interface to the gripper controller 00063 //and tell the action client that we want to spin a thread by default 00064 gripper_client_ = new GripperClient("gripper_action", true); 00065 event_client_ = new EventClient("event_detector",true); 00066 00067 00068 //wait for the gripper action server to come up 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 //Open the gripper 00084 void open(double position_open){ 00085 00086 pr2_controllers_msgs::Pr2GripperCommandGoal open; 00087 open.command.position = position_open; // position open (9 cm) 00088 open.command.max_effort = -1.0; // max force 00089 00090 ROS_INFO("Sending open goal"); 00091 gripper_client_->sendGoal(open); 00092 // do not wait, return right away so that the user knows contact was made 00093 } 00094 00095 //move into event_detector mode to drop an object 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; // set the contact acceleration to n m/s^2 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 // create messages that are used to published feedback/result 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 // helper variables 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 // check that preempt has not been requested by the client 00155 if (as_.isPreemptRequested() || !ros::ok()) 00156 { 00157 ROS_INFO("%s: Preempted", action_name_.c_str()); 00158 // set the action state to preempted 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 // set the action state to succeeded 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