00001
00002
00003 import roslib
00004 roslib.load_manifest('srs_grasping')
00005 import rospy
00006 import os
00007 import grasping_functions
00008
00009 from srs_object_database_msgs.srv import *
00010 from srs_grasping.srv import *
00011 from srs_msgs.msg import GraspingErrorCodes
00012
00013 class databaseutils():
00014
00015 def __init__(self, graspingutils=None):
00016
00017 self.graspingutils = graspingutils;
00018 self.get_mesh_service = rospy.ServiceProxy('/get_model_mesh', GetMesh);
00019 self.get_grasps_service = rospy.ServiceProxy('/get_model_grasp', GetGrasp);
00020 self.get_object_info_service = rospy.ServiceProxy('/get_models', GetObjectId);
00021 self.insert_obj_service = rospy.ServiceProxy('/insert_object_service', InsertObject)
00022
00023 if self.graspingutils is None:
00024 self.graspingutils = grasping_functions.graspingutils();
00025
00026 def get_mesh(self, object_id):
00027 try:
00028 resp = self.get_mesh_service(model_ids=[object_id])
00029 except rospy.ServiceException, e:
00030 rospy.logerr("Service did not process request: %s", str(e))
00031 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST
00032
00033 if len(resp.msg) == 0:
00034 rospy.logerr("The object with ID=%d has not mesh file in the DB", object_id);
00035 return GraspingErrorCodes.UNKNOWN_OBJECT
00036
00037 mesh_file = "/tmp/mesh.iv"
00038 f = open(mesh_file, 'w')
00039 res = f.write(resp.msg[0].data)
00040 f.close()
00041
00042 return mesh_file
00043
00044 def insert_grasps(self, object_id, grasp_file):
00045 try:
00046 resp = self.insert_obj_service(model_id=object_id, data_grasp=grasp_file)
00047 except rospy.ServiceException, e:
00048 rospy.logerr("Service did not process request: %s", str(e))
00049 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST
00050
00051 return GraspingErrorCodes.SUCCESS;
00052
00053 def get_grasps(self, object_id):
00054
00055 server_result = GetDBGraspsResponse();
00056
00057 try:
00058 resp = self.get_grasps_service(model_ids=[object_id]);
00059 except rospy.ServiceException, e:
00060 rospy.logerr("Service did not process request: %s", str(e))
00061 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST
00062
00063
00064 if len(resp.msg) == 0:
00065 rospy.loginfo("No grasping data for this object.");
00066 return GraspingErrorCodes.NON_GENERATED_INFO;
00067
00068 try:
00069 grasp_file = "/tmp/grasp.xml";
00070 f = open(grasp_file, 'w');
00071 f.write(resp.msg[0].bs);
00072 f.close();
00073
00074 GRASPS = self.graspingutils.get_grasps(grasp_file);
00075 if GRASPS == GraspingErrorCodes.CORRUPTED_GRASP_FILE:
00076 rospy.logerr("ERROR reading the grasp file");
00077 return GraspingErrorCodes.CORRUPTED_GRASP_FILE;
00078
00079 os.remove(grasp_file);
00080
00081 rospy.loginfo(str(len(GRASPS))+" grasping configuration for this object.");
00082
00083 server_result.grasp_configuration = GRASPS;
00084 return server_result;
00085
00086 except rospy.ServiceException, e:
00087 rospy.logerr("Service did not process request: %s", str(e));
00088 rospy.logerr("No grasping data for this object.");
00089 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST;
00090
00091 def get_object_id(self, object_name):
00092 try:
00093 resp = self.get_object_info_service("name", object_name)
00094 except rospy.ServiceException, e:
00095 rospy.logerr("Service did not process request: %s", str(e))
00096 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST
00097
00098 if len(resp.model_ids) == 0:
00099 rospy.logerr("No info for this object.");
00100 return GraspingErrorCodes.OBJECT_INFO_NOT_FOUND;
00101
00102 return resp.model_ids[0];
00103
00104 def get_object_name(self, object_id):
00105 try:
00106 resp = self.get_object_info_service("id", str(object_id))
00107 except rospy.ServiceException, e:
00108 rospy.logerr("Service did not process request: %s", str(e))
00109 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST
00110
00111 if len(resp.model_desc) == 0:
00112 rospy.logerr("No info for this object.");
00113 return GraspingErrorCodes.OBJECT_INFO_NOT_FOUND;
00114
00115 return resp.model_desc[0];
00116
00117 def get_graspingutils(self):
00118 return self.graspingutils;
