public void wakeUp() { if (debug) PrintTools.debug(this, "Waking up"); initialize.set(true); }
public void setInput(HandDesiredConfigurationMessage handDesiredConfigurationMessage) { this.outgoingHandDesiredConfigurationMessage = handDesiredConfigurationMessage; hasInputBeenSet.set(true); if (DEBUG) PrintTools.debug(this, "Input has been set: " + outgoingHandDesiredConfigurationMessage); }
public void writeData() throws IOException { PrintTools.debug(DEBUG, this, "writeData()"); tcpYoWhiteBoard.writeData(); }
public void readData() throws IOException { PrintTools.debug(DEBUG, this, "readData()"); tcpYoWhiteBoard.readData(); }
public void destroy() { sleep(); if (debug) PrintTools.debug(this, "Destroyed"); } }
@Override public void initialize() { PrintTools.debug(this, "CutforceSimulator initialized"); doControl(); }
private void assertPosesAreWithinThresholds(FramePose3D framePose1, FramePose3D framePose2) { double positionDistance = framePose1.getPositionDistance(framePose2); double orientationDistance = framePose1.getOrientationDistance(framePose2); PrintTools.debug(this, " desired Chest Pose :\n" + framePose1); PrintTools.debug(this, " actual Chest Pose :\n" + framePose2); PrintTools.debug(this, " positionDistance = " + positionDistance); PrintTools.debug(this, " orientationDistance = " + orientationDistance); if (!Double.isNaN(POSITION_THRESHOLD)) { assertEquals("Pose position error :" + positionDistance + " exceeds threshold: " + POSITION_THRESHOLD, 0.0, positionDistance, POSITION_THRESHOLD); } assertEquals("Pose orientation error :" + orientationDistance + " exceeds threshold: " + ORIENTATION_THRESHOLD, 0.0, orientationDistance, ORIENTATION_THRESHOLD); } }
public void sendDeviceSettingToUI(Map<String, Object> params) { BlackFlyParameterPacket packet = HumanoidMessageTools.createBlackFlyParameterPacket(false, (Double) params.get("gain"), (Double) params.get("exposure"), (Double) params.get("frame_rate"), (Double) params.get("shutter_speed"), (Boolean) params.get("auto_exposure"), (Boolean) params.get("auto_gain"), (Boolean) params.get("auto_shutter"), side); packetCommunicator.send(packet); PrintTools.debug(DEBUG, this, "packet to UI " + packet); }
public Map<String, Object> setBlackFlyParameters(BlackFlyParameterPacket packet) { PrintTools.debug(DEBUG, this, "packet fr UI " + packet); Map<String, Object> parameters = new HashMap<>(); parameters.put("auto_exposure", packet.getAutoExposure()); parameters.put("auto_gain", packet.getAutoGain()); parameters.put("auto_shutter", packet.getAutoShutter()); parameters.put("exposure", packet.getExposure()); parameters.put("gain", packet.getGain()); parameters.put("shutter_speed", packet.getShutter()); parameters.put("frame_rate", packet.getFrameRate()); return dynamicReconfigureClient.setParameters(parameters); }
private static void setTimeIntervalByTrimming(Trajectory segmentToPack, Trajectory segment1, Trajectory segment2) { double latestStartingTime = Math.max(segment1.getInitialTime(), segment2.getInitialTime()); double earliestEndingTime = Math.min(segment1.getFinalTime(), segment2.getFinalTime()); if (earliestEndingTime <= latestStartingTime) { PrintTools.debug(segment1.toString()); PrintTools.debug(segment2.toString()); throw new RuntimeException("Got null intersection for time intervals during trajectory operation"); } segmentToPack.setInitialTime(latestStartingTime); segmentToPack.setFinalTime(earliestEndingTime); }
public void sleep() { // processPlanningStatisticsRequest(); if (debug) PrintTools.debug(this, "Going to sleep"); cancelAllActiveStages(); cleanupAllPlanningStages(); for (FootstepPlanningStage planningTask : stepPlanningTasks.iterator()) stepPlanningTasks.remove(planningTask).cancel(true); }
public void receivedPacket(TextToSpeechPacket packet) { PrintTools.debug(this, "Received TextToSpeechPacket: " + packet.getTextToSpeakAsString()); String textToSpeak = packet.getTextToSpeakAsString(); textToSpeak = "<prosody pitch=\"60Hz\" rate=\"-10%\" volume=\"x-loud\">" + textToSpeak + "</prosody>"; ttsClient.playText(textToSpeak); }
@Override public void onBehaviorPaused() { for (RobotSide robotSide : RobotSide.values()) { publisher.publish(HumanoidMessageTools.createHandDesiredConfigurationMessage(robotSide, HandConfiguration.STOP)); } isPaused.set(true); if (DEBUG) PrintTools.debug(this, "Pausing Behavior"); }
@Override public void onBehaviorResumed() { isPaused.set(false); hasPacketBeenSet.set(false); if (hasInputBeenSet()) { sendHandDesiredConfigurationToController(); } if (DEBUG) PrintTools.debug(this, "Resuming Behavior"); }
private void setupTest() { FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); String className = getClass().getSimpleName(); PrintTools.debug("simulationTestingParameters.getKeepSCSUp " + simulationTestingParameters.getKeepSCSUp()); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setTestEnvironment(flatGround); drcSimulationTestHelper.createSimulation(className); robotModel = getRobotModel(); ThreadTools.sleep(1000); }
private GetRequest requestPost(String url) { PrintTools.debug(DEBUG, "LOGGING IN AS: " + loginInfo.getUsername()); return Unirest.get(url).basicAuth(loginInfo.getUsername(), loginInfo.getPassword()); } }
private void sendHandDesiredConfigurationToController() { if (!isPaused.getBooleanValue() && !isAborted.getBooleanValue()) { publisher.publish(outgoingHandDesiredConfigurationMessage); hasPacketBeenSet.set(true); startTime.set(yoTime.getDoubleValue()); if (DEBUG) PrintTools.debug(this, "Sending HandDesiredConfigurationMessage to Controller: " + outgoingHandDesiredConfigurationMessage); } }
private GetRequest requestPost(String url) { PrintTools.debug("LOGGING IN AS: " + loginInfo.getUsername() + " " + loginInfo.getPassword()); return Unirest.get(url).basicAuth(loginInfo.getUsername(), loginInfo.getPassword()); } }
public void getFramePositionInitial(FramePoint3D positionToPack) { compute(xTrajectory.getInitialTime()); PrintTools.debug("Initial time = " + xTrajectory.getInitialTime()); positionToPack.setToZero(referenceFrame); positionToPack.set(getPosition()); }
public void getFramePositionFinal(FramePoint3D positionToPack) { compute(xTrajectory.getFinalTime()); PrintTools.debug("Final time = " + xTrajectory.getFinalTime()); positionToPack.setToZero(referenceFrame); positionToPack.set(getPosition()); }