get_db_grasps.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 #   Copyright (c) 2012 \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: March 2012
00024 #
00025 # \brief
00026 #   Implements a service that returns all the grasping configurations for a given object_id.
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 time
00061 import rospy
00062 
00063 import grasping_functions
00064 from srs_grasping.srv import *
00065 from srs_msgs.msg import GraspingErrorCodes
00066 
00067 
00068 class get_db_grasps():
00069 
00070         def __init__(self):
00071                 print "-------------------------------------------------------------------------";
00072                 rospy.loginfo("Waiting /get_model_grasp service...");
00073                 rospy.wait_for_service('/get_model_grasp');
00074                 rospy.loginfo("/get_db_grasps service is ready.");
00075  
00076 
00077         def get_db_grasps(self, server_goal):
00078                 x = time.time();
00079                 rospy.loginfo("/get_db_grasps service has been called...");
00080 
00081 
00082                 server_result = GetDBGraspsResponse();
00083                 server_result.error_code.val = 0;
00084                 server_result.grasp_configuration = [];
00085 
00086                 resp = grasping_functions.databaseutils.get_grasps(server_goal.object_id);
00087                 if resp < 0:
00088                         resp2 = grasping_functions.openraveutils.generator(server_goal.object_id);
00089                         if resp2 is GraspingErrorCodes.SUCCESS:
00090                                 resp_aux = grasping_functions.databaseutils.get_grasps(server_goal.object_id);
00091                                 if resp_aux < 0:
00092                                         server_result.error_code.val = resp_aux;
00093                                 else:
00094                                         server_result.error_code.val = GraspingErrorCodes.SUCCESS;
00095                                         server_result.grasp_configuration = resp_aux.grasp_configuration;
00096                         else:
00097                                 server_result.error_code.val = resp2;
00098                 else:
00099                         server_result.error_code.val = GraspingErrorCodes.SUCCESS;
00100                         server_result.grasp_configuration = resp.grasp_configuration;
00101 
00102                 print "Time employed: " + str(time.time() - x);
00103                 print "---------------------------------------";
00104                 return server_result;
00105 
00106 
00107         def get_db_grasps_server(self):
00108                 s = rospy.Service('/get_db_grasps', GetDBGrasps, self.get_db_grasps);
00109 
00110 
00111 if __name__ == '__main__':
00112         rospy.init_node('get_db_grasps')
00113         SCRIPT = get_db_grasps()
00114         SCRIPT.get_db_grasps_server();
00115         rospy.spin()


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