@Before
public void setUp()
{
registry = new YoVariableRegistry(getClass().getSimpleName());
this.yoTime = new YoDouble("yoTime", registry);
this.ros2Node = ROS2Tools.createRos2Node(PubSubImplementation.INTRAPROCESS, "ihmc_humanoid_behavior_dispatcher_test");
drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel());
drcSimulationTestHelper.createSimulation(getSimpleRobotName());
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(getClass().getSimpleName() + " before test.");
robot = drcSimulationTestHelper.getRobot();
fullRobotModel = getRobotModel().createFullRobotModel();
walkingControllerParameters = getRobotModel().getWalkingControllerParameters();
yoGraphicsListRegistry = new YoGraphicsListRegistry();
behaviorDispatcher = setupBehaviorDispatcher(getRobotModel().getSimpleRobotName(), fullRobotModel, ros2Node, yoGraphicsListRegistry, registry);
CapturePointUpdatable capturePointUpdatable = createCapturePointUpdateable(drcSimulationTestHelper, registry, yoGraphicsListRegistry);
behaviorDispatcher.addUpdatable(capturePointUpdatable);
DRCRobotSensorInformation sensorInfo = getRobotModel().getSensorInformation();
for (RobotSide robotSide : RobotSide.values)
{
WristForceSensorFilteredUpdatable wristSensorUpdatable = new WristForceSensorFilteredUpdatable(drcSimulationTestHelper.getRobotName(), robotSide,
fullRobotModel, sensorInfo,
robotDataReceiver.getForceSensorDataHolder(),
IHMCHumanoidBehaviorManager.BEHAVIOR_YO_VARIABLE_SERVER_DT,
drcSimulationTestHelper.getRos2Node(), registry);
behaviorDispatcher.addUpdatable(wristSensorUpdatable);
}
referenceFrames = robotDataReceiver.getReferenceFrames();
}