planner_tests.h
Go to the documentation of this file.
00001 #include <gtest/gtest.h>
00002 
00003 #include <ros/console.h>
00004 #include <descartes_core/path_planner_base.h>
00005 #include <descartes_trajectory_test/cartesian_robot.h>
00006 
00007 #include "utils/trajectory_maker.h"
00008 
00009 // Google Test Framework for Path Planners
00010 
00011 template <class T>
00012 class PathPlannerTest : public testing::Test
00013 {
00014 protected:
00015   descartes_core::PathPlannerBasePtr makePlanner()
00016   {
00017     descartes_core::PathPlannerBasePtr planner(new T());
00018     EXPECT_TRUE(planner->initialize(robot_)) << "Failed to initalize robot model";
00019     return planner;
00020   }
00021 
00022   PathPlannerTest()
00023     : velocity_limits_(6, 1.0), robot_(new descartes_trajectory_test::CartesianRobot(5.0, 0.001, velocity_limits_))
00024   {
00025     ros::Time::init();
00026   }
00027 
00028   std::vector<double> velocity_limits_;
00029   descartes_core::RobotModelConstPtr robot_;
00030 };
00031 
00032 TYPED_TEST_CASE_P(PathPlannerTest);
00033 
00034 TYPED_TEST_P(PathPlannerTest, construction)
00035 {
00036   using namespace descartes_core;
00037 
00038   PathPlannerBasePtr planner = this->makePlanner();
00039 }
00040 
00041 TYPED_TEST_P(PathPlannerTest, basicConfigure)
00042 {
00043   descartes_core::PathPlannerBasePtr planner = this->makePlanner();
00044   descartes_core::PlannerConfig config;
00045   planner->getConfig(config);
00046   EXPECT_TRUE(planner->setConfig(config));
00047 }
00048 
00049 TYPED_TEST_P(PathPlannerTest, preservesTiming)
00050 {
00051   using namespace descartes_tests;
00052   using namespace descartes_core;
00053 
00054   PathPlannerBasePtr planner = this->makePlanner();
00055 
00056   std::vector<TrajectoryPtPtr> input, output;
00057   // Make input trajectory
00058   input = makeConstantVelocityTrajectory(Eigen::Vector3d(-1.0, 0, 0),  // start position
00059                                          Eigen::Vector3d(1.0, 0, 0),   // end position
00060                                          0.9,                          // tool velocity
00061                                          20);                          // samples
00062   // Double the dt of every pt to provide some variety
00063   double dt = input.front().get()->getTiming().upper;
00064   for (auto& pt : input)
00065   {
00066     pt.get()->setTiming(TimingConstraint(dt));
00067     dt *= 1.5;
00068   }
00069   // // Solve
00070   ASSERT_TRUE(planner->planPath(input));
00071   // Get the result
00072   ASSERT_TRUE(planner->getPath(output));
00073   // Compare timing
00074   ASSERT_TRUE(input.size() == output.size());
00075   for (size_t i = 0; i < input.size(); ++i)
00076   {
00077     double t1 = input[i].get()->getTiming().upper;
00078     double t2 = output[i].get()->getTiming().upper;
00079     EXPECT_DOUBLE_EQ(t1, t2) << "Input/output timing should correspond for same index: " << t1 << " " << t2;
00080   }
00081 }
00082 
00083 TYPED_TEST_P(PathPlannerTest, simpleVelocityCheck)
00084 {
00085   using namespace descartes_core;
00086 
00087   PathPlannerBasePtr planner = this->makePlanner();
00088 
00089   std::vector<descartes_core::TrajectoryPtPtr> input;
00090   input = descartes_tests::makeConstantVelocityTrajectory(Eigen::Vector3d(-1.0, 0, 0),  // start position
00091                                                           Eigen::Vector3d(1.0, 0, 0),   // end position
00092                                                           0.9,  // tool velocity (< 1.0 m/s limit)
00093                                                           10);  // samples
00094   ASSERT_TRUE(!input.empty());
00095   // The nominal trajectory (0.9 m/s) is less than max tool speed of 1.0 m/s
00096   EXPECT_TRUE(planner->planPath(input));
00097   // Unconstraining a point should still succeed
00098   input.back().get()->setTiming(descartes_core::TimingConstraint());
00099   EXPECT_TRUE(planner->planPath(input));
00100   // Making a dt for a segment very small should induce failure
00101   input.back().get()->setTiming(descartes_core::TimingConstraint(0.001));
00102   EXPECT_FALSE(planner->planPath(input)) << "Trajectory pt has very small dt; planner should fail for velocity out of "
00103                                             "bounds";
00104 }
00105 
00106 TYPED_TEST_P(PathPlannerTest, zigzagTrajectory)
00107 {
00108   using namespace descartes_core;
00109 
00110   PathPlannerBasePtr planner = this->makePlanner();
00111 
00112   std::vector<descartes_core::TrajectoryPtPtr> input;
00113   input = descartes_tests::makeZigZagTrajectory(-1.0,  // start position
00114                                                 1.0,   // end position
00115                                                 0.5,
00116                                                 0.1,  // tool velocity (< 1.0 m/s limit)
00117                                                 10);  // samples
00118   ASSERT_TRUE(!input.empty());
00119   // The nominal trajectory (0.9 m/s) is less than max tool speed of 1.0 m/s
00120   EXPECT_TRUE(planner->planPath(input));
00121 }
00122 
00123 REGISTER_TYPED_TEST_CASE_P(PathPlannerTest, construction, basicConfigure, preservesTiming, simpleVelocityCheck,
00124                            zigzagTrajectory);


descartes_planner
Author(s): Jorge Nicho
autogenerated on Thu Jun 6 2019 21:36:12