public REAPlanarRegionPublicNetworkProvider(RegionFeaturesProvider regionFeaturesProvider, Ros2Node ros2Node, MessageTopicNameGenerator publisherTopicNameGenerator, MessageTopicNameGenerator subscriberTopicNameGenerator) { this.regionFeaturesProvider = regionFeaturesProvider; publisher = ROS2Tools.createPublisher(ros2Node, PlanarRegionsListMessage.class, publisherTopicNameGenerator); ROS2Tools.createCallbackSubscription(ros2Node, RequestPlanarRegionsListMessage.class, subscriberTopicNameGenerator, this::handlePacket); }
public void startServer(NetworkPorts portToOpen) throws IOException { if (ros2Node != null) { LogTools.error("There is already a log server running."); } else { ros2Node = ROS2Tools.createRealtimeRos2Node(PubSubImplementation.FAST_RTPS, "lidar_log"); lidarScanPublisher = ROS2Tools.createPublisher(ros2Node, LidarScanMessage.class, ROS2Tools.getDefaultTopicNameGenerator()); ros2Node.spin(); } if (currentLoggingTask == null) currentLoggingTask = executorService.scheduleAtFixedRate(this::readData, 0L, 10L, TimeUnit.MILLISECONDS); }
public static MessageTopicNameGenerator getSubscriberTopicNameGenerator(String robotName) { return ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.BEHAVIOR_MODULE, ROS2TopicQualifier.INPUT); } }
public VideoPacketHandler(Ros2Node ros2Node) { publisher = ROS2Tools.createPublisher(ros2Node, VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); }
uiPlanningResultReference = messager.createInput(PlanningResultTopic); ros2Node = ROS2Tools.createRealtimeRos2Node(pubSubImplementation, "ihmc_footstep_planner_test"); .createPublisher(ros2Node, FootstepPlanningRequestPacket.class, FootstepPlannerCommunicationProperties.subscriberTopicNameGenerator(robotName)); footstepPlannerParametersPublisher = ROS2Tools .createPublisher(ros2Node, FootstepPlannerParametersPacket.class, FootstepPlannerCommunicationProperties.subscriberTopicNameGenerator(robotName)); toolboxStatePublisher = ROS2Tools .createPublisher(ros2Node, ToolboxStateMessage.class, FootstepPlannerCommunicationProperties.subscriberTopicNameGenerator(robotName)); ROS2Tools.createCallbackSubscription(ros2Node, FootstepPlanningToolboxOutputStatus.class, FootstepPlannerCommunicationProperties.publisherTopicNameGenerator(robotName), s -> processFootstepPlanningOutputStatus(s.takeNextData()));
public StereoVisionPointCloudPublisher(FullHumanoidRobotModelFactory modelFactory, Ros2Node ros2Node, String robotConfigurationDataTopicName) { robotName = modelFactory.getRobotDescription().getName(); fullRobotModel = modelFactory.createFullRobotModel(); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, robotConfigurationDataTopicName, s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); pointcloudPublisher = ROS2Tools.createPublisher(ros2Node, StereoVisionPointCloudMessage.class, ROS2Tools.getDefaultTopicNameGenerator(robotName)); }
public void setupCommunication(String robotName, RealtimeRos2Node realtimeRos2Node) { for (RobotSide robotSide : RobotSide.values) { ROS2Tools.createCallbackSubscription(realtimeRos2Node, HandDesiredConfigurationMessage.class, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName), handDesiredConfigurationMessageSubscribers.get(robotSide)); ROS2Tools.createCallbackSubscription(realtimeRos2Node, ValkyrieHandFingerTrajectoryMessage.class, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName), valkyrieHandFingerTrajectoryMessageSubscribers.get(robotSide)); } }
public ValkyrieFingerTrajectoryMessagePublisher(Ros2Node ros2Node, MessageTopicNameGenerator subscriberTopicNameGenerator) { publisher = ROS2Tools.createPublisher(ros2Node, ValkyrieHandFingerTrajectoryMessage.class, subscriberTopicNameGenerator); }
.createPublisher(localNode, FootstepPlanningRequestPacket.class, ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.FOOTSTEP_PLANNER_TOOLBOX, ROS2Tools.ROS2TopicQualifier.INPUT)); localNode.spin();
public TextToSpeechNetworkModule() { ROS2Tools.createCallbackSubscription(ros2Node, TextToSpeechPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), s -> receivedPacket(s.takeNextData())); }
public ToolboxModule(String robotName, FullHumanoidRobotModel fullRobotModelToLog, LogModelProvider modelProvider, boolean startYoVariableServer, int updatePeriodMilliseconds, PubSubImplementation pubSubImplementation) throws IOException { this.robotName = robotName; this.modelProvider = modelProvider; this.startYoVariableServer = startYoVariableServer; this.fullRobotModel = fullRobotModelToLog; realtimeRos2Node = ROS2Tools.createRealtimeRos2Node(pubSubImplementation, "ihmc_" + CaseFormat.UPPER_CAMEL.to(CaseFormat.LOWER_UNDERSCORE, name)); commandInputManager = new CommandInputManager(name, createListOfSupportedCommands()); statusOutputManager = new StatusMessageOutputManager(createListOfSupportedStatus()); controllerNetworkSubscriber = new ControllerNetworkSubscriber(getSubscriberTopicNameGenerator(), commandInputManager, getPublisherTopicNameGenerator(), statusOutputManager, realtimeRos2Node); executorService = Executors.newScheduledThreadPool(1, threadFactory); timeWithoutInputsBeforeGoingToSleep.set(0.5); commandInputManager.registerHasReceivedInputListener(new HasReceivedInputListener() { private final Set<Class<? extends Command<?, ?>>> silentCommands = silentCommands(); @Override public void hasReceivedInput(Class<? extends Command<?, ?>> commandClass) { if (!silentCommands.contains(commandClass)) receivedInput.set(true); } }); controllerNetworkSubscriber.addMessageFilter(createMessageFilter()); ROS2Tools.createCallbackSubscription(realtimeRos2Node, ToolboxStateMessage.class, getSubscriberTopicNameGenerator(), s -> receivedPacket(s.takeNextData())); registerExtraPuSubs(realtimeRos2Node); realtimeRos2Node.spin(); }
private void runPlanObjectivePackets() ROS2Tools.createCallbackSubscription(localNode, FootstepPlannerParametersPacket.class, ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.FOOTSTEP_PLANNER_TOOLBOX, ROS2Tools.ROS2TopicQualifier.INPUT), s -> processFootstepPlannerParametersPacket(s.takeNextData())); ROS2Tools.createCallbackSubscription(localNode, VisibilityGraphsParametersPacket.class, ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.FOOTSTEP_PLANNER_TOOLBOX, ROS2Tools.ROS2TopicQualifier.INPUT), s -> processVisibilityGraphsParametersPacket(s.takeNextData())); localNode.spin();
public GetVideoPacketExampleBehavior(String robotName, Ros2Node ros2Node) { super(robotName, ros2Node); // coactiveBehaviorsNetworkManager = ros2Node; FIXME createSubscriber(VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), videoPacketQueue::put); }
public HandControlThread(RobotSide robotSide) { String nodeName = robotSide.getLowerCaseName() + "_" + CaseFormat.UPPER_CAMEL.to(CaseFormat.LOWER_UNDERSCORE, getClass().getSimpleName()); realtimeRos2Node = ROS2Tools.createRealtimeRos2Node(PubSubImplementation.FAST_RTPS, nodeName); } }
private void setupMocapModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException { if (params.isMocapModuleEnabled()) { MocapPlanarRegionsListManager planarRegionsListManager = new MocapPlanarRegionsListManager(); Ros2Node ros2Node = ROS2Tools.createRos2Node(PubSubImplementation.FAST_RTPS, "ihmc_mocap_localization_node"); ROS2Tools.createCallbackSubscription(ros2Node, PlanarRegionsListMessage.class, REACommunicationProperties.publisherTopicNameGenerator, s -> planarRegionsListManager.receivedPacket(s.takeNextData())); new IHMCMOCAPLocalizationModule(robotModel, planarRegionsListManager); String methodName = "setupMocapModule"; printModuleConnectedDebugStatement(PacketDestination.MOCAP_MODULE, methodName); } }
public AbstractBehavior(String robotName, String namePrefix, Ros2Node ros2Node) { this.robotName = robotName; this.ros2Node = ros2Node; behaviorName = FormattingTools.addPrefixAndKeepCamelCaseForMiddleOfExpression(namePrefix, getClass().getSimpleName()); registry = new YoVariableRegistry(behaviorName); yoBehaviorStatus = new YoEnum<BehaviorStatus>(namePrefix + "Status", registry, BehaviorStatus.class); hasBeenInitialized = new YoBoolean("hasBeenInitialized", registry); isPaused = new YoBoolean("isPaused" + behaviorName, registry); isAborted = new YoBoolean("isAborted" + behaviorName, registry); percentCompleted = new YoDouble("percentCompleted", registry); behaviorsServices = new ArrayList<>(); footstepPlanningToolboxSubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.FOOTSTEP_PLANNER_TOOLBOX, ROS2TopicQualifier.INPUT); footstepPlanningToolboxPubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.FOOTSTEP_PLANNER_TOOLBOX, ROS2TopicQualifier.OUTPUT); kinematicsToolboxSubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_TOOLBOX, ROS2TopicQualifier.INPUT); kinematicsToolboxPubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_TOOLBOX, ROS2TopicQualifier.OUTPUT); kinematicsPlanningToolboxSubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_PLANNING_TOOLBOX, ROS2TopicQualifier.INPUT); kinematicsPlanningToolboxPubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_PLANNING_TOOLBOX, ROS2TopicQualifier.OUTPUT); controllerSubGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); behaviorSubGenerator = IHMCHumanoidBehaviorManager.getSubscriberTopicNameGenerator(robotName); behaviorPubGenerator = IHMCHumanoidBehaviorManager.getPublisherTopicNameGenerator(robotName); textToSpeechPublisher = createPublisher(TextToSpeechPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); }
@Before public void setUp() { registry = new YoVariableRegistry(getClass().getSimpleName()); this.yoTime = new YoDouble("yoTime", registry); this.ros2Node = ROS2Tools.createRos2Node(PubSubImplementation.INTRAPROCESS, "ihmc_humanoid_behavior_dispatcher_test"); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.createSimulation(getSimpleRobotName()); MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(getClass().getSimpleName() + " before test."); robot = drcSimulationTestHelper.getRobot(); fullRobotModel = getRobotModel().createFullRobotModel(); walkingControllerParameters = getRobotModel().getWalkingControllerParameters(); yoGraphicsListRegistry = new YoGraphicsListRegistry(); behaviorDispatcher = setupBehaviorDispatcher(getRobotModel().getSimpleRobotName(), fullRobotModel, ros2Node, yoGraphicsListRegistry, registry); CapturePointUpdatable capturePointUpdatable = createCapturePointUpdateable(drcSimulationTestHelper, registry, yoGraphicsListRegistry); behaviorDispatcher.addUpdatable(capturePointUpdatable); DRCRobotSensorInformation sensorInfo = getRobotModel().getSensorInformation(); for (RobotSide robotSide : RobotSide.values) { WristForceSensorFilteredUpdatable wristSensorUpdatable = new WristForceSensorFilteredUpdatable(drcSimulationTestHelper.getRobotName(), robotSide, fullRobotModel, sensorInfo, robotDataReceiver.getForceSensorDataHolder(), IHMCHumanoidBehaviorManager.BEHAVIOR_YO_VARIABLE_SERVER_DT, drcSimulationTestHelper.getRos2Node(), registry); behaviorDispatcher.addUpdatable(wristSensorUpdatable); } referenceFrames = robotDataReceiver.getReferenceFrames(); }
uiPlanningResultReference = messager.createInput(PlanningResultTopic); ros2Node = ROS2Tools.createRealtimeRos2Node(pubSubImplementation, "ihmc_footstep_planner_test"); .createPublisher(ros2Node, FootstepPlanningRequestPacket.class, FootstepPlannerCommunicationProperties.subscriberTopicNameGenerator(robotName)); footstepPlannerParametersPublisher = ROS2Tools .createPublisher(ros2Node, FootstepPlannerParametersPacket.class, FootstepPlannerCommunicationProperties.subscriberTopicNameGenerator(robotName)); toolboxStatePublisher = ROS2Tools .createPublisher(ros2Node, ToolboxStateMessage.class, FootstepPlannerCommunicationProperties.subscriberTopicNameGenerator(robotName)); ROS2Tools.createCallbackSubscription(ros2Node, FootstepPlanningToolboxOutputStatus.class, FootstepPlannerCommunicationProperties.publisherTopicNameGenerator(robotName), s -> processFootstepPlanningOutputStatus(s.takeNextData()));
public IHMCETHRosLocalizationUpdateSubscriber(String robotName, final RosMainNode rosMainNode, Ros2Node ros2Node, final PPSTimestampOffsetProvider ppsTimeOffsetProvider) ROS2Tools.createCallbackSubscription(ros2Node, LocalizationPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), s -> receivedPacket(s.takeNextData())); localizationPointMapPublisher = ROS2Tools.createPublisher(ros2Node, LocalizationPointMapPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); IHMCROS2Publisher<StampedPosePacket> stampedPosePublisher = ROS2Tools.createPublisher(ros2Node, StampedPosePacket.class, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName)); RosPoseStampedSubscriber rosPoseStampedSubscriber = new RosPoseStampedSubscriber() IHMCROS2Publisher<LocalizationStatusPacket> localizationStatusPublisher = ROS2Tools.createPublisher(ros2Node, LocalizationStatusPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); AbstractRosTopicSubscriber<Float64> overlapSubscriber = new AbstractRosTopicSubscriber<std_msgs.Float64>(std_msgs.Float64._TYPE)
public <T> void createSubscriber(Class<T> messageType, MessageTopicNameGenerator generator, ObjectConsumer<T> consumer) { ROS2Tools.createCallbackSubscription(ros2Node, messageType, generator, s -> consumer.consumeObject(s.takeNextData())); }