Commits

Ioan Sucan committed 2044aac

expanding test suite

Comments (0)

Files changed (1)

tests/geometric/2dmap/2dmap.cpp

     {
     }
 
-    bool test2DDisks(const Circles2D &circles, bool show = false, double *time = NULL, double *pathLength = NULL)
+    double test2DCircles(const Circles2D &circles, bool show = false, double *time = NULL, double *pathLength = NULL)
     {
-        bool result = true;
-
         /* instantiate space information */
         base::SpaceInformationPtr si = geometric::spaceInformation2DCircles(circles);
 
         planner->setProblemDefinition(pdef);
         planner->setup();
 
-        /* start counting time */
-        ompl::time::point startTime = ompl::time::now();
+	base::ScopedState<> start(si);
+	base::ScopedState<> goal(si);
+	unsigned int good = 0;
+	
+	for (std::size_t i = 0 ; i < circles.getQueryCount() ; ++i)
+	{
+	    const Circles2D::Query &q = circles.getQuery(i);
+	    start[0] = q.startX_;
+	    start[1] = q.startY_;
+	    goal[0] = q.goalX_;
+	    goal[1] = q.goalY_;
+	    pdef->setStartAndGoalStates(start, goal, 1e-3);
+	    planner->clear();
 
-        /* call the planner to solve the problem */
-        if (planner->solve(SOLUTION_TIME))
-        {
-            ompl::time::duration elapsed = ompl::time::now() - startTime;
-            if (time)
-                *time += ompl::time::seconds(elapsed);
-            if (show)
-                printf("Found solution in %f seconds!\n", ompl::time::seconds(elapsed));
+	    /* start counting time */
+	    ompl::time::point startTime = ompl::time::now();
 
-            geometric::PathGeometric *path = static_cast<geometric::PathGeometric*>(pdef->getSolutionPath().get());
+	    if (planner->solve(SOLUTION_TIME))
+	    {
+		ompl::time::duration elapsed = ompl::time::now() - startTime;
+		good++;
+		if (time)
+		    *time += ompl::time::seconds(elapsed);
+		if (show)
+		    printf("Found solution in %f seconds!\n", ompl::time::seconds(elapsed));
 
+		geometric::PathGeometric *path = static_cast<geometric::PathGeometric*>(pdef->getSolutionPath().get());
+
+		/* make the solution more smooth */
+		geometric::PathSimplifierPtr sm(new geometric::PathSimplifier(si));
+		
+		startTime = ompl::time::now();
+		sm->simplify(*path, SOLUTION_TIME);
+		elapsed = ompl::time::now() - startTime;
+		
+		if (pathLength)
+		    *pathLength += path->length();
+		
+		if (time)
+		    *time += ompl::time::seconds(elapsed);
+	    }
 	}
 	
-	
-	return true;
+	if (pathLength)
+	    *pathLength /= (double)circles.getQueryCount();
+	if (time)
+	    *time /= (double)circles.getQueryCount();
+
+	return (double)good / (double)circles.getQueryCount();
     }
     
     bool test2DEnv(const Environment2D &env, bool show = false, double *time = NULL, double *pathLength = NULL)
         }
     }
 
+    void run2DCirclesTest(TestPlanner *p, double *success, double *avgruntime, double *avglength)
+    {
+        *success = 100.0 * p->test2DCircles(circles_, false, avgruntime, avglength);
+
+        if (verbose_)
+        {
+            printf("    Success rate: %f%%\n", *success);
+            printf("    Average runtime: %f\n", *avgruntime);
+            printf("    Average path length: %f\n", *avglength);
+        }
+    }
+
+    void runAllTests(void)
+    {
+	double success    = 0.0;
+	double avgruntime = 0.0;
+	double avglength  = 0.0;
+	
+	simpleTest();
+	
+	TestPlanner *p = new RRTTest();
+	run2DMapTest(p, &success, &avgruntime, &avglength);
+	BOOST_CHECK(success >= 99.0);
+	BOOST_CHECK(avgruntime < 0.01);
+	BOOST_CHECK(avglength < 100.0);
+
+	success    = 0.0;
+	avgruntime = 0.0;
+	avglength  = 0.0;
+    
+	run2DCirclesTest(p, &success, &avgruntime, &avglength);
+	BOOST_CHECK(success >= 99.0);
+	BOOST_CHECK(avgruntime < 0.01);
+	BOOST_CHECK(avglength < 10.0);
+	
+	delete p;
+    }
+    
 protected:
 
     PlanTest(void)
     {
-        verbose_ = true;
+        verbose_ = false;
         boost::filesystem::path path(TEST_RESOURCES_DIR);
         loadEnvironment((path / "env1.txt").string().c_str(), env_);
 
 
 BOOST_AUTO_TEST_CASE(geometric_RRT)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
+    runAllTests();
+}
 
-    simpleTest();
-
-    TestPlanner *p = new RRTTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.01);
-    BOOST_CHECK(avglength < 100.0);
-}
 
 BOOST_AUTO_TEST_CASE(geometric_RRTConnect)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new RRTConnectTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.01);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_pRRT)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new pRRTTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.02);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_TRRT)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new TRRTTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.02);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_pSBL)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new pSBLTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.2);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 
 BOOST_AUTO_TEST_CASE(geometric_KPIECE1)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new KPIECE1Test();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.1);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_LBKPIECE1)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new LBKPIECE1Test();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.1);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_BKPIECE1)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new BKPIECE1Test();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.1);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_EST)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new ESTTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.1);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_LazyRRT)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new LazyRRTTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 70.0);
-    BOOST_CHECK(avgruntime < 1.0);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_PRM)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new PRMTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.1);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_CASE(geometric_SBL)
 {
-    double success    = 0.0;
-    double avgruntime = 0.0;
-    double avglength  = 0.0;
-
-    simpleTest();
-
-    TestPlanner *p = new SBLTest();
-    run2DMapTest(p, &success, &avgruntime, &avglength);
-    delete p;
-
-    BOOST_CHECK(success >= 99.0);
-    BOOST_CHECK(avgruntime < 0.1);
-    BOOST_CHECK(avglength < 100.0);
+    runAllTests();
 }
 
 BOOST_AUTO_TEST_SUITE_END()
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.