40 #define Vector2 BaseDistance::Vector2
44 std::vector<Vector2> points;
46 double d, eps=0.00001;
52 points.push_back(
Vector2(3.2f, 0.0f));
54 assert((d-2.4)<eps && n.x == 3.2f && n.y == 0.0f);
56 points.push_back(
Vector2(0.0f, 2.8f));
58 assert((d-2.3 < eps) && n.x == 0.0f && n.y == 2.8f);
60 points.push_back(
Vector2(0.0f, -2.9f));
62 assert((d-2.2 < eps) && n.x == 0.0f && n.y == -2.9f);
64 points.push_back(
Vector2(-2.4f, 0.0f));
66 assert((d-2.1 < eps) && n.x == -2.4f && n.y == 0.0f);
70 points.push_back(
Vector2(1.76f, 1.6f));
72 assert((d-1.46 < eps) && n.x == 1.76f && n.y == 1.6f);
74 points.push_back(
Vector2(-1.18f, 1.55f));
76 assert((d-1.37 < eps) && n.x == -1.18f && n.y == 1.55f);
78 points.push_back(
Vector2(-1.05f, -1.7f));
80 assert((d-1.25 < eps) && n.x == -1.05f && n.y == -1.7f);
82 points.push_back(
Vector2(0.95f, -1.82f));
84 assert((d-1.13 < eps) && n.x == 0.95f && n.y == -1.82f);
88 for(
int i=0; i < 1000; i++) {
89 std::random_shuffle(points.begin(), points.end());
91 assert((d-1.13 < eps) && n.x == 0.95f && n.y == -1.82f);
96 int main(
int argc,
char *argv[])
98 ros::init(argc, argv,
"BaseDistance_test");
double distance(std::vector< Vector2 > &points, Vector2 *nearest, double dx=0, double dy=0, double dth=0)
Calculates the distance to closest of points from nearest footprint wall The distance is calculated n...
void setFootprint(double front, double rear, double left, double right, double tolerance)
Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_.
int main(int argc, char *argv[])