real_bad_grasp = [real_grasp[i] for i in ind_bad_grasp]
# end of grasp detection
# get sorted ind by the score values
sorted_value_ind = list(
index for index, item in sorted(enumerate(real_score_value),
key=lambda item: item[1],
reverse=True))
# sort grasps using the ind
sorted_real_good_grasp = [real_good_grasp[i] for i in sorted_value_ind]
real_good_grasp = sorted_real_good_grasp
# get the sorted score value, from high to low
real_score_value = sorted(real_score_value, reverse=True)
marker_array = MarkerArray()
marker_array_single = MarkerArray()
grasp_msg_list = GraspConfigList()
for i in range(len(real_good_grasp)):
grasp_msg = get_grasp_msg(real_good_grasp[i], real_score_value[i])
grasp_msg_list.grasps.append(grasp_msg)
for i in range(len(real_good_grasp)):
show_grasp_marker(marker_array, real_good_grasp[i], gripper,
(0, 1, 0), marker_life_time)
if show_bad_grasp:
for i in range(len(real_bad_grasp)):
show_grasp_marker(marker_array, real_bad_grasp[i], gripper,
(1, 0, 0), marker_life_time)
id_ = 0
for m in marker_array.markers:
def handle_get_grasp(req):
global grasps
# set a timer here to keep track of wait time....?
start = rospy.get_time()
lapse = 0
threshold = 10
# keep waiting until get grasp with close location......
graspList = GraspConfigList()
notFound = True
while notFound:
print req
cube = rospy.get_param('/detect_grasps/workspace')
# wait until the subscriber gets location objects from topic
top_grasp = GraspConfig()
top_grasp.score.data = 0
while grasps == []:
current = rospy.get_time()
lapse = current - start
if lapse % 1 == 0:
print lapse
if lapse > threshold:
print "ERROR: Took too long to get grasp, will now force exit."
# return top_grasp
return (graspList, len(graspList.grasps))
# search for a grasp close to the object location
# rospy.loginfo('Top grasp was:')
top_grasp = GraspConfig()
top_grasp.score.data = 0
print grasps
for grasp in grasps:
# not tested yet, but if can't launch inside from roslaunch api becuase can't set parms in launch file, then test something like this to get the graps at the specified location!
# if abs( req.x - grasp.surface.x) < eps and abs( req.y - grasp.surface.y) < eps:
# if req.x > cube[0] and req.x < cube[1] and req.y > cube[2] and req.y < cube[3] and req.z > cube[4] and req.z < cube[5]:
if grasp.score > top_grasp.score:
top_grasp = grasp
notFound = False
print "GRASP FOUND!"
graspList.grasps.append(grasp)
notFound = False
# otherwise no close grasps found yet, so reset grasps and try again!
if notFound:
print "ERRRORRRRR NO CLOSE GRASP FOUND!!!!!! Trying again! \n\n\n"
# grasps = []
top_grasp = None
# rospy.sleep(0.1)
current = rospy.get_time()
lapse = current - start
print lapse
if lapse > threshold:
print "ERROR: Took too long to get CLOSE grasp, will now force exit."
# return top_grasp
return (graspList, len(graspList.grasps))
# grasp was found, return it!
# print "Returning grasp with highest score [%s]"%(top_grasp)
grasps = [
] # need to clear this before exiting, but can't becuase used before assigned
# return top_grasp
return (graspList, len(graspList.grasps))