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