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