databaseutils.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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;


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