is_grasped.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 #   Copyright (c) 2013 \n
00007 #   Robotnik Automation SLL \n\n
00008 #
00009 #################################################################
00010 #
00011 # \note
00012 #   Project name: srs
00013 # \note
00014 #   ROS stack name: srs
00015 # \note
00016 #   ROS package name: srs_grasping
00017 #
00018 # \author
00019 #   Author: Manuel Rodriguez, email:mrodriguez@robotnik.es
00020 # \author
00021 #   Supervised by: Manuel Rodriguez, email:mrodriguez@robotnik.es
00022 #
00023 # \date Date of creation: April 2013
00024 #
00025 # \brief
00026 #   Service to check if the hand is grasping something.
00027 #
00028 #################################################################
00029 #
00030 # Redistribution and use in source and binary forms, with or without
00031 # modification, are permitted provided that the following conditions are met:
00032 #
00033 #     - Redistributions of source code must retain the above copyright
00034 #       notice, this list of conditions and the following disclaimer. \n
00035 #     - Redistributions in binary form must reproduce the above copyright
00036 #       notice, this list of conditions and the following disclaimer in the
00037 #       documentation and/or other materials provided with the distribution. \n
00038 #     - Neither the name of the Robotnik Automation SLL nor the names of its
00039 #       contributors may be used to endorse or promote products derived from
00040 #       this software without specific prior written permission. \n
00041 #
00042 # This program is free software: you can redistribute it and/or modify
00043 # it under the terms of the GNU Lesser General Public License LGPL as 
00044 # published by the Free Software Foundation, either version 3 of the 
00045 # License, or (at your option) any later version.
00046 # 
00047 # This program is distributed in the hope that it will be useful,
00048 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00049 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00050 # GNU Lesser General Public License LGPL for more details.
00051 # 
00052 # You should have received a copy of the GNU Lesser General Public 
00053 # License LGPL along with this program. 
00054 # If not, see <http://www.gnu.org/licenses/>.
00055 #
00056 #################################################################
00057 
00058 import roslib
00059 roslib.load_manifest('srs_grasping')
00060 import rospy
00061 import time
00062 
00063 from cob_srvs.srv import Trigger, TriggerResponse
00064 from schunk_sdh.msg import TactileSensor
00065 
00066 class is_grasped():
00067 
00068         def __init__(self):
00069                 self.f1 = []; self.f2 = []; self.f3 = [];
00070                 self.sdh_state = rospy.Subscriber("/sdh_controller/tactile_data", TactileSensor, self.get_sdh_state)
00071                 rospy.loginfo("/srs_grasping/is_grasped service is ready.");
00072 
00073 
00074         def get_sdh_state(self, msg):
00075                 self.f1 = msg.tactile_matrix[0].tactile_array + msg.tactile_matrix[1].tactile_array;
00076                 self.f2 = msg.tactile_matrix[2].tactile_array + msg.tactile_matrix[3].tactile_array;
00077                 self.f3 = msg.tactile_matrix[4].tactile_array + msg.tactile_matrix[5].tactile_array;
00078 
00079         def is_grasped(self, request):
00080                 rospy.loginfo("/srs_grasping/is_grasped service has been called...");
00081 
00082                 res = TriggerResponse()
00083 
00084                 while self.sdh_state.get_num_connections() == 0:
00085                         rospy.loginfo("[srs_grasping/is_grasped] Waiting for /sdh_controller/state connections...")
00086                         time.sleep(0.3);
00087                         continue;
00088 
00089                 size = len(self.f1)
00090                 if size == 0:
00091                         res.success.data = False
00092                         res.error_message.data = "no data received"
00093                         rospy.loginfo("[srs_grasping/is_grasped]: %s", res.error_message.data);
00094                         return res;
00095 
00096                 f1=0; f2=0; f3=0;
00097                 for i in range(0,size):
00098                         if self.f1[i] > 0: #> 500
00099                                 f1+=1;
00100                         if self.f2[i] > 0:
00101                                 f2+=1;
00102                         if self.f3[i] > 0:
00103                                 f3+=1;
00104 
00105                 num_fingers=0;
00106                 if f1>0:
00107                         num_fingers+=1;
00108                 if f2>0:
00109                         num_fingers+=1;
00110                 if f3>0:
00111                         num_fingers+=1;
00112 
00113                 if num_fingers>=1: #>=2
00114                         res.success.data = True
00115                         res.error_message.data = "Number of fingers touching the object: ",str(num_fingers)
00116                 else:
00117                         res.success.data = False
00118                         res.error_message.data = "Fingers are not touching the object"
00119 
00120                 rospy.loginfo("[srs_grasping/is_grasped]: %s", res.error_message.data);
00121                 return res;
00122 
00123 
00124         def is_grasped_server(self):
00125                 s = rospy.Service('/srs_grasping/is_grasped', Trigger, self.is_grasped);
00126 
00127 
00128 ## Main routine for running the grasp server
00129 if __name__ == '__main__':
00130         rospy.init_node('is_grasped_node');
00131         SCRIPT = is_grasped();
00132         SCRIPT.is_grasped_server();
00133         rospy.spin();


srs_grasping
Author(s): Robotnik Automation SLL
autogenerated on Mon Oct 6 2014 08:59:42