How to use

publish

method

in

us.ihmc.communication.IHMCRealtimeROS2Publisher

@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()));

controllerCrashPublisher.publish(MessageTools.createControllerCrashNotificationPacket(ControllerCrashLocation.ESTIMATOR_WRITE, e.getMessage()));

@Override
public void write(long timestamp)
{
 try
 {
   if (runController.getBooleanValue())
   {
    if (outputProcessor != null)
    {
      outputProcessor.processAfterController(controllerTimestamp.getLongValue());
    }
    totalDelay.set(timestamp - lastEstimatorStartTime.getLongValue());
    threadDataSynchronizer.publishControllerData();
    if (robotVisualizer != null)
    {
      robotVisualizerUpdateTimer.startMeasurement();
      robotVisualizer.update(controllerTimestamp.getLongValue(), registry);
      robotVisualizerUpdateTimer.stopMeasurement();
    }
    rootFrame.getTransformToDesiredFrame(rootToWorldTransform, ReferenceFrame.getWorldFrame());
    yoGraphicsListRegistry.setControllerTransformToWorld(rootToWorldTransform);
   }
 }
 catch (Exception e)
 {
   crashNotificationPublisher.publish(MessageTools.createControllerCrashNotificationPacket(ControllerCrashLocation.CONTROLLER_WRITE, e.getMessage()));
   throw new RuntimeException(e);
 }
}

@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = timeout)
public void testWalkingBetweenBollardsAStarPlanner()
{
 DRCStartingLocation startingLocation = () -> new OffsetAndYawRobotInitialSetup(-1.5, 0.0, 0.007, 0.0);
 FramePose3D goalPose = new FramePose3D();
 goalPose.setX(1.5);
 setupSimulation(bollardPlanarRegions, startingLocation);
 CollisionCheckerScript collisionChecker = getCollisionChecker(500);
 drcSimulationTestHelper.createSimulation("FootstepPlannerEndToEndTest");
 drcSimulationTestHelper.getSimulationConstructionSet().addScript(collisionChecker);
 FootstepPlannerParameters parameters = getRobotModel().getFootstepPlannerParameters();
 FootstepPlannerParametersPacket parametersPacket = new FootstepPlannerParametersPacket();
 RemoteUIMessageConverter.copyFootstepPlannerParametersToPacket(parametersPacket, parameters);
 parametersPacket.setCheckForBodyBoxCollisions(true);
 parametersPacket.setReturnBestEffortPlan(false);
 footstepPlannerParametersPublisher.publish(parametersPacket);
 runEndToEndTestAndKeepSCSUpIfRequested(FootstepPlannerType.A_STAR, bollardPlanarRegions, goalPose);
}