def annotate_grasps(self):
object_id = GraspingHelper.get_name(self.objects)
gripper = GraspingHelper.get_gripper()
frame_id = "/reference/" + gripper + "_gripper"
print("Frame id: " + frame_id)
self.grasps = []
keep_going = True
print "saving time"
time = rospy.Time.now()
index = 0
while keep_going:
response = "continue"
while (len(response) > 0 and response not in self.commands.keys()):
response = raw_input("Press enter to annotate the grasp or type 'save' to end annotation. ")
grasps = self.commands[response](self.transformer, gripper, frame_id, str(object_id), index, time)
if response == "save":
return
index += 1
self.grasps.extend(grasps)
try:
to_publish_grasps = MoveHelper.set_grasps_at_pose(self.object_poses[object_id], self.grasps, self.transformer)
self.publish_grasp_markers(to_publish_grasps, object_id)
except KeyError as e:
pass
def visualize_grasps(self, object_poses):
for object_name, object_pose in object_poses.iteritems():
graspResponse = self.graspService(object_name)
if not graspResponse.success:
rospy.logerr("No grasps were found for object " + object_name)
return
grasps = MoveHelper.set_grasps_at_pose(object_pose,
graspResponse.grasps,
self.transformer,
object_pose.header.frame_id)
self.publishMarkers(grasps, object_name)
def pick(self, object_pose, object_name):
self.group.detach_object()
graspResponse = self.graspService(object_name)
if not graspResponse.success:
rospy.logerr("No grasps were found for object " + object_name)
return
self.group.set_planning_time(20)
self.group.set_start_state_to_current_state()
grasps = MoveHelper.set_grasps_at_pose(object_pose, graspResponse.grasps, self.transformer)
self.publishMarkers(grasps, object_name)
result = self.group.pick(object_name, grasps * 5)
return result
def pick(self, pose, object_name):
self.group.detach_object()
graspResponse = self.graspService(object_name)
if not graspResponse.success:
rospy.logerr("No grasps were found for object " + object_name)
return
self.group.set_planning_time(20)
self.group.set_start_state_to_current_state()
grasps = MoveHelper.set_grasps_at_pose(pose, graspResponse.grasps, self.transformer)
self.publishMarkers(grasps, object_name)
result = self.group.pick(object_name, grasps * 5)
return result
def pick(self, object_pose, object_name):
self.group.detach_object()
graspResponse = self.graspService(object_name)
if not graspResponse.success:
rospy.logerr("No grasps were found for object " + object_name)
return
self.group.set_planning_time(20)
self.group.set_start_state_to_current_state()
grasps = MoveHelper.set_grasps_at_pose(object_pose,
graspResponse.grasps,
self.transformer,
object_pose.header.frame_id)
#print(str(grasps))
self.publishMarkers(grasps, object_name)
for grasp in grasps:
self.group.set_pose_target(grasp.grasp_pose)
self.group.plan()
self.group.go()
