Go to the documentation of this file.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 #include <katana_gazebo_plugins/katana_gripper_grasp_controller.h>
00029
00030 namespace katana_gazebo_plugins
00031 {
00032
00033
00034
00036 static const double GRIPPER_OPEN_ANGLE = 0.30;
00037
00039 static const double GRIPPER_CLOSED_ANGLE = -0.44;
00040
00042 static const double DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD = -0.43;
00043
00045 static const double GRIPPER_OPENING_CLOSING_DURATION = 6.0;
00046
00047 KatanaGripperGraspController::KatanaGripperGraspController(ros::NodeHandle private_nodehandle) :
00048 last_command_was_close_(false), desired_angle_(0.0), current_angle_(0.0), has_new_desired_angle_(false)
00049 {
00050 ros::NodeHandle root_nh("");
00051
00052 private_nodehandle.param<double> ("gripper_object_presence_threshold", gripper_object_presence_threshold_,
00053 DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD);
00054
00055 std::string query_service_name = root_nh.resolveName("gripper_grasp_status");
00056 query_srv_ = root_nh.advertiseService(query_service_name, &KatanaGripperGraspController::serviceCallback, this);
00057 ROS_INFO_STREAM("katana gripper grasp query service started on topic " << query_service_name);
00058
00059 std::string gripper_grasp_posture_controller = root_nh.resolveName("gripper_grasp_posture_controller");
00060 action_server_
00061 = new actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction>(
00062 root_nh,
00063 gripper_grasp_posture_controller,
00064 boost::bind(
00065 &KatanaGripperGraspController::executeCB,
00066 this,
00067 _1),
00068 false);
00069 action_server_->start();
00070 ROS_INFO_STREAM("katana gripper grasp hand posture action server started on topic " << gripper_grasp_posture_controller);
00071 }
00072
00073 KatanaGripperGraspController::~KatanaGripperGraspController()
00074 {
00075 delete action_server_;
00076 }
00077
00078 void KatanaGripperGraspController::executeCB(
00079 const object_manipulation_msgs::GraspHandPostureExecutionGoalConstPtr &goal)
00080 {
00081 switch (goal->goal)
00082 {
00083 case object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP:
00084 if (goal->grasp.grasp_posture.position.empty())
00085 {
00086 ROS_ERROR("katana gripper grasp execution: position vector empty in requested grasp");
00087 action_server_->setAborted();
00088 return;
00089 }
00090
00091
00092
00093
00094 setDesiredAngle(GRIPPER_CLOSED_ANGLE);
00095 last_command_was_close_ = true;
00096 break;
00097
00098 case object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP:
00099 if (goal->grasp.pre_grasp_posture.position.empty())
00100 {
00101 ROS_ERROR("katana gripper grasp execution: position vector empty in requested pre_grasp");
00102 action_server_->setAborted();
00103 return;
00104 }
00105
00106
00107
00108
00109 setDesiredAngle(goal->grasp.pre_grasp_posture.position[0]);
00110 last_command_was_close_ = false;
00111 break;
00112
00113 case object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE:
00114 setDesiredAngle(GRIPPER_OPEN_ANGLE);
00115 last_command_was_close_ = false;
00116 break;
00117
00118 default:
00119 ROS_ERROR("katana gripper grasp execution: unknown goal code (%d)", goal->goal);
00120 action_server_->setAborted();
00121 return;
00122 }
00123
00124
00125 ros::Duration(GRIPPER_OPENING_CLOSING_DURATION).sleep();
00126
00127
00128
00129 static const bool move_gripper_success = true;
00130
00131
00132 if (move_gripper_success)
00133 {
00134 ROS_INFO("Gripper goal achieved");
00135 action_server_->setSucceeded();
00136 }
00137 else
00138 {
00139 if (goal->goal == object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP)
00140 {
00141
00142
00143 action_server_->setSucceeded();
00144 }
00145 else
00146 {
00147 ROS_WARN("Gripper goal not achieved for pre-grasp or release");
00148
00149 action_server_->setSucceeded();
00150 }
00151 }
00152 }
00153
00154 bool KatanaGripperGraspController::serviceCallback(object_manipulation_msgs::GraspStatus::Request &request,
00155 object_manipulation_msgs::GraspStatus::Response &response)
00156 {
00157 if (!last_command_was_close_)
00158 {
00159 ROS_INFO("Gripper grasp query false: the last gripper command was not 'close' (= not holding any object)");
00160 response.is_hand_occupied = false;
00161 return true;
00162 }
00163
00164 if (current_angle_ < gripper_object_presence_threshold_)
00165 {
00166 ROS_INFO("Gripper grasp query false: gripper angle %f below threshold %f (= not holding any object)",
00167 current_angle_, gripper_object_presence_threshold_);
00168 response.is_hand_occupied = false;
00169 }
00170 else
00171 {
00172 ROS_INFO("Gripper grasp query true: gripper angle %f above threshold %f (= holding an object)",
00173 current_angle_, gripper_object_presence_threshold_);
00174 response.is_hand_occupied = true;
00175 }
00176 return true;
00177 }
00178
00179 void KatanaGripperGraspController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) {
00180 p = 0.4;
00181 i = 0.1;
00182 d = 0.0;
00183 }
00184
00185
00186 }