@Override public void sendPelvisPoseErrorPacket(PelvisPoseErrorPacket pelvisPoseErrorPacket) { if (poseErrorPublisher != null) poseErrorPublisher.publish(pelvisPoseErrorPacket); }
@Override public void sendLocalizationResetRequest(LocalizationPacket localizationPacket) { if (localizationPublisher != null) localizationPublisher.publish(localizationPacket); } }
private void requestAbortPlanning() { if (verbose) PrintTools.info("Sending out a sleep request."); toolboxStatePublisher.publish(MessageTools.createToolboxStateMessage(ToolboxState.SLEEP)); }
private void sendMessageToUI(String message) { textToSpeechPublisher.publish(MessageTools.createTextToSpeechPacket(message)); } }
private void sendMessageToUI(String message) { textToSpeechPublisher.publish(MessageTools.createTextToSpeechPacket(message)); }
private void requestPlannerStatistics() { plannerStatisticsRequestPublisher.publish(new PlanningStatisticsRequestMessage()); }
@Override public void run() { HandJointAnglePacket copyForReading = packetCopier.getCopyForReading(); if (copyForReading != null) { if (publisher != null) { publisher.publish(copyForReading); } } } }, 0, WORKER_SLEEP_TIME_MILLIS, TimeUnit.MILLISECONDS);
private void sendMessageToUI(String message) { textToSpeechPublisher.publish(MessageTools.createTextToSpeechPacket(message)); }
private void sendMessageToUI(String message) { textToSpeechPublisher.publish(MessageTools.createTextToSpeechPacket(message)); }
private void requestNewPlan() { toolboxStatePublisher.publish(MessageTools.createToolboxStateMessage(ToolboxState.WAKE_UP)); if (verbose) PrintTools.info("Told the toolbox to wake up."); FootstepPlannerParametersPacket plannerParametersPacket = new FootstepPlannerParametersPacket(); FootstepPlannerParameters footstepPlannerParameters = plannerParametersReference.get(); copyFootstepPlannerParametersToPacket(plannerParametersPacket, footstepPlannerParameters); plannerParametersPublisher.publish(plannerParametersPacket); VisibilityGraphsParametersPacket visibilityGraphsParametersPacket = new VisibilityGraphsParametersPacket(); VisibilityGraphsParameters visibilityGraphsParameters = visibilityGraphParametersReference.get(); copyVisibilityGraphsParametersToPacket(visibilityGraphsParametersPacket, visibilityGraphsParameters); visibilityGraphsParametersPublisher.publish(visibilityGraphsParametersPacket); if (verbose) PrintTools.info("Sent out some parameters"); submitFootstepPlanningRequestPacket(); }
lidarScanPublisher.publish(lidarScanMessage);
private void checkAndPublishIfInvalidResult() { if (resultReference.get().validForExecution()) return; FootstepPlanningToolboxOutputStatus result = new FootstepPlanningToolboxOutputStatus(); FootstepPlanningResult planningResult = resultReference.getAndSet(null); planarRegionsList.ifPresent(regions -> result.getPlanarRegionsList().set(PlanarRegionMessageConverter.convertToPlanarRegionsListMessage(regions))); result.setFootstepPlanningResult(planningResult.toByte()); result.setPlanId(plannerRequestIdReference.get()); outputStatusPublisher.publish(result); if (verbose) PrintTools.info("Finished planning, but result isn't valid, so publishing blank result on the network."); }
private void submitFootstepPlanningRequestPacket() { FootstepPlanningRequestPacket packet = new FootstepPlanningRequestPacket(); packet.getStanceFootPositionInWorld().set(plannerStartPositionReference.get()); packet.getStanceFootOrientationInWorld().set(plannerStartOrientationReference.get()); packet.getGoalPositionInWorld().set(plannerGoalPositionReference.get()); packet.getGoalOrientationInWorld().set(plannerGoalOrientationReference.get()); if (plannerInitialSupportSideReference.get() != null) packet.setInitialStanceRobotSide(plannerInitialSupportSideReference.get().toByte()); if (plannerTimeoutReference.get() != null) packet.setTimeout(plannerTimeoutReference.get()); if (plannerTypeReference.get() != null) packet.setRequestedFootstepPlannerType(plannerTypeReference.get().toByte()); if (plannerRequestIdReference.get() != null) packet.setPlannerRequestId(plannerRequestIdReference.get()); if (plannerHorizonLengthReference.get() != null) packet.setHorizonLength(plannerHorizonLengthReference.get()); if (plannerPlanarRegionReference.get() != null) packet.getPlanarRegionsListMessage().set(PlanarRegionMessageConverter.convertToPlanarRegionsListMessage(plannerPlanarRegionReference.get())); footstepPlanningRequestPublisher.publish(packet); }
private void publishResultingPlan() { double totalWaitTime = 0.0; double maxWaitTime = 10.0; long sleepDuration = 1; while (resultReference.get() == null) { if (totalWaitTime > maxWaitTime) { if (verbose) PrintTools.info("Timed out waiting for a plan when we received a valid result for execution."); return; } ThreadTools.sleep(sleepDuration); totalWaitTime += Conversions.millisecondsToSeconds(sleepDuration); } if (verbose) PrintTools.info("Finished planning, publishing the result on the network."); FootstepPlanningToolboxOutputStatus result = new FootstepPlanningToolboxOutputStatus(); FootstepPlanningResult planningResult = resultReference.getAndSet(null); if (!planningResult.validForExecution()) throw new RuntimeException("The result is completely invalid, which is a problem."); FootstepPlan footstepPlan = footstepPlanReference.getAndSet(null); planarRegionsList.ifPresent(regions -> result.getPlanarRegionsList().set(PlanarRegionMessageConverter.convertToPlanarRegionsListMessage(regions))); result.setFootstepPlanningResult(planningResult.toByte()); result.getFootstepDataList().set(convertToFootstepDataListMessage(footstepPlan)); result.setPlanId(plannerRequestIdReference.get()); receivedPlanIdReference.set(plannerRequestIdReference.get());; outputStatusPublisher.publish(result); }
@Override public void doControl() { for (RobotSide robotSide : RobotSide.values) { packet.setRobotSide(robotSide.toByte()); packet.getJointAngles().reset(); for (ValkyrieHandJointName jointEnum : ValkyrieHandJointName.values) { double q = handJoints.get(robotSide).get(jointEnum).getQ(); packet.getJointAngles().add(q); } publisher.publish(packet); } }
@Override public void run() { try { if (runController.getBooleanValue()) { if (firstTick.getBooleanValue()) { robotController.initialize(); if (outputProcessor != null) { outputProcessor.initialize(); } firstTick.set(false); } controllerTimer.startMeasurement(); robotController.doControl(); controllerTimer.stopMeasurement(); } } catch (Exception e) { crashNotificationPublisher.publish(MessageTools.createControllerCrashNotificationPacket(ControllerCrashLocation.CONTROLLER_RUN, e.getMessage())); throw new RuntimeException(e); } }
controllerCrashPublisher.publish(MessageTools.createControllerCrashNotificationPacket(ControllerCrashLocation.ESTIMATOR_RUN, e.getMessage()));
packet.getPlanarRegionsListMessage().set(PlanarRegionMessageConverter.convertToPlanarRegionsListMessage(planarRegionsList)); footstepPlanningRequestPublisher.publish(packet);
outputPacket.getLowLevelPlannerGoal().getOrientation().set(lowLevelGoalOrientation); footstepOutputStatusPublisher.publish(outputPacket);
controllerCrashPublisher.publish(MessageTools.createControllerCrashNotificationPacket(ControllerCrashLocation.ESTIMATOR_WRITE, e.getMessage()));