def get_grasp_msg(real_good_grasp_, score_value_):
grasp_bottom_center_modify = real_good_grasp_[4]
approach = real_good_grasp_[1]
binormal = real_good_grasp_[2]
minor_pc = real_good_grasp_[3]
grasp_config_ = GraspConfig()
top_p_ = grasp_bottom_center_modify + approach * ags.gripper.hand_depth
grasp_config_.sample.x = grasp_bottom_center_modify[0]
grasp_config_.sample.y = grasp_bottom_center_modify[1]
grasp_config_.sample.z = grasp_bottom_center_modify[2]
grasp_config_.top.x = top_p_[0]
grasp_config_.top.y = top_p_[1]
grasp_config_.top.z = top_p_[2]
grasp_config_.approach.x = approach[0]
grasp_config_.approach.y = approach[1]
grasp_config_.approach.z = approach[2]
grasp_config_.binormal.x = binormal[0]
grasp_config_.binormal.y = binormal[1]
grasp_config_.binormal.z = binormal[2]
grasp_config_.axis.x = minor_pc[0]
grasp_config_.axis.y = minor_pc[1]
grasp_config_.axis.z = minor_pc[2]
grasp_config_.score.data = score_value_
return grasp_config_
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))
import numpy as np
from gpd.msg import GraspConfig
from gpd.msg import GraspConfigList
from std_msgs.msg import Int32
from tf.transformations import quaternion_from_euler
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
import geometry_msgs.msg
import moveit_commander
import moveit_msgs.msg
from moveit_commander.conversions import pose_to_list
FFC_recived = []
grasps = GraspConfig()
x_offset = -0.026
y_offset = -0.01
z_offset = -0.03
gripper_length = 0.275
vertical_threshold = 0.0
forward_dis1 = 0.1075
forward_dis2 = 0.05
# Get the msg from /cloud_indexed topic.
def callback(msg):
global FFC_recived
grasps = msg
FFC_recived.append(grasps)