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/katana_gripper_grasp_controller.h>
00029 
00030 namespace katana
00031 {
00032 
00033 KatanaGripperGraspController::KatanaGripperGraspController(boost::shared_ptr<AbstractKatana> katana) :
00034   katana_(katana)
00035 {
00036   ros::NodeHandle root_nh("");
00037   ros::NodeHandle pn("~");
00038 
00039   pn.param<double> ("goal_threshold", goal_threshold_, 0.01);
00040 
00041   std::string query_service_name = root_nh.resolveName("gripper_grasp_status");
00042   query_srv_ = root_nh.advertiseService(query_service_name, &KatanaGripperGraspController::serviceCallback, this);
00043   ROS_INFO_STREAM("katana gripper grasp query service started on topic " << query_service_name);
00044 
00045   std::string gripper_grasp_posture_controller = root_nh.resolveName("gripper_grasp_posture_controller");
00046   action_server_ = new actionlib::SimpleActionServer<control_msgs::GripperCommandAction>(root_nh,
00047       gripper_grasp_posture_controller, boost::bind(&KatanaGripperGraspController::executeCB, this, _1), false);
00048   action_server_->start();
00049   ROS_INFO_STREAM("katana gripper grasp hand posture action server started on topic " << gripper_grasp_posture_controller);
00050 
00051 }
00052 
00053 KatanaGripperGraspController::~KatanaGripperGraspController()
00054 {
00055   delete action_server_;
00056 }
00057 
00058 void KatanaGripperGraspController::executeCB(const control_msgs::GripperCommandGoalConstPtr &goal)
00059 {
00060   ROS_INFO("Moving gripper to position: %f", goal->command.position);
00061   bool moveJointSuccess = katana_->moveJoint(GRIPPER_INDEX, goal->command.position);
00062   // wait for gripper to open/close
00063   ros::Duration(GRIPPER_OPENING_CLOSING_DURATION).sleep();
00064   
00065   control_msgs::GripperCommandResult result;
00066   result.position = katana_->getMotorAngles()[GRIPPER_INDEX];
00067   result.reached_goal = false;
00068   result.stalled = false;
00069   
00070   if (moveJointSuccess && fabs(result.position - goal->command.position) < goal_threshold_)
00071   {  
00072     ROS_INFO("Gripper goal reached");
00073     result.reached_goal = true;
00074     action_server_->setSucceeded(result); 
00075   }
00076   else if (moveJointSuccess && fabs(result.position - goal->command.position) > goal_threshold_)
00077   {
00078     ROS_INFO("Gripper stalled.");
00079     result.stalled = true;
00080     action_server_->setSucceeded(result);
00081   }
00082   else
00083   {
00084     ROS_WARN("Goal position (%f) outside gripper range. Or some other stuff happened.", goal->command.position);
00085     action_server_->setAborted(result);
00086   }
00087 
00088 
00089 }
00090 
00091 bool KatanaGripperGraspController::serviceCallback(control_msgs::QueryTrajectoryState::Request &request, 
00092                                                    control_msgs::QueryTrajectoryState::Response &response)
00093 {
00094   response.position.resize(1);
00095   response.position[0] = katana_->getMotorAngles()[GRIPPER_INDEX];
00096   return true;
00097 }
00098 
00099 }


katana
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:35