pr2_gripper_grasp_controller: pr2_gripper_grasp_controller: pr2_gripper_grasp

00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 #include <ros/ros.h>
00035 
00036 #include <actionlib/server/simple_action_server.h>
00037 #include <actionlib/client/simple_action_client.h>
00038 
00039 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00040 
00041 #include <sensor_msgs/JointState.h>
00042 
00043 #include <object_manipulation_msgs/GraspHandPostureExecutionAction.h>
00044 #include <object_manipulation_msgs/GraspStatus.h>
00045 
00046 static const std::string JOINT_STATES_TOPIC = "joint_states";
00047 
00048 class PR2GripperGraspController
00049 {
00050 private:
00051 
00053   ros::NodeHandle root_nh_;
00054 
00056   ros::NodeHandle priv_nh_;
00057 
00059   actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> *gripper_action_client_;
00060 
00062   actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction> *action_server_;
00063 
00065   ros::ServiceServer query_srv_;
00066 
00068   std::string gripper_virtual_joint_name_;
00069 
00070   //-------------------------------- Constants ---------------------------------
00071 
00073   static const double GRIPPER_OPEN;
00075   static const double GRIPPER_CLOSED;
00076 
00078   double gripper_object_presence_threshold_;  
00080   static const double DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD;
00081 
00082   //------------------------------ Functionality -------------------------------
00083 
00085   bool getGripperValue(double &value)
00086   {
00087     //get the joint states
00088     sensor_msgs::JointState::ConstPtr joint_states = 
00089       ros::topic::waitForMessage<sensor_msgs::JointState>(JOINT_STATES_TOPIC, root_nh_);
00090     if (!joint_states)
00091     {
00092       ROS_ERROR("pr2 gripper grasp status: joint states not received");
00093       return false;
00094     }
00095     
00096     //find the gripper joint
00097     size_t i;
00098     for (i=0; i<joint_states->name.size(); i++)
00099     {
00100       if (joint_states->name[i] == gripper_virtual_joint_name_) break;
00101     }
00102     if (i==joint_states->name.size())
00103     {
00104       ROS_ERROR("pr2_gripper grasp status: gripper joint %s not found in joint state", 
00105                 gripper_virtual_joint_name_.c_str());
00106       return false;
00107     }
00108     if (joint_states->position.size() <= i)
00109     {
00110       ROS_ERROR("pr2_gripper grasp status: malformed joint state message received");
00111       return false;
00112     }
00113     value = joint_states->position[i];
00114     return true;
00115   }
00116 
00117   void executeCB(const object_manipulation_msgs::GraspHandPostureExecutionGoalConstPtr &goal)
00118   {
00119     //prepare the gripper command based on the goal
00120     pr2_controllers_msgs::Pr2GripperCommandGoal gripper_command;
00121     switch (goal->goal)
00122     {
00123     case object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP:
00124       if (goal->grasp.grasp_posture.position.empty())
00125       {
00126         ROS_ERROR("pr2 gripper grasp execution: position vector empty in requested grasp");
00127         action_server_->setAborted();
00128         return;
00129       }
00130       gripper_command.command.position = GRIPPER_CLOSED;
00131       //gripper_command.command.position = goal->grasp.grasp_posture.position[0] / 0.5 * 0.0857;
00132       if (goal->grasp.grasp_posture.effort.empty())
00133       {
00134         if(goal->max_contact_force > 0)
00135         {
00136           ROS_INFO("limiting max contact force to %f", goal->max_contact_force);
00137           gripper_command.command.max_effort = goal->max_contact_force;
00138         }
00139         else gripper_command.command.max_effort = 100;
00140         ROS_WARN("pr2 gripper grasp execution: effort vector empty in requested grasp, using max force");
00141         //action_server_->setAborted();
00142         //return;
00143       }
00144       //we use the effort in the first joint for going to the grasp
00145       else 
00146       {
00147         if(goal->grasp.grasp_posture.effort.at(0) < goal->max_contact_force || goal->max_contact_force == 0)
00148           gripper_command.command.max_effort = goal->grasp.grasp_posture.effort.at(0);
00149         else gripper_command.command.max_effort = goal->max_contact_force;
00150       }      
00151       break;
00152     case object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP:
00153       if (goal->grasp.pre_grasp_posture.position.empty())
00154       {
00155         ROS_ERROR("pr2 gripper grasp execution: position vector empty in requested pre_grasp");
00156         action_server_->setAborted();
00157         return;
00158       }
00159       //gripper_command.command.position = GRIPPER_OPEN;
00160       gripper_command.command.position = goal->grasp.pre_grasp_posture.position.at(0) / 0.5 * 0.0857;
00161       ROS_DEBUG("pre grasp posture opening is %.3f", gripper_command.command.position);
00162       if (goal->grasp.pre_grasp_posture.effort.empty())
00163       {
00164         gripper_command.command.max_effort = 100;
00165         ROS_WARN("pr2 gripper grasp execution: effort vector empty in requested pre_grasp, using max force");
00166         //action_server_->setAborted();
00167         //return;
00168       }
00169       else 
00170         gripper_command.command.max_effort = goal->grasp.pre_grasp_posture.effort.at(0);
00171       //gripper_command.command.max_effort = 10000;
00172         
00173       break;      
00174     case object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE:
00175       gripper_command.command.position = GRIPPER_OPEN;
00176       gripper_command.command.max_effort = 100;
00177       break;
00178     default:
00179       ROS_ERROR("pr2 gripper grasp execution: unknown goal code (%d)", goal->goal);
00180       action_server_->setAborted();
00181       return;
00182     }
00183 
00184     //send the command to the gripper
00185     gripper_action_client_->sendGoal(gripper_command);
00186     gripper_action_client_->waitForResult();
00187 
00188     //process the result
00189     if(gripper_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 
00190     {
00191       ROS_INFO("Gripper goal achieved");
00192       action_server_->setSucceeded();
00193     } 
00194     else 
00195     {
00196       if (goal->goal == object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP)
00197       {
00198         //this is probably correct behavior, since there is an object in the gripper
00199         //we can not expect to reach our goal
00200         action_server_->setSucceeded();
00201       }
00202       else
00203       {
00204         ROS_WARN("Gripper goal not achieved for pre-grasp or release");
00205         //this might become a failure later, for now we're still investigating
00206         action_server_->setSucceeded();
00207       }
00208     }
00209   }
00210 
00211   bool serviceCallback(object_manipulation_msgs::GraspStatus::Request &request,
00212                        object_manipulation_msgs::GraspStatus::Response &response)
00213   {
00214     double gripper_value;
00215     if (!getGripperValue(gripper_value)) return false;    
00216 
00217     double min_gripper_opening = gripper_object_presence_threshold_;
00218     /*
00219     if (request.grasp.grasp_posture.position.empty())
00220     {
00221       ROS_ERROR("pr2 gripper grasp execution: position vector empty in grasp being checked");
00222       return false;
00223     }
00224     double min_gripper_opening = request.grasp.grasp_posture.position[0] / 0.5 * 0.0857 - .01;
00225     */
00226     if (gripper_value < min_gripper_opening) 
00227     {
00228       ROS_INFO("Gripper grasp query false: gripper value %f below threshold %f", 
00229                gripper_value, min_gripper_opening);
00230       response.is_hand_occupied = false;
00231     }
00232     else 
00233     {
00234       ROS_DEBUG("Gripper grasp query true: gripper value %f above threshold %f", 
00235                gripper_value, min_gripper_opening);
00236       response.is_hand_occupied = true;
00237     }
00238     return true;
00239   }
00240                        
00241 
00242 public:
00243   PR2GripperGraspController() :
00244     root_nh_(""),
00245     priv_nh_("~")
00246   {
00247     std::string gripper_action_name = root_nh_.resolveName("gripper_action_name");
00248     gripper_action_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>
00249       (gripper_action_name, true);
00250     while(!gripper_action_client_->waitForServer(ros::Duration(2.0)) && root_nh_.ok())
00251     {
00252       ROS_INFO_STREAM("Waiting for gripper action client on topic " << gripper_action_name);
00253     }
00254     if (!root_nh_.ok()) exit(0);
00255     ROS_INFO_STREAM("Using gripper action client on topic " << gripper_action_name);
00256 
00257     priv_nh_.param<std::string>("gripper_virtual_joint_name", gripper_virtual_joint_name_, "undefined");
00258     if ( gripper_virtual_joint_name_ == "undefined" )
00259     {
00260       ROS_ERROR("Gripper virtual joint name not defined; set the parameter gripper_virtual_joint_name in the launch file");
00261       exit(0);
00262     }
00263 
00264     priv_nh_.param<double>("gripper_object_presence_threshold", gripper_object_presence_threshold_,
00265                            DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD);
00266 
00267     std::string query_service_name = root_nh_.resolveName("grasp_query_name");   
00268     query_srv_ = root_nh_.advertiseService(query_service_name, &PR2GripperGraspController::serviceCallback, this);    
00269     ROS_INFO_STREAM("pr2_gripper grasp query service started on topic " << query_service_name);
00270 
00271     std::string posture_action_name = root_nh_.resolveName("posture_action_name");
00272     action_server_ = new actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction>
00273       (root_nh_, posture_action_name, boost::bind(&PR2GripperGraspController::executeCB, this, _1), false);
00274     action_server_->start();
00275     ROS_INFO_STREAM("pr2_gripper grasp hand posture action server started on topic " << posture_action_name);
00276   }
00277 
00278   ~PR2GripperGraspController()
00279   {
00280     delete action_server_;
00281     delete gripper_action_client_;
00282   }
00283 
00284 };
00285 
00286 const double PR2GripperGraspController::GRIPPER_OPEN = 0.086;
00287 const double PR2GripperGraspController::GRIPPER_CLOSED = 0.0;
00288 const double PR2GripperGraspController::DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD = 0.0021;
00289 
00290 int main(int argc, char **argv)
00291 {
00292   ros::init(argc, argv, "pr2_gripper_grasp_controller");
00293   PR2GripperGraspController node;
00294   ros::spin();
00295   return 0;  
00296 }
pr2_gripper_grasp_controller.cpp
