@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;
}
