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;
}