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 MomentumBasedControllerFactory(ContactableBodiesFactory contactableBodiesFactory, SideDependentList<String> footForceSensorNames, SideDependentList<String> footContactSensorNames, SideDependentList<String> wristSensorNames, WalkingControllerParameters walkingControllerParameters, ArmControllerParameters armControllerParameters, CapturePointPlannerParameters capturePointPlannerParameters, HighLevelState initialBehavior) { this.footSensorNames = footForceSensorNames; this.footContactSensorNames = footContactSensorNames; this.wristSensorNames = wristSensorNames; this.contactableBodiesFactory = contactableBodiesFactory; this.initialBehavior = initialBehavior; this.walkingControllerParameters = walkingControllerParameters; this.capturePointPlannerParameters = capturePointPlannerParameters; commandInputManager = new CommandInputManager(ControllerAPIDefinition.getControllerSupportedCommands()); statusOutputManager = new StatusMessageOutputManager(ControllerAPIDefinition.getControllerSupportedStatusMessages()); managerFactory = new HighLevelControlManagerFactory(statusOutputManager, registry); managerFactory.setArmControlParameters(armControllerParameters); managerFactory.setCapturePointPlannerParameters(capturePointPlannerParameters); managerFactory.setWalkingControllerParameters(walkingControllerParameters); }
public <T> void createSubscriberFromController(Class<T> messageType, ObjectConsumer<T> consumer) { createSubscriber(messageType, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), consumer); }
public <T> IHMCROS2Publisher<T> createPublisherForController(Class<T> messageType) { return createPublisher(messageType, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName)); }
List<Class<? extends Command<?, ?>>> controllerSupportedCommands = ControllerAPIDefinition.getControllerSupportedCommands(); for(int i = 0; i < controllerSupportedCommands.size(); i++)
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(); }
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 DRCSimulationTestHelper(SimulationTestingParameters simulationTestParameters, DRCRobotModel robotModel, CommonAvatarEnvironmentInterface testEnvironment) { this.robotModel = robotModel; this.walkingControlParameters = robotModel.getWalkingControllerParameters(); this.simulationTestingParameters = simulationTestParameters; robotName = robotModel.getSimpleRobotName(); if (testEnvironment != null) this.testEnvironment = testEnvironment; simulationStarter = new DRCSimulationStarter(robotModel, this.testEnvironment); fullRobotModel = robotModel.createFullRobotModel(); HumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullRobotModel); scriptedFootstepGenerator = new ScriptedFootstepGenerator(referenceFrames, fullRobotModel, walkingControlParameters); guiInitialSetup = new DRCGuiInitialSetup(false, false, simulationTestingParameters); networkProcessorParameters.enableNetworkProcessor(false); networkProcessorParameters.enableLocalControllerCommunicator(true); List<Class<? extends Command<?, ?>>> controllerSupportedCommands = ControllerAPIDefinition.getControllerSupportedCommands(); for (Class<? extends Command<?, ?>> command : controllerSupportedCommands) { Class<?> messageClass = ROS2Tools.newMessageInstance(command).getMessageClass(); IHMCROS2Publisher<?> defaultPublisher = createPublisherForController(messageClass); defaultControllerPublishers.put(messageClass, defaultPublisher); } defaultControllerPublishers.put(WholeBodyTrajectoryMessage.class, createPublisherForController(WholeBodyTrajectoryMessage.class)); defaultControllerPublishers.put(MessageCollection.class, createPublisherForController(MessageCollection.class)); defaultControllerPublishers.put(ValkyrieHandFingerTrajectoryMessage.class, createPublisherForController(ValkyrieHandFingerTrajectoryMessage.class)); }
@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 registerExtraPuSubs(RealtimeRos2Node realtimeRos2Node) { MessageTopicNameGenerator controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); ROS2Tools.createCallbackSubscription(realtimeRos2Node, RobotConfigurationData.class, controllerPubGenerator, s -> { if (wholeBodyTrajectoryToolboxController != null) wholeBodyTrajectoryToolboxController.updateRobotConfigurationData(s.takeNextData()); }); } }
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)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConstructor() { List<Class<? extends Command<?, ?>>> controllerSupportedCommands = ControllerAPIDefinition.getControllerSupportedCommands(); CommandInputManager commandInputManager = new CommandInputManager(controllerSupportedCommands); YoDouble yoTime = new YoDouble("yoTime", null); CommandConsumerWithDelayBuffers commandConsumer = new CommandConsumerWithDelayBuffers(commandInputManager, yoTime); assertNotNull(commandConsumer); for(Class clazz: controllerSupportedCommands) { assertFalse(commandConsumer.isNewCommandAvailable(clazz)); assertNull(commandConsumer.pollNewestCommand(clazz)); assertEquals(0, commandConsumer.pollNewCommands(clazz).size()); } }
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 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]); }
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(); } }
List<Class<? extends Command<?, ?>>> controllerSupportedCommands = ControllerAPIDefinition.getControllerSupportedCommands(); CommandInputManager commandInputManager = new CommandInputManager(controllerSupportedCommands); YoDouble yoTime = new YoDouble("yoTime", null);
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()); }
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 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); }
List<Class<? extends Command<?, ?>>> controllerSupportedCommands = ControllerAPIDefinition.getControllerSupportedCommands(); CommandInputManager commandInputManager = new CommandInputManager(controllerSupportedCommands); YoDouble yoTime = new YoDouble("yoTime", null);