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; } }