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