std::unique_ptr<Path> RRTPlanner::run(
MotionInstant start, const MotionCommand* cmd,
const MotionConstraints& motionConstraints,
const Geometry2d::ShapeSet* obstacles, std::unique_ptr<Path> prevPath) {
// This planner only works with commands of type 'PathTarget'
assert(cmd->getCommandType() == Planning::MotionCommand::PathTarget);
Planning::PathTargetCommand target =
*static_cast<const Planning::PathTargetCommand*>(cmd);
MotionInstant goal = target.pathGoal;
// Simple case: no path
if (start.pos == goal.pos) {
InterpolatedPath* path = new InterpolatedPath();
path->setStartTime(RJ::timestamp());
path->waypoints.emplace_back(
MotionInstant(start.pos, Geometry2d::Point()), 0);
return unique_ptr<Path>(path);
}
// Locate a goal point that is obstacle-free
boost::optional<Geometry2d::Point> prevGoal;
if (prevPath) prevGoal = prevPath->end().motion.pos;
goal.pos = EscapeObstaclesPathPlanner::findNonBlockedGoal(
goal.pos, prevGoal, *obstacles);
// Replan if needed, otherwise return the previous path unmodified
if (shouldReplan(start, goal, motionConstraints, obstacles,
prevPath.get())) {
// Run bi-directional RRT to generate a path.
auto points = runRRT(start, goal, motionConstraints, obstacles);
// Optimize out uneccesary waypoints
optimize(points, obstacles, motionConstraints, start.vel, goal.vel);
// Check if Planning or optimization failed
if (points.size() < 2) {
debugLog("PathPlanning Failed");
auto path = make_unique<InterpolatedPath>();
path->waypoints.emplace_back(MotionInstant(start.pos, Point()), 0);
path->waypoints.emplace_back(MotionInstant(start.pos, Point()), 0);
return std::move(path);
}
// Generate and return a cubic bezier path using the waypoints
return generateCubicBezier(points, *obstacles, motionConstraints,
start.vel, goal.vel);
} else {
return prevPath;
}
}
std::unique_ptr<Path> RRTPlanner::run(
MotionInstant start, const MotionCommand* cmd,
const MotionConstraints& motionConstraints,
const Geometry2d::ShapeSet* obstacles, std::unique_ptr<Path> prevPath) {
// This planner only works with commands of type 'PathTarget'
assert(cmd->getCommandType() == Planning::MotionCommand::PathTarget);
Planning::PathTargetCommand target =
*static_cast<const Planning::PathTargetCommand*>(cmd);
MotionInstant goal = target.pathGoal;
// Simple case: no path
if (start.pos == goal.pos) {
InterpolatedPath* path = new InterpolatedPath();
path->setStartTime(RJ::timestamp());
path->waypoints.emplace_back(
MotionInstant(start.pos, Geometry2d::Point()), 0);
return unique_ptr<Path>(path);
}
// Locate a goal point that is obstacle-free
boost::optional<Geometry2d::Point> prevGoal;
if (prevPath) prevGoal = prevPath->end().pos;
goal.pos = EscapeObstaclesPathPlanner::findNonBlockedGoal(
goal.pos, prevGoal, *obstacles);
// Replan if needed, otherwise return the previous path unmodified
if (shouldReplan(start, goal, motionConstraints, obstacles,
prevPath.get())) {
// Run bi-directional RRT to generate a path.
InterpolatedPath* path =
runRRT(start, goal, motionConstraints, obstacles);
// If RRT failed, the path will be empty, so we need to add a single
// point to make it valid.
if (path && path->waypoints.empty()) {
path->waypoints.emplace_back(
MotionInstant(start.pos, Geometry2d::Point()), 0);
}
return unique_ptr<Path>(path);
} else {
return prevPath;
}
}
void PathPlanner::mainMenu(){
cout<<"\n\nWelcome to Eric's Caffeine Induced Path Planner!\n";
bool choiceFlag= true;
string choice;
while(choiceFlag){
cout<<"Please select from the following menu options.\n\n";
cout<<"1: Run Inner Trace Algorithm\n";
cout<<"2: Run a silly Square Spiral\n";
cout<<"3: Run a Rapidly Exploring Random Tree Search Algorithm\n";
cin>>choice;
if(isdigit(choice[0])){
//Really dirty hack to get the int value. Im sorry :(
int option= choice[0] - '0';
switch(option){
case 1:
runTrace();
break;
case 2:
runSquareSpiral();
break;
case 3:
runRRT();
break;
}
}
else if(choice[0] == 'q' || choice[0] =='Q'){
cout<<"\n\nThanks for using Eric's Caffeine Induced Path Planner!\n";
cout<<"Bye\n";
break;
}
else{
cout<<"Please enter a number listed on the options menu!\n";
}
}
}
