katana_gripper_grasp_controller.cpp
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) 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_gazebo_plugins/katana_gripper_grasp_controller.h>
00029 
00030 namespace katana_gazebo_plugins
00031 {
00032 
00033 // copied from katana_constants.h:
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 GRIPPER_OPENING_CLOSING_DURATION = 3.0;
00043 
00044 KatanaGripperGraspController::KatanaGripperGraspController(ros::NodeHandle private_nodehandle) :
00045   desired_angle_(0.0), current_angle_(0.0), has_new_desired_angle_(false)
00046 {
00047   ros::NodeHandle root_nh("");
00048 
00049   private_nodehandle.param<double> ("goal_threshold", goal_threshold_, 0.01);
00050 
00051   std::string query_service_name = root_nh.resolveName("gripper_grasp_status");
00052   query_srv_ = root_nh.advertiseService(query_service_name, &KatanaGripperGraspController::serviceCallback, this);
00053   ROS_INFO_STREAM("katana gripper grasp query service started on topic " << query_service_name);
00054 
00055   std::string gripper_grasp_posture_controller = root_nh.resolveName("gripper_grasp_posture_controller");
00056   action_server_ = new actionlib::SimpleActionServer<control_msgs::GripperCommandAction>(root_nh,
00057       gripper_grasp_posture_controller, boost::bind(&KatanaGripperGraspController::executeCB, this, _1), false);
00058   action_server_->start();
00059   ROS_INFO_STREAM("katana gripper grasp hand posture action server started on topic " << gripper_grasp_posture_controller);
00060 }
00061 
00062 KatanaGripperGraspController::~KatanaGripperGraspController()
00063 {
00064   delete action_server_;
00065 }
00066 
00067 void KatanaGripperGraspController::executeCB(const control_msgs::GripperCommandGoalConstPtr &goal)
00068 {
00069   ROS_INFO("Moving gripper to position: %f", goal->command.position);
00070 
00071   control_msgs::GripperCommandResult result;
00072   result.position = current_angle_;
00073   result.reached_goal = false;
00074   result.stalled = false;
00075 
00076   if(goal->command.position < GRIPPER_CLOSED_ANGLE || goal->command.position > GRIPPER_OPEN_ANGLE)
00077   {
00078     ROS_WARN("Goal position (%f) outside gripper range. Or some other stuff happened.", goal->command.position);
00079     action_server_->setAborted(result);
00080   }
00081   else
00082   {
00083     setDesiredAngle(goal->command.position);
00084     ros::Duration(GRIPPER_OPENING_CLOSING_DURATION).sleep();
00085     if(fabs(goal->command.position - current_angle_) > goal_threshold_)
00086     {
00087       ROS_INFO("Gripper stalled.");
00088       result.stalled = true;
00089     }
00090     else
00091     {
00092       ROS_INFO("Gripper goal reached.");
00093       result.reached_goal = true;
00094     }
00095     result.position = current_angle_;
00096     action_server_->setSucceeded(result);
00097   }
00098 }
00099 
00100 bool KatanaGripperGraspController::serviceCallback(control_msgs::QueryTrajectoryState::Request &request,
00101                                                    control_msgs::QueryTrajectoryState::Response &response)
00102 {
00103   response.position.resize(1);
00104   response.position[0] = current_angle_;
00105   return true;
00106 }
00107 
00108 void KatanaGripperGraspController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) {
00109   p = 0.4;
00110   i = 0.1;
00111   d = 0.0;
00112 }
00113 
00114 
00115 }


katana_gazebo_plugins
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:42:23