katana_gripper_grasp_controller.h
Go to the documentation of this file.00001 /* 00002 * UOS-ROS packages - Robot Operating System code by the University of Osnabrück 00003 * Copyright (C) 2011 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.h 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 #ifndef KATANA_GRIPPER_GRASP_CONTROLLER_H_ 00028 #define KATANA_GRIPPER_GRASP_CONTROLLER_H_ 00029 00030 #include <ros/ros.h> 00031 00032 #include <actionlib/server/simple_action_server.h> 00033 00034 #include <object_manipulation_msgs/GraspHandPostureExecutionAction.h> 00035 #include <object_manipulation_msgs/GraspStatus.h> 00036 00037 #include <katana_gazebo_plugins/gazebo_ros_katana_gripper_action_interface.h> 00038 00039 namespace katana_gazebo_plugins 00040 { 00041 00042 class KatanaGripperGraspController : public IGazeboRosKatanaGripperAction 00043 { 00044 public: 00045 KatanaGripperGraspController(ros::NodeHandle private_nodehandle); 00046 virtual ~KatanaGripperGraspController(); 00047 00048 GRKAPoint getNextDesiredPoint(ros::Time time) 00049 { 00050 // reset the flag has_new_desired_angle_ because we only emit one angle 00051 if (has_new_desired_angle_) 00052 has_new_desired_angle_ = false; 00053 00054 GRKAPoint point = {desired_angle_, 0.0}; 00055 return point; 00056 } 00057 00058 void setCurrentPoint(GRKAPoint point) 00059 { 00060 this->current_angle_ = point.position; 00061 } 00062 00063 bool hasActiveGoal() const 00064 { 00065 return has_new_desired_angle_; 00066 } 00067 00068 void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 00069 00070 void cancelGoal() 00071 { 00072 has_new_desired_angle_ = false; 00073 } 00074 00075 private: 00077 actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction> *action_server_; 00078 00080 ros::ServiceServer query_srv_; 00081 00082 bool last_command_was_close_; 00083 00085 double gripper_object_presence_threshold_; 00086 00087 void executeCB(const object_manipulation_msgs::GraspHandPostureExecutionGoalConstPtr &goal); 00088 00089 bool serviceCallback(object_manipulation_msgs::GraspStatus::Request &request, 00090 object_manipulation_msgs::GraspStatus::Response &response); 00091 00092 void setDesiredAngle(double desired_angle) 00093 { 00094 desired_angle_ = desired_angle; 00095 has_new_desired_angle_ = true; 00096 } 00097 00098 double desired_angle_; 00099 double current_angle_; 00100 bool has_new_desired_angle_; 00101 }; 00102 00103 } 00104 00105 #endif /* KATANA_GRIPPER_GRASP_CONTROLLER_H_ */