$search
00001 /* 00002 * UOS-ROS packages - Robot Operating System code by the University of Osnabrück 00003 * Copyright (C) 2010 University of Osnabrück 00004 * 00005 * This program is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU General Public License 00007 * as published by the Free Software Foundation; either version 2 00008 * of the License, or (at your option) any later version. 00009 * 00010 * This program is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 * GNU General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU General Public License 00016 * along with this program; if not, write to the Free Software 00017 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00018 * 00019 * katana_gripper_grasp_controller.cpp 00020 * 00021 * Created on: 29.01.2011 00022 * Author: Martin Günther <mguenthe@uos.de> 00023 * 00024 * based on pr2_gripper_grasp_controller 00025 * 00026 */ 00027 00028 #include <katana/katana_gripper_grasp_controller.h> 00029 00030 namespace katana 00031 { 00032 00033 KatanaGripperGraspController::KatanaGripperGraspController(boost::shared_ptr<AbstractKatana> katana) : 00034 katana_(katana), last_command_was_close_(false) 00035 { 00036 ros::NodeHandle root_nh(""); 00037 ros::NodeHandle pn("~"); 00038 00039 pn.param<double> ("gripper_object_presence_threshold", gripper_object_presence_threshold_, 00040 DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD); 00041 00042 std::string query_service_name = root_nh.resolveName("grasp_query_name"); 00043 query_srv_ = root_nh.advertiseService(query_service_name, &KatanaGripperGraspController::serviceCallback, this); 00044 ROS_INFO_STREAM("katana gripper grasp query service started on topic " << query_service_name); 00045 00046 std::string posture_action_name = root_nh.resolveName("posture_action_name"); 00047 action_server_ = new actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction>(root_nh, 00048 posture_action_name, boost::bind(&KatanaGripperGraspController::executeCB, this, _1), false); 00049 action_server_->start(); 00050 ROS_INFO_STREAM("katana gripper grasp hand posture action server started on topic " << posture_action_name); 00051 } 00052 00053 KatanaGripperGraspController::~KatanaGripperGraspController() 00054 { 00055 delete action_server_; 00056 } 00057 00058 void KatanaGripperGraspController::executeCB(const object_manipulation_msgs::GraspHandPostureExecutionGoalConstPtr &goal) 00059 { 00060 switch (goal->goal) 00061 { 00062 case object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP: 00063 if (goal->grasp.grasp_posture.position.empty()) 00064 { 00065 ROS_ERROR("katana gripper grasp execution: position vector empty in requested grasp"); 00066 action_server_->setAborted(); 00067 return; 00068 } 00069 00070 // well, we don't really use the grasp_posture.position value here, we just instruct 00071 // the gripper to close all the way... 00072 // that might change in the future and we might do something more interesting 00073 katana_->moveJoint(GRIPPER_INDEX, GRIPPER_CLOSED_ANGLE); 00074 00075 last_command_was_close_ = true; 00076 break; 00077 00078 case object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP: 00079 if (goal->grasp.pre_grasp_posture.position.empty()) 00080 { 00081 ROS_ERROR("katana gripper grasp execution: position vector empty in requested pre_grasp"); 00082 action_server_->setAborted(); 00083 return; 00084 } 00085 00086 // well, we don't really use the pre_grasp_posture.position value here, we just instruct 00087 // the gripper to open all the way... 00088 // that might change in the future and we might do something more interesting 00089 for (unsigned int i = 0; i < goal->grasp.pre_grasp_posture.position.size(); i++) 00090 { 00091 katana_->moveJoint(katana_->getJointIndex(goal->grasp.pre_grasp_posture.name[i]), 00092 goal->grasp.pre_grasp_posture.position[i]); 00093 } 00094 00095 last_command_was_close_ = false; 00096 break; 00097 00098 case object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE: 00099 00100 katana_->moveJoint(GRIPPER_INDEX, GRIPPER_OPEN_ANGLE); 00101 last_command_was_close_ = false; 00102 break; 00103 00104 default: 00105 ROS_ERROR("katana gripper grasp execution: unknown goal code (%d)", goal->goal); 00106 action_server_->setAborted(); 00107 return; 00108 } 00109 00110 // wait for gripper to open/close 00111 ros::Duration(GRIPPER_OPENING_CLOSING_DURATION).sleep(); 00112 00113 // If we ever do anything else than setSucceeded() in the future, we will have to really 00114 // check if the desired opening angle was reached by the gripper here. 00115 static const bool move_gripper_success = true; 00116 00117 // process the result 00118 if (move_gripper_success) 00119 { 00120 ROS_INFO("Gripper goal achieved"); 00121 action_server_->setSucceeded(); 00122 } 00123 else 00124 { 00125 if (goal->goal == object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP) 00126 { 00127 // this is probably correct behavior, since there is an object in the gripper 00128 // we can not expect the gripper to fully close 00129 action_server_->setSucceeded(); 00130 } 00131 else 00132 { 00133 ROS_WARN("Gripper goal not achieved for pre-grasp or release"); 00134 // this might become a failure later, for now we're still investigating 00135 action_server_->setSucceeded(); 00136 } 00137 } 00138 } 00139 00140 bool KatanaGripperGraspController::serviceCallback(object_manipulation_msgs::GraspStatus::Request &request, 00141 object_manipulation_msgs::GraspStatus::Response &response) 00142 { 00143 if (!last_command_was_close_) 00144 { 00145 ROS_INFO("Gripper grasp query false: the last gripper command was not 'close' (= not holding any object)"); 00146 response.is_hand_occupied = false; 00147 return true; 00148 } 00149 00150 double gripperAngle = katana_->getMotorAngles()[GRIPPER_INDEX]; 00151 if (gripperAngle < gripper_object_presence_threshold_) 00152 { 00153 ROS_INFO("Gripper grasp query false: gripper angle %f below threshold %f (= not holding any object)", 00154 gripperAngle, gripper_object_presence_threshold_); 00155 response.is_hand_occupied = false; 00156 } 00157 else 00158 { 00159 ROS_INFO("Gripper grasp query true: gripper angle %f above threshold %f (= holding an object)", 00160 gripperAngle, gripper_object_presence_threshold_); 00161 response.is_hand_occupied = true; 00162 } 00163 return true; 00164 } 00165 00166 }