Commits

Ioan Sucan committed 58b3858

refactor 2dmap tests to allow for additional tests; add setup for planning among circles (disks)

Comments (0)

Files changed (7)

tests/geometric/2dmap/2DcirclesSetup.h

+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013, Willow Garage
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#ifndef OMPL_TEST_2DCIRCLES_SETUP_
+#define OMPL_TEST_2DCIRCLES_SETUP_
+
+#include <boost/filesystem.hpp>
+
+#include "ompl/base/SpaceInformation.h"
+#include "ompl/base/spaces/RealVectorStateSpace.h"
+
+#include "../../resources/circles2D.h"
+
+namespace ompl
+{
+    namespace geometric
+    {
+	
+	class StateValidityChecker2DCircles : public base::StateValidityChecker
+	{
+	public:	    
+	    
+	    StateValidityChecker2DCircles(const base::SpaceInformationPtr &si, const Circles2D &circles) :
+		base::StateValidityChecker(si),
+		circles_(circles)
+	    {
+	    }
+	    
+	    virtual bool isValid(const base::State *state) const
+	    {
+		const double *xy = state->as<base::RealVectorStateSpace::StateType>()->values;
+		return circles_.noOverlap(xy[0], xy[1]);
+	    }
+	    
+	private:
+	    const Circles2D &circles_;
+	};
+	
+	static base::SpaceInformationPtr spaceInformation2DCircles(const Circles2D &circles)
+	{
+	    base::RealVectorStateSpace *space = new base::RealVectorStateSpace();
+	    space->addDimension(circles.minX_, circles.maxX_);
+	    space->addDimension(circles.minY_, circles.maxY_);
+	    base::SpaceInformationPtr si(new base::SpaceInformation(base::StateSpacePtr(space)));
+	    StateValidityChecker2DCircles *svc = new StateValidityChecker2DCircles(si, circles);
+	    si->setStateValidityChecker(base::StateValidityCheckerPtr(svc));  
+	    si->setStateValidityCheckingResolution(0.002);
+	    si->setup();
+	    return si;
+	}
+	
+	
+    }
+}
+
+#endif

tests/geometric/2dmap/2DmapSetup.h

 
 /* Author: Ioan Sucan */
 
+#ifndef OMPL_TEST_2DMAP_SETUP_
+#define OMPL_TEST_2DMAP_SETUP_
+
 #include <boost/filesystem.hpp>
 
 #include "ompl/base/SpaceInformation.h"
 
 
         /** \brief Construct an instance of space information (done automatically when using SimpleSetup) */
-        static base::SpaceInformationPtr spaceInformation2DMap(Environment2D &env)
+        static base::SpaceInformationPtr spaceInformation2DMap(const Environment2D &env)
         {
             base::RealVectorStateSpace *sSpace = new StateSpace2DMap();
 
         }
 
         /** \brief Construct a problem definition  (done automatically when using SimpleSetup) */
-        static base::ProblemDefinitionPtr problemDefinition2DMap(const base::SpaceInformationPtr &si, Environment2D &env)
+        static base::ProblemDefinitionPtr problemDefinition2DMap(const base::SpaceInformationPtr &si, const Environment2D &env)
         {
             base::ProblemDefinitionPtr pdef(new base::ProblemDefinition(si));
 
     }
 
 }
+
+#endif

tests/geometric/2dmap/2dmap.cpp

 #include <boost/test/unit_test.hpp>
 
 #include "2DmapSetup.h"
+#include "2DcirclesSetup.h"
 #include <iostream>
 
 #include "ompl/base/spaces/RealVectorStateProjections.h"
     {
     }
 
-    virtual bool execute(Environment2D &env, bool show = false, double *time = NULL, double *pathLength = NULL)
+    bool test2DDisks(const Circles2D &circles, bool show = false, double *time = NULL, double *pathLength = NULL)
+    {
+        bool result = true;
+
+        /* instantiate space information */
+        base::SpaceInformationPtr si = geometric::spaceInformation2DCircles(circles);
+
+        /* instantiate problem definition */
+	base::ProblemDefinitionPtr pdef(new base::ProblemDefinition(si));	
+	
+        /* instantiate motion planner */
+        base::PlannerPtr planner = newPlanner(si);
+        planner->setProblemDefinition(pdef);
+        planner->setup();
+
+        /* start counting time */
+        ompl::time::point startTime = ompl::time::now();
+
+        /* 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));
+
+            geometric::PathGeometric *path = static_cast<geometric::PathGeometric*>(pdef->getSolutionPath().get());
+
+	}
+	
+	
+	return true;
+    }
+    
+    bool test2DEnv(const Environment2D &env, bool show = false, double *time = NULL, double *pathLength = NULL)
     {
         bool result = true;
 
 
     void simpleTest(void)
     {
-        geometric::SimpleSetup2DMap s(env);
+        geometric::SimpleSetup2DMap s(env_);
         s.setup();
         base::PlannerTest pt(s.getPlanner());
         pt.test();
     }
 
-    void runPlanTest(TestPlanner *p, double *success, double *avgruntime, double *avglength)
+    void run2DMapTest(TestPlanner *p, double *success, double *avgruntime, double *avglength)
     {
         double time   = 0.0;
         double length = 0.0;
         int    good   = 0;
         int    N      = 100;
 
-        for (int i = 0 ; i < N ; ++i)
-            if (p->execute(env, false, &time, &length))
+	for (int i = 0 ; i < N ; ++i)
+            if (p->test2DEnv(env_, false, &time, &length))
                 good++;
 
         *success    = 100.0 * (double)good / (double)N;
         *avgruntime = time / (double)N;
         *avglength  = length / (double)N;
 
-        if (verbose)
+        if (verbose_)
         {
             printf("    Success rate: %f%%\n", *success);
             printf("    Average runtime: %f\n", *avgruntime);
 
     PlanTest(void)
     {
-        verbose = true;
+        verbose_ = true;
         boost::filesystem::path path(TEST_RESOURCES_DIR);
-        path = path / "env1.txt";
-        loadEnvironment(path.string().c_str(), env);
+        loadEnvironment((path / "env1.txt").string().c_str(), env_);
 
-        if (env.width * env.height == 0)
+        if (env_.width * env_.height == 0)
         {
             BOOST_FAIL( "The environment has a 0 dimension. Cannot continue" );
         }
+	
+	circles_.loadCircles((path / "circle_obstacles.txt").string());
+	circles_.loadQueries((path / "circle_queries.txt").string());
     }
 
-    Environment2D env;
-    bool          verbose;
+    Environment2D env_;
+    Circles2D     circles_;
+    bool          verbose_;
 };
 
 BOOST_FIXTURE_TEST_SUITE( MyPlanTestFixture, PlanTest )
     simpleTest();
 
     TestPlanner *p = new RRTTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new RRTConnectTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new pRRTTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new TRRTTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new pSBLTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new KPIECE1Test();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new LBKPIECE1Test();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new BKPIECE1Test();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new ESTTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new LazyRRTTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 70.0);
     simpleTest();
 
     TestPlanner *p = new PRMTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);
     simpleTest();
 
     TestPlanner *p = new SBLTest();
-    runPlanTest(p, &success, &avgruntime, &avglength);
+    run2DMapTest(p, &success, &avgruntime, &avglength);
     delete p;
 
     BOOST_CHECK(success >= 99.0);

tests/resources/circle_obstacles.txt

+units: meter 			
+	x	y	r
+1	27	15	5
+2	15	22	4
+3	40	28	3
+4	30	30	4
+5	21	34	4
+6	39	44.5	6
+7	10	9	2
+8	40	10	2
+9	53	40	4
+10	50	20	2
+11	20	50	3
+12	40	60	2
+13	50	50	3
+14	20	10	2
+15	10	40	4
+16	40	20	2.5
+17	30	40	2.5
+18	50	30	3.25
+19	45	35	2
+20	35	55	2.5
+21	36	34.75	2
+22	33.5	22	4
+23	25	45	2.5
+24	15	12	2
+25	5	20	3
+26	30	0	4
+27	50	10	3
+28	62	50	4
+29	45	13	2.5
+30	45	2.5	2
+31	23	24.7	2.5
+32	14	1.6	2.5
+33	34	9	2.5
+34	20	1.6	2.5
+35	58	27	3
+36	60	20	3
+37	37.1	14.7	2.6
+38	17.74	43.04	3
+39	55	15	3
+40	45.7	23.66	3
+41	24.5	39.8	2
+42	55.2	5.22	3
+43	10.55	52.36	3
+44	12.5	31.45	3.5
+45	38.3	4.3	3
+46	24.2	6.4	3
+47	44	-2.5	2
+48	6.4	47.4	3
+49	18.1	28	2.5
+50	26.87	37.23	1
+51	26.6	51.33	2.5
+52	15.74	56.55	3
+53	15	51.15	1.5
+54	20.2	18.06	2.3
+55	27.3	22.6	1.8
+56	23.4	20.5	1.2
+57	44.1	7.3	2.5
+58	35.5	27.2	1.2
+59	13.25	46.5	2.4
+60	49.4	4.4	2
+61	31.6	36	1.5
+62	24.5	29.2	1.5
+63	15.8	37.1	1.5
+64	37.2	-2.4	2.2
+65	40.8	-1	1.2
+66	29.5	7.1	1.8
+67	44.82	18.2	2
+68	49.4	15	1.6
+69	41.2	14.5	1.4
+70	17.8	14.7	1.4

tests/resources/circle_queries.txt

+0	start:	42.3113	51.0478
+0	goal:	59.3943	35.0738
+1	start:	7.28857	4.33811
+1	goal:	10.933	14.7456
+2	start:	28.5255	57.3041
+2	goal:	35.7033	32.4758
+3	start:	61.2226	56.8893
+3	goal:	48.6357	38.2136
+4	start:	11.3932	24.5666
+4	goal:	44.1842	61.0589
+5	start:	16.7579	53.4638
+5	goal:	31.003	9.30758
+6	start:	65.1177	22.0149
+6	goal:	61.17	16.2962
+7	start:	4.6053	59.2635
+7	goal:	39.1563	36.6896
+8	start:	60.0969	-4.22661
+8	goal:	5.91287	29.384
+9	start:	45.0552	9.93644
+9	goal:	56.3082	19.5768
+10	start:	58.9605	53.2088
+10	goal:	22.4408	56.7885
+11	start:	55.1958	51.7751
+11	goal:	59.1977	-2.2662
+12	start:	31.9192	17.1741
+12	goal:	47.2135	60.0302
+13	start:	27.6291	5.64244
+13	goal:	43.8565	30.2322
+14	start:	8.70325	34.8771
+14	goal:	56.6349	57.3219
+15	start:	24.9704	-0.121513
+15	goal:	5.87427	28.6967
+16	start:	44.312	61.6471
+16	goal:	9.93112	27.1867
+17	start:	40.7882	35.8865
+17	goal:	16.1553	15.8263
+18	start:	25.2464	21.4667
+18	goal:	56.4315	36.767
+19	start:	3.50384	52.1568
+19	goal:	59.7886	45.2024
+20	start:	44.1913	61.6946
+20	goal:	26.2942	34.8852
+21	start:	15.0327	-1.65489
+21	goal:	7.03989	29.3183
+22	start:	2.35585	60.5385
+22	goal:	54.797	50.7455
+23	start:	8.35529	-3.21333
+23	goal:	26.0371	24.3486
+24	start:	63.8516	44.1515
+24	goal:	12.8495	58.1823
+25	start:	7.07713	31.4065
+25	goal:	40.497	13.1309
+26	start:	34.901	-2.75233
+26	goal:	32.7597	-4.39465
+27	start:	35.7333	5.9447
+27	goal:	24.3921	30.8189
+28	start:	52.773	34.2975
+28	goal:	26.6193	40.2714
+29	start:	51.8091	7.13617
+29	goal:	21.5079	29.5972
+30	start:	27.5499	5.72098
+30	goal:	3.66027	56.0602
+31	start:	50.4648	59.1616
+31	goal:	64.6737	5.30115
+32	start:	20.658	46.8018
+32	goal:	8.02595	33.8899
+33	start:	32.3302	60.8166
+33	goal:	5.96426	34.0627
+34	start:	30.4287	51.6144
+34	goal:	53.8133	28.4159
+35	start:	57.0086	58.6258
+35	goal:	65.7114	30.7433
+36	start:	53.2322	29.3032
+36	goal:	3.39036	8.17497
+37	start:	43.2752	52.951
+37	goal:	11.838	59.4149
+38	start:	45.9323	40.5625
+38	goal:	8.54812	23.8339
+39	start:	41.137	31.1623
+39	goal:	34.688	-1.67694
+40	start:	12.9148	5.35256
+40	goal:	60.0215	6.88571
+41	start:	32.5639	59.1527
+41	goal:	56.2908	51.9554
+42	start:	52.9716	18.0689
+42	goal:	65.2936	43.8161
+43	start:	9.67563	49.1385
+43	goal:	62.2963	4.30874
+44	start:	5.10464	1.28634
+44	goal:	21.4038	41.0583
+45	start:	57.8399	54.8619
+45	goal:	51.7023	23.815
+46	start:	22.4881	42.1935
+46	goal:	14.5652	17.297
+47	start:	10.6108	3.91623
+47	goal:	46.6856	43.4564
+48	start:	17.1212	61.6547
+48	goal:	36.8823	61.6147
+49	start:	6.75267	-3.39522
+49	goal:	8.33106	1.45901
+50	start:	8.12415	22.9693
+50	goal:	9.22204	17.3941
+51	start:	17.4271	35.94
+51	goal:	34.0256	26.2084
+52	start:	32.3028	44.5274
+52	goal:	17.0949	59.5723
+53	start:	31.7747	59.2672
+53	goal:	7.2243	34.0054
+54	start:	63.6721	20.8081
+54	goal:	57.1056	9.16775
+55	start:	32.7623	6.41451
+55	goal:	54.5502	21.3748
+56	start:	3.48173	13.6616
+56	goal:	14.2799	7.77722
+57	start:	65.7012	15.9047
+57	goal:	51.2098	17.7197
+58	start:	9.11622	11.4793
+58	goal:	64.911	16.7554
+59	start:	28.6603	54.1324
+59	goal:	47.3178	20.6214
+60	start:	65.1854	46.4573
+60	goal:	28.0475	24.9325
+61	start:	11.4993	48.3692
+61	goal:	8.65013	26.9941
+62	start:	23.7612	0.948153
+62	goal:	44.3059	29.9562
+63	start:	58.688	38.1355
+63	goal:	63.3858	13.452
+64	start:	61.5983	31.3097
+64	goal:	10.0636	61.3316
+65	start:	29.4291	53.0716
+65	goal:	55.8652	51.2472
+66	start:	24.8255	20.9092
+66	goal:	21.3946	38.6623
+67	start:	4.68936	29.9292
+67	goal:	13.2677	-1.48996
+68	start:	3.89227	44.6636
+68	goal:	29.6609	-4.00834
+69	start:	17.8237	18.9286
+69	goal:	15.9596	-3.61392
+70	start:	60.9689	10.3166
+70	goal:	43.4419	31.6965
+71	start:	3.54928	59.344
+71	goal:	22.0288	27.3067
+72	start:	25.9429	1.48675
+72	goal:	27.0161	25.9931
+73	start:	4.59471	37.2862
+73	goal:	10.2256	25.4115
+74	start:	21.7146	58.6578
+74	goal:	41.3042	-3.09863
+75	start:	16.5112	46.7924
+75	goal:	59.5404	14.8189
+76	start:	8.2762	27.1594
+76	goal:	28.2891	20.1355
+77	start:	58.7939	41.6096
+77	goal:	47.2151	40.3553
+78	start:	32.4678	3.1871
+78	goal:	48.4635	60.5457
+79	start:	47.7089	7.62234
+79	goal:	12.6615	56.2299
+80	start:	8.00967	16.0007
+80	goal:	59.8419	38.5848
+81	start:	64.3562	58.5354
+81	goal:	3.17669	24.3496
+82	start:	23.779	41.9578
+82	goal:	13.2648	9.84625
+83	start:	8.68149	44.172
+83	goal:	9.0923	-1.82479
+84	start:	57.7443	55.4952
+84	goal:	27.1109	38.5942
+85	start:	57.25	17.7444
+85	goal:	2.26782	14.688
+86	start:	62.9226	57.327
+86	goal:	18.8077	13.4382
+87	start:	62.3105	30.8814
+87	goal:	63.7011	23.935
+88	start:	29.7053	50.6047
+88	goal:	59.2889	11.2714
+89	start:	54.8875	21.1252
+89	goal:	34.3343	29.813
+90	start:	13.42	54.4055
+90	goal:	8.722	30.6886
+91	start:	52.8565	7.95299
+91	goal:	33.6929	49.0867
+92	start:	4.48986	2.93917
+92	goal:	36.148	-0.0284564
+93	start:	45.5961	44.7052
+93	goal:	21.3899	15.1501
+94	start:	5.58478	15.5585
+94	goal:	25.4197	59.9413
+95	start:	46.5793	0.304953
+95	goal:	40.4977	12.1915
+96	start:	18.3735	-2.53268
+96	goal:	60.6745	54.8599
+97	start:	9.29441	3.61165
+97	goal:	3.81345	23.3005
+98	start:	61.9047	-2.5052
+98	goal:	21.7512	13.5132
+99	start:	45.1587	40.3311
+99	goal:	17.7359	61.845

tests/resources/circles2D.h

+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#ifndef OMPL_TEST_CIRCLES_2D_
+#define OMPL_TEST_CIRCLES_2D_
+
+#include <fstream>
+#include <vector>
+
+struct Circles2D
+{  
+    struct Circle
+    {
+	Circle(double x, double y, double r) : x_(x), y_(y), r_(r), r2_(r*r)
+	{
+	}
+	
+	double x_, y_, r_, r2_;
+    };
+
+    struct Query
+    {
+	double startX_, startY_, goalX_, goalY_;
+    };
+    
+    Circles2D(void)
+    {
+	minX_ = minY_ = 0.0;
+	maxX_ = maxY_ = 0.0;
+    }
+    
+    void loadCircles(const std::string &filename)
+    {
+	//    std::cout << "Loading " << filename << std::endl;
+	std::ifstream fin(filename.c_str());
+	// ignore first two lines
+	char dummy[4096];
+	fin.getline(dummy, sizeof(dummy));
+	fin.getline(dummy, sizeof(dummy));
+	while (true)
+	{
+	    int id;
+	    double x, y, r;
+	    fin >> id >> x >> y >> r;
+	    if (fin.eof() || !fin.good())
+		break;
+	    circles_.push_back(Circle(x,y,r));
+	    //      std::cout << "Added circle " << id << " at center " << x << ", " << y << " of radius " << r << std::endl;
+	}
+	fin.close();
+	
+	// find a bounding box for the environment
+	if (circles_.empty())
+	{
+	    minX_ = minY_ = 0.0;
+	    maxX_ = maxY_ = 0.0;
+	}
+	else
+	{
+	    minX_ = minY_ = std::numeric_limits<double>::infinity();
+	    maxX_ = maxY_ = -std::numeric_limits<double>::infinity();
+	    for (std::size_t i = 0 ; i < circles_.size() ; ++i)
+	    {
+		if (circles_[i].x_ - circles_[i].r_ < minX_)
+		    minX_ = circles_[i].x_ - circles_[i].r_;
+		if (circles_[i].y_ - circles_[i].r_ < minY_)
+		    minY_ = circles_[i].y_ - circles_[i].r_;
+		if (circles_[i].x_ + circles_[i].r_ > maxX_)
+		    maxX_ = circles_[i].x_ + circles_[i].r_;
+		if (circles_[i].y_ + circles_[i].r_ > maxY_)
+		    maxY_ = circles_[i].y_ + circles_[i].r_;
+	    }
+	}
+	//    std::cout << "Bounding box is [" << minX_ << ", " << minY_ << "] x [" << maxX_ << ", " << maxY_ << "]" << std::endl;
+    }
+    
+    void loadQueries(const std::string &filename)
+    {
+	std::ifstream fin(filename.c_str());
+	while (fin.good() && !fin.eof())
+	{
+	    std::string dummy;
+	    Query q;
+	    fin >> dummy >> dummy >> q.startX_ >> q.startY_;
+	    if (fin.eof())
+		break;
+	    fin >> dummy >> dummy >> q.goalX_ >> q.goalY_;
+	    queries_.push_back(q);
+	}
+    }
+    
+    const Query& getQuery(std::size_t index) const
+    {
+	return queries_[index];
+    }
+    
+    std::size_t getQueryCount(void) const
+    {
+	return queries_.size();
+    }
+    
+    bool noOverlap(double x, double y) const
+    {
+	for (std::size_t i = 0 ; i < circles_.size() ; ++i)
+	{
+	    double dx = circles_[i].x_ - x;
+	    double dy = circles_[i].y_ - y;
+	    if (dx * dx + dy * dy < circles_[i].r2_)
+		return false;
+	}
+	return true;
+    }
+    
+    std::vector<Circle> circles_;
+    std::vector<Query> queries_;
+    double minX_;
+    double maxX_;
+    double minY_;
+    double maxY_;
+};
+
+#endif

tests/resources/environment2D.h

 
 /* Author: Ioan Sucan */
 
-#ifndef ENVIDONMENT_2D_
-#define ENVIDONMENT_2D_
+#ifndef OMPL_TEST_ENVIRONMENT_2D_
+#define OMPL_TEST_ENVIRONMENT_2D_
 
 #include <fstream>
 #include <vector>
     in.close();
 }
 
-static void printEnvironment(std::ostream &out, Environment2D &env)
+static void printEnvironment(std::ostream &out, const Environment2D &env)
 {
 
     for (unsigned int i = 0 ; i < env.grid.size() ; ++i)