@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 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); }
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); }
private void sendOutgoingPacketToControllerAndNetworkProcessor() { if (!isPaused.getBooleanValue() && !isAborted.getBooleanValue()) { armTrajectoryPublisher.publish(outgoingMessage); hasPacketBeenSent.set(true); if (DEBUG) PrintTools.debug(this, "sending packet to controller and network processor: " + outgoingMessage); } }
private void sendHeadTrajectoryMessage(double trajectoryTime, Quaternion desiredOrientation) { ReferenceFrame chestCoMFrame = fullRobotModel.getChest().getBodyFixedFrame(); HeadTrajectoryMessage headTrajectoryMessage = HumanoidMessageTools.createHeadTrajectoryMessage(trajectoryTime, desiredOrientation, ReferenceFrame.getWorldFrame(), chestCoMFrame); headTrajectoryPublisher.publish(headTrajectoryMessage); // footstepSentTimer.reset(); }
private void sendFootstepPlan() { FootstepPlanningToolboxOutputStatus plannerResult = this.plannerResult.getAndSet(null); FootstepDataListMessage footstepDataListMessage = plannerResult.getFootstepDataList(); footstepDataListMessage.setDefaultSwingDuration(swingTime.getValue()); footstepDataListMessage.setDefaultTransferDuration(transferTime.getDoubleValue()); footstepDataListMessage.setDestination(PacketDestination.CONTROLLER.ordinal()); footstepPublisher.publish(footstepDataListMessage); }
public void dispatchBehavior(AbstractBehavior behaviorToTest) throws SimulationExceededMaximumTimeException { HumanoidBehaviorType testBehaviorType = HumanoidBehaviorType.TEST; behaviorDispatcher.addBehavior(testBehaviorType, behaviorToTest); behaviorDispatcher.start(); HumanoidBehaviorTypePacket requestTestBehaviorPacket = HumanoidMessageTools.createHumanoidBehaviorTypePacket(testBehaviorType); humanoidBehabiorTypePublisher.publish(requestTestBehaviorPacket); boolean success = simulateAndBlockAndCatchExceptions(1.0); assertTrue("Caught an exception when testing the behavior, the robot probably fell.", success); }
public void update(boolean planarRegionsHaveBeenUpdated) { processRequests(); if (regionFeaturesProvider.getPlanarRegionsList() == null) return; if (regionFeaturesProvider.getPlanarRegionsList().isEmpty()) return; publisher.publish(PlanarRegionMessageConverter.convertToPlanarRegionsListMessage(regionFeaturesProvider.getPlanarRegionsList())); }
private void sendPelvisPosePacketToController() { if (!isPaused.getBooleanValue() && !isAborted.getBooleanValue()) { publisher.publish(outgoingPelvisOrientationTrajectoryMessage); hasPacketBeenSent.set(true); startTime.set(yoTime.getDoubleValue()); trajectoryTime.set(outgoingPelvisOrientationTrajectoryMessage.getSo3Trajectory().getTaskspaceTrajectoryPoints().getLast().getTime()); } }
private void sendChestPoseToController() { if (!isPaused.getBooleanValue() && !isAborted.getBooleanValue()) { publisher.publish(outgoingChestTrajectoryMessage); hasPacketBeenSent.set(true); startTime.set(yoTime.getDoubleValue()); trajectoryTime.set(outgoingChestTrajectoryMessage.getSo3Trajectory().getTaskspaceTrajectoryPoints().getLast().getTime()); } }
private void sendMessageToController() { if (!isPaused.getBooleanValue() && !isAborted.getBooleanValue()) { publisher.publish(outgoingPelvisHeightTrajectoryMessage); packetHasBeenSent.set(true); startTime.set(yoTime.getDoubleValue()); trajectoryTime.set(outgoingPelvisHeightTrajectoryMessage.getEuclideanTrajectory().getTaskspaceTrajectoryPoints().getLast().getTime()); } }
private void sendPelvisPosePacketToController() { if (!isPaused.getBooleanValue() && !isAborted.getBooleanValue()) { publisher.publish(outgoingPelvisTrajectoryMessage); hasPacketBeenSent.set(true); startTime.set(yoTime.getDoubleValue()); trajectoryTime.set(outgoingPelvisTrajectoryMessage.getSe3Trajectory().getTaskspaceTrajectoryPoints().getLast().getTime()); } }