public <T> void createSubscriberFromController(Class<T> messageType, ObjectConsumer<T> consumer) { createSubscriber(messageType, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), consumer); }
public BehaviorService(String robotName, String name, Ros2Node ros2Node) { this.robotName = robotName; this.ros2Node = ros2Node; registry = new YoVariableRegistry(name); controllerSubGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); }
public ZeroPoseMockRobotConfigurationDataPublisherModule(final DRCRobotModel robotModel) { fullRobotModel = robotModel.createFullRobotModel(); forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions(); publisher = ROS2Tools.createPublisher(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotModel.getSimpleRobotName())); Thread t = new Thread(this); t.start(); }
@Override public void registerExtraPuSubs(RealtimeRos2Node realtimeRos2Node) { MessageTopicNameGenerator controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); ROS2Tools.createCallbackSubscription(realtimeRos2Node, RobotConfigurationData.class, controllerPubGenerator, s -> { if (wholeBodyTrajectoryToolboxController != null) wholeBodyTrajectoryToolboxController.updateRobotConfigurationData(s.takeNextData()); }); } }
public ValkyrieHandStateCommunicator(String robotName, FullHumanoidRobotModel fullRobotModel, ValkyrieHandModel handModel, RealtimeRos2Node realtimeRos2Node) { publisher = ROS2Tools.createPublisher(realtimeRos2Node, HandJointAnglePacket.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName)); for (RobotSide robotside : RobotSide.values) { for (ValkyrieHandJointName jointEnum : ValkyrieHandJointName.values) { OneDoFJointBasics joint = fullRobotModel.getOneDoFJointByName(jointEnum.getJointName(robotside)); handJoints.get(robotside).put(jointEnum, joint); } } packet = HumanoidMessageTools.createHandJointAnglePacket(null, false, false, new double[ValkyrieHandJointName.values.length]); }
@Override public MultiThreadedRobotControlElement createSimulatedHandController(FloatingRootJointRobot simulatedRobot, ThreadDataSynchronizerInterface threadDataSynchronizer, RealtimeRos2Node realtimeRos2Node, CloseableAndDisposableRegistry closeableAndDisposableRegistry) { if (!ValkyrieConfigurationRoot.VALKYRIE_WITH_ARMS) return null; else return new SimulatedValkyrieFingerController(simulatedRobot, threadDataSynchronizer, realtimeRos2Node, closeableAndDisposableRegistry, this, ControllerAPIDefinition.getPublisherTopicNameGenerator(getSimpleRobotName()), ControllerAPIDefinition.getSubscriberTopicNameGenerator(getSimpleRobotName())); }
private CapturePointUpdatable createCapturePointUpdateable(YoGraphicsListRegistry yoGraphicsListRegistry) { CapturabilityBasedStatusSubscriber capturabilityBasedStatusSubsrciber = new CapturabilityBasedStatusSubscriber(); ROS2Tools.createCallbackSubscription(ros2Node, CapturabilityBasedStatus.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), s -> capturabilityBasedStatusSubsrciber.receivedPacket(s.takeNextData())); CapturePointUpdatable ret = new CapturePointUpdatable(capturabilityBasedStatusSubsrciber, yoGraphicsListRegistry, registry); return ret; }
@Override public void registerExtraPuSubs(RealtimeRos2Node realtimeRos2Node) { MessageTopicNameGenerator controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); ROS2Tools.createCallbackSubscription(realtimeRos2Node, RobotConfigurationData.class, controllerPubGenerator, s -> controller.receivedPacket(s.takeNextData())); ROS2Tools.createCallbackSubscription(realtimeRos2Node, CapturabilityBasedStatus.class, controllerPubGenerator, s -> controller.receivedPacket(s.takeNextData())); }
ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), s -> javaFXRobotVisualizer.submitNewConfiguration(s.takeNextData())); view3dFactory.addNodeToView(javaFXRobotVisualizer.getRootNode());
@Override public void registerExtraPuSubs(RealtimeRos2Node realtimeRos2Node) { MessageTopicNameGenerator controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); ROS2Tools.createCallbackSubscription(realtimeRos2Node, RobotConfigurationData.class, controllerPubGenerator, s -> { if (kinematicsToolBoxController != null) kinematicsToolBoxController.updateRobotConfigurationData(s.takeNextData()); }); ROS2Tools.createCallbackSubscription(realtimeRos2Node, CapturabilityBasedStatus.class, controllerPubGenerator, s -> { if (kinematicsToolBoxController != null) kinematicsToolBoxController.updateCapturabilityBasedStatus(s.takeNextData()); }); }
@Override public void registerExtraPuSubs(RealtimeRos2Node realtimeRos2Node) { MessageTopicNameGenerator controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); ROS2Tools.createCallbackSubscription(realtimeRos2Node, RobotConfigurationData.class, controllerPubGenerator, s -> { if (kinematicsPlanningToolboxController != null) kinematicsPlanningToolboxController.updateRobotConfigurationData(s.takeNextData()); }); ROS2Tools.createCallbackSubscription(realtimeRos2Node, CapturabilityBasedStatus.class, controllerPubGenerator, s -> { if (kinematicsPlanningToolboxController != null) kinematicsPlanningToolboxController.updateCapturabilityBasedStatus(s.takeNextData()); }); }
public ValkyrieSensorSuiteManager(String robotName, FullHumanoidRobotModelFactory fullRobotModelFactory, CollisionBoxProvider collisionBoxProvider, DRCROSPPSTimestampOffsetProvider ppsTimestampOffsetProvider, DRCRobotSensorInformation sensorInformation, ValkyrieJointMap jointMap, RobotTarget target) { this.robotName = robotName; this.ppsTimestampOffsetProvider = ppsTimestampOffsetProvider; this.fullRobotModelFactory = fullRobotModelFactory; this.sensorInformation = sensorInformation; DRCRobotLidarParameters multisenseLidarParameters = sensorInformation.getLidarParameters(ValkyrieSensorInformation.MULTISENSE_LIDAR_ID); String sensorName = multisenseLidarParameters.getSensorNameInSdf(); String rcdTopicName = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName).generateTopicName(RobotConfigurationData.class); lidarScanPublisher = new LidarScanPublisher(sensorName, fullRobotModelFactory, ros2Node, rcdTopicName); lidarScanPublisher.setPPSTimestampOffsetProvider(ppsTimestampOffsetProvider); lidarScanPublisher.setCollisionBoxProvider(collisionBoxProvider); if (ENABLE_STEREO_PUBLISHER) { stereoVisionPointCloudPublisher = new StereoVisionPointCloudPublisher(fullRobotModelFactory, ros2Node, rcdTopicName); stereoVisionPointCloudPublisher.setPPSTimestampOffsetProvider(ppsTimestampOffsetProvider); } else { stereoVisionPointCloudPublisher = null; } }
public RobotiqControlThread(String robotName, RobotSide robotSide, CloseableAndDisposableRegistry closeableAndDisposableRegistry) { super(robotSide); this.robotSide = robotSide; robotiqHand = new RobotiqHandCommunicator(robotSide); handDesiredConfigurationMessageSubscriber = new HandDesiredConfigurationMessageSubscriber(robotSide); manualHandControlProvider = new ManualHandControlProvider(robotSide); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); IHMCRealtimeROS2Publisher<HandJointAnglePacket> handJointAnglePublisher = ROS2Tools.createPublisher(realtimeRos2Node, HandJointAnglePacket.class, publisherTopicNameGenerator); jointAngleCommunicator = new HandJointAngleCommunicator(robotSide, handJointAnglePublisher, closeableAndDisposableRegistry); ROS2Tools.createCallbackSubscription(realtimeRos2Node, HandDesiredConfigurationMessage.class, subscriberTopicNameGenerator, handDesiredConfigurationMessageSubscriber); ROS2Tools.createCallbackSubscription(realtimeRos2Node, ManualHandControlPacket.class, subscriberTopicNameGenerator, manualHandControlProvider); realtimeRos2Node.spin(); }
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()); }
@Override public void initializeSimulatedSensors(ObjectCommunicator scsSensorsCommunicator) { ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); DRCRobotCameraParameters multisenseLeftEyeCameraParameters = sensorInformation.getCameraParameters(ValkyrieSensorInformation.MULTISENSE_SL_LEFT_CAMERA_ID); CameraDataReceiver cameraDataReceiver = new SCSCameraDataReceiver(multisenseLeftEyeCameraParameters.getRobotSide(), fullRobotModelFactory, multisenseLeftEyeCameraParameters.getSensorNameInSdf(), robotConfigurationDataBuffer, scsSensorsCommunicator, ros2Node, ppsTimestampOffsetProvider); cameraDataReceiver.start(); lidarScanPublisher.receiveLidarFromSCS(scsSensorsCommunicator); lidarScanPublisher.setScanFrameToLidarSensorFrame(); lidarScanPublisher.start(); }
robotName = robotModel.getSimpleRobotName().toLowerCase(); String rosTopicPrefix = "/ihmc_ros/" + robotName; String rcdTopicName = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotModel.getSimpleRobotName()) .generateTopicName(RobotConfigurationData.class); ppsTimestampOffsetProvider.attachToRosMainNode(rosMainNode); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotModel.getSimpleRobotName()), s -> ppsTimestampOffsetProvider.receivedPacket(s.takeNextData()));
private BehaviorDispatcher<HumanoidBehaviorType> setupBehaviorDispatcher(String robotName, FullHumanoidRobotModel fullRobotModel, Ros2Node ros2Node, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry registry) { ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), s -> robotDataReceiver.receivedPacket(s.takeNextData())); BehaviorControlModeSubscriber desiredBehaviorControlSubscriber = new BehaviorControlModeSubscriber(); ROS2Tools.createCallbackSubscription(ros2Node, BehaviorControlModePacket.class, IHMCHumanoidBehaviorManager.getSubscriberTopicNameGenerator(robotName), s -> desiredBehaviorControlSubscriber.receivedPacket(s.takeNextData())); HumanoidBehaviorTypeSubscriber desiredBehaviorSubscriber = new HumanoidBehaviorTypeSubscriber(); ROS2Tools.createCallbackSubscription(ros2Node, HumanoidBehaviorTypePacket.class, IHMCHumanoidBehaviorManager.getSubscriberTopicNameGenerator(robotName), s -> desiredBehaviorSubscriber.receivedPacket(s.takeNextData())); YoVariableServer yoVariableServer = null; yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(false); BehaviorDispatcher<HumanoidBehaviorType> ret = new BehaviorDispatcher<>(robotName, yoTime, robotDataReceiver, desiredBehaviorControlSubscriber, desiredBehaviorSubscriber, ros2Node, yoVariableServer, HumanoidBehaviorType.class, HumanoidBehaviorType.STOP, registry, yoGraphicsListRegistry); return ret; }
@Override public void initializePhysicalSensors(URI sensorURI) { if (sensorURI == null) { throw new IllegalArgumentException("The ros uri was null, val's physical sensors require a ros uri to be set! Check your Network Parameters.ini file"); } ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); RosMainNode rosMainNode = new RosMainNode(sensorURI, "darpaRoboticsChallange/networkProcessor"); DRCRobotCameraParameters multisenseLeftEyeCameraParameters = sensorInformation.getCameraParameters(ValkyrieSensorInformation.MULTISENSE_SL_LEFT_CAMERA_ID); DRCRobotLidarParameters multisenseLidarParameters = sensorInformation.getLidarParameters(ValkyrieSensorInformation.MULTISENSE_LIDAR_ID); DRCRobotPointCloudParameters multisenseStereoParameters = sensorInformation.getPointCloudParameters(ValkyrieSensorInformation.MULTISENSE_STEREO_ID); boolean shouldUseRosParameterSetters = sensorInformation.setupROSParameterSetters(); MultiSenseSensorManager multiSenseSensorManager = new MultiSenseSensorManager(fullRobotModelFactory, robotConfigurationDataBuffer, rosMainNode, ros2Node, ppsTimestampOffsetProvider, multisenseLeftEyeCameraParameters, multisenseLidarParameters, multisenseStereoParameters, shouldUseRosParameterSetters); lidarScanPublisher.receiveLidarFromROS(multisenseLidarParameters.getRosTopic(), rosMainNode); lidarScanPublisher.setScanFrameToWorldFrame(); lidarScanPublisher.start(); if (ENABLE_STEREO_PUBLISHER) { stereoVisionPointCloudPublisher.receiveStereoPointCloudFromROS(multisenseStereoParameters.getRosTopic(), rosMainNode); stereoVisionPointCloudPublisher.start(); } multiSenseSensorManager.initializeParameterListeners(); ppsTimestampOffsetProvider.attachToRosMainNode(rosMainNode); rosMainNode.execute(); }
private void setupStateEstimationThread() { String robotName = robotModel.get().getSimpleRobotName(); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator; if (externalPelvisCorrectorSubscriber.hasValue()) { pelvisPoseCorrectionCommunicator = externalPelvisCorrectorSubscriber.get(); } else { pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeRos2Node.get(), publisherTopicNameGenerator); ROS2Tools.createCallbackSubscription(realtimeRos2Node.get(), StampedPosePacket.class, subscriberTopicNameGenerator, s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData())); } stateEstimationThread = new DRCEstimatorThread(robotName, robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(), robotModel.get(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node.get(), pelvisPoseCorrectionCommunicator, simulationOutputWriter, yoVariableServer, gravity.get()); }
RobotMotionStatusHolder robotMotionStatusFromController = new RobotMotionStatusHolder(); DRCRobotSensorInformation sensorInformation = drcRobotModel.getSensorInformation(); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(drcRobotModel.getSimpleRobotName()); final DRCPoseCommunicator poseCommunicator = new DRCPoseCommunicator(humanoidFullRobotModel, jointConfigurationGatherer, auxiliaryRobotDataProvider, publisherTopicNameGenerator, realtimeRos2Node, sensorOutputMapReadOnly,