void initializePathPlanner(){
//initializeSegments(filename);
planner = PathPlanner(segments);
planner.populateTrajectory();
StateTrajectory* traj = planner.getPathTrajectory();
for (int i = 0; i < traj->size(); i++){
cout << traj->at(i).toString() << endl;
}
}
void getInputSegments(){
segments.push_back(Line(20, 55, 60, 55));
segments.push_back(Line(20, 5, 20, 55)); //TODO: real input
segments.push_back(Line(60, 5, 20, 5));
segments.push_back(Line(60, 55, 60, 5));
//segments.push_back(Line(20, 5, 40, 63));
planner = PathPlanner(segments);
planner.populateTrajectory();
StateTrajectory* traj = planner.getPathTrajectory();
for (int i = 0; i < traj->size(); i++){
cout << traj->at(i).toString() << endl;
}
}
biped* searchTrajectory(){
stateTraj = planner.getPathTrajectory();
RobotSegment curstate = stateTraj->back();
for(int i = stateTraj->size() - 2; i >= 0; i --){
RobotSegment goalstate = stateTraj->at(i);
checker = new BipedChecker(&grid, goalstate.robot_pos[0],
goalstate.robot_pos[1], inflate_h, inflate_z);
//hardcoded r? TODO: fix
biped* output=helper.search(curstate.robot_pos[0], curstate.robot_pos[1],
curstate.theta, goalstate.robot_pos[0],
goalstate.robot_pos[1], 3, goalstate.theta,
checker, maxDepth, viewDepth);
bipedTrajectory.push_back(output);
curstate = goalstate;
//stateTraj->pop_back();
}
}
//Function to search a trajectory of states
void searchTrajectory(){
stateTraj = planner.getPathTrajectory();
//RobotSegment curstate = RobotSegment(init.x(), init.y(), initTheta, Line(0, 0, 0, 0));
RobotSegment curstate = stateTraj->back();
//stateTraj->pop_back();
for(int i = stateTraj->size() - 2; i >= 0; i --){
RobotSegment goalstate = stateTraj->at(i);
checker = new BipedChecker(&grid, goalstate.robot_pos[0],
goalstate.robot_pos[1], inflate_h, inflate_z);
cout << "Moving from " << curstate.toString() << " to " << goalstate.toString() <<endl ;
biped* output = helper.search(curstate.robot_pos[0], curstate.robot_pos[1],
curstate.theta, goalstate.robot_pos[0],
goalstate.robot_pos[1], 3, goalstate.theta,
checker, maxDepth, viewDepth); //hardcoded r? TODO: fix
bipedTrajectory.push_back(output);
curstate = goalstate;
//stateTraj->pop_back();
}
}