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         gripper_command.command.max_effort = 10000;
00135         ROS_WARN("pr2 gripper grasp execution: effort vector empty in requested grasp, using max force");
00136         //action_server_->setAborted();
00137         //return;
00138       }
00139       //we use the effort in the first joint for going to the grasp
00140       else 
00141         gripper_command.command.max_effort = goal->grasp.grasp_posture.effort.at(0);
00142 
00143       break;
00144     case object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP:
00145       if (goal->grasp.pre_grasp_posture.position.empty())
00146       {
00147         ROS_ERROR("pr2 gripper grasp execution: position vector empty in requested pre_grasp");
00148         action_server_->setAborted();
00149         return;
00150       }
00151       //gripper_command.command.position = GRIPPER_OPEN;
00152       gripper_command.command.position = goal->grasp.pre_grasp_posture.position.at(0) / 0.5 * 0.0857;
00153       ROS_DEBUG("pre grasp posture opening is %.3f", gripper_command.command.position);
00154       if (goal->grasp.pre_grasp_posture.effort.empty())
00155       {
00156         gripper_command.command.max_effort = 10000;
00157         ROS_WARN("pr2 gripper grasp execution: effort vector empty in requested pre_grasp, using max force");
00158         //action_server_->setAborted();
00159         //return;
00160       }
00161       else 
00162         gripper_command.command.max_effort = goal->grasp.pre_grasp_posture.effort.at(0);
00163       //gripper_command.command.max_effort = 10000;
00164         
00165       break;      
00166     case object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE:
00167       gripper_command.command.position = GRIPPER_OPEN;
00168       gripper_command.command.max_effort = 10000;
00169       break;
00170     default:
00171       ROS_ERROR("pr2 gripper grasp execution: unknown goal code (%d)", goal->goal);
00172       action_server_->setAborted();
00173       return;
00174     }
00175 
00176     //send the command to the gripper
00177     gripper_action_client_->sendGoal(gripper_command);
00178     gripper_action_client_->waitForResult();
00179 
00180     //process the result
00181     if(gripper_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 
00182     {
00183       ROS_INFO("Gripper goal achieved");
00184       action_server_->setSucceeded();
00185     } 
00186     else 
00187     {
00188       if (goal->goal == object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP)
00189       {
00190         //this is probably correct behavior, since there is an object in the gripper
00191         //we can not expect to reach our goal
00192         action_server_->setSucceeded();
00193       }
00194       else
00195       {
00196         ROS_WARN("Gripper goal not achieved for pre-grasp or release");
00197         //this might become a failure later, for now we're still investigating
00198         action_server_->setSucceeded();
00199       }
00200     }
00201   }
00202 
00203   bool serviceCallback(object_manipulation_msgs::GraspStatus::Request &request,
00204                        object_manipulation_msgs::GraspStatus::Response &response)
00205   {
00206     double gripper_value;
00207     if (!getGripperValue(gripper_value)) return false;    
00208     if (gripper_value < gripper_object_presence_threshold_) 
00209     {
00210       ROS_INFO("Gripper grasp query false: gripper value %f below threshold %f", 
00211                gripper_value, gripper_object_presence_threshold_);
00212       response.is_hand_occupied = false;
00213     }
00214     else 
00215     {
00216       ROS_INFO("Gripper grasp query true: gripper value %f above threshold %f", 
00217                gripper_value, gripper_object_presence_threshold_);
00218       response.is_hand_occupied = true;
00219     }
00220     return true;
00221   }
00222                        
00223 
00224 public:
00225   PR2GripperGraspController() :
00226     root_nh_(""),
00227     priv_nh_("~")
00228   {
00229     std::string gripper_action_name = root_nh_.resolveName("gripper_action_name");
00230     gripper_action_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>
00231       (gripper_action_name, true);
00232     while(!gripper_action_client_->waitForServer(ros::Duration(2.0)) && root_nh_.ok())
00233     {
00234       ROS_INFO_STREAM("Waiting for gripper action client on topic " << gripper_action_name);
00235     }
00236     if (!root_nh_.ok()) exit(0);
00237     ROS_INFO_STREAM("Using gripper action client on topic " << gripper_action_name);
00238 
00239     priv_nh_.param<std::string>("gripper_virtual_joint_name", gripper_virtual_joint_name_, "undefined");
00240     if ( gripper_virtual_joint_name_ == "undefined" )
00241     {
00242       ROS_ERROR("Gripper virtual joint name not defined; set the parameter gripper_virtual_joint_name in the launch file");
00243       exit(0);
00244     }
00245 
00246     priv_nh_.param<double>("gripper_object_presence_threshold", gripper_object_presence_threshold_,
00247                            DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD);
00248 
00249     std::string query_service_name = root_nh_.resolveName("grasp_query_name");   
00250     query_srv_ = root_nh_.advertiseService(query_service_name, &PR2GripperGraspController::serviceCallback, this);    
00251     ROS_INFO_STREAM("pr2_gripper grasp query service started on topic " << query_service_name);
00252 
00253     std::string posture_action_name = root_nh_.resolveName("posture_action_name");
00254     action_server_ = new actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction>
00255       (root_nh_, posture_action_name, boost::bind(&PR2GripperGraspController::executeCB, this, _1));
00256     ROS_INFO_STREAM("pr2_gripper grasp hand posture action server started on topic " << posture_action_name);
00257   }
00258 
00259   ~PR2GripperGraspController()
00260   {
00261     delete action_server_;
00262     delete gripper_action_client_;
00263   }
00264 
00265 };
00266 
00267 const double PR2GripperGraspController::GRIPPER_OPEN = 0.086;
00268 const double PR2GripperGraspController::GRIPPER_CLOSED = 0.0;
00269 const double PR2GripperGraspController::DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD = 0.0021;
00270 
00271 int main(int argc, char **argv)
00272 {
00273   ros::init(argc, argv, "pr2_gripper_grasp_controller");
00274   PR2GripperGraspController node;
00275   ros::spin();
00276   return 0;  
00277 }
pr2_gripper_grasp_controller.cpp
