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 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); }
public void startPlaying() { PrintTools.info(garbageCollectAndPrintUsedMemoryInMB(false) + "startPlaying() "); ExecutorService executor = Executors.newSingleThreadExecutor( ThreadTools.getNamedThreadFactory( getClass().getSimpleName() + videoPlayerThreadIndex++)); executor.execute( new Runnable() { @Override public void run() { while (true) { if (currentPathToVideo == null) { ThreadTools.sleep(10); } else { boolean demuxerCreatedSuccessfully = tryToMakeDemuxer(currentPathToVideo); if (demuxerCreatedSuccessfully) { playVideo(demuxer); } else { currentPathToVideo = null; } if (demuxer != null) { demuxer.delete(); } } readyForUpdate = true; } } }); executor.shutdown(); }
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; }
public void mapBooleanVariableToComponent( YoVariableHolder holder, SaitekX52Mapping mapping, String variableName, boolean toggle, boolean invert) { BooleanYoVariable yoVariable = (BooleanYoVariable) holder.getVariable(variableName); if (yoVariable != null) { mapBooleanVariableToComponent(mapping, yoVariable, toggle, invert); } else { PrintTools.warn(this, "Variable " + variableName + " could not be found!"); } }
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; }
public void mapDoubleVariableToComponent( YoVariableHolder holder, SaitekX52Mapping mapping, String variableName, double minValue, double maxValue, double deadZone, boolean invert) { DoubleYoVariable yoVariable = (DoubleYoVariable) holder.getVariable(variableName); if (yoVariable != null) { mapDoubleVariableToComponent(mapping, yoVariable, minValue, maxValue, deadZone, invert); } else { PrintTools.warn(this, "Variable " + variableName + " could not be found!"); } }