private void setupMultiThreadedRobotController()
{
if (scsInitialSetup.get().getRunMultiThreaded())
{
threadedRobotController = new MultiThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet);
}
else
{
PrintTools.warn(this, "Running simulation in single threaded mode", true);
threadedRobotController = new SingleThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet);
}
int estimatorTicksPerSimulationTick = (int) Math.round(robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT());
int controllerTicksPerSimulationTick = (int) Math.round(robotModel.get().getControllerDT() / robotModel.get().getSimulateDT());
int slowPublisherTicksPerSimulationTick = (int) Math.round(10 * robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT());
threadedRobotController.addController(stateEstimationThread, estimatorTicksPerSimulationTick, false);
threadedRobotController.addController(controllerThread, controllerTicksPerSimulationTick, true);
MultiThreadedRobotControlElement simulatedHandController = robotModel.get().createSimulatedHandController(humanoidFloatingRootJointRobot,
threadDataSynchronizer, realtimeRos2Node.get(),
closeableAndDisposableRegistry);
if (simulatedHandController != null)
{
threadedRobotController.addController(simulatedHandController, controllerTicksPerSimulationTick, false);
}
}