Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('cob_grasp_generation')
00004 import rospy
00005
00006 import actionlib
00007 import moveit_msgs.msg
00008 import cob_grasp_generation.msg
00009 from cob_grasp_generation import or_grasp_generation
00010
00011 class CobGraspGenerationActionServer(object):
00012
00013 def __init__(self, name):
00014 self._as_generate = actionlib.SimpleActionServer('generate_grasps', cob_grasp_generation.msg.GenerateGraspsAction, execute_cb=self.generate_cb)
00015 self._as_generate.start()
00016 self._as_query = actionlib.SimpleActionServer('query_grasps', cob_grasp_generation.msg.QueryGraspsAction, execute_cb=self.query_cb)
00017 self._as_query.start()
00018 self._as_show = actionlib.SimpleActionServer('show_grasps', cob_grasp_generation.msg.ShowGraspsAction, execute_cb=self.show_cb)
00019 self._as_show.start()
00020
00021 self.orgg = or_grasp_generation.ORGraspGeneration()
00022
00023 def generate_cb(self, goal):
00024 success = False
00025 num_grasps = 0
00026
00027 rospy.loginfo('Generating grasps for object: >> %s <<' % (goal.object_name))
00028
00029 if self.orgg.check_database(goal.object_name):
00030 rospy.logwarn('Grasps for object %s exist in the database.', goal.object_name)
00031 success = False
00032 else:
00033 self.orgg.setup_environment(goal.object_name, goal.viewer)
00034 rospy.loginfo('GraspTable for object %s does not exist. Now planning Grasps for the object',goal.object_name)
00035 num_grasps = self.orgg.generate_grasps(goal.object_name, goal.replan)
00036 if (num_grasps > 0):
00037 success = True
00038
00039 result = cob_grasp_generation.msg.GenerateGraspsResult()
00040 result.success = success
00041 result.num_grasps = num_grasps
00042
00043 if success:
00044 rospy.loginfo('Generate: Succeeded')
00045 self._as_generate.set_succeeded(result)
00046 else:
00047 rospy.logwarn('Generate: Failed')
00048 self._as_generate.set_aborted(result)
00049
00050
00051
00052 def query_cb(self, goal):
00053 success = False
00054 grasp_list = []
00055
00056 rospy.loginfo('Querying grasps for object: >> %s <<' % (goal.object_name))
00057
00058 if self.orgg.check_database(goal.object_name):
00059 self.orgg.setup_environment(goal.object_name, viewer=False)
00060 rospy.loginfo('GraspTable for object %s exist in the database.', goal.object_name)
00061 rospy.loginfo('Returning grasp list for selected object.')
00062 grasp_list = self.orgg.get_grasps(goal.object_name, goal.grasp_id, goal.num_grasps, goal.threshold)
00063 else:
00064 rospy.logwarn('GraspTable for object %s does not exist!',goal.object_name)
00065
00066 if not (grasp_list == []):
00067 success = True
00068
00069 result = cob_grasp_generation.msg.QueryGraspsResult()
00070 result.success = success
00071 result.grasp_list = grasp_list
00072
00073 if success:
00074 rospy.loginfo('Query: Succeeded')
00075 self._as_query.set_succeeded(result)
00076 else:
00077 rospy.logwarn('Query: Failed')
00078 self._as_query.set_aborted(result)
00079
00080
00081 def show_cb(self, goal):
00082 success = False
00083
00084 rospy.loginfo('Show grasp %i for object %s.' % (goal.grasp_id, goal.object_name))
00085
00086 if self.orgg.check_database(goal.object_name):
00087 self.orgg.setup_environment(goal.object_name, viewer=True)
00088 rospy.loginfo('Display Grasp. Object: %s | ID: %i' % (goal.object_name, goal.grasp_id))
00089 self.orgg.show_grasp(goal.object_name, goal.grasp_id, goal.sort_by_quality)
00090 success = True
00091 else:
00092 rospy.logerr('GraspTable for Object %s does not exist!' % (goal.object_name))
00093
00094 result = cob_grasp_generation.msg.ShowGraspsResult()
00095 result.success = success
00096
00097 if success:
00098 rospy.loginfo('Show: Succeeded')
00099 self._as_show.set_succeeded(result)
00100 else:
00101 rospy.logwarn('Show: Failed')
00102 self._as_show.set_aborted(result)
00103
00104
00105 if __name__ == '__main__':
00106 rospy.init_node('generate_grasps')
00107 CobGraspGenerationActionServer('generate_grasps_server')
00108 rospy.spin()