00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <gtest/gtest.h>
00035 #include <dwb_plugins/simple_goal_checker.h>
00036 #include <dwb_plugins/stopped_goal_checker.h>
00037
00038 using dwb_plugins::SimpleGoalChecker;
00039 using dwb_plugins::StoppedGoalChecker;
00040
00041 void checkMacro(dwb_local_planner::GoalChecker& gc,
00042 double x0, double y0, double theta0,
00043 double x1, double y1, double theta1,
00044 double xv, double yv, double thetav,
00045 bool expected_result)
00046 {
00047 gc.reset();
00048 geometry_msgs::Pose2D pose0, pose1;
00049 pose0.x = x0;
00050 pose0.y = y0;
00051 pose0.theta = theta0;
00052 pose1.x = x1;
00053 pose1.y = y1;
00054 pose1.theta = theta1;
00055 nav_2d_msgs::Twist2D v;
00056 v.x = xv;
00057 v.y = yv;
00058 v.theta = thetav;
00059 if (expected_result)
00060 EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v));
00061 else
00062 EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v));
00063 }
00064
00065 void sameResult(dwb_local_planner::GoalChecker& gc0, dwb_local_planner::GoalChecker& gc1,
00066 double x0, double y0, double theta0,
00067 double x1, double y1, double theta1,
00068 double xv, double yv, double thetav,
00069 bool expected_result)
00070 {
00071 checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
00072 checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
00073 }
00074
00075 void trueFalse(dwb_local_planner::GoalChecker& gc0, dwb_local_planner::GoalChecker& gc1,
00076 double x0, double y0, double theta0,
00077 double x1, double y1, double theta1,
00078 double xv, double yv, double thetav)
00079 {
00080 checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true);
00081 checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false);
00082 }
00083
00084 TEST(VelocityIterator, two_checks)
00085 {
00086 ros::NodeHandle x;
00087 SimpleGoalChecker gc;
00088 StoppedGoalChecker sgc;
00089 gc.initialize(x);
00090 sgc.initialize(x);
00091 sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
00092 sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false);
00093 sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false);
00094 sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false);
00095 sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true);
00096 trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0);
00097 trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0);
00098 trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0);
00099 trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1);
00100 }
00101
00102 int main(int argc, char **argv)
00103 {
00104 ros::init(argc, argv, "goal_checker");
00105 testing::InitGoogleTest(&argc, argv);
00106 return RUN_ALL_TESTS();
00107 }