Ejemplo n.º 1
0
/* ************************************************************************* */
TEST( testPath, nearestSegment) {
	Geometry2d::Point p0, p1(1.0, 0.0), p2(2.0, 0.0), p3(3.0, 0.0);

	Planning::Path path;
	path.points.push_back(p0);
	path.points.push_back(p1);
	path.points.push_back(p2);
	path.points.push_back(p3);

	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);
}
Ejemplo n.º 2
0
/* ************************************************************************* */
TEST( testPath, startFrom3 ) {

	Geometry2d::Point p0, p1(1.0, 0.0), p2(2.0, 0.0), p3(3.0, 0.0);

	Planning::Path path;
	path.points.push_back(p0);
	path.points.push_back(p1);
	path.points.push_back(p2);
	path.points.push_back(p3);

	Point pt(0.5,-0.01);
	Planning::Path act;
	path.startFrom(pt, act);

	// verify
	ASSERT_EQ(5, act.size());
	EXPECT_TRUE(pt == act.points[0]);
//	EXPECT_TRUE(p1 == act.points[1]); // fails
}
Ejemplo n.º 3
0
/* ************************************************************************* */
TEST( testPath, startFrom1 ) {

	Geometry2d::Point p0, p1(1.0, 0.0), p2(2.0, 0.0), p3(3.0, 0.0);

	Planning::Path path;
	path.points.push_back(p0);
	path.points.push_back(p1);
	path.points.push_back(p2);
	path.points.push_back(p3);

	// simple case - on same axis as path
	Planning::Path act;
	path.startFrom(Point(-1.0, 0.0), act);

	// verify
	ASSERT_EQ(4, act.size());
	EXPECT_FLOAT_EQ(-1.0, act.points[0].x);
	EXPECT_FLOAT_EQ(0.0, act.points[0].y);
	EXPECT_TRUE(act.points[1] == p1);
}
Ejemplo n.º 4
0
void Planning::Path::startFrom(const Geometry2d::Point& pt, Planning::Path& result) const {

	// path will start at the current robot pose
	result.clear();
	result.points.push_back(pt);

	if (points.empty())
		return;

	// handle simple paths
	if (points.size() == 1) {
		result.points.push_back(points.front());
		return;
	}

	// find where to start the path
	Geometry2d::Segment close_segment;
	float dist = -1;
	unsigned int i = (points.front().nearPoint(pt, 0.02)) ? 1 : 0;
	vector<Geometry2d::Point>::const_iterator path_start = ++points.begin();
	for (; i < (points.size() - 1); ++i)
	{
		Geometry2d::Segment s(points[i], points[i+1]);
		const float d = s.distTo(pt);
		if (dist < 0 || d < dist)
		{
			close_segment = s;
			dist = d;
		}
	}

	// slice path
	// new path will be pt, [closest point on nearest segment], [i+1 to end]
	if (dist > 0.0 && dist < 0.02) {
		Geometry2d::Point intersection_pt = close_segment.nearestPoint(pt);
		result.points.push_back(intersection_pt);
	}

	result.points.insert(result.points.end(), path_start, points.end());

}
Ejemplo n.º 5
0
Geometry2d::Point OurRobot::findGoalOnPath(const Geometry2d::Point& pose,
		const Planning::Path& path,	const ObstacleGroup& obstacles) {
		const bool blend_verbose = false;

		// empty path case - leave robot stationary
		if (path.empty())
			return pose;

		// find closest point on path to pose
		float max = path.points[0].distTo(pose);
		unsigned int ip = 0;
		for (unsigned i=0; i<path.points.size(); i++) {
			if (path.points[i].distTo(pose) < max) {
				max = path.points[i].distTo(pose);
				ip = i;
			}
		}

		if (blend_verbose) addText(QString("cur pt %1=(%2,%3)").arg(ip).arg(path.points[ip].x).arg(path.points[ip].y));

		// go to nearest point if only point or closest point is the goal
		if (path.size() == 1) {
			if (blend_verbose) addText(QString("blend:simple_path"));
			return path.points[0];
		}

		// can't mix, just go to endpoint
		if (path.size() == 2) {
			if (blend_verbose) addText(QString("blend:size2"));
			return path.points[1];
		}

		// FIXME: does not blend the last segment
		// All other cases: proportionally blend the next two points together for a smoother
		// path, so long as it is still viable

		if (blend_verbose) addText(QString("blend:segments=%1").arg(path.points.size()-1));

		// pull out relevant points
		Point p0 = pos;
		Point p1;
		Point p2;

		if (path.size() > ip+2) {
			p1 = path.points[ip+1];
			p2 = path.points[ip+2];
		} else if (path.size() > ip+1) {
			p1 = path.points[ip];
			p2 = path.points[ip+1];
		} else {
			p1 = path.points[ip-1];
			p2 = path.points[ip];
		}

		Geometry2d::Segment target_seg(p1, p2);

		if (blend_verbose) addText(QString("pos=(%1,%2)").arg(pos.x,5).arg(pos.y,5));
		if (blend_verbose) addText(QString("path[0]=(%1,%2)").arg(path.points[0].x).arg(path.points[0].y));
		if (blend_verbose) addText(QString("p1=(%1,%2)").arg(p1.x,5).arg(p1.y,5));
		if (blend_verbose) addText(QString("p2=(%1,%2)").arg(p2.x,5).arg(p2.y,5));

		// final endpoint handling
		if (target_seg.nearPointPerp(p0, 0.02) && p0.nearPoint(p2, 0.03)) {
			if (2 == path.size()-1) {
				if (blend_verbose) addText(QString("blend:at_end"));
				return p2;
			} else {
				// reset this segment to next one
				if (blend_verbose) addText(QString("blend:reset_segment"));
				Point temp(p1);
				p1 = p2;
				p2 = temp;
				target_seg = Geometry2d::Segment(p1, p2);
			}
		}

		float dist1 = p0.distTo(p1), dist2 = p1.distTo(p2);
		if (blend_verbose) addText(QString("blend:d1=%1,d2=%2").arg(dist1).arg(dist2));

		// endpoint handling
		if (dist1 < 0.02) {
			if (blend_verbose) addText(QString("blend:dist1small=%1").arg(dist1));
			return p2; /// just go to next point
		}

		// short segment handling
		if (p1.distTo(p2) < 0.05) {
			if (blend_verbose) addText(QString("blend:dist2small=%1").arg(dist2));
			return p2; /// just go to next point
		}

		// close to segment - go to end of segment
		if (target_seg.nearPoint(p0, 0.03)) {
			if (blend_verbose) addText(QString("blend:closeToSegment"));
			return p2;
		}

		// mix the next point between the first and second point
		// if we are far away from p1, want scale to be closer to p1
		// if we are close to p1, want scale to be closer to p2
		float scale = 1 - clamp(dist1/dist2, 0.0f, 1.0f);
		Geometry2d::Point targetPos = p1 + (p2-p1)*scale;
		if (blend_verbose) {
			addText(QString("blend:scale=%1").arg(scale));
			addText(QString("blend:dist1=%1").arg(dist1));
			addText(QString("blend:dist2=%1").arg(dist2));
		}

		// check for collisions on blended path
		Geometry2d::Segment shortcut(p0, targetPos);

		Geometry2d::Point result = p1;
		if (!obstacles.hit(shortcut)) {
			if (blend_verbose) addText(QString("blend:shortcut_succeed"));
			result = targetPos;
		} else if (result.nearPoint(pose, 0.05)) {
			if (blend_verbose) addText(QString("blend:shortcut_failed"));
			result = result + (result-pose).normalized() * 0.10;
		}

		if (blend_verbose) addText(QString("point (%1, %2)").arg(result.x).arg(result.y));

		return result;
}