def grasp_goal_cb(userdata, goal):
############################################################
# Creating grasp goal
grasp_goal = OverheadGraspGoal()
grasp_goal.is_grasp = True
grasp_goal.disable_head = False
grasp_goal.disable_coll = False
grasp_goal.grasp_type = OverheadGraspGoal.VISION_GRASP
grasp_goal.x = GRASP_LOCATION[0]
grasp_goal.y = arm_mult * GRASP_LOCATION[1]
grasp_goal.behavior_name = "overhead_grasp"
grasp_goal.sig_level = 0.999
############################################################
return grasp_goal
def place_goal_cb(userdata, goal):
print "object Number", userdata.object_number
############################################################
# Creating place place_goal
place_goal = OverheadGraspGoal()
place_goal.is_grasp = False
place_goal.disable_head = False
place_goal.disable_coll = False
place_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
place_goal.x = PLACE_LOCATIONS[userdata.object_number - 1][0]
place_goal.y = arm_mult * PLACE_LOCATIONS[userdata.object_number -
1][1]
place_goal.roll = PLACE_LOCATIONS[userdata.object_number - 1][2]
place_goal.behavior_name = "overhead_grasp"
place_goal.sig_level = 0.999
############################################################
return place_goal
def do_action(self, req):
rospy.loginfo("[grasp_obj_button] Table clicked, grasping object...")
# put 3d pt in grasping frame
req.pixel3d.header.stamp = rospy.Time(0)
grasp_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d)
# move to grasp setup position
setup_goal = OverheadGraspSetupGoal()
setup_goal.disable_head = False
setup_goal.open_gripper = True
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
############################################################
# Creating grasp goal
grasp_goal = OverheadGraspGoal()
grasp_goal.is_grasp = True
grasp_goal.disable_head = False
grasp_goal.disable_coll = False
grasp_goal.grasp_type=OverheadGraspGoal.VISION_GRASP
grasp_goal.x = grasp_pt.point.x
grasp_goal.y = grasp_pt.point.y
grasp_goal.behavior_name = "overhead_grasp"
grasp_goal.sig_level = 0.999
############################################################
# grasp object
self.grasp_client.send_goal(grasp_goal)
self.grasp_client.wait_for_result()
result = self.grasp_client.get_result()
rospy.loginfo("[grasp_obj_button] Grasp result: %s" % result)
obj_in_hand = result.grasp_result == "Object grasped"
# go back to grasp setup
setup_goal.open_gripper = False
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
return ButtonActionResponse()