private void checkTimings(List<FootstepTiming> upcomingFootstepTimings) { if (upcomingFootstepTimings.isEmpty()) return; boolean timingsValid = upcomingFootstepTimings.get(0).hasAbsoluteTime(); boolean atLeastOneFootstepHadTiming = upcomingFootstepTimings.get(0).hasAbsoluteTime(); double lastTime = upcomingFootstepTimings.get(0).getSwingStartTime(); timingsValid = timingsValid && lastTime > 0.0; for (int footstepIdx = 1; footstepIdx < upcomingFootstepTimings.size(); footstepIdx++) { FootstepTiming footstep = upcomingFootstepTimings.get(footstepIdx); boolean timeIncreasing = footstep.getSwingStartTime() > lastTime; timingsValid = timingsValid && footstep.hasAbsoluteTime() && timeIncreasing; atLeastOneFootstepHadTiming = atLeastOneFootstepHadTiming || footstep.hasAbsoluteTime(); lastTime = footstep.getSwingStartTime(); if (!timingsValid) break; } if (atLeastOneFootstepHadTiming && !timingsValid) { PrintTools.warn("Recieved footstep data with invalid timings. Using swing and transfer times instead."); for (int footstepIdx = 1; footstepIdx < upcomingFootstepTimings.size(); footstepIdx++) upcomingFootstepTimings.get(footstepIdx).removeAbsoluteTime(); } } }
private FootstepTiming createFootstepTiming(FootstepDataCommand footstep) { FootstepTiming timing = new FootstepTiming(); if (footstep.hasTimings()) timing.setTimings(footstep.getSwingTime(), footstep.getTransferTime()); else { if (upcomingFootstepTimings.isEmpty()) timing.setTimings(defaultSwingTime.getDoubleValue(), defaultInitialTransferTime.getDoubleValue()); else timing.setTimings(defaultSwingTime.getDoubleValue(), defaultTransferTime.getDoubleValue()); } if (footstep.hasAbsoluteTime()) timing.setAbsoluteTime(timing.getSwingStartTime()); return timing; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConstructor() { FootstepData testObject = new FootstepData(); assertTrue(testObject != null); Footstep testFootstep = new Footstep(RobotSide.LEFT); FootstepTiming testFootstepTiming = new FootstepTiming(0.2, 0.1); testFootstepTiming.setAbsoluteTime(0.1, 0.5); testObject.set(testFootstep, testFootstepTiming); assertTrue(testObject.getFootstep() == testFootstep); assertTrue(testObject.getStepTime() == testFootstepTiming.getStepTime()); assertTrue(testObject.getSwingSide() == testFootstep.getRobotSide()); assertTrue(testObject.getSupportSide() == testFootstep.getRobotSide().getOppositeSide()); assertTrue(testObject.getSwingTime() == testFootstepTiming.getSwingTime()); assertTrue(testObject.getTransferTime() == testFootstepTiming.getTransferTime()); assertTrue(MathTools.epsilonEquals(testObject.getTransferStartTime(), testFootstepTiming.getExecutionStartTime(), Epsilons.ONE_BILLIONTH)); assertTrue(testObject.getSwingStartTime() == testFootstepTiming.getSwingStartTime()); assertTrue(testObject.getFootstepPoseReferenceFrame() == testFootstep.getFootstepPose().getReferenceFrame()); assertTrue(testObject.getFramePose() == testFootstep.getFootstepPose()); testObject.setSwingTime(0.8); assertTrue(testObject.getSwingTime() == 0.8); Footstep newFootstep = new Footstep(RobotSide.LEFT); testObject.setFootstep(newFootstep); assertTrue(testObject.getFootstep() == newFootstep); }