public <T> IHMCROS2Publisher<T> createPublisherForController(Class<T> messageType) { return createPublisher(messageType, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName)); }
public ValkyriePunchMessenger(String robotName, Ros2Node ros2Node) { MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); armTrajectoryPublisher = ROS2Tools.createPublisher(ros2Node, ArmTrajectoryMessage.class, subscriberTopicNameGenerator); highLevelStatePublisher = ROS2Tools.createPublisher(ros2Node, HighLevelStateMessage.class, subscriberTopicNameGenerator); }
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 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 ROSiRobotCommandDispatcher(String robotName, RealtimeRos2Node realtimeRos2Node, String rosHostIP) { ROS2Tools.createCallbackSubscription(realtimeRos2Node, HandDesiredConfigurationMessage.class, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName), handDesiredConfigurationMessageSubscriber); String rosURI = "http://" + rosHostIP + ":11311"; rosHandCommunicator = new ROSiRobotCommunicator(rosURI); try { NodeConfiguration nodeConfiguration = RosTools.createNodeConfiguration(new URI(rosURI)); NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault(); nodeMainExecutor.execute(rosHandCommunicator, nodeConfiguration); } catch (URISyntaxException e) { e.printStackTrace(); } }
@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())); }
@Override public void start(Stage primaryStage) throws Exception { String robotTargetString = getParameters().getNamed().getOrDefault("robotTarget", "REAL_ROBOT"); RobotTarget robotTarget = RobotTarget.valueOf(robotTargetString); PrintTools.info("-------------------------------------------------------------------"); PrintTools.info(" -------- Loading parameters for RobotTarget: " + robotTarget + " -------"); PrintTools.info("-------------------------------------------------------------------"); ValkyrieRobotModel robotModel = new ValkyrieRobotModel(robotTarget, false, "DEFAULT", null, false, true); String robotName = robotModel.getSimpleRobotName(); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); ValkyrieFingerTrajectoryMessagePublisher handFingerTrajectoryMessagePublisher = new ValkyrieFingerTrajectoryMessagePublisher(ros2Node, subscriberTopicNameGenerator); ui = new JoystickBasedGraspingMainUI(robotName, primaryStage, ros2Node, robotModel, handFingerTrajectoryMessagePublisher); }
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()); }
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(); }
externalPelvisPoseSubscriber = new PelvisPoseCorrectionCommunicator(null, null); ROS2Tools.createCallbackSubscription(estimatorRealtimeRos2Node, StampedPosePacket.class, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName), externalPelvisPoseSubscriber);
localizationPointMapPublisher = ROS2Tools.createPublisher(ros2Node, LocalizationPointMapPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); IHMCROS2Publisher<StampedPosePacket> stampedPosePublisher = ROS2Tools.createPublisher(ros2Node, StampedPosePacket.class, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName)); RosPoseStampedSubscriber rosPoseStampedSubscriber = new RosPoseStampedSubscriber()
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()); }
ROS2Tools.MessageTopicNameGenerator controllerSubGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName);
desiredPinkys.put(RobotSide.LEFT, messager.createInput(GraspingJavaFXTopics.LeftPinky, 0.0)); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName);
RequestWristForceSensorCalibrationSubscriber requestWristForceSensorCalibrationSubscriber = new RequestWristForceSensorCalibrationSubscriber(); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); ROS2Tools.createCallbackSubscription(realtimeRos2Node, StateEstimatorModePacket.class, subscriberTopicNameGenerator, subscriber -> stateEstimatorModeSubscriber.receivedPacket(subscriber.takeNextData()));