private void setupYoRegistries() { yoGraphicsListRegistry = new YoGraphicsListRegistry(); yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(true); yoGraphicsListRegistryForDetachedOverhead = new YoGraphicsListRegistry(); }
yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(false); ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);
private void createGraphicsAndArtifacts(YoGraphicsListRegistry yoGraphicsListRegistry) { yoGraphicsListRegistry.registerYoGraphic("Frames", leftMidZUpFrameViz); yoGraphicsListRegistry.registerYoGraphic("Frames", rightMidZUpFrameViz); yoGraphicsListRegistry.registerYoGraphic("target", targetViz); yoGraphicsListRegistry.registerArtifact("target", targetViz.createArtifact()); yoGraphicsListRegistry.registerArtifact("centroidViz", centroidViz.createArtifact()); yoGraphicsListRegistry.registerYoGraphic("nominalYaw", nominalYawGraphic); YoArtifactPolygon supportPolygonArtifact = new YoArtifactPolygon("quadSupportPolygonArtifact", supportPolygon, Color.blue, false); YoArtifactPolygon currentTriplePolygonArtifact = new YoArtifactPolygon("currentTriplePolygonArtifact", currentTriplePolygon, Color.GREEN, false); yoGraphicsListRegistry.registerArtifact("nominalYawArtifact", nominalYawArtifact); yoGraphicsListRegistry.registerArtifact("supportPolygon", supportPolygonArtifact); yoGraphicsListRegistry.registerArtifact("currentTriplePolygon", currentTriplePolygonArtifact); yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(true); }
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; }
yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(false); ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);
private BehaviorDispatcher setupBehaviorDispatcher(FullRobotModel fullRobotModel, Ros2Node ros2Node, HumanoidRobotDataReceiver robotDataReceiver, YoGraphicsListRegistry yoGraphicsListRegistry) { 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, yoTimeBehaviorDispatcher, robotDataReceiver, desiredBehaviorControlSubscriber, desiredBehaviorSubscriber, ros2Node, yoVariableServer, HumanoidBehaviorType.class, HumanoidBehaviorType.STOP, registry, yoGraphicsListRegistry); ret.addUpdatable(capturePointUpdatable); if (wristForceSensorUpdatables != null) { ret.addUpdatable(wristForceSensorUpdatables.get(RobotSide.LEFT)); ret.addUpdatable(wristForceSensorUpdatables.get(RobotSide.RIGHT)); } ret.finalizeStateMachine(); return ret; }
scs.setRobot(robot); yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(true); yoGraphicsListRegistry.registerYoGraphic("centroid", centroidGraphic); yoGraphicsListRegistry.registerArtifact("copSnapping", centerOfMassViz.createArtifact());