std::unique_ptr<Path> DirectTargetPathPlanner::run(PlanRequest& planRequest) {
const MotionInstant& startInstant = planRequest.start;
const auto& motionConstraints = planRequest.constraints.mot;
const Geometry2d::ShapeSet& obstacles = planRequest.obstacles;
std::unique_ptr<Path>& prevPath = planRequest.prevPath;
const Planning::DirectPathTargetCommand& command =
dynamic_cast<const Planning::DirectPathTargetCommand&>(
*planRequest.motionCommand);
if (shouldReplan(planRequest)) {
Geometry2d::Point endTarget = command.pathGoal.pos;
const auto direction = (endTarget - startInstant.pos).normalized();
float endSpeed = command.pathGoal.vel.mag();
auto path = std::make_unique<TrapezoidalPath>(
startInstant.pos, vectorInDirection(startInstant.vel, direction),
endTarget, vectorInDirection(command.pathGoal.vel, direction),
motionConstraints);
path->setStartTime(RJ::now());
return std::move(path);
} else {
return std::move(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().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;
}
}
