private void assertPosesAreWithinThresholds(FramePose2d framePose1, FramePose2d framePose2) {
    double positionDistance = framePose1.getPositionDistance(framePose2);
    double orientationDistance = framePose1.getOrientationDistance(framePose2);

    if (DEBUG) {
      PrintTools.debug(this, " desired Midfeet Pose :\n" + framePose1 + "\n");
      PrintTools.debug(this, " actual Midfeet Pose :\n" + framePose2 + "\n");

      PrintTools.debug(this, " positionDistance = " + positionDistance);
      PrintTools.debug(this, " orientationDistance = " + orientationDistance);
    }

    assertEquals(0.0, positionDistance, POSITION_THRESHOLD);
    assertEquals(0.0, orientationDistance, ORIENTATION_THRESHOLD);
  }
  private boolean tryToMakeDemuxer(final Path path) {
    PrintTools.debug(
        this,
        garbageCollectAndPrintUsedMemoryInMB(false)
            + " tryToMakeDemuxer "
            + path.getFileName().toString());

    timedOut = true;
    ThreadTools.executeWithTimeout(
        MP4VideoDemuxer.class.getSimpleName() + demuxerThreadIndex++,
        new Runnable() {
          @Override
          public void run() {
            try {
              demuxer = new MP4VideoDemuxer(path.toFile());
              timedOut = false;
            } catch (IOException e) {
              e.printStackTrace();
            }
          }
        },
        1000,
        TimeUnit.MILLISECONDS);
    if (timedOut) PrintTools.error("Demuxer.init() timed out!");
    return timedOut;
  }
  private boolean executeBehavior(final AbstractBehavior behavior, double trajectoryTime)
      throws SimulationExceededMaximumTimeException {
    final double simulationRunTime = trajectoryTime + EXTRA_SIM_TIME_FOR_SETTLING;

    if (DEBUG) {
      System.out.println("\n");
      PrintTools.debug(
          this, "starting behavior: " + behavior.getName() + "   t = " + yoTime.getDoubleValue());
    }

    Thread behaviorThread =
        new Thread() {
          public void run() {
            {
              double startTime = Double.NaN;
              boolean simStillRunning = true;
              boolean initalized = false;

              while (simStillRunning) {
                if (!initalized) {
                  startTime = yoTime.getDoubleValue();
                  initalized = true;
                }

                double timeSpentSimulating = yoTime.getDoubleValue() - startTime;
                simStillRunning = timeSpentSimulating < simulationRunTime;

                robotDataReceiver.updateRobotModel();
                behavior.doControl();
              }
            }
          }
        };

    behaviorThread.start();

    boolean ret = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(simulationRunTime);

    PrintTools.debug(
        this, "done with behavior: " + behavior.getName() + "   t = " + yoTime.getDoubleValue());

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