katana_gripper_grasp_controller.h
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2011 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * katana_gripper_grasp_controller.h
20  *
21  * Created on: 29.01.2011
22  * Author: Martin Günther <mguenthe@uos.de>
23  *
24  * based on pr2_gripper_grasp_controller
25  */
26 
27 #ifndef KATANA_GRIPPER_GRASP_CONTROLLER_H_
28 #define KATANA_GRIPPER_GRASP_CONTROLLER_H_
29 
30 #include <ros/ros.h>
31 
33 
34 #include <control_msgs/GripperCommandAction.h>
35 #include <control_msgs/QueryTrajectoryState.h>
36 
38 
39 namespace katana_gazebo_plugins
40 {
41 
43 {
44 public:
47 
49  {
50  // reset the flag has_new_desired_angle_ because we only emit one angle
52  has_new_desired_angle_ = false;
53 
54  GRKAPoint point = {desired_angle_, 0.0};
55  return point;
56  }
57 
59  {
60  this->current_angle_ = point.position;
61  }
62 
63  bool hasActiveGoal() const
64  {
66  }
67 
68  void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
69 
70  void cancelGoal()
71  {
72  has_new_desired_angle_ = false;
73  }
74 
75 private:
78 
81 
84 
85  void executeCB(const control_msgs::GripperCommandGoalConstPtr &goal);
86 
87  bool serviceCallback(control_msgs::QueryTrajectoryState::Request &request,
88  control_msgs::QueryTrajectoryState::Response &response);
89 
90  void setDesiredAngle(double desired_angle)
91  {
92  desired_angle_ = desired_angle;
94  }
95 
100 };
101 
102 }
103 
104 #endif /* KATANA_GRIPPER_GRASP_CONTROLLER_H_ */
d
actionlib::SimpleActionServer< control_msgs::GripperCommandAction > * action_server_
Action server for the grasp posture action.
ros::ServiceServer query_srv_
Server for the posture query service.
void getGains(double &p, double &i, double &d, double &i_max, double &i_min)
void executeCB(const control_msgs::GripperCommandGoalConstPtr &goal)
double gripper_object_presence_threshold_
A joint angle below this value indicates there is nothing inside the gripper.
bool serviceCallback(control_msgs::QueryTrajectoryState::Request &request, control_msgs::QueryTrajectoryState::Response &response)


katana_gazebo_plugins
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:34