@SuppressWarnings("unchecked") public void publishToController(Object message) { defaultControllerPublishers.get(message.getClass()).publish(message); }
@Override protected void setBehaviorInput() { if (DEBUG) { publishTextToSpeack("Telling Planner To Wake Up"); } toolboxStatePublisher.publish(MessageTools.createToolboxStateMessage(ToolboxState.WAKE_UP)); } };
@Override public void onFrame(VideoSource videoSource, byte[] data, long timeStamp, Point3DReadOnly position, QuaternionReadOnly orientation, IntrinsicParameters intrinsicParameters) { if (DEBUG) { PrintTools.debug(DEBUG, this, "Sending new VideoPacket FPS: " + 1.0 / timer.averageLap()); timer.lap(); } publisher.publish(HumanoidMessageTools.createVideoPacket(videoSource, timeStamp, data, position, orientation, intrinsicParameters)); }
@Override public void onFrame(VideoSource videoSource, byte[] data, long timeStamp, Point3DReadOnly position, QuaternionReadOnly orientation, IntrinsicParameters intrinsicParameters) { publisher.publish(HumanoidMessageTools.createVideoPacket(videoSource, timeStamp, data, position, orientation, intrinsicParameters)); }
private void stopArmMotion() { if (outgoingMessage != null) { stopAllTrajectoryPublisher.publish(new StopAllTrajectoryMessage()); } }
@Override protected void setBehaviorInput() { if (DEBUG) { publishTextToSpeack("Requesting Plan"); } footstepPlanningRequestPublisher.publish(HumanoidMessageTools.createFootstepPlanningRequestPacket(initialStanceFootPose, initialStanceSide, goalPose)); } };
private void sendFootsteps() { FootstepDataListMessage footstepsToSend = footstepsToSendReference.getAndSet(null); if (footstepsToSend != null && isWalking.get()) { footstepPublisher.publish(footstepsToSend); } if (!isWalking.get()) sendPauseMessage(); }
@Override public void onFrame(VideoSource videoSource, byte[] data, long timeStamp, Point3DReadOnly position, QuaternionReadOnly orientation, IntrinsicParameters intrinsicParameters) { if (DEBUG) { PrintTools.debug(this, videoSource.name() + " fisheye data size size is " + data.length); } publisher.publish(HumanoidMessageTools.createFisheyePacket(videoSource, timeStamp, data, position, orientation, intrinsicParameters)); }
private void stopArmMotion() { if (outgoingMessage != null) { stopAllTrajectoryPublisher.publish(new StopAllTrajectoryMessage()); } }
public void publishTextToSpeack(String textToSpeak) { textToSpeechPublisher.publish(MessageTools.createTextToSpeechPacket(textToSpeak)); }
@Override public void onNewMessage(std_msgs.Float64 message) { overlap = message.getData(); LocalizationStatusPacket localizationOverlapPacket = HumanoidMessageTools.createLocalizationStatusPacket(overlap, null); localizationStatusPublisher.publish(localizationOverlapPacket); } };
public void sendBehaviorToDispatcher(AbstractBehavior behaviorToTest) throws SimulationExceededMaximumTimeException { HumanoidBehaviorType testBehaviorType = HumanoidBehaviorType.TEST; behaviorDispatcher.addBehavior(testBehaviorType, behaviorToTest); HumanoidBehaviorTypePacket requestTestBehaviorPacket = HumanoidMessageTools.createHumanoidBehaviorTypePacket(testBehaviorType); humanoidBehabiorTypePublisher.publish(requestTestBehaviorPacket); }
@Override public void onBehaviorPaused() { pauseWalkingPublisher.publish(HumanoidMessageTools.createPauseWalkingMessage(true)); isPaused.set(true); if (DEBUG) PrintTools.debug(this, "Pausing Behavior"); }
@Override public void onNewMessage(std_msgs.String message) { String status = message.getData(); LocalizationStatusPacket localizationOverlapPacket = HumanoidMessageTools.createLocalizationStatusPacket(overlap, status); localizationStatusPublisher.publish(localizationOverlapPacket); // FIXME probably should not be using the same publisher as in the previous subscriber. }
private void sendPauseMessage() { PauseWalkingMessage pauseWalkingMessage = new PauseWalkingMessage(); pauseWalkingMessage.setPause(true); pauseWalkingPublisher.publish(pauseWalkingMessage); }
public void stop() { animationTimer.stop(); executorService.shutdownNow(); PauseWalkingMessage pauseWalkingMessage = new PauseWalkingMessage(); pauseWalkingMessage.setPause(true); pauseWalkingPublisher.publish(pauseWalkingMessage); }
private void processAndSendPointCloud(UnpackedPointCloud pointCloudData) { Point3D[] points = pointCloudData.getPoints(); LocalizationPointMapPacket localizationMapPacket = new LocalizationPointMapPacket(); HumanoidMessageTools.packLocalizationPointMap(points, localizationMapPacket); localizationPointMapPublisher.publish(localizationMapPacket); }
private void clearPlanarRegionsList() { RequestPlanarRegionsListMessage requestPlanarRegionsListMessage = MessageTools.createRequestPlanarRegionsListMessage(PlanarRegionsRequestType.CLEAR); requestPlanarRegionsListMessage.setDestination(PacketDestination.REA_MODULE.ordinal()); planarRegionsRequestPublisher.publish(requestPlanarRegionsListMessage); }
@Override public void doTransitionOutOfAction() { super.doTransitionOutOfAction(); //found the door location, inform the UI of its location publisher.publish(HumanoidMessageTools.createDoorLocationPacket(searchForDoorBehavior.getLocation())); } };
@Override public void sendFreezeRequest() { HighLevelStateMessage message = new HighLevelStateMessage(); message.setHighLevelControllerName(HighLevelControllerName.EXIT_WALKING.toByte()); highLevelStatePublisher.publish(message); }