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()