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/standard_traj_generator.h>
00036 #include <dwb_plugins/limited_accel_generator.h>
00037 #include <nav_core2/exceptions.h>
00038 #include <vector>
00039 #include <algorithm>
00040
00041 using dwb_plugins::StandardTrajectoryGenerator;
00042
00043 geometry_msgs::Pose2D origin;
00044 nav_2d_msgs::Twist2D zero;
00045 nav_2d_msgs::Twist2D forward;
00046
00047 void checkLimits(const std::vector<nav_2d_msgs::Twist2D>& twists,
00048 double exp_min_x, double exp_max_x, double exp_min_y, double exp_max_y,
00049 double exp_min_theta, double exp_max_theta,
00050 double exp_max_xy = -1.0,
00051 double exp_min_xy = -1.0, double exp_min_speed_theta = -1.0)
00052 {
00053 ASSERT_GT(twists.size(), 0U);
00054 nav_2d_msgs::Twist2D first = twists[0];
00055
00056 double min_x = first.x, max_x = first.x, min_y = first.y, max_y = first.y;
00057 double min_theta = first.theta, max_theta = first.theta;
00058 double max_xy = hypot(first.x, first.y);
00059
00060 for (nav_2d_msgs::Twist2D twist : twists)
00061 {
00062 min_x = std::min(min_x, twist.x);
00063 min_y = std::min(min_y, twist.y);
00064 min_theta = std::min(min_theta, twist.theta);
00065 max_x = std::max(max_x, twist.x);
00066 max_y = std::max(max_y, twist.y);
00067 max_theta = std::max(max_theta, twist.theta);
00068 double hyp = hypot(twist.x, twist.y);
00069 max_xy = std::max(max_xy, hyp);
00070
00071 if (exp_min_xy >= 0 && exp_min_speed_theta >= 0)
00072 {
00073 EXPECT_TRUE(fabs(twist.theta) >= exp_min_speed_theta || hyp >= exp_min_xy);
00074 }
00075 }
00076 EXPECT_DOUBLE_EQ(min_x, exp_min_x);
00077 EXPECT_DOUBLE_EQ(max_x, exp_max_x);
00078 EXPECT_DOUBLE_EQ(min_y, exp_min_y);
00079 EXPECT_DOUBLE_EQ(max_y, exp_max_y);
00080 EXPECT_DOUBLE_EQ(min_theta, exp_min_theta);
00081 EXPECT_DOUBLE_EQ(max_theta, exp_max_theta);
00082 if (exp_max_xy >= 0)
00083 {
00084 EXPECT_DOUBLE_EQ(max_xy, exp_max_xy);
00085 }
00086 }
00087
00088 TEST(VelocityIterator, standard_gen)
00089 {
00090 ros::NodeHandle nh("st_gen");
00091 StandardTrajectoryGenerator gen;
00092 gen.initialize(nh);
00093 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00094 EXPECT_EQ(twists.size(), 1926U);
00095 checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, 0.55, 0.1, 0.4);
00096 }
00097
00098 TEST(VelocityIterator, max_xy)
00099 {
00100 ros::NodeHandle nh("max_xy");
00101 nh.setParam("max_speed_xy", 1.0);
00102 StandardTrajectoryGenerator gen;
00103 gen.initialize(nh);
00104
00105 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00106
00107 EXPECT_EQ(twists.size(), 2010U);
00108 checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1));
00109 }
00110
00111 TEST(VelocityIterator, min_xy)
00112 {
00113 ros::NodeHandle nh("min_xy");
00114 nh.setParam("min_speed_xy", -1);
00115 StandardTrajectoryGenerator gen;
00116 gen.initialize(nh);
00117 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00118
00119 EXPECT_EQ(twists.size(), 2015U);
00120 checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
00121 }
00122
00123 TEST(VelocityIterator, min_theta)
00124 {
00125 ros::NodeHandle nh("min_theta");
00126 nh.setParam("min_speed_theta", -1);
00127 StandardTrajectoryGenerator gen;
00128 gen.initialize(nh);
00129 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00130
00131 EXPECT_EQ(twists.size(), 2015U);
00132 checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
00133 }
00134
00135 TEST(VelocityIterator, no_limits)
00136 {
00137 ros::NodeHandle nh("no_limits");
00138 nh.setParam("max_speed_xy", -1.0);
00139 nh.setParam("min_speed_xy", -1);
00140 nh.setParam("min_speed_theta", -1);
00141 StandardTrajectoryGenerator gen;
00142 gen.initialize(nh);
00143 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00144
00145 EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
00146 checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
00147 }
00148
00149 TEST(VelocityIterator, no_limits_samples)
00150 {
00151 ros::NodeHandle nh("no_limits_samples");
00152 nh.setParam("max_speed_xy", -1.0);
00153 nh.setParam("min_speed_xy", -1);
00154 nh.setParam("min_speed_theta", -1);
00155 int x_samples = 10, y_samples = 3, theta_samples = 5;
00156 nh.setParam("vx_samples", x_samples);
00157 nh.setParam("vy_samples", y_samples);
00158 nh.setParam("vtheta_samples", theta_samples);
00159 StandardTrajectoryGenerator gen;
00160 gen.initialize(nh);
00161 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00162 EXPECT_EQ(twists.size(), static_cast<unsigned int>(x_samples * y_samples * theta_samples - 1));
00163 checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
00164 }
00165
00166 TEST(VelocityIterator, dwa_gen_exception)
00167 {
00168 ros::NodeHandle nh("dwa_gen_exception");
00169 nh.setParam("use_dwa", true);
00170 StandardTrajectoryGenerator gen;
00171 EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
00172 }
00173
00174 TEST(VelocityIterator, no_dwa_gen_exception)
00175 {
00176 ros::NodeHandle nh("no_dwa_gen_exception");
00177 nh.setParam("use_dwa", false);
00178 dwb_plugins::LimitedAccelGenerator gen;
00179 EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
00180 }
00181
00182 TEST(VelocityIterator, dwa_gen)
00183 {
00184 ros::NodeHandle nh("dwa_gen");
00185 nh.setParam("use_dwa", true);
00186 nh.setParam("min_speed_theta", -1);
00187 dwb_plugins::LimitedAccelGenerator gen;
00188 gen.initialize(nh);
00189 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00190
00191 EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
00192 checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
00193 }
00194
00195 TEST(VelocityIterator, dwa_gen_no_param)
00196 {
00197 ros::NodeHandle nh("dwa_gen_no_param");
00198 nh.setParam("min_speed_theta", -1);
00199 dwb_plugins::LimitedAccelGenerator gen;
00200 gen.initialize(nh);
00201 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
00202 EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
00203 checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
00204 }
00205
00206 TEST(VelocityIterator, nonzero)
00207 {
00208 ros::NodeHandle nh("nonzero");
00209 nh.setParam("use_dwa", true);
00210 nh.setParam("min_speed_theta", -1);
00211 dwb_plugins::LimitedAccelGenerator gen;
00212 gen.initialize(nh);
00213 nav_2d_msgs::Twist2D initial;
00214 initial.x = 0.1;
00215 initial.y = -0.08;
00216 initial.theta = 0.05;
00217 std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(initial);
00218 EXPECT_EQ(twists.size(), 2519U);
00219 checkLimits(twists, 0.0, 0.225, -0.1, 0.045, -0.11000000000000003, 0.21,
00220 0.24622144504490268, 0.0, 0.1);
00221 }
00222
00223 void matchPose(const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b)
00224 {
00225 EXPECT_DOUBLE_EQ(a.x, b.x);
00226 EXPECT_DOUBLE_EQ(a.y, b.y);
00227 EXPECT_DOUBLE_EQ(a.theta, b.theta);
00228 }
00229
00230 void matchPose(const geometry_msgs::Pose2D& a, const double x, const double y, const double theta)
00231 {
00232 EXPECT_DOUBLE_EQ(a.x, x);
00233 EXPECT_DOUBLE_EQ(a.y, y);
00234 EXPECT_DOUBLE_EQ(a.theta, theta);
00235 }
00236
00237 void matchTwist(const nav_2d_msgs::Twist2D& a, const nav_2d_msgs::Twist2D& b)
00238 {
00239 EXPECT_DOUBLE_EQ(a.x, b.x);
00240 EXPECT_DOUBLE_EQ(a.y, b.y);
00241 EXPECT_DOUBLE_EQ(a.theta, b.theta);
00242 }
00243
00244 void matchTwist(const nav_2d_msgs::Twist2D& a, const double x, const double y, const double theta)
00245 {
00246 EXPECT_DOUBLE_EQ(a.x, x);
00247 EXPECT_DOUBLE_EQ(a.y, y);
00248 EXPECT_DOUBLE_EQ(a.theta, theta);
00249 }
00250
00251 const double DEFAULT_SIM_TIME = 1.7;
00252
00253 TEST(TrajectoryGenerator, basic)
00254 {
00255 ros::NodeHandle nh("basic");
00256 StandardTrajectoryGenerator gen;
00257 nh.setParam("linear_granularity", 0.5);
00258 gen.initialize(nh);
00259 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
00260 matchTwist(res.velocity, forward);
00261 EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
00262 int n = res.poses.size();
00263 EXPECT_EQ(n, 3);
00264 ASSERT_GT(n, 0);
00265
00266 matchPose(res.poses[0], origin);
00267 matchPose(res.poses[n - 1], DEFAULT_SIM_TIME * forward.x, 0, 0);
00268 }
00269
00270 TEST(TrajectoryGenerator, basic_no_last_point)
00271 {
00272 ros::NodeHandle nh("basic_no_last_point");
00273 StandardTrajectoryGenerator gen;
00274 nh.setParam("include_last_point", false);
00275 nh.setParam("linear_granularity", 0.5);
00276 gen.initialize(nh);
00277 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
00278 matchTwist(res.velocity, forward);
00279 EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME / 2);
00280 int n = res.poses.size();
00281 EXPECT_EQ(n, 2);
00282 ASSERT_GT(n, 0);
00283
00284 matchPose(res.poses[0], origin);
00285 matchPose(res.poses[n - 1], 0.255, 0, 0);
00286 }
00287
00288 TEST(TrajectoryGenerator, too_slow)
00289 {
00290 ros::NodeHandle nh("too_slow");
00291 StandardTrajectoryGenerator gen;
00292 nh.setParam("linear_granularity", 0.5);
00293 gen.initialize(nh);
00294 nav_2d_msgs::Twist2D cmd;
00295 cmd.x = 0.2;
00296 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
00297 matchTwist(res.velocity, cmd);
00298 EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
00299 int n = res.poses.size();
00300 EXPECT_EQ(n, 2);
00301 ASSERT_GT(n, 0);
00302
00303 matchPose(res.poses[0], origin);
00304 }
00305
00306 TEST(TrajectoryGenerator, holonomic)
00307 {
00308 ros::NodeHandle nh("holonomic");
00309 StandardTrajectoryGenerator gen;
00310 nh.setParam("linear_granularity", 0.5);
00311 gen.initialize(nh);
00312 nav_2d_msgs::Twist2D cmd;
00313 cmd.x = 0.3;
00314 cmd.y = 0.2;
00315 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
00316 matchTwist(res.velocity, cmd);
00317 EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
00318 int n = res.poses.size();
00319 EXPECT_EQ(n, 3);
00320 ASSERT_GT(n, 0);
00321
00322 matchPose(res.poses[0], origin);
00323 matchPose(res.poses[n - 1], cmd.x*DEFAULT_SIM_TIME, cmd.y*DEFAULT_SIM_TIME, 0);
00324 }
00325
00326 TEST(TrajectoryGenerator, twisty)
00327 {
00328 ros::NodeHandle nh("twisty");
00329 StandardTrajectoryGenerator gen;
00330 nh.setParam("linear_granularity", 0.5);
00331 nh.setParam("angular_granularity", 0.025);
00332 gen.initialize(nh);
00333 nav_2d_msgs::Twist2D cmd;
00334 cmd.x = 0.3;
00335 cmd.y = -0.2;
00336 cmd.theta = 0.111;
00337 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
00338 matchTwist(res.velocity, cmd);
00339 EXPECT_NEAR(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME, 1.0E-5);
00340
00341 int n = res.poses.size();
00342 EXPECT_EQ(n, 9);
00343 ASSERT_GT(n, 0);
00344
00345 matchPose(res.poses[0], origin);
00346 matchPose(res.poses[n - 1], 0.5355173615993063, -0.29635287789821596, cmd.theta * DEFAULT_SIM_TIME);
00347 }
00348
00349 TEST(TrajectoryGenerator, sim_time)
00350 {
00351 ros::NodeHandle nh("sim_time");
00352 const double sim_time = 2.5;
00353 nh.setParam("sim_time", sim_time);
00354 nh.setParam("linear_granularity", 0.5);
00355 StandardTrajectoryGenerator gen;
00356 gen.initialize(nh);
00357 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
00358 matchTwist(res.velocity, forward);
00359 EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), sim_time);
00360 int n = res.poses.size();
00361 EXPECT_EQ(n, 3);
00362 ASSERT_GT(n, 0);
00363
00364 matchPose(res.poses[0], origin);
00365 matchPose(res.poses[n - 1], sim_time * forward.x, 0, 0);
00366 }
00367
00368 TEST(TrajectoryGenerator, accel)
00369 {
00370 ros::NodeHandle nh("accel");
00371 nh.setParam("sim_time", 5.0);
00372 nh.setParam("discretize_by_time", true);
00373 nh.setParam("sim_granularity", 1.0);
00374 nh.setParam("acc_lim_x", 0.1);
00375 nh.setParam("min_speed_xy", -1.0);
00376 StandardTrajectoryGenerator gen;
00377 gen.initialize(nh);
00378
00379 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
00380 matchTwist(res.velocity, forward);
00381 EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
00382 ASSERT_EQ(res.poses.size(), 6U);
00383 matchPose(res.poses[0], origin);
00384 matchPose(res.poses[1], 0.1, 0, 0);
00385 matchPose(res.poses[2], 0.3, 0, 0);
00386 matchPose(res.poses[3], 0.6, 0, 0);
00387 matchPose(res.poses[4], 0.9, 0, 0);
00388 matchPose(res.poses[5], 1.2, 0, 0);
00389 }
00390
00391 TEST(TrajectoryGenerator, dwa)
00392 {
00393 ros::NodeHandle nh("dwa");
00394 nh.setParam("use_dwa", true);
00395 nh.setParam("sim_period", 1.0);
00396 nh.setParam("sim_time", 5.0);
00397 nh.setParam("discretize_by_time", true);
00398 nh.setParam("sim_granularity", 1.0);
00399 nh.setParam("acc_lim_x", 0.1);
00400 nh.setParam("min_speed_xy", -1.0);
00401 dwb_plugins::LimitedAccelGenerator gen;
00402 gen.initialize(nh);
00403
00404 dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
00405 matchTwist(res.velocity, forward);
00406 EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
00407 ASSERT_EQ(res.poses.size(), 6U);
00408 matchPose(res.poses[0], origin);
00409 matchPose(res.poses[1], 0.3, 0, 0);
00410 matchPose(res.poses[2], 0.6, 0, 0);
00411 matchPose(res.poses[3], 0.9, 0, 0);
00412 matchPose(res.poses[4], 1.2, 0, 0);
00413 matchPose(res.poses[5], 1.5, 0, 0);
00414 }
00415
00416 int main(int argc, char **argv)
00417 {
00418 forward.x = 0.3;
00419 ros::init(argc, argv, "twist_gen");
00420 testing::InitGoogleTest(&argc, argv);
00421 return RUN_ALL_TESTS();
00422 }