@Override
public ArrayList<FootstepData> getStepsAlongPath(RobotSide firstStepSide) {
if (overheadPathList.isEmpty()) {
throw new RuntimeException(this.getClass().getSimpleName() + ": No Path for generator");
}
// if (!hasHeightMap())
// {
// TODO: @Agraber do simple planning assuming flat ground at the initial footstep height
// throw new RuntimeException(this.getClass().getSimpleName() + ": No HeightMap");
// }
ArrayList<FootstepData> generatedSteps = new ArrayList<>();
RobotSide lastStepSide = firstStepSide.getOppositeSide();
SideDependentList<FootstepData> lastFootsteps = initialFeet;
for (FootstepOverheadPath overheadPath : overheadPathList) {
// for each segment, generate a path to the end, then use that as the initial for the next
super.initialize(lastFootsteps, overheadPath);
List<FootstepData> segmentFootsteps = super.getStepsAlongPath(lastStepSide.getOppositeSide());
for (FootstepData footstep : segmentFootsteps) {
generatedSteps.add(footstep);
lastStepSide = footstep.getRobotSide();
lastFootsteps.put(lastStepSide, footstep);
}
// check that the segment reached the goal
FramePose2d segmentGoal = overheadPath.getPoseAtDistance(overheadPath.getTotalDistance());
if (!isGoalFootstep(lastFootsteps.get(lastStepSide), segmentGoal, horizontalDistance)) {
break;
}
}
return generatedSteps;
}
@Override
public void initialize(
SideDependentList<FootstepData> originalFeet, FootstepOverheadPath overheadPath) {
overheadPathList.clear();
overheadPathList.add(overheadPath);
initialFeet = originalFeet;
super.initialize(originalFeet, overheadPath);
}