def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb' ) args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb rospy.init_node('stackit') iksvc, ns = ik_command.connect_service(limb) moveit_commander.roscpp_initialize(sys.argv) rate = rospy.Rate(1) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander(limb+"_arm") group.allow_replanning(True) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() obj_manager = ObjectManager() while len(obj_manager.collision_objects) <= 0: rate.sleep() objects = obj_manager.collision_objects object_height = params['object_height'] """if len(objects) > 1: stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0]) stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0) objects.pop(len(objects)-1) elif len(objects) == 1: stack_pose = projectPose(objects[0].primitive_poses[0])""" stack_pose = Pose(position=Point(0.593, -0.212, object_height-0.130), orientation=Quaternion(0.6509160466, 0.758886809948, -0.0180992582839, -0.0084573527776) ) processed_win = "Processed image" raw_win = "Hand camera" cv2.namedWindow(processed_win) #cv2.namedWindow(raw_win) rospy.on_shutdown(obj_manager.remove_known_objects) for obj in objects: obj_pose = obj.primitive_poses[0] #group.pick(obj.id) #group.place(obj.id, stack_pose) print "Got pose:", obj.primitive_poses[0] pose = projectPose(obj.primitive_poses[0]) pose = incrementPoseMsgZ(pose, object_height*1.7) print "Modified pose:", pose #if obj.id in obj_manager.id_operations: # obj_manager.id_operations[obj.id] = CollisionObject.REMOVE #obj_manager.publish(obj) print "setting target to pose" # Move to the next block group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(pose) plan = group.plan() # is there a better way of checking this? plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan1..." #rospy.sleep(3) group.go(wait=True) imgproc = ObjectFinder("star", None, None) imgproc.subscribe("/cameras/"+limb+"_hand_camera/image") imgproc.publish(limb) vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): blobArray = [] for centroid, axis in zip(imgproc.centroids, imgproc.axes): blob = BlobInfo() centroid = Point(*centroid) blob.centroid = centroid if axis is None: axis = -1*numpy.ones(6) blob.axis = Polygon([Point(*axis[0:3].tolist()), Point(*axis[3:6].tolist())]) blobArray.append(blob) msg = BlobInfoArray() msg.blobs = blobArray imgproc.handler_pub.publish(msg) if imgproc.processed is not None: cv2.imshow(processed_win, imgproc.processed) cv2.waitKey(10) vc.unsubscribe() imgproc.unsubscribe() print "Adding attached message" #Add attached message #obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...? obj_manager.publish_attached(obj, limb) #touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"] #group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(pose, 2*object_height) # test this ik_command.service_request_pose(iksvc, pose, limb, blocking = True) else: print "Unable to plan path" continue # what to do? # Move to the stacking position group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(stack_pose) plan = group.plan() plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan2..." #rospy.sleep(3) group.go(wait=True) gripper_if.open(block=True) group.detach_object(obj.id) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(stack_pose, 2*object_height) ik_command.service_request_pose(iksvc, pose, limb, blocking = True) obj.operation = CollisionObject.REMOVE obj_manager.publish(obj) # Get the next stack pose stack_pose = incrementPoseMsgZ(stack_pose, object_height*3/4) """if obj.id in obj_manager.id_operations:
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument('-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb') args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb rospy.init_node('stackit') iksvc, ns = ik_command.connect_service(limb) moveit_commander.roscpp_initialize(sys.argv) rate = rospy.Rate(1) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander(limb + "_arm") group.allow_replanning(True) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() obj_manager = ObjectManager() while len(obj_manager.collision_objects) <= 0: rate.sleep() objects = obj_manager.collision_objects object_height = params['object_height'] """if len(objects) > 1: stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0]) stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0) objects.pop(len(objects)-1) elif len(objects) == 1: stack_pose = projectPose(objects[0].primitive_poses[0])""" stack_pose = Pose(position=Point(0.593, -0.212, object_height - 0.130), orientation=Quaternion(0.6509160466, 0.758886809948, -0.0180992582839, -0.0084573527776)) processed_win = "Processed image" raw_win = "Hand camera" cv2.namedWindow(processed_win) #cv2.namedWindow(raw_win) rospy.on_shutdown(obj_manager.remove_known_objects) for obj in objects: obj_pose = obj.primitive_poses[0] #group.pick(obj.id) #group.place(obj.id, stack_pose) print "Got pose:", obj.primitive_poses[0] pose = projectPose(obj.primitive_poses[0]) pose = incrementPoseMsgZ(pose, object_height * 1.7) print "Modified pose:", pose #if obj.id in obj_manager.id_operations: # obj_manager.id_operations[obj.id] = CollisionObject.REMOVE #obj_manager.publish(obj) print "setting target to pose" # Move to the next block group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(pose) plan = group.plan() # is there a better way of checking this? plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan1..." #rospy.sleep(3) group.go(wait=True) imgproc = ObjectFinder("star", None, None) imgproc.subscribe("/cameras/" + limb + "_hand_camera/image") imgproc.publish(limb) vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): blobArray = [] for centroid, axis in zip(imgproc.centroids, imgproc.axes): blob = BlobInfo() centroid = Point(*centroid) blob.centroid = centroid if axis is None: axis = -1 * numpy.ones(6) blob.axis = Polygon([ Point(*axis[0:3].tolist()), Point(*axis[3:6].tolist()) ]) blobArray.append(blob) msg = BlobInfoArray() msg.blobs = blobArray imgproc.handler_pub.publish(msg) if imgproc.processed is not None: cv2.imshow(processed_win, imgproc.processed) cv2.waitKey(10) vc.unsubscribe() imgproc.unsubscribe() print "Adding attached message" #Add attached message #obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...? obj_manager.publish_attached(obj, limb) #touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"] #group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(pose, 2 * object_height) # test this ik_command.service_request_pose(iksvc, pose, limb, blocking=True) else: print "Unable to plan path" continue # what to do? # Move to the stacking position group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(stack_pose) plan = group.plan() plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan2..." #rospy.sleep(3) group.go(wait=True) gripper_if.open(block=True) group.detach_object(obj.id) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(stack_pose, 2 * object_height) ik_command.service_request_pose(iksvc, pose, limb, blocking=True) obj.operation = CollisionObject.REMOVE obj_manager.publish(obj) # Get the next stack pose stack_pose = incrementPoseMsgZ(stack_pose, object_height * 3 / 4) """if obj.id in obj_manager.id_operations: