float getTime(InterpolatedPath& path, int index, const MotionConstraints& motionConstraints, float startSpeed, float endSpeed) { return Trapezoidal::getTime( path.length(0, index), path.length(), motionConstraints.maxSpeed, motionConstraints.maxAcceleration, startSpeed, endSpeed); }
TEST(InterpolatedPath, evaluate) { Point p0(1, 1), p1(1, 2), p2(2, 2); InterpolatedPath path; path.waypoints.emplace_back(MotionInstant(p0, Point()), 0s); path.waypoints.emplace_back(MotionInstant(p1, p1), 3s); path.waypoints.emplace_back(MotionInstant(p2, Point()), 6s); // path should be invalid and at end state when t > duration auto out = path.evaluate(1000s); ASSERT_FALSE(out); }
InterpolatedPath* RRTPlanner::runRRT(MotionInstant start, MotionInstant goal, const MotionConstraints& motionConstraints, const Geometry2d::ShapeSet* obstacles) { InterpolatedPath* path = new InterpolatedPath(); path->setStartTime(RJ::timestamp()); // Initialize two RRT trees FixedStepTree startTree; FixedStepTree goalTree; startTree.init(start.pos, obstacles); goalTree.init(goal.pos, obstacles); startTree.step = goalTree.step = .15f; // Run bi-directional RRT algorithm Tree* ta = &startTree; Tree* tb = &goalTree; for (unsigned int i = 0; i < _maxIterations; ++i) { Geometry2d::Point r = RandomFieldLocation(); Tree::Point* newPoint = ta->extend(r); if (newPoint) { // try to connect the other tree to this point if (tb->connect(newPoint->pos)) { // trees connected // done with global path finding // the path is from start to goal // runRRT will handle the rest break; } } swap(ta, tb); } Tree::Point* p0 = startTree.last(); Tree::Point* p1 = goalTree.last(); // sanity check if (!p0 || !p1 || p0->pos != p1->pos) { return path; } // extract path from RRTs // add the start tree first...normal order (aka from root to p0) startTree.addPath(*path, p0); // add the goal tree in reverse (aka p1 to root) goalTree.addPath(*path, p1, true); path = optimize(*path, obstacles, motionConstraints, start.vel, goal.vel); return path; }
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; } }
TEST(Path, nearestSegment) { Point p0, p1(1.0, 0.0), p2(2.0, 0.0), p3(3.0, 0.0); InterpolatedPath path; path.waypoints.emplace_back(MotionInstant(p0, Point()), 0s); path.waypoints.emplace_back(MotionInstant(p1, Point()), 0s); path.waypoints.emplace_back(MotionInstant(p2, Point()), 0s); path.waypoints.emplace_back(MotionInstant(p3, Point()), 0s); Segment actSeg = path.nearestSegment(Point(0.5, -0.5)); EXPECT_FLOAT_EQ(p0.x(), actSeg.pt[0].x()); EXPECT_FLOAT_EQ(p0.y(), actSeg.pt[0].y()); EXPECT_FLOAT_EQ(p1.x(), actSeg.pt[1].x()); EXPECT_FLOAT_EQ(p1.y(), actSeg.pt[1].y()); }
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; } }
TEST(InterpolatedPath, subPath1) { InterpolatedPath path; path.waypoints.emplace_back(MotionInstant(Point(1, 1), Point(0, 0)), 0s); path.waypoints.emplace_back(MotionInstant(Point(1, 2), Point(1, 1)), 3s); path.waypoints.emplace_back(MotionInstant(Point(1, 3), Point(0, 0)), 6s); // test invalid parameters to subPath() EXPECT_THROW(path.subPath(-1s, 5s), invalid_argument); EXPECT_THROW(path.subPath(8s, 20s), invalid_argument); // start time beyond bounds of path EXPECT_THROW(path.subPath(5s, -2s), invalid_argument); // make a subpath that cuts off one second from the start and end of the // original unique_ptr<Path> subPath = path.subPath(1s, 5s); RJ::Seconds midTime((5 - 1) / 2.0); auto mid = subPath->evaluate(midTime); ASSERT_TRUE(mid); EXPECT_FLOAT_EQ(Point(1, 1).x(), mid->motion.vel.x()); // mid velocity of subpath should be the same as velocity of original path EXPECT_FLOAT_EQ(Point(1, 1).y(), mid->motion.vel.y()); EXPECT_FLOAT_EQ(1, mid->motion.pos.x()); EXPECT_FLOAT_EQ(2, mid->motion.pos.y()); // test the subpath at t = 0 auto start = subPath->evaluate(0s); ASSERT_TRUE(start); // the starting velocity of the subpath should be somewhere between the 0 // and the velocity at the middle EXPECT_GT(start->motion.vel.mag(), 0); EXPECT_LT(start->motion.vel.mag(), Point(1, 1).mag()); // the starting position of the subpath should be somewhere between the // start pos of the original path and the middle point EXPECT_GT(start->motion.pos.y(), 1); EXPECT_LT(start->motion.pos.x(), 2); }
TEST(InterpolatedPath, subpath2) { // Create a test path InterpolatedPath path; path.waypoints.emplace_back(MotionInstant(Point(1, 0), Point(0, 0)), 0s); path.waypoints.emplace_back(MotionInstant(Point(1, 2), Point(-1, -1)), 1s); path.waypoints.emplace_back(MotionInstant(Point(-2, 19), Point(1, 1)), 3s); path.waypoints.emplace_back(MotionInstant(Point(1, 6), Point(0, 0)), 9s); // Create 6 subPaths of length 1.5 vector<unique_ptr<Path>> subPaths; RJ::Seconds diff = 1500ms; for (RJ::Seconds i = 0ms; i < 9s; i += diff) { subPaths.push_back(path.subPath(i, i + diff)); } // Compare the subPaths to the origional path and check that the results of // evaluating the paths are close enough for (int i = 0; i < 6; i++) { for (auto j = 0ms; j < 1500ms; j += 1ms) { auto time = i * 1500ms + j; auto org = path.evaluate(time); auto sub = subPaths[i]->evaluate(j); ASSERT_TRUE(org); ASSERT_TRUE(sub); EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001) << "i+j=" << to_string(time); EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001) << "i+j=" << to_string(time); EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001) << "i+j=" << to_string(time); EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001) << "i+j=" << to_string(time); } } }
TEST(CompositePath, CompositeSubPath) { // Create a test path InterpolatedPath path; path.waypoints.emplace_back(MotionInstant(Point(1, 0), Point(0, 0)), 0s); path.waypoints.emplace_back(MotionInstant(Point(1, 2), Point(-1, -1)), 1s); path.waypoints.emplace_back(MotionInstant(Point(-2, 19), Point(1, 1)), 3s); path.waypoints.emplace_back(MotionInstant(Point(1, 6), Point(0, 0)), 9s); // Create 6 subPaths and rejoin them together into one compositePath CompositePath compositePath; auto diff = 1500ms; for (auto i = 0ms; i < 7500ms; i += diff) { compositePath.append(path.subPath(i, i + diff)); } compositePath.append(path.subPath(7500ms)); // Compare that the compositePath and origonal path are mostly equal for (RJ::Seconds i = 0ms; i <= 10s; i += 1ms) { auto org = path.evaluate(i); auto sub = compositePath.evaluate(i); if (!org && !sub) break; ASSERT_TRUE(org); ASSERT_TRUE(sub); EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001) << "i=" << to_string(i); EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001) << "i=" << to_string(i); EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001) << "i=" << to_string(i); EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001) << "i=" << to_string(i); } // Create 9 subPaths from the compositePaths vector<unique_ptr<Path>> subPaths; diff = 1000ms; for (auto i = 0ms; i < 8s; i += diff) { subPaths.push_back(compositePath.subPath(i, i + diff)); } subPaths.push_back(compositePath.subPath(8000ms)); // Compare the subPaths of the compositePaths to the origional path and // check that the results of evaluating the paths are close enough for (int i = 0; i < 9; i++) { for (auto j = 0ms; j < 1s; j += 100ms) { auto time = i * 1000ms + j; auto org = path.evaluate(time); auto sub = subPaths[i]->evaluate(j); ASSERT_TRUE(org); ASSERT_TRUE(sub); EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001) << "newPathTime=" << to_string(time); EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001) << "newPathTime=" << to_string(time); EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001) << "newPathTime=" << to_string(time); EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001) << "newPathTime=" << to_string(time); } } }