public void testFootstepAndPointsFromDataFile() throws NumberFormatException, InsufficientDataException, IOException
{
QuadTreeFootstepSnappingParameters snappingParameters = new AtlasFootstepSnappingParameters();
ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);
double maskSafetyBuffer = 0.01;
double boundingBoxDimension = 0.3;
footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension);
String baseName = "footstepListsForTesting/";
String resourceName = baseName + "DataFromConvexHullSnapper1422988400956.txt";
InputStream resourceAsStream = getClass().getClassLoader().getResourceAsStream(resourceName);
FootstepPointsDataReader dataReader = new FootstepPointsDataReader(resourceAsStream);
FootstepDataMessage footstepData = new FootstepDataMessage();
footstepData.setRobotSide(RobotSide.LEFT.toByte());
FootSpoof spoof = new FootSpoof("basicSpoof");
FramePose2D desiredPose = new FramePose2D(ReferenceFrame.getWorldFrame());
List<Point3D> listOfPoints = new ArrayList<>();
while (dataReader.hasAnotherFootstepAndPoints())
{
listOfPoints = dataReader.getNextSetPointsAndFootstep(footstepData);
desiredPose.setIncludingFrame(ReferenceFrame.getWorldFrame(), footstepData.getLocation().getX(), footstepData.getLocation().getY(),
footstepData.getOrientation().getYaw());
Footstep footstep = footstepSnapper.generateFootstepUsingHeightMap(desiredPose, spoof.getRigidBody(), spoof.getSoleFrame(),
RobotSide.fromByte(footstepData.getRobotSide()), listOfPoints, 0.0);
assertTrue(footstep.getFootstepType() != Footstep.FootstepType.BAD_FOOTSTEP);
}
}