@ContinuousIntegrationTest( estimatedDuration = 50.0, categoriesOverride = IntegrationCategory.EXCLUDE) @Test(timeout = 300000) public void testWalkForwardsX() throws SimulationExceededMaximumTimeException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0); assertTrue(success); double walkDistance = RandomTools.generateRandomDouble(new Random(), 1.0, 2.0); Vector2d walkDirection = new Vector2d(1, 0); double trajectoryTime = walkDistance / ASSUMED_WALKING_SPEED_mPerSec; WalkToGoalBehavior walkToGoalBehavior = testWalkToGoalBehavior(walkDistance, walkDirection, trajectoryTime); assertTrue(walkToGoalBehavior.isDone()); BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows()); }
private WalkToGoalBehavior testWalkToGoalBehavior( double walkDistance, Vector2d walkDirection, double trajectoryTime) throws SimulationExceededMaximumTimeException { FramePose2d initialMidFeetPose = getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot); FramePose2d desiredMidFeetPose = offsetMidFeetPose2d(initialMidFeetPose, walkDistance, walkDirection); final WalkToGoalBehavior walkToGoalBehavior = new WalkToGoalBehavior( communicationBridge, fullRobotModel, yoTime, getRobotModel().getPhysicalProperties().getAnkleHeight()); communicationBridge.attachGlobalListener( walkToGoalBehavior.getControllerGlobalPacketConsumer()); walkToGoalBehavior.initialize(); WalkToGoalBehaviorPacket requestedGoal = new WalkToGoalBehaviorPacket( desiredMidFeetPose.getX(), desiredMidFeetPose.getY(), desiredMidFeetPose.getYaw(), RobotSide.RIGHT); walkToGoalBehavior.consumeObjectFromNetworkProcessor(requestedGoal); WalkToGoalBehaviorPacket walkToGoalFindPathPacket = new WalkToGoalBehaviorPacket(WalkToGoalAction.FIND_PATH); walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalFindPathPacket); WalkToGoalBehaviorPacket walkToGoalExecutePacket = new WalkToGoalBehaviorPacket(WalkToGoalAction.EXECUTE); walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalExecutePacket); walkToGoalBehavior.doControl(); assertTrue(walkToGoalBehavior.hasInputBeenSet()); boolean success = executeBehavior(walkToGoalBehavior, trajectoryTime); assertTrue(success); FramePose2d finalMidFeetPose = getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot); // FramePose2d finalMidFeetPose = getCurrentMidFeetPose2d(referenceFrames); PrintTools.debug(this, " initial Midfeet Pose :\n" + initialMidFeetPose + "\n"); assertPosesAreWithinThresholds(desiredMidFeetPose, finalMidFeetPose); return walkToGoalBehavior; }