katana_gripper_grasp_controller.cpp
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) 2010 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.cpp
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 
29 
30 namespace katana_gazebo_plugins
31 {
32 
33 // copied from katana_constants.h:
34 
36 static const double GRIPPER_OPEN_ANGLE = 0.30;
37 
39 static const double GRIPPER_CLOSED_ANGLE = -0.44;
40 
42 static const double GRIPPER_OPENING_CLOSING_DURATION = 3.0;
43 
45  desired_angle_(0.0), current_angle_(0.0), has_new_desired_angle_(false)
46 {
47  ros::NodeHandle root_nh("");
48 
49  private_nodehandle.param<double> ("goal_threshold", goal_threshold_, 0.01);
50 
51  std::string query_service_name = root_nh.resolveName("gripper_grasp_status");
53  ROS_INFO_STREAM("katana gripper grasp query service started on topic " << query_service_name);
54 
55  std::string gripper_grasp_posture_controller = root_nh.resolveName("gripper_grasp_posture_controller");
57  gripper_grasp_posture_controller, boost::bind(&KatanaGripperGraspController::executeCB, this, _1), false);
59  ROS_INFO_STREAM("katana gripper grasp hand posture action server started on topic " << gripper_grasp_posture_controller);
60 }
61 
63 {
64  delete action_server_;
65 }
66 
67 void KatanaGripperGraspController::executeCB(const control_msgs::GripperCommandGoalConstPtr &goal)
68 {
69  ROS_INFO("Moving gripper to position: %f", goal->command.position);
70 
71  control_msgs::GripperCommandResult result;
72  result.position = current_angle_;
73  result.reached_goal = false;
74  result.stalled = false;
75 
76  if(goal->command.position < GRIPPER_CLOSED_ANGLE || goal->command.position > GRIPPER_OPEN_ANGLE)
77  {
78  ROS_WARN("Goal position (%f) outside gripper range. Or some other stuff happened.", goal->command.position);
79  action_server_->setAborted(result);
80  }
81  else
82  {
83  setDesiredAngle(goal->command.position);
84  ros::Duration(GRIPPER_OPENING_CLOSING_DURATION).sleep();
85  if(fabs(goal->command.position - current_angle_) > goal_threshold_)
86  {
87  ROS_INFO("Gripper stalled.");
88  result.stalled = true;
89  }
90  else
91  {
92  ROS_INFO("Gripper goal reached.");
93  result.reached_goal = true;
94  }
95  result.position = current_angle_;
97  }
98 }
99 
100 bool KatanaGripperGraspController::serviceCallback(control_msgs::QueryTrajectoryState::Request &request,
101  control_msgs::QueryTrajectoryState::Response &response)
102 {
103  response.position.resize(1);
104  response.position[0] = current_angle_;
105  return true;
106 }
107 
108 void KatanaGripperGraspController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) {
109  p = 0.4;
110  i = 0.1;
111  d = 0.0;
112 }
113 
114 
115 }
std::string resolveName(const std::string &name, bool remap=true) const
bool sleep() const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
#define ROS_WARN(...)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
static const double GRIPPER_OPENING_CLOSING_DURATION
The maximum time it takes to open or close the gripper.
static const double GRIPPER_OPEN_ANGLE
Constants for gripper fully open or fully closed (should be equal to the value in the urdf descriptio...
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)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void executeCB(const control_msgs::GripperCommandGoalConstPtr &goal)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
static const double GRIPPER_CLOSED_ANGLE
Constants for gripper fully open or fully closed (should be equal to the value in the urdf descriptio...
#define ROS_INFO_STREAM(args)
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