def table_callback(self, msg):
try:
table_array = TableArray()
table_array.header.frame_id = "world"
highest_table = None
for table in msg.tables:
new_table = Table()
new_table.header.frame_id = "world"
table_pose = PoseStamped(pose=table.pose, header=table.header)
table_pose.header.stamp = self.transformer.getLatestCommonTime(
"world", table.header.frame_id)
new_table.pose = self.transformer.transformPose(
"world", table_pose).pose
new_table.convex_hull = table.convex_hull
table_array.tables.append(new_table)
if highest_table is None or new_table.pose.position.z > highest_table.pose.position.z:
highest_table = new_table
self.publisher.publish(table_array)
position = highest_table.pose.position
#position.z -= 0.1
MoveHelper.add_table(position=position)
# rospy.sleep(10)
self.is_finished = True
except:
self.is_finished = False
def scene_callback(self, msg):
kinect_in_scene = False
for object in msg.world.collision_objects:
should_remove = False
time_since_seen = rospy.Time.now()
if object.id not in self.last_time_seen.keys():
should_remove = True
else:
time_since_seen = rospy.Time.now() - self.last_time_seen[
object.id]
should_remove |= time_since_seen > default_object_timeout
if should_remove:
rospy.loginfo("Object " + object.id +
" has not been seen in at least " +
str(time_since_seen.to_sec()) +
". Removing object from MoveIt collision scene")
#self.scene.remove_world_object(object.id)
if object.id == "kinect":
kinect_in_scene = True
if not kinect_in_scene:
MoveHelper.add_kinect(self.transformer)
def pick_and_place(self, objects, object_poses):
self.is_picking = True
grasps = None
object_name = ""
random.shuffle(objects)
for object in objects:
object_name = object.type.key
rospy.loginfo("Getting grasp for object " + object_name)
graspResponse = self.graspService(object_name)
if graspResponse.success:
rospy.loginfo("grasps were found for object " + object_name)
grasps = graspResponse.grasps
break
if grasps is None:
rospy.logerr(
"Failed to find any grasps for the objects identified")
self.is_picking = False
return
rospy.loginfo("Finding a valid place pose")
place_poses = self.getValidPlacePoses()
rospy.loginfo("Attempting to pick up object " + object_name)
MoveHelper.move_to_neutral("left", True)
pickSuccess = False
try:
pickSuccess = self.pick(object_poses[object_name], object_name)
except Exception as e:
traceback.print_exc()
#if isinstance(e, TypeError):
# pickSuccess = True
#else:
raise e
finally:
self.is_placing = pickSuccess
self.is_picking = False
if not pickSuccess:
rospy.logerr("Object pick up failed")
return
place_result = False
try:
for place_pose in place_poses:
rospy.loginfo("Attempting to place object")
if self.place(object_name, object_poses[object_name],
place_pose):
break
except Exception as e:
traceback.print_exc()
raise e
finally:
self.is_placing = False
def publish_grasp_markers(self, grasps, object_id): #for grasp in grasps: # print(str(grasp.grasp_pose)) markers = MoveHelper.create_grasp_markers(grasps, object_id) for marker in markers: #print(str(marker)) self.markers_publisher.publish(marker)
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_and_place(self, object_id, object_pose, destination_point):
object_name = object_id
print("Attempting to pick up object " + object_name)
self.is_picking = True
rospy.loginfo("Finding a valid place pose")
place_poses = self.getValidPlacePoses(destination_point)
rospy.loginfo("Attempting to pick up object " + object_name)
MoveHelper.move_to_neutral("left", True)
pickSuccess = False
try:
pickSuccess = self.pick(object_pose, object_name)
rospy.loginfo("Pick success: " + str(pickSuccess))
except Exception as e:
traceback.print_exc()
raise e
finally:
self.is_placing = pickSuccess
self.is_picking = False
if not pickSuccess:
rospy.logerr("Object pick up failed")
return
place_result = False
try:
for place_pose in place_poses:
rospy.loginfo("Attempting to place object")
if self.place(object_name, object_pose, place_pose):
break
except Exception as e:
traceback.print_exc()
raise e
finally:
self.is_placing = False
def go(self, args):
#moveit_commander.roscpp_initialize(args)
#left_arm = baxter_interface.limb.Limb("left")
#left_arm.move_to_neutral()
object_name = "spoon"
MoveHelper.move_to_neutral("left", True)
self.scene.remove_world_object(object_name)
rospy.sleep(5.0)
pose = self.addObject(object_name)
place_poses = self.getValidPlacePoses()
pickSuccess = False
try:
pickSuccess = self.pick(pose, object_name)
except Exception as e:
traceback.print_exc()
if isinstance(e, TypeError):
pickSuccess = True
else:
raise e
if not pickSuccess:
rospy.logerr("Object pick up failed")
return
place_result = False
try:
for place_pose in place_poses:
rospy.loginfo("Attempting to place object")
rospy.loginfo(str(place_pose))
if self.place(object_name, place_pose):
break
except Exception as e:
traceback.print_exc()
raise e
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 markers_callback(self, msg):
object_poses = dict()
self.objects = ["kinect", "table", "tripod", "boundary1", "boundary2"]
for object in self.objects:
self.last_time_seen[object] = rospy.Time.now()
if len(msg.objects) == 0:
rospy.logerr("No objects identified")
return
for object in msg.objects:
self.last_time_seen[object.type.key] = rospy.Time.now()
pose = GraspingHelper.getPoseStampedFromPoseWithCovariance(
object.pose)
self.add_object_at_pose(str(object.type.key), pose)
object_poses[str(object.type.key)] = pose
self.objects.append(object.type.key)
marker = MoveHelper.create_pose_marker(pose, object.type.key, 2,
15, (1, 0, 0, 1))
self.markers_publisher.publish(marker)
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()
def publishMarkers(self, grasps, object_name):
markers = MoveHelper.create_grasp_markers(grasps=grasps,
object_name=object_name,
lifetime=0.01)
for marker in markers:
self.markers_publisher.publish(marker)
def publishMarkers(self, grasps, object_name): markers = MoveHelper.create_grasp_markers(grasps, object_name) for marker in markers: self.markers_publisher.publish(marker)
def go(self, args):
moveit_commander.roscpp_initialize(args)
MoveHelper.move_to_neutral("left", True)
rospy.sleep(5.0)
rospy.spin()
