@Before
public void setUp() {
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " before test.");
behaviorCommunicatorServer =
PacketCommunicator.createIntraprocessPacketCommunicator(
NetworkPorts.BEHAVIOUR_MODULE_PORT, new IHMCCommunicationKryoNetClassList());
behaviorCommunicatorClient =
PacketCommunicator.createIntraprocessPacketCommunicator(
NetworkPorts.BEHAVIOUR_MODULE_PORT, new IHMCCommunicationKryoNetClassList());
try {
behaviorCommunicatorClient.connect();
behaviorCommunicatorServer.connect();
} catch (IOException e) {
throw new RuntimeException(e);
}
drcSimulationTestHelper =
new DRCSimulationTestHelper(
testEnvironment,
getSimpleRobotName(),
DRCObstacleCourseStartingLocation.DEFAULT,
simulationTestingParameters,
getRobotModel());
Robot robotToTest = drcSimulationTestHelper.getRobot();
yoTime = robotToTest.getYoTime();
robot = drcSimulationTestHelper.getRobot();
fullRobotModel = getRobotModel().createFullRobotModel();
forceSensorDataHolder =
new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions()));
robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);
drcSimulationTestHelper.attachListener(RobotConfigurationData.class, robotDataReceiver);
PacketRouter<PacketDestination> networkProcessor = new PacketRouter<>(PacketDestination.class);
networkProcessor.attachPacketCommunicator(
PacketDestination.CONTROLLER, drcSimulationTestHelper.getControllerCommunicator());
networkProcessor.attachPacketCommunicator(
PacketDestination.BEHAVIOR_MODULE, behaviorCommunicatorClient);
communicationBridge =
new BehaviorCommunicationBridge(
behaviorCommunicatorServer, robotToTest.getRobotsYoVariableRegistry());
}
private boolean executeBehavior(final AbstractBehavior behavior, double trajectoryTime)
throws SimulationExceededMaximumTimeException {
final double simulationRunTime = trajectoryTime + EXTRA_SIM_TIME_FOR_SETTLING;
if (DEBUG) {
System.out.println("\n");
PrintTools.debug(
this, "starting behavior: " + behavior.getName() + " t = " + yoTime.getDoubleValue());
}
Thread behaviorThread =
new Thread() {
public void run() {
{
double startTime = Double.NaN;
boolean simStillRunning = true;
boolean initalized = false;
while (simStillRunning) {
if (!initalized) {
startTime = yoTime.getDoubleValue();
initalized = true;
}
double timeSpentSimulating = yoTime.getDoubleValue() - startTime;
simStillRunning = timeSpentSimulating < simulationRunTime;
robotDataReceiver.updateRobotModel();
behavior.doControl();
}
}
}
};
behaviorThread.start();
boolean ret = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(simulationRunTime);
PrintTools.debug(
this, "done with behavior: " + behavior.getName() + " t = " + yoTime.getDoubleValue());
return ret;
}
@ContinuousIntegrationTest(
estimatedDuration = 50.0,
categoriesOverride = IntegrationCategory.EXCLUDE)
@Test(timeout = 300000)
public void testWalkForwardsX() throws SimulationExceededMaximumTimeException {
BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0);
assertTrue(success);
double walkDistance = RandomTools.generateRandomDouble(new Random(), 1.0, 2.0);
Vector2d walkDirection = new Vector2d(1, 0);
double trajectoryTime = walkDistance / ASSUMED_WALKING_SPEED_mPerSec;
WalkToGoalBehavior walkToGoalBehavior =
testWalkToGoalBehavior(walkDistance, walkDirection, trajectoryTime);
assertTrue(walkToGoalBehavior.isDone());
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
@Ignore("Needs to be improved")
@ContinuousIntegrationTest(estimatedDuration = 67.1)
@Test(timeout = 340000)
public void testMultiStepForwardAndContinueWalking()
throws SimulationExceededMaximumTimeException, InterruptedException,
ControllerFailureException {
BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
setupTest(getRobotModel());
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
setForwardPushParameters();
Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
BooleanYoVariable walk =
(BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");
// disable walking
walk.set(false);
blockingSimulationRunner.simulateAndBlock(4.0);
// push the robot
pushRobotController.applyForceDelayed(
pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);
// simulate for a little while longer
blockingSimulationRunner.simulateAndBlock(forceDuration + 6.0);
// re-enable walking
walk.set(true);
blockingSimulationRunner.simulateAndBlock(6.0);
Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
Vector3d plusMinusVector =
new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
BoundingBox3d boundingBox =
BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());
createVideo(scs);
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
@After
public void destroySimulationAndRecycleMemory() {
if (simulationTestingParameters.getKeepSCSUp()) {
ThreadTools.sleepForever();
}
// Do this here in case a test fails. That way the memory will be recycled.
if (drcSimulationTestHelper != null) {
drcSimulationTestHelper.destroySimulation();
drcSimulationTestHelper = null;
}
if (behaviorCommunicatorClient != null) {
behaviorCommunicatorClient.close();
}
if (behaviorCommunicatorServer != null) {
behaviorCommunicatorServer.close();
}
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " after test.");
}
