[ompl] 02/18: Imported Upstream version 1.2.1+ds1
Leopold Palomo-Avellaneda
leo at alaxarxa.net
Sat Jan 28 18:26:02 UTC 2017
This is an automated email from the git hooks/post-receive script.
lepalom-guest pushed a commit to branch master
in repository ompl.
commit 071eb95fbea8e99f19e9656e046581a6f8adf801
Author: Leopold Palomo-Avellaneda <leo at alaxarxa.net>
Date: Wed Jan 25 09:34:36 2017 +0100
Imported Upstream version 1.2.1+ds1
---
.appveyor.yml | 40 +
.travis.yml | 51 +-
CMakeLists.txt | 99 +--
CMakeModules/CPackSettings.cmake | 3 -
CMakeModules/CompilerSettings.cmake | 9 +-
CMakeModules/FindEigen3.cmake | 85 +-
CMakeModules/FindGCCXML.cmake | 26 -
CMakeModules/FindPython.cmake | 15 +-
CMakeModules/Findcastxml.cmake | 60 ++
CMakeModules/OMPLUtils.cmake | 47 +-
CMakeModules/OMPLVersion.cmake | 6 +-
CMakeModules/PythonBindingsUtils.cmake | 67 +-
CMakeModules/create_symlinks.sh.in | 3 -
CMakeModules/generate_header.cmake | 4 +-
CMakeModules/ompl.pc.in | 4 +-
CMakeModules/workaround_for_gccxml_bug.cmake | 11 -
README.md | 11 +-
appveyor.yml | 41 -
demos/CForestCircleGridBenchmark.cpp | 2 +-
demos/CMakeLists.txt | 10 +-
demos/GeometricCarPlanning.cpp | 4 +-
demos/HybridSystemPlanning.cpp | 10 +-
demos/HypercubeBenchmark.cpp | 4 +-
demos/KinematicChainBenchmark.cpp | 2 +-
demos/Koules/Koules.cpp | 2 +-
demos/Koules/KoulesSetup.cpp | 2 +-
demos/Koules/KoulesSimulator.cpp | 6 +-
demos/Koules/KoulesSimulator.h | 5 +-
demos/Koules/KoulesStateSpace.cpp | 2 +-
demos/LTLWithTriangulation.cpp | 13 +-
demos/OptimalPlanning.cpp | 31 +-
demos/OptimalPlanning.py | 4 +-
demos/PlannerData.cpp | 12 +-
demos/Point2DPlanning.cpp | 2 +-
demos/Point2DPlanning.py | 4 +-
demos/RandomWalkPlanner.py | 3 +-
demos/RigidBodyPlanning.cpp | 4 +-
demos/RigidBodyPlanningWithControls.cpp | 11 +-
demos/RigidBodyPlanningWithIK.cpp | 4 +-
...RigidBodyPlanningWithIntegrationAndControls.cpp | 2 +-
.../RigidBodyPlanningWithODESolverAndControls.cpp | 2 +-
demos/StateSampling.cpp | 6 +-
demos/ThunderLightning.cpp | 4 +-
demos/TriangulationDemo.cpp | 4 +-
demos/VFRRT/VectorFieldConservative.cpp | 175 ++++
demos/VFRRT/VectorFieldNonconservative.cpp | 123 +++
demos/VFRRT/plotConservative.py | 69 ++
demos/VFRRT/plotNonconservative.py | 54 ++
doc/Doxyfile | 2 +-
doc/css/ompl.css | 4 +
doc/header.html | 3 +-
doc/header.html.in | 1 -
doc/markdown/CForest.md | 8 +-
doc/markdown/CForest.md.in | 8 +-
doc/markdown/FAQ.md | 2 +-
doc/markdown/FindOMPL.cmake | 14 +-
doc/markdown/api_overview.md | 6 +-
doc/markdown/api_overview.md.in | 6 +-
doc/markdown/benchmark.md | 8 +-
doc/markdown/boost.md | 192 -----
doc/markdown/buildOptions.md | 5 +-
doc/markdown/citations.md | 43 +-
doc/markdown/code/svc.cpp | 2 +-
doc/markdown/contact.md | 5 +
doc/markdown/contrib.md | 2 +-
doc/markdown/developers.md | 4 +-
doc/markdown/download.md.in | 1 +
doc/markdown/education.md | 2 +-
doc/markdown/installPyPlusPlus.md | 19 +-
doc/markdown/installation.md | 303 +++----
doc/markdown/integration.md | 45 ++
doc/markdown/mainpage.md.in | 3 +-
doc/markdown/optimalPlanning.md | 2 +
doc/markdown/plannerarena.md | 4 +-
doc/markdown/planners.md | 10 +-
doc/markdown/python.md | 8 +-
doc/markdown/register.md | 5 +
doc/markdown/releaseNotes.md | 18 +
doc/markdown/thirdparty.md | 9 +-
doc/mkdocs.sh | 2 +-
install-ompl-ubuntu.sh | 111 +++
install-ompl-ubuntu.sh.in | 111 +++
py-bindings/CMakeLists.txt | 22 +-
py-bindings/bindings_generator.py.in | 67 +-
py-bindings/generate_bindings.py | 125 +--
py-bindings/headers_base.txt | 116 +--
py-bindings/headers_control.txt | 51 +-
py-bindings/headers_geometric.txt | 74 +-
py-bindings/headers_morse.txt | 20 +-
py-bindings/headers_tools.txt | 16 +-
py-bindings/headers_util.txt | 14 +-
py-bindings/ompl/__init__.py | 18 +-
py-bindings/ompl/bindings_generator.py | 67 +-
py-bindings/ompl/control/__init__.py | 2 +-
py-bindings/ompl_py_base.h | 2 +-
py-bindings/ompl_py_control.h | 5 +-
py-bindings/ompl_py_geometric.h | 6 +-
py-bindings/ompl_py_morse.h | 2 +-
py-bindings/ompl_py_tools.h | 2 +-
.../{py_boost_function.hpp => py_std_function.hpp} | 21 +-
scripts/ompl_benchmark_statistics.py | 26 +-
scripts/plannerarena/global.R | 73 ++
scripts/plannerarena/plannerarena | 8 +-
scripts/plannerarena/{app.R => server.R} | 626 +++++++--------
scripts/plannerarena/ui.R | 112 +++
scripts/plannerarena/www/plannerarena.css | 2 +-
src/CMakeLists.txt | 35 -
src/boost/python/converter/registered.hpp | 104 +++
.../python/converter/shared_ptr_from_python.hpp | 63 ++
.../python/converter/shared_ptr_to_python.hpp | 28 +
src/boost/python/detail/is_shared_ptr.hpp | 17 +
src/boost/python/detail/is_xxx.hpp | 13 +
src/boost/python/detail/value_is_shared_ptr.hpp | 17 +
src/boost/python/detail/value_is_xxx.hpp | 32 +
src/boost/python/object/inheritance.hpp | 132 +++
src/boost/python/to_python_indirect.hpp | 109 +++
src/boost/python/to_python_value.hpp | 171 ++++
src/ompl/CMakeLists.txt | 4 +-
src/ompl/base/GenericParam.h | 7 +-
src/ompl/base/Goal.h | 8 +-
src/ompl/base/MotionValidator.h | 4 +-
src/ompl/base/OptimizationObjective.h | 12 +-
src/ompl/base/Path.h | 8 +-
src/ompl/base/Planner.h | 31 +-
src/ompl/base/PlannerData.h | 11 +-
src/ompl/base/PlannerTerminationCondition.h | 8 +-
src/ompl/base/ProblemDefinition.h | 15 +-
src/ompl/base/ProjectionEvaluator.h | 8 +-
src/ompl/base/ScopedState.h | 2 +-
src/ompl/base/SolutionNonExistenceProof.h | 2 +-
src/ompl/base/SpaceInformation.h | 24 +-
src/ompl/base/State.h | 2 +-
src/ompl/base/StateSampler.h | 30 +-
src/ompl/base/StateSpace.h | 13 +-
src/ompl/base/StateStorage.h | 6 +-
src/ompl/base/StateValidityChecker.h | 2 +-
src/ompl/base/TypedSpaceInformation.h | 145 ++++
.../TypedStateValidityChecker.h} | 62 +-
src/ompl/base/ValidStateSampler.h | 12 +-
src/ompl/base/goals/GoalLazySamples.h | 13 +-
src/ompl/base/goals/GoalRegion.h | 2 +-
src/ompl/base/goals/GoalState.h | 2 +-
src/ompl/base/goals/src/GoalLazySamples.cpp | 32 +-
src/ompl/base/goals/src/GoalRegion.cpp | 2 +-
src/ompl/base/goals/src/GoalStates.cpp | 5 +-
.../VFMechanicalWorkOptimizationObjective.h | 99 +++
.../VFUpstreamCriterionOptimizationObjective.h | 115 +++
.../src/PathLengthOptimizationObjective.cpp | 7 +-
src/ompl/base/samplers/InformedStateSampler.h | 14 +-
.../samplers/MinimumClearanceValidStateSampler.h | 94 +++
.../samplers/informed/PathLengthDirectInfSampler.h | 4 +-
.../informed/src/PathLengthDirectInfSampler.cpp | 6 +-
.../samplers/src/GaussianValidStateSampler.cpp | 4 +-
.../src/MaximizeClearanceValidStateSampler.cpp | 4 +-
.../src/MinimumClearanceValidStateSampler.cpp | 94 +++
src/ompl/base/spaces/SO3StateSpace.h | 2 +-
src/ompl/base/spaces/src/DiscreteStateSpace.cpp | 2 +-
src/ompl/base/spaces/src/RealVectorStateSpace.cpp | 9 +-
src/ompl/base/spaces/src/SO2StateSpace.cpp | 4 +-
src/ompl/base/spaces/src/SO3StateSpace.cpp | 17 +-
src/ompl/base/spaces/src/TimeStateSpace.cpp | 4 +-
src/ompl/base/src/Goal.cpp | 2 +-
src/ompl/base/src/OptimizationObjective.cpp | 6 +-
src/ompl/base/src/Planner.cpp | 22 +-
src/ompl/base/src/PlannerData.cpp | 28 +-
src/ompl/base/src/PlannerTerminationCondition.cpp | 31 +-
src/ompl/base/src/ProblemDefinition.cpp | 33 +-
src/ompl/base/src/ProjectionEvaluator.cpp | 36 +-
src/ompl/base/src/SpaceInformation.cpp | 4 +-
src/ompl/base/src/StateSpace.cpp | 65 +-
src/ompl/base/src/StateStorage.cpp | 6 +-
src/ompl/base/src/ValidStateSampler.cpp | 6 +-
src/ompl/contrib/rrt_star/RRTstar.h | 2 -
src/ompl/control/ControlSampler.h | 12 +-
src/ompl/control/ControlSpace.h | 10 +-
src/ompl/control/DirectedControlSampler.h | 12 +-
src/ompl/control/ODESolver.h | 15 +-
src/ompl/control/PlannerData.h | 2 +-
src/ompl/control/SimpleSetup.h | 2 +-
src/ompl/control/SpaceInformation.h | 4 +-
src/ompl/control/StatePropagator.h | 3 +-
src/ompl/control/SteeredControlSampler.h | 1 +
src/ompl/control/planners/est/EST.h | 6 +-
src/ompl/control/planners/est/src/EST.cpp | 16 +-
src/ompl/control/planners/kpiece/KPIECE1.h | 4 +-
src/ompl/control/planners/kpiece/src/KPIECE1.cpp | 20 +-
src/ompl/control/planners/ltl/Automaton.h | 6 +-
src/ompl/control/planners/ltl/LTLPlanner.h | 8 +-
.../control/planners/ltl/LTLProblemDefinition.h | 2 +-
.../control/planners/ltl/LTLSpaceInformation.h | 2 +-
src/ompl/control/planners/ltl/ProductGraph.h | 38 +-
.../planners/ltl/PropositionalDecomposition.h | 4 +-
src/ompl/control/planners/ltl/World.h | 31 +-
src/ompl/control/planners/ltl/src/Automaton.cpp | 53 +-
src/ompl/control/planners/ltl/src/LTLPlanner.cpp | 78 +-
.../planners/ltl/src/LTLProblemDefinition.cpp | 36 +
src/ompl/control/planners/ltl/src/ProductGraph.cpp | 177 +++--
.../ltl/src/PropositionalDecomposition.cpp | 36 +
src/ompl/control/planners/ltl/src/World.cpp | 71 +-
src/ompl/control/planners/pdst/PDST.h | 16 +-
src/ompl/control/planners/pdst/src/PDST.cpp | 24 +-
src/ompl/control/planners/rrt/RRT.h | 6 +-
src/ompl/control/planners/rrt/src/RRT.cpp | 17 +-
src/ompl/control/planners/sst/SST.h | 290 +++++++
src/ompl/control/planners/sst/src/SST.cpp | 453 +++++++++++
src/ompl/control/planners/syclop/Decomposition.h | 2 +-
.../control/planners/syclop/GridDecomposition.h | 6 +-
src/ompl/control/planners/syclop/Syclop.h | 41 +-
src/ompl/control/planners/syclop/SyclopRRT.h | 2 +-
.../planners/syclop/src/GridDecomposition.cpp | 2 +-
src/ompl/control/planners/syclop/src/Syclop.cpp | 18 +-
src/ompl/control/planners/syclop/src/SyclopEST.cpp | 4 +-
src/ompl/control/planners/syclop/src/SyclopRRT.cpp | 7 +-
.../control/spaces/src/DiscreteControlSpace.cpp | 2 +-
.../control/spaces/src/RealVectorControlSpace.cpp | 9 +-
src/ompl/control/src/ControlSpace.cpp | 4 +-
src/ompl/control/src/PlannerData.cpp | 2 +-
.../control/src/SimpleDirectedControlSampler.cpp | 2 +-
src/ompl/datastructures/BinaryHeap.h | 8 +-
src/ompl/datastructures/DynamicSSSP.h | 6 +-
src/ompl/datastructures/GreedyKCenters.h | 3 +-
src/ompl/datastructures/Grid.h | 16 +-
src/ompl/datastructures/GridB.h | 8 +-
src/ompl/datastructures/GridN.h | 2 +-
src/ompl/datastructures/LPAstarOnGraph.h | 21 +-
src/ompl/datastructures/NearestNeighbors.h | 5 +-
src/ompl/datastructures/NearestNeighborsFLANN.h | 14 +-
src/ompl/datastructures/NearestNeighborsGNAT.h | 14 +-
.../NearestNeighborsGNATNoThreadSafety.h | 14 +-
src/ompl/datastructures/Permutation.h | 24 +-
src/ompl/extensions/morse/MorseControlSpace.h | 2 +-
src/ompl/extensions/morse/MorseEnvironment.h | 38 +-
src/ompl/extensions/morse/MorseGoal.h | 10 +-
src/ompl/extensions/morse/MorseProjection.h | 12 +-
src/ompl/extensions/morse/MorseSimpleSetup.h | 6 +-
src/ompl/extensions/morse/MorseStateSpace.h | 2 +-
.../extensions/morse/MorseTerminationCondition.h | 12 +-
src/ompl/extensions/morse/src/MorseGoal.cpp | 4 +-
.../extensions/morse/src/MorseStatePropagator.cpp | 2 +-
src/ompl/extensions/morse/src/MorseStateSpace.cpp | 6 +-
src/ompl/extensions/opende/OpenDEEnvironment.h | 8 +-
.../extensions/opende/src/OpenDEEnvironment.cpp | 3 +-
.../extensions/opende/src/OpenDESimpleSetup.cpp | 4 +-
.../extensions/opende/src/OpenDEStateSpace.cpp | 3 +-
.../extensions/triangle/TriangularDecomposition.h | 1 -
.../triangle/src/TriangularDecomposition.cpp | 79 +-
src/ompl/geometric/HillClimbing.h | 2 +-
src/ompl/geometric/PathGeometric.h | 6 +-
src/ompl/geometric/PathHybridization.h | 2 +-
src/ompl/geometric/PathSimplifier.h | 6 +-
src/ompl/geometric/SimpleSetup.h | 2 +-
.../geometric/planners/AnytimePathShortening.cpp | 14 +-
src/ompl/geometric/planners/bitstar/BITstar.h | 81 +-
.../planners/bitstar/datastructures/IdGenerator.h | 16 +-
.../bitstar/datastructures/IntegratedQueue.h | 195 +++--
.../planners/bitstar/datastructures/Vertex.h | 40 +-
.../bitstar/datastructures/src/IntegratedQueue.cpp | 196 +++--
.../planners/bitstar/datastructures/src/Vertex.cpp | 62 +-
.../geometric/planners/bitstar/src/BITstar.cpp | 312 ++++----
src/ompl/geometric/planners/cforest/CForest.h | 8 +-
.../planners/cforest/CForestStateSampler.h | 4 +-
.../planners/cforest/CForestStateSpaceWrapper.h | 2 +-
.../geometric/planners/cforest/src/CForest.cpp | 26 +-
.../planners/cforest/src/CForestStateSampler.cpp | 6 +-
src/ompl/geometric/planners/est/BiEST.h | 181 +++++
src/ompl/geometric/planners/est/EST.h | 144 +---
.../geometric/planners/est/{EST.h => ProjEST.h} | 20 +-
src/ompl/geometric/planners/est/src/BiEST.cpp | 317 ++++++++
src/ompl/geometric/planners/est/src/EST.cpp | 136 ++--
.../planners/est/src/{EST.cpp => ProjEST.cpp} | 42 +-
.../planners/experience/LightningRetrieveRepair.h | 2 +-
.../planners/experience/ThunderRetrieveRepair.h | 2 +-
.../experience/src/LightningRetrieveRepair.cpp | 4 +-
.../experience/src/ThunderRetrieveRepair.cpp | 6 +-
src/ompl/geometric/planners/fmt/BFMT.h | 624 +++++++++++++++
src/ompl/geometric/planners/fmt/FMT.h | 69 +-
src/ompl/geometric/planners/fmt/src/BFMT.cpp | 884 +++++++++++++++++++++
src/ompl/geometric/planners/fmt/src/FMT.cpp | 235 +++++-
src/ompl/geometric/planners/kpiece/BKPIECE1.h | 4 +-
.../geometric/planners/kpiece/Discretization.h | 12 +-
src/ompl/geometric/planners/kpiece/KPIECE1.h | 4 +-
src/ompl/geometric/planners/kpiece/LBKPIECE1.h | 4 +-
.../geometric/planners/kpiece/src/BKPIECE1.cpp | 20 +-
src/ompl/geometric/planners/kpiece/src/KPIECE1.cpp | 20 +-
.../geometric/planners/kpiece/src/LBKPIECE1.cpp | 24 +-
src/ompl/geometric/planners/pdst/PDST.h | 14 +-
src/ompl/geometric/planners/pdst/src/PDST.cpp | 16 +-
.../geometric/planners/prm/ConnectionStrategy.h | 16 +-
src/ompl/geometric/planners/prm/LazyPRM.h | 10 +-
src/ompl/geometric/planners/prm/PRM.h | 22 +-
src/ompl/geometric/planners/prm/SPARS.h | 30 +-
src/ompl/geometric/planners/prm/SPARStwo.h | 41 +-
.../geometric/planners/prm/src/GoalVisitor.hpp | 2 +
src/ompl/geometric/planners/prm/src/LazyPRM.cpp | 33 +-
src/ompl/geometric/planners/prm/src/PRM.cpp | 41 +-
src/ompl/geometric/planners/prm/src/SPARS.cpp | 73 +-
src/ompl/geometric/planners/prm/src/SPARStwo.cpp | 67 +-
src/ompl/geometric/planners/rrt/BiTRRT.h | 8 +-
src/ompl/geometric/planners/rrt/LBTRRT.h | 12 +-
src/ompl/geometric/planners/rrt/LazyLBTRRT.h | 12 +-
src/ompl/geometric/planners/rrt/LazyRRT.h | 6 +-
src/ompl/geometric/planners/rrt/RRT.h | 6 +-
src/ompl/geometric/planners/rrt/RRTConnect.h | 12 +-
src/ompl/geometric/planners/rrt/RRTstar.h | 8 +-
src/ompl/geometric/planners/rrt/TRRT.h | 6 +-
src/ompl/geometric/planners/rrt/VFRRT.h | 155 ++++
src/ompl/geometric/planners/rrt/pRRT.h | 13 +-
src/ompl/geometric/planners/rrt/src/BiTRRT.cpp | 23 +-
.../geometric/planners/rrt/src/InformedRRTstar.cpp | 6 +
src/ompl/geometric/planners/rrt/src/LBTRRT.cpp | 22 +-
src/ompl/geometric/planners/rrt/src/LazyLBTRRT.cpp | 44 +-
src/ompl/geometric/planners/rrt/src/LazyRRT.cpp | 18 +-
src/ompl/geometric/planners/rrt/src/RRT.cpp | 18 +-
src/ompl/geometric/planners/rrt/src/RRTConnect.cpp | 18 +-
src/ompl/geometric/planners/rrt/src/RRTstar.cpp | 25 +-
src/ompl/geometric/planners/rrt/src/TRRT.cpp | 18 +-
src/ompl/geometric/planners/rrt/src/VFRRT.cpp | 293 +++++++
src/ompl/geometric/planners/rrt/src/pRRT.cpp | 25 +-
src/ompl/geometric/planners/sbl/SBL.h | 4 +-
src/ompl/geometric/planners/sbl/pSBL.h | 19 +-
src/ompl/geometric/planners/sbl/src/SBL.cpp | 18 +-
src/ompl/geometric/planners/sbl/src/pSBL.cpp | 23 +-
src/ompl/geometric/planners/sst/SST.h | 299 +++++++
src/ompl/geometric/planners/sst/src/SST.cpp | 423 ++++++++++
src/ompl/geometric/planners/stride/STRIDE.h | 6 +-
src/ompl/geometric/planners/stride/src/STRIDE.cpp | 16 +-
src/ompl/geometric/src/GeneticSearch.cpp | 2 +-
src/ompl/geometric/src/PathGeometric.cpp | 10 +-
src/ompl/geometric/src/PathHybridization.cpp | 8 +-
src/ompl/geometric/src/PathSimplifier.cpp | 6 +-
src/ompl/tools/benchmark/Benchmark.h | 8 +-
src/ompl/tools/benchmark/src/Benchmark.cpp | 148 ++--
src/ompl/tools/benchmark/src/MachineSpecs.cpp | 8 +-
src/ompl/tools/config/SelfConfig.h | 2 +
src/ompl/tools/config/src/SelfConfig.cpp | 42 +-
src/ompl/tools/debug/PlannerMonitor.h | 10 +-
src/ompl/tools/debug/Profiler.h | 15 +-
src/ompl/tools/debug/src/PlannerMonitor.cpp | 14 +-
src/ompl/tools/debug/src/Profiler.cpp | 15 +-
src/ompl/tools/experience/ExperienceSetup.h | 2 +-
src/ompl/tools/lightning/DynamicTimeWarp.h | 2 +-
src/ompl/tools/lightning/Lightning.h | 2 +-
src/ompl/tools/lightning/LightningDB.h | 4 +-
src/ompl/tools/lightning/src/LightningDB.cpp | 2 +-
src/ompl/tools/multiplan/ParallelPlan.h | 6 +-
src/ompl/tools/multiplan/src/ParallelPlan.cpp | 9 +-
src/ompl/tools/thunder/SPARSdb.h | 35 +-
src/ompl/tools/thunder/Thunder.h | 4 +-
src/ompl/tools/thunder/ThunderDB.h | 4 +-
src/ompl/tools/thunder/src/SPARSdb.cpp | 31 +-
src/ompl/tools/thunder/src/Thunder.cpp | 12 +-
src/ompl/util/ClassForward.h | 4 +-
src/ompl/util/Console.h | 4 +-
.../ompl_py_control.h => src/ompl/util/Hash.h | 39 +-
src/ompl/util/ProlateHyperspheroid.h | 4 +-
src/ompl/util/RandomNumbers.h | 45 +-
src/ompl/util/Time.h | 37 +-
src/ompl/util/src/Console.cpp | 8 +-
src/ompl/util/src/GeometricEquations.cpp | 11 +-
src/ompl/util/src/PPM.cpp | 5 +-
src/ompl/util/src/ProlateHyperspheroid.cpp | 6 +-
src/ompl/util/src/RandomNumbers.cpp | 103 ++-
tests/CMakeLists.txt | 122 +--
tests/base/StateSpaceTest.h | 13 +
tests/base/ptc.cpp | 6 +-
tests/base/state_operations.cpp | 6 +-
tests/base/state_spaces.cpp | 2 +-
tests/datastructures/nearestneighbors.cpp | 14 +-
tests/extensions/morse/morse_plan.cpp | 10 +-
tests/geometric/2d/2DmapSetup1.h | 2 +-
tests/geometric/2d/2denvs.cpp | 30 +-
tests/geometric/test_geometric.py | 12 +
tests/regression_tests/RegressionTest.cpp | 13 +-
.../RegressionTestCirclesProblem.inl.h | 22 +-
tests/regression_tests/VERSIONS | 3 +
tests/util/random/random.cpp | 9 +-
...boost_function.cpp => test_py_std_function.cpp} | 16 +-
...y_boost_function.py => test_py_std_function.py} | 6 +-
378 files changed, 10308 insertions(+), 3699 deletions(-)
diff --git a/.appveyor.yml b/.appveyor.yml
new file mode 100644
index 0000000..65dc84a
--- /dev/null
+++ b/.appveyor.yml
@@ -0,0 +1,40 @@
+# AppVeyor file
+# http://www.appveyor.com/docs/appveyor-yml
+
+version: "{build}"
+os: Visual Studio 2015
+
+clone_folder: C:\projects\ompl
+clone_depth: 1
+
+branches:
+ only:
+ - master
+platform: x64
+
+environment:
+ CTEST_OUTPUT_ON_FAILURE: 1
+ BOOST_ROOT: C:\Libraries\boost_1_59_0
+ BOOST_LIBRARYDIR: C:\Libraries\boost_1_59_0\lib64-msvc-14.0
+
+configuration: Release
+
+before_build:
+ - cmd: set
+ - cmd: mkdir build
+ - cmd: cd build
+ - cmd: cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%configuration% -DOMPL_REGISTRATION=OFF -DOMPL_BUILD_DEMOS=OFF -DBOOST_ROOT="%BOOST_ROOT%" -DBOOST_LIBRARYDIR="%BOOST_LIBRARYDIR%" ..
+
+build:
+ project: C:\projects\ompl\build\ompl.sln
+ parallel: true
+
+after_build:
+ - cmd: cmake --build . --target package --config %configuration%
+
+# tests seem to hang
+#test_script:
+# - cmd: ctest -C %configuration%
+
+artifacts:
+ - path: 'build\omplapp*.zip'
diff --git a/.travis.yml b/.travis.yml
index 94dec2a..58add4d 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,19 +1,20 @@
-sudo: false
cache:
apt: true
directories:
- - /home/travis/build/ompl/ompl/build/pyplusplus
+ - ${HOME}/castxml
+ - /usr/local
language: cpp
-os:
- - linux
- - osx
-
-env:
-# - BUILD_TYPE=Debug
- - BUILD_TYPE=Release
-
matrix:
+ include:
+ - os: linux
+ dist: trusty
+ sudo: required
+ compiler: gcc
+ env: PYTHONPATH=/usr/local/lib/python2.7/dist-packages
+ - os: osx
+ osx_image: xcode7.2
+ compiler: clang
allow_failures:
- os: osx
@@ -24,13 +25,21 @@ addons:
- python-dev
- libode-dev
- libeigen3-dev
+ - python-pip
install:
- if [ "$TRAVIS_OS_NAME" = "osx" ]; then
- brew update;
- brew outdated boost || brew upgrade boost;
- brew install ode eigen;
+ brew install boost-python ode eigen python cmake;
+ if [ ! -e ${HOME}/castxml ]; then
+ curl https://midas3.kitware.com/midas/download/item/318762/castxml-macosx.tar.gz | tar zxf - -C ${HOME};
+ fi;
+ else
+ if [ ! -e ${HOME}/castxml ]; then
+ wget -O - https://midas3.kitware.com/midas/download/item/318227/castxml-linux.tar.gz | tar zxf - -C ${HOME};
+ fi;
fi
+ - sudo -H pip -v install https://github.com/gccxml/pygccxml/archive/v1.7.2.tar.gz https://bitbucket.org/ompl/pyplusplus/get/1.6.tar.gz
+
script:
# Create build directory
@@ -38,17 +47,11 @@ script:
- cd build
# Configure
- - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DOMPL_LOCAL_PYPLUSPLUS_INSTALL=ON -DOMPL_REGISTRATION=OFF -DCMAKE_INSTALL_PREFIX=tmp ..
-
- # Skip Python bindings on OS X, since brew doesn't have a compiler that can compile gccxml.
- # On Linux, we need to make sure cmake finds the locally installed gccxml
- # and not the one in /usr/bin
- - if [ "$TRAVIS_OS_NAME" = "linux" ]; then
- if [ ! -x "pyplusplus/bin/gccxml" ]; then make installpyplusplus; fi;
- rm CMakeCache.txt;
- cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DOMPL_LOCAL_PYPLUSPLUS_INSTALL=ON -DOMPL_REGISTRATION=OFF -DCMAKE_INSTALL_PREFIX=tmp -DGCCXML=pyplusplus/bin/gccxml ..;
- make -j2 update_bindings;
- fi
+ - cmake -DOMPL_REGISTRATION=OFF -DCMAKE_INSTALL_PREFIX=tmp -DCASTXML:FILEPATH=${HOME}/castxml/bin/castxml ..
+
+ # TIMES OUT BECAUSE IT DOES NOT PRODUCE OUTPUT FOR MORE THAN 10 MINUTES
+ # generate python bindings
+ #- make -j4 update_bindings
# Build
- make -j4
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 46ae14d..223e123 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,7 @@
cmake_minimum_required(VERSION 2.8)
+cmake_policy(SET CMP0017 NEW)
if(NOT ${CMAKE_VERSION} VERSION_LESS 3.0.0)
- cmake_policy(SET CMP0042 OLD)
+ cmake_policy(SET CMP0042 NEW)
endif()
project(ompl CXX C)
@@ -23,7 +24,16 @@ if(MSVC OR MSVC90 OR MSVC10)
set(MSVC ON)
endif (MSVC OR MSVC90 OR MSVC10)
-set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
+# add "${prefix}/share/cmake/Modules", which is where Eigen3's module is in MacPorts.
+if(${CMAKE_VERSION} VERSION_LESS 2.8.12)
+ get_filename_component(CMAKE_ROOT_DIR "${CMAKE_ROOT}" PATH)
+else()
+ get_filename_component(CMAKE_ROOT_DIR "${CMAKE_ROOT}" DIRECTORY)
+endif()
+set(CMAKE_MODULE_PATH
+ "${CMAKE_MODULE_PATH}"
+ "${CMAKE_ROOT_DIR}/cmake/Modules"
+ "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
include(GNUInstallDirs)
include(CompilerSettings)
include(OMPLVersion)
@@ -52,36 +62,9 @@ else(IS_ICPC)
add_definitions(-DBOOST_TEST_DYN_LINK)
endif(IS_ICPC)
-# Do one quiet find_package on Boost to see if it is recent enough to
-# have the try_join_for call
-find_package(Boost QUIET 1.50)
-
-# If Boost is not found at all, the check for version 1.50 below will die in
-# the most ungraceful manner because Boost_VERSION is not defined. Define the
-# variable here for a more useful error.
-if (NOT ${Boost_FOUND})
- set(Boost_VERSION 0)
-endif()
-
-# try_join_for requires the chrono library, so if we will use
-# try_join_for, we need to include the chrono component
-# Must recheck the Boost version, since update_bindings will re-run CMake
-# and this will pass for versions of Boost < 1.50
-if (${Boost_FOUND} AND ${Boost_VERSION} GREATER 104900) # Boost version is at least 1.50
- # we're using chrono
- find_package(Boost COMPONENTS date_time thread serialization filesystem system program_options unit_test_framework chrono REQUIRED)
-else()
- # don't use chrono
- find_package(Boost COMPONENTS date_time thread serialization filesystem system program_options unit_test_framework REQUIRED)
-endif()
+find_package(Boost 1.54 COMPONENTS serialization filesystem system program_options unit_test_framework REQUIRED)
include_directories(SYSTEM ${Boost_INCLUDE_DIR})
-if(${Boost_VERSION} LESS 105300)
- # Include bundled version of boost::odeint if it isn't installed natively
- set(ODEINT_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/external")
- include_directories(SYSTEM "${ODEINT_INCLUDE_DIR}")
-endif()
-
# on OS X we need to check whether to use libc++ or libstdc++ with clang++
if(APPLE AND CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
include(GetPrerequisites)
@@ -112,7 +95,7 @@ find_package(Boost COMPONENTS python QUIET)
find_package(Eigen3 QUIET)
if (EIGEN3_FOUND)
set(OMPL_HAVE_EIGEN3 1)
- include_directories("${EIGEN3_INCLUDE_DIRS}")
+ include_directories("${EIGEN3_INCLUDE_DIR}")
endif()
# MORSE is only needed for Modular OpenRobots Simulation Engine bindings
@@ -143,15 +126,6 @@ endif()
# R is needed for running Planner Arena locally
find_program(R_EXEC R)
-option(OMPL_LOCAL_PYPLUSPLUS_INSTALL "Whether Py++ and dependencies should be installed in the build directory" OFF)
-set(SUDO "sudo")
-set(CMAKE_GCCXML_ARGS "")
-if(OMPL_LOCAL_PYPLUSPLUS_INSTALL)
- set(SUDO "")
- set(CMAKE_GCCXML_ARGS "-DCMAKE_INSTALL_PREFIX=${PROJECT_BINARY_DIR}/pyplusplus")
- set(DISTUTILS_ARGS "--prefix=${PROJECT_BINARY_DIR}/pyplusplus")
-endif()
-
add_subdirectory(py-bindings)
add_subdirectory(src)
add_subdirectory(tests)
@@ -159,27 +133,30 @@ add_subdirectory(demos)
add_subdirectory(scripts)
add_subdirectory(doc)
-if (NOT MSVC)
- target_link_flags(ompl)
- set(PKG_NAME "ompl")
- set(PKG_DESC "The Open Motion Planning Library")
- set(PKG_OMPL_LIBS "-lompl ${ompl_LINK_FLAGS}")
- set(pkg_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/ompl.pc")
- configure_file("${pkg_conf_file}.in" "${pkg_conf_file}" @ONLY)
- install(FILES "${pkg_conf_file}"
- DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig/
- COMPONENT ompl
- RENAME "ompl${OMPL_INSTALL_SUFFIX}.pc")
-
- install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/doc/markdown/FindOMPL.cmake"
- DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/ompl${OMPL_INSTALL_SUFFIX}"
- COMPONENT ompl
- RENAME ompl-config.cmake)
-
- install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/ompl.conf"
- DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/ompl${OMPL_INSTALL_SUFFIX}"
- COMPONENT ompl)
-endif()
+target_link_flags(ompl)
+set(PKG_NAME "ompl")
+set(PKG_DESC "The Open Motion Planning Library")
+set(PKG_EXTERNAL_DEPS "${ompl_PKG_DEPS}")
+set(PKG_OMPL_LIBS "-lompl ${ompl_LINK_FLAGS}")
+set(pkg_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/ompl.pc")
+configure_file("${pkg_conf_file}.in" "${pkg_conf_file}" @ONLY)
+install(FILES "${pkg_conf_file}"
+ DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig/
+ COMPONENT ompl
+ RENAME "ompl${OMPL_INSTALL_SUFFIX}.pc")
+
+install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/doc/markdown/FindOMPL.cmake"
+ DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/ompl${OMPL_INSTALL_SUFFIX}"
+ COMPONENT ompl
+ RENAME ompl-config.cmake)
+
+install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/ompl.conf"
+ DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/ompl${OMPL_INSTALL_SUFFIX}"
+ COMPONENT ompl)
+
+# script to install ompl on Ubuntu
+configure_file("${CMAKE_CURRENT_SOURCE_DIR}/install-ompl-ubuntu.sh.in"
+ "${CMAKE_CURRENT_SOURCE_DIR}/install-ompl-ubuntu.sh" @ONLY)
# uninstall target
configure_file(
diff --git a/CMakeModules/CPackSettings.cmake b/CMakeModules/CPackSettings.cmake
index 8daab95..e4f946a 100644
--- a/CMakeModules/CPackSettings.cmake
+++ b/CMakeModules/CPackSettings.cmake
@@ -35,10 +35,7 @@ set(CPACK_SOURCE_IGNORE_FILES
"/html/"
"/bindings/"
"TODO"
- "exposed_decl.pypp.txt"
"ompl.pc$"
- "installPyPlusPlus.bat$"
- "installPyPlusPlus.sh$"
"create_symlinks.sh$"
"uninstall_symlinks.sh$"
"config.h$"
diff --git a/CMakeModules/CompilerSettings.cmake b/CMakeModules/CompilerSettings.cmake
index ecef2db..cba0f87 100644
--- a/CMakeModules/CompilerSettings.cmake
+++ b/CMakeModules/CompilerSettings.cmake
@@ -1,3 +1,8 @@
+# force C++11 mode (MS Visual Studio doesn't know about this flag)
+if(NOT MSVC)
+ add_definitions(-std=c++11)
+endif()
+
if(CMAKE_COMPILER_IS_GNUCXX)
add_definitions(-W -Wall -Wextra #-Wconversion
-Wcast-qual -Wwrite-strings -Wunreachable-code -Wpointer-arith
@@ -7,13 +12,13 @@ if(CMAKE_COMPILER_IS_GNUCXX)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS_RELEASE}")
endif(CMAKE_COMPILER_IS_GNUCXX)
if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
- add_definitions(-W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas -Qunused-arguments -Wno-c++11-extensions)
+ add_definitions(-W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas -Qunused-arguments -Wno-deprecated-register -Wno-mismatched-tags)
# prepend optimizion flag (in case the default setting doesn't include one)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS_RELEASE}")
endif()
if(MSVC OR MSVC90 OR MSVC10)
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /W1 /D_ITERATOR_DEBUG_LEVEL=0")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /W1 -DNOMINMAX")
endif(MSVC OR MSVC90 OR MSVC10)
if(CMAKE_CXX_COMPILER_ID STREQUAL "Intel")
diff --git a/CMakeModules/FindEigen3.cmake b/CMakeModules/FindEigen3.cmake
index 786ea8c..9c546a0 100644
--- a/CMakeModules/FindEigen3.cmake
+++ b/CMakeModules/FindEigen3.cmake
@@ -1,8 +1,81 @@
-include(FindPackageHandleStandardArgs)
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+# find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+# EIGEN3_FOUND - system has eigen lib with correct version
+# EIGEN3_INCLUDE_DIR - the eigen include directory
+# EIGEN3_VERSION - eigen version
-find_package(PkgConfig)
-if(PKG_CONFIG_FOUND)
- pkg_check_modules(EIGEN3 eigen3)
-endif()
+# Copyright (c) 2006, 2007 Montel Laurent, <montel at kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael at free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1 at gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+ if(NOT Eigen3_FIND_VERSION_MAJOR)
+ set(Eigen3_FIND_VERSION_MAJOR 2)
+ endif(NOT Eigen3_FIND_VERSION_MAJOR)
+ if(NOT Eigen3_FIND_VERSION_MINOR)
+ set(Eigen3_FIND_VERSION_MINOR 91)
+ endif(NOT Eigen3_FIND_VERSION_MINOR)
+ if(NOT Eigen3_FIND_VERSION_PATCH)
+ set(Eigen3_FIND_VERSION_PATCH 0)
+ endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+ set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+ file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+ set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+ if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK FALSE)
+ else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK TRUE)
+ endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+ if(NOT EIGEN3_VERSION_OK)
+
+ message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+ "but at least version ${Eigen3_FIND_VERSION} is required")
+ endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+ # in cache already
+ _eigen3_check_version()
+ set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+ find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+ PATHS
+ ${CMAKE_INSTALL_PREFIX}/include
+ ${KDE4_INCLUDE_DIR}
+ PATH_SUFFIXES eigen3 eigen
+ )
+
+ if(EIGEN3_INCLUDE_DIR)
+ _eigen3_check_version()
+ endif(EIGEN3_INCLUDE_DIR)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+ mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
-find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIRS)
diff --git a/CMakeModules/FindGCCXML.cmake b/CMakeModules/FindGCCXML.cmake
deleted file mode 100644
index 579da37..0000000
--- a/CMakeModules/FindGCCXML.cmake
+++ /dev/null
@@ -1,26 +0,0 @@
-# - Find the GCC-XML front-end executable.
-#
-# This module will define the following variables:
-# GCCXML - the GCC-XML front-end executable.
-
-#=============================================================================
-# Copyright 2001-2009 Kitware, Inc.
-#
-# Distributed under the OSI-approved BSD License (the "License");
-# see accompanying file Copyright.txt for details.
-#
-# This software is distributed WITHOUT ANY WARRANTY; without even the
-# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-# See the License for more information.
-#=============================================================================
-# (To distribute this file outside of CMake, substitute the full
-# License text for the above reference.)
-
-# added extra path, to check for gccxml installed in build directory
-find_program(GCCXML NAMES gccxml
- PATHS "${PROJECT_BINARY_DIR}/pyplusplus/bin"
- [HKEY_CURRENT_USER\\Software\\Kitware\\GCC_XML;loc]
- "$ENV{ProgramFiles}/GCC_XML"
- "C:/Program Files/GCC_XML")
-
-mark_as_advanced(GCCXML)
diff --git a/CMakeModules/FindPython.cmake b/CMakeModules/FindPython.cmake
index 5b6bd27..36117e5 100644
--- a/CMakeModules/FindPython.cmake
+++ b/CMakeModules/FindPython.cmake
@@ -140,17 +140,24 @@ function(find_python_module module)
RESULT_VARIABLE _status
OUTPUT_VARIABLE _verloc
ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
- list(GET _verloc 1 _location)
- list(GET _verloc 0 _version)
- message("${_status} ${_verloc} ${_version}")
if(NOT _status)
+ list(LENGTH _verloc _verloclength)
+ if(_verloclength GREATER 1)
+ list(GET _verloc 1 _location)
+ list(GET _verloc 0 _version)
+ else()
+ set(_version "0")
+ endif()
+ # get rid of version prefixes and suffixes so that
+ # "v1.0rc2" becomes "1.0"
+ string(REGEX MATCH "[0-9.]+" _version "${_version}")
if (NOT ${_version} VERSION_LESS ${_minversion})
set(PY_${module_upper} ${_location} CACHE STRING
"Location of Python module ${module}")
set(PY_${module_upper}_VERSION ${_version} CACHE STRING
"Version of Python module ${module}")
else()
- message(SEND_ERROR "Module '${module}' version ${_version} found, but minimum version ${_minversion} required.")
+ message(WARNING "Module '${module}' version ${_version} found, but minimum version ${_minversion} required.")
endif()
endif(NOT _status)
endif (_minversion STREQUAL "")
diff --git a/CMakeModules/Findcastxml.cmake b/CMakeModules/Findcastxml.cmake
new file mode 100644
index 0000000..654876e
--- /dev/null
+++ b/CMakeModules/Findcastxml.cmake
@@ -0,0 +1,60 @@
+include(FindPackageHandleStandardArgs)
+
+if(NOT CASTXML)
+ find_program(CASTXML NAMES castxml)
+endif()
+
+if (CASTXML)
+ set(CASTXMLCFLAGS "-std=c++11")
+
+ if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+ set(CASTXMLCOMPILER "g++")
+ else()
+ if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ set(CASTXMLCOMPILER "clang++")
+ else()
+ if (MSVC)
+ set(CASTXMLCOMPILER "msvc8")
+ endif()
+ endif()
+ endif()
+
+ set(CASTXMLCONFIG "[gccxml]\nxml_generator=castxml\nxml_generator_path=${CASTXML}\ncompiler=${CASTXMLCOMPILER}\ncompiler_path=${CMAKE_CXX_COMPILER}\n")
+
+ set(_candidate_include_path
+ "${OMPL_INCLUDE_DIR}"
+ "${OMPLAPP_INCLUDE_DIR}"
+ "${PYTHON_INCLUDE_DIRS}"
+ "${Boost_INCLUDE_DIR}"
+ "${ASSIMP_INCLUDE_DIRS}"
+ "${ODEINT_INCLUDE_DIR}"
+ "${EIGEN3_INCLUDE_DIR}"
+ "${OMPL_INCLUDE_DIR}/../py-bindings")
+ if(MINGW)
+ execute_process(COMMAND "${CMAKE_CXX_COMPILER}" "-dumpversion"
+ OUTPUT_VARIABLE _version OUTPUT_STRIP_TRAILING_WHITESPACE)
+ get_filename_component(_path "${CMAKE_CXX_COMPILER}" DIRECTORY)
+ get_filename_component(_path "${_path}" DIRECTORY)
+ list(APPEND _candidate_include_path
+ "${_path}/include"
+ "${_path}/lib/gcc/mingw32/${_version}/include"
+ "${_path}/lib/gcc/mingw32/${_version}/include/c++"
+ "${_path}/lib/gcc/mingw32/${_version}/include/c++/mingw32")
+ endif()
+ list(REMOVE_DUPLICATES _candidate_include_path)
+ set(CASTXMLINCLUDEPATH ".")
+ foreach(dir ${_candidate_include_path})
+ if(EXISTS ${dir})
+ set(CASTXMLINCLUDEPATH "${CASTXMLINCLUDEPATH};${dir}")
+ endif()
+ endforeach()
+ set(CASTXMLCONFIG "${CASTXMLCONFIG}include_paths=${CASTXMLINCLUDEPATH}\n")
+ if(CASTXMLCFLAGS)
+ set(CASTXMLCONFIG "${CASTXMLCONFIG}cflags=${CASTXMLCFLAGS}\n")
+ endif()
+ set(CASTXMLCONFIGPATH "${PROJECT_BINARY_DIR}/castxml.cfg")
+ file(WRITE "${CASTXMLCONFIGPATH}" "${CASTXMLCONFIG}")
+ set(CASTXMLCONFIGPATH "${CASTXMLCONFIGPATH}" PARENT_SCOPE)
+endif()
+
+find_package_handle_standard_args(castxml DEFAULT_MSG CASTXML)
diff --git a/CMakeModules/OMPLUtils.cmake b/CMakeModules/OMPLUtils.cmake
index 35bc2c7..6296cd7 100644
--- a/CMakeModules/OMPLUtils.cmake
+++ b/CMakeModules/OMPLUtils.cmake
@@ -2,7 +2,6 @@ macro(add_ompl_test test_name)
add_executable(${ARGV})
target_link_libraries(${test_name}
ompl
- ${Boost_DATE_TIME_LIBRARY}
${Boost_PROGRAM_OPTIONS_LIBRARY}
${Boost_SERIALIZATION_LIBRARY}
${Boost_FILESYSTEM_LIBRARY}
@@ -16,16 +15,49 @@ macro(add_ompl_python_test test_file)
add_test(${test_name} "${PYTHON_EXEC}" "${CMAKE_CURRENT_SOURCE_DIR}/${test_file}" "-v")
endmacro(add_ompl_python_test)
+# Computes the link flags and package dependencies for a list of targets. This command:
+#
+# target_link_flags(target1 target2 ...)
+#
+# sets the following variables in the calling context:
+#
+# target1_LINK_FLAGS
+# target1_PKG_DEPS
+#
+# Note that the link flags for *all* targets are combined into two variables.
+# The second variable is used for libraries that were found with pkg-config.
+# This function is intended for generating pkg-config *.pc files.
function(target_link_flags)
set(_link_flags "")
+ set(_pkg_deps "")
foreach(_target ${ARGV})
get_target_property(_link_libs ${_target} LINK_LIBRARIES)
foreach(_lib ${_link_libs})
get_filename_component(_basename ${_lib} NAME_WE)
get_filename_component(_ext ${_lib} EXT)
- if(_ext STREQUAL ${CMAKE_SHARED_LIBRARY_SUFFIX})
- string(REPLACE ${CMAKE_SHARED_LIBRARY_PREFIX} "" _libname ${_basename})
- list(APPEND _link_flags "-l${_libname}")
+ # add -lfoo to link flags
+ if (_lib MATCHES "-l.+")
+ list(APPEND _link_flags "${_lib}")
+ else()
+ # add link flags for dynamic libraries
+ if(_ext STREQUAL ${CMAKE_SHARED_LIBRARY_SUFFIX})
+ string(REPLACE ${CMAKE_SHARED_LIBRARY_PREFIX} "" _libname ${_basename})
+ list(APPEND _link_flags "-l${_libname}")
+ else()
+ # OS X frameworks, which are also dynamic libraries
+ if (_ext STREQUAL ".framework")
+ list(APPEND _link_flags "-framework ${_basename}")
+ else()
+ # libraries found by pkg-config are just returned as "foo",
+ # not "libfoo.so".
+ if(NOT _ext)
+ list(FIND ARGV ${_basename} _index)
+ if (_index EQUAL -1)
+ list(APPEND _pkg_deps "${_basename}")
+ endif()
+ endif()
+ endif()
+ endif()
endif()
endforeach()
endforeach()
@@ -35,6 +67,13 @@ function(target_link_flags)
endforeach()
string(STRIP "${_link_flags_str}" _link_flags_str)
set(${ARGV0}_LINK_FLAGS "${_link_flags_str}" PARENT_SCOPE)
+
+ list(REMOVE_DUPLICATES _pkg_deps)
+ foreach(_dep ${_pkg_deps})
+ set(_pkg_dep_str "${_pkg_dep_str} ${_dep}")
+ endforeach()
+ string(STRIP "${_pkg_dep_str}" _pkg_dep_str)
+ set(${ARGV0}_PKG_DEPS "${_pkg_dep_str}" PARENT_SCOPE)
endfunction()
option(OMPL_VERSIONED_INSTALL "Append version suffix to binaries, libraries, and include dir." OFF)
diff --git a/CMakeModules/OMPLVersion.cmake b/CMakeModules/OMPLVersion.cmake
index 12dcc5e..81a8af5 100644
--- a/CMakeModules/OMPLVersion.cmake
+++ b/CMakeModules/OMPLVersion.cmake
@@ -1,8 +1,8 @@
# set the version in a way CMake can use
set(OMPL_MAJOR_VERSION 1)
-set(OMPL_MINOR_VERSION 1)
-set(OMPL_PATCH_VERSION 0)
+set(OMPL_MINOR_VERSION 2)
+set(OMPL_PATCH_VERSION 1)
set(OMPL_VERSION "${OMPL_MAJOR_VERSION}.${OMPL_MINOR_VERSION}.${OMPL_PATCH_VERSION}")
# increment this when we have ABI changes
-set(OMPL_ABI_VERSION 11)
+set(OMPL_ABI_VERSION 12)
diff --git a/CMakeModules/PythonBindingsUtils.cmake b/CMakeModules/PythonBindingsUtils.cmake
index 8edfe40..c313cc0 100644
--- a/CMakeModules/PythonBindingsUtils.cmake
+++ b/CMakeModules/PythonBindingsUtils.cmake
@@ -3,28 +3,9 @@ find_package(Boost COMPONENTS python)
# You can optionally specify the desired version like so:
# find_package(Python 2.6)
find_package(Python QUIET)
-set(ENV{PYTHONPATH} "${PROJECT_BINARY_DIR}/pyplusplus/lib/python${PYTHON_VERSION}/site-packages:$ENV{PYTHONPATH}")
-find_python_module(pyplusplus QUIET)
-find_python_module(pygccxml QUIET)
-find_package(GCCXML QUIET)
-
-if(APPLE)
- # The latest gccxml can be *compiled* with clang, but cannot *simulate*
- # clang. If you compiled gccxml with clang, then you have to specify a
- # g++ compiler by adding the following to PYOMPL_EXTRA_CFLAGS:
- # --gccxml-compiler /opt/local/bin/g++-mp-4.8
- # (You can use other versions of g++ as well.) Note that /usr/bin/g++
- # is actually clang++ in Xcode 5.0, so that won't work.
- #
- # Gccxml mistakenly thinks that OS X is a 32-bit architecture.
- set(PYOMPL_EXTRA_CFLAGS "-m64")
-endif(APPLE)
-
-# Trick gccxml to ignore some compiler intrinsics that are used in Boost.Atomic
-# in Boost 1.55.
-if(CMAKE_COMPILER_IS_GNUCXX AND Boost_VERSION VERSION_GREATER "105400")
- set(PYOMPL_EXTRA_CFLAGS "${PYOMPL_EXTRA_CFLAGS} -DBOOST_INTEL_CXX_VERSION")
-endif()
+find_python_module(pyplusplus 1.6.0)
+find_python_module(pygccxml 1.7.2)
+find_package(castxml)
if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY)
include_directories(${PYTHON_INCLUDE_DIRS})
@@ -41,7 +22,7 @@ if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY)
endif()
if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY AND PY_PYPLUSPLUS
- AND PY_PYGCCXML AND GCCXML)
+ AND PY_PYGCCXML AND CASTXML)
# make sure targets are defined only once
if(NOT TARGET generate_headers)
# top-level target for updating all-in-one header file for each module
@@ -54,59 +35,55 @@ if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY AND PY_PYPLUSPLUS
mark_as_advanced(PY_OMPL_GENERATE)
endif()
-function(create_module_header_file_target module dir)
+function(create_module_header_file_target module)
# create list of absolute paths to header files, which we
# will add as a list of dependencies for the target
file(READ "headers_${module}.txt" headers_string)
separate_arguments(rel_headers UNIX_COMMAND "${headers_string}")
set(headers "")
foreach(header ${rel_headers})
+ get_filename_component(header "../${header}" ABSOLUTE)
list(APPEND headers "${header}")
endforeach(header)
# target for all-in-one header for module
add_custom_target(${module}.h
- COMMAND ${CMAKE_COMMAND} -D module=${module} -D exclude=${ARGV2}
+ COMMAND ${CMAKE_COMMAND} -D dir="${CMAKE_CURRENT_SOURCE_DIR}/.." -D module=${module} -D exclude=${ARGV1}
-P "${OMPL_CMAKE_UTIL_DIR}/generate_header.cmake"
- DEPENDS ${headers} WORKING_DIRECTORY "${dir}"
+ DEPENDS ${headers}
COMMENT "Preparing C++ header file for Python binding generation for module ${module}")
add_dependencies(generate_headers ${module}.h)
endfunction(create_module_header_file_target)
-function(create_module_code_generation_target module dir)
+function(create_module_code_generation_target module)
# target for regenerating code. Cmake is run so that the list of
# sources for the py_ompl_${module} target (see below) is updated.
add_custom_target(update_${module}_bindings
- COMMAND env
- PYTHONPATH="${PROJECT_BINARY_DIR}/pyplusplus/lib/python${PYTHON_VERSION}/site-packages:$ENV{PYTHONPATH}"
- ${PYTHON_EXEC}
+ COMMAND time ${PYTHON_EXEC}
"${CMAKE_CURRENT_SOURCE_DIR}/generate_bindings.py" "${module}"
- "1>${CMAKE_BINARY_DIR}/pyplusplus_${module}.log" "2>&1"
- COMMAND ${CMAKE_COMMAND} -D "PATH=${dir}/bindings/${module}"
- -P "${OMPL_CMAKE_UTIL_DIR}/workaround_for_gccxml_bug.cmake"
+ "2>&1" | tee "${CMAKE_BINARY_DIR}/pyplusplus_${module}.log"
COMMAND ${CMAKE_COMMAND} ${CMAKE_BINARY_DIR}
- WORKING_DIRECTORY ${dir}
COMMENT "Creating C++ code for Python module ${module} (see pyplusplus_${module}.log)")
add_dependencies(update_${module}_bindings ${module}.h)
add_dependencies(update_bindings update_${module}_bindings)
endfunction(create_module_code_generation_target)
-function(create_module_generation_targets module dir)
- create_module_header_file_target("${module}" "${dir}" "${ARGV2}")
- create_module_code_generation_target("${module}" "${dir}")
+function(create_module_generation_targets module)
+ create_module_header_file_target("${module}" "${ARGV1}")
+ create_module_code_generation_target("${module}")
endfunction(create_module_generation_targets)
-function(create_module_target module dir)
+function(create_module_target module)
# target for each python module
- aux_source_directory("${dir}/bindings/${module}" PY${module}BINDINGS)
+ aux_source_directory("${CMAKE_CURRENT_BINARY_DIR}/bindings/${module}" PY${module}BINDINGS)
list(LENGTH PY${module}BINDINGS NUM_SOURCE_FILES)
if(NUM_SOURCE_FILES GREATER 0)
- if(ARGC GREATER 2)
- set(_dest_dir "${ARGV2}")
- if(ARGC GREATER 3)
- set(_extra_libs "${ARGV3}")
+ if(ARGC GREATER 1)
+ set(_dest_dir "${ARGV1}")
+ if(ARGC GREATER 2)
+ set(_extra_libs "${ARGV2}")
endif()
else()
- set(_dest_dir "${dir}/ompl")
+ set(_dest_dir "${CMAKE_CURRENT_SOURCE_DIR}/ompl")
endif()
if(WIN32)
# If this is built as a 'module', the compiler complains upon link,
@@ -149,7 +126,7 @@ function(create_module_target module dir)
install(TARGETS py_ompl_${module}
DESTINATION "${OMPL_PYTHON_INSTALL_DIR}/ompl/${module}/"
COMPONENT ${_component})
- include_directories("${dir}/bindings/${module}" "${dir}")
+ include_directories("${CMAKE_CURRENT_BINARY_DIR}/bindings/${module}" "${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
else(NUM_SOURCE_FILES GREATER 0)
if(PY_OMPL_GENERATE)
message(STATUS "Code for module ${module} not found; type \"make update_bindings\"")
diff --git a/CMakeModules/create_symlinks.sh.in b/CMakeModules/create_symlinks.sh.in
index 689bbc4..3adcda5 100755
--- a/CMakeModules/create_symlinks.sh.in
+++ b/CMakeModules/create_symlinks.sh.in
@@ -12,9 +12,6 @@ if [ ! -d .symlinks ]; then
mkdir -p .symlinks/@CMAKE_INSTALL_INCLUDEDIR@
cd .symlinks/include
ln -s ompl at OMPL_INSTALL_SUFFIX@ ompl
- if [[ @Boost_VERSION@ < 105300 ]]; then
- ln -s omplext_odeint at OMPL_INSTALL_SUFFIX@ omplext_odeint
- fi
if [ $1 ]; then
ln -s omplapp at OMPL_INSTALL_SUFFIX@ omplapp
fi
diff --git a/CMakeModules/generate_header.cmake b/CMakeModules/generate_header.cmake
index 1218809..86f053a 100644
--- a/CMakeModules/generate_header.cmake
+++ b/CMakeModules/generate_header.cmake
@@ -1,10 +1,10 @@
-file(READ "headers_${module}.txt" headers_string)
+file(READ "${dir}/py-bindings/headers_${module}.txt" headers_string)
separate_arguments(headers UNIX_COMMAND "${headers_string}")
set(module_header "bindings/${module}.h")
file(WRITE "${module_header}" "")
foreach(header ${headers})
if(NOT exclude OR NOT header MATCHES "${exclude}")
- file(READ "${header}" _header_txt)
+ file(READ "${dir}/${header}" _header_txt)
file(APPEND "${module_header}" "${_header_txt}")
endif()
endforeach(header)
diff --git a/CMakeModules/ompl.pc.in b/CMakeModules/ompl.pc.in
index 04fd20c..916ce09 100644
--- a/CMakeModules/ompl.pc.in
+++ b/CMakeModules/ompl.pc.in
@@ -1,8 +1,8 @@
# This file was generated by CMake for @PROJECT_NAME@
prefix=@CMAKE_INSTALL_PREFIX@
exec_prefix=${prefix}
-libdir=${prefix}/lib
-includedir=${prefix}/include
+libdir=${prefix}/@CMAKE_INSTALL_LIBDIR@
+includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@
Name: @PKG_NAME@
Description: @PKG_DESC@
diff --git a/CMakeModules/workaround_for_gccxml_bug.cmake b/CMakeModules/workaround_for_gccxml_bug.cmake
deleted file mode 100644
index 4a7c915..0000000
--- a/CMakeModules/workaround_for_gccxml_bug.cmake
+++ /dev/null
@@ -1,11 +0,0 @@
-# Function to work around a gccxml bug that results in a code that cannot be compiled
-#
-# For some boost::function's it generates code like this: boost::function<int()(int)>
-# This script fixes that, albeit in a very fragile way. This could probably be improved
-# by someone with better regexp writing skills.
-file(GLOB fnames "${PATH}/*.pypp.*")
-foreach(fname ${fnames})
- file(READ ${fname} _text)
- string(REPLACE " ()(" " (" _new_text "${_text}")
- file(WRITE "${fname}" "${_new_text}")
-endforeach(fname)
diff --git a/README.md b/README.md
index 36243f9..9a79fb2 100644
--- a/README.md
+++ b/README.md
@@ -9,16 +9,16 @@ detailed installation instructions.
OMPL has the following required dependencies:
-* [Boost](http://www.boost.org) (version 1.48 or higher)
+* [Boost](http://www.boost.org) (version 1.54 or higher)
* [CMake](http://www.cmake.org) (version 2.8.7 or higher)
The following dependencies are optional:
* [ODE](http://ode.org) (needed to compile support for planning using the Open Dynamics Engine)
-* [Py++](https://bitbucket.org/ompl/pyplusplus) (needed to generate Python bindings)
+* [Py++](https://bitbucket.org/ompl/ompl/src/tip/doc/markdown/installPyPlusPlus.md) (needed to generate Python bindings)
* [Doxygen](http://www.doxygen.org) (needed to create a local copy of the documentation at
http://ompl.kavrakilab.org/core)
-* [Eigen](http://eigen.tuxfamily.org) (needed for an informed sampling technique to improve the optimization of path length)
+* [Eigen](http://eigen.tuxfamily.org) (needed for an informed sampling technique to improve the optimization of path length and for the Vector Field RRT planner)
Once dependencies are installed, you can build OMPL on Linux, OS X,
and MS Windows. Go to the top-level directory of OMPL and type the
@@ -27,7 +27,6 @@ following commands:
mkdir -p build/Release
cd build/Release
cmake ../..
- # next two steps are optional
- make installpyplusplus && cmake . # download & install Py++
- make update_bindings # if you want Python bindings
+ # next step is optional
+ make -j 4 update_bindings # if you want Python bindings
make -j 4 # replace "4" with the number of cores on your machine
diff --git a/appveyor.yml b/appveyor.yml
deleted file mode 100644
index 8ccdc9f..0000000
--- a/appveyor.yml
+++ /dev/null
@@ -1,41 +0,0 @@
-# AppVeyor file
-# http://www.appveyor.com/docs/appveyor-yml
-
-version: "{build}"
-os: unstable
-
-clone_folder: C:\projects\ompl
-clone_depth: 1
-
-branches:
- only:
- - master
-platform: x64
-
-environment:
- CTEST_OUTPUT_ON_FAILURE: 1
- BOOST_ROOT: C:\Libraries\boost
- BOOST_LIBRARYDIR: C:\Libraries\boost\lib64-msvc-12.0
-
-configuration: Release
-
-before_build:
- - cmd: set
- - cmd: mkdir build
- - cmd: cd build
- - cmd: cmake -G "Visual Studio 12 Win64" -DCMAKE_BUILD_TYPE=%configuration% -DOMPL_REGISTRATION=OFF -DOMPL_BUILD_DEMOS=OFF -DBOOST_ROOT="%BOOST_ROOT%" -DBOOST_LIBRARYDIR="%BOOST_LIBRARYDIR%" ..
-
-build:
- project: C:\projects\ompl\build\ompl.sln
- parallel: true
-
-# after_build:
-# - cmd: cmake --build . --target package --config %configuration%
-
-# no time to run tests
-# test_script:
-# - cmd: cd build
-# - cmd: ctest -C %configuration%
-
-# artifacts:
-# - path: ompl\*.zip
diff --git a/demos/CForestCircleGridBenchmark.cpp b/demos/CForestCircleGridBenchmark.cpp
index b57763b..784eff1 100644
--- a/demos/CForestCircleGridBenchmark.cpp
+++ b/demos/CForestCircleGridBenchmark.cpp
@@ -113,7 +113,7 @@ int main(int argc, char **argv)
og::SimpleSetup ss(space);
// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, obstacleRadius*obstacleRadius, _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid, obstacleRadius*obstacleRadius, std::placeholders::_1));
// define start & goal states
ob::ScopedState<ob::SE2StateSpace> start(space), goal(space);
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index fd75552..497ca9a 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -8,8 +8,6 @@ if (OMPL_BUILD_DEMOS)
ompl
${Boost_FILESYSTEM_LIBRARY}
${Boost_SYSTEM_LIBRARY}
- ${Boost_THREAD_LIBRARY}
- ${Boost_DATE_TIME_LIBRARY}
${Boost_PROGRAM_OPTIONS_LIBRARY})
endmacro(add_ompl_demo)
@@ -33,6 +31,11 @@ if (OMPL_BUILD_DEMOS)
add_ompl_demo(demo_PlannerProgressProperties PlannerProgressProperties.cpp)
add_ompl_demo(demo_CForestCircleGridBenchmark CForestCircleGridBenchmark.cpp)
+ if(OMPL_HAVE_EIGEN3)
+ add_ompl_demo(demo_VectorFieldConservative VFRRT/VectorFieldConservative.cpp)
+ add_ompl_demo(demo_VectorFieldNonconservative VFRRT/VectorFieldNonconservative.cpp)
+ endif()
+
if (OMPL_EXTENSION_OPENDE)
add_ompl_demo(demo_OpenDERigidBodyPlanning OpenDERigidBodyPlanning.cpp)
endif()
@@ -53,3 +56,6 @@ install(FILES ${OMPL_DEMO_CXX_FILES} ${OMPL_DEMO_PY_FILES}
install(DIRECTORY Koules
DESTINATION "${OMPL_DEMO_INSTALL_DIR}"
COMPONENT ompl)
+install(DIRECTORY VFRRT
+ DESTINATION "${OMPL_DEMO_INSTALL_DIR}"
+ COMPONENT ompl)
diff --git a/demos/GeometricCarPlanning.cpp b/demos/GeometricCarPlanning.cpp
index b9ec984..20d58bc 100644
--- a/demos/GeometricCarPlanning.cpp
+++ b/demos/GeometricCarPlanning.cpp
@@ -80,8 +80,8 @@ void plan(ob::StateSpacePtr space, bool easy)
// set state validity checking for this space
ob::SpaceInformationPtr si(ss.getSpaceInformation());
- ss.setStateValidityChecker(boost::bind(
- easy ? &isStateValidEasy : &isStateValidHard, si.get(), _1));
+ ss.setStateValidityChecker(std::bind(
+ easy ? &isStateValidEasy : &isStateValidHard, si.get(), std::placeholders::_1));
// set the start and goal states
if (easy)
diff --git a/demos/HybridSystemPlanning.cpp b/demos/HybridSystemPlanning.cpp
index 85177a6..583d196 100644
--- a/demos/HybridSystemPlanning.cpp
+++ b/demos/HybridSystemPlanning.cpp
@@ -143,10 +143,12 @@ int main(int, char**)
oc::SimpleSetup setup(cmanifold);
setup.setStartAndGoalStates(start, goal, 5.);
- setup.setStateValidityChecker(boost::bind(
- &isStateValid, setup.getSpaceInformation().get(), _1));
- setup.setStatePropagator(boost::bind(
- &propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
+ setup.setStateValidityChecker(std::bind(
+ &isStateValid, setup.getSpaceInformation().get(), std::placeholders::_1));
+ setup.setStatePropagator(std::bind(
+ &propagate, setup.getSpaceInformation().get(),
+ std::placeholders::_1, std::placeholders::_2,
+ std::placeholders::_3, std::placeholders::_4));
setup.getSpaceInformation()->setPropagationStepSize(.1);
setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
diff --git a/demos/HypercubeBenchmark.cpp b/demos/HypercubeBenchmark.cpp
index 9eee7b9..62af093 100644
--- a/demos/HypercubeBenchmark.cpp
+++ b/demos/HypercubeBenchmark.cpp
@@ -74,7 +74,7 @@ void addPlanner(ompl::tools::Benchmark& benchmark, ompl::base::PlannerPtr planne
{
ompl::base::ParamSet& params = planner->params();
if (params.hasParam(std::string("range")))
- params.setParam(std::string("range"), boost::lexical_cast<std::string>(range));
+ params.setParam(std::string("range"), std::to_string(range));
benchmark.addPlanner(planner);
}
@@ -106,7 +106,7 @@ int main(int argc, char **argv)
int run_count = 20;
ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
ompl::tools::Benchmark b(ss, "HyperCube");
- b.addExperimentParameter("num_dims", "INTEGER", boost::lexical_cast<std::string>(ndim));
+ b.addExperimentParameter("num_dims", "INTEGER", std::to_string(ndim));
addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())), range);
addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())), range);
diff --git a/demos/KinematicChainBenchmark.cpp b/demos/KinematicChainBenchmark.cpp
index 0ea1940..f7a1d1a 100644
--- a/demos/KinematicChainBenchmark.cpp
+++ b/demos/KinematicChainBenchmark.cpp
@@ -308,7 +308,7 @@ int main(int argc, char **argv)
int run_count = 20;
ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
ompl::tools::Benchmark b(ss, "KinematicChain");
- b.addExperimentParameter("num_links", "INTEGER", boost::lexical_cast<std::string>(numLinks));
+ b.addExperimentParameter("num_links", "INTEGER", std::to_string(numLinks));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())));
diff --git a/demos/Koules/Koules.cpp b/demos/Koules/Koules.cpp
index 5958eb5..e74c1d7 100644
--- a/demos/Koules/Koules.cpp
+++ b/demos/Koules/Koules.cpp
@@ -131,7 +131,7 @@ void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
{
// Create a benchmark class
ompl::tools::Benchmark b(ks, "Koules");
- b.addExperimentParameter("num_koules", "INTEGER", boost::lexical_cast<std::string>(
+ b.addExperimentParameter("num_koules", "INTEGER", std::to_string(
(ks.getStateSpace()->getDimension() - 5) / 4));
// Add the planner to evaluate
b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
diff --git a/demos/Koules/KoulesSetup.cpp b/demos/Koules/KoulesSetup.cpp
index 5841f02..12aca23 100644
--- a/demos/Koules/KoulesSetup.cpp
+++ b/demos/Koules/KoulesSetup.cpp
@@ -129,7 +129,7 @@ void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerN
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// set directed control sampler; when using the PDST planner, propagate as long as possible
si_->setDirectedControlSamplerAllocator(
- boost::bind(&KoulesDirectedControlSamplerAllocator, _1, getGoal(), plannerName == "pdst"));
+ std::bind(&KoulesDirectedControlSamplerAllocator, std::placeholders::_1, getGoal(), plannerName == "pdst"));
// set planner
setPlanner(getConfiguredPlannerInstance(plannerName));
// set validity checker
diff --git a/demos/Koules/KoulesSimulator.cpp b/demos/Koules/KoulesSimulator.cpp
index 926fbf2..b766370 100644
--- a/demos/Koules/KoulesSimulator.cpp
+++ b/demos/Koules/KoulesSimulator.cpp
@@ -247,7 +247,7 @@ void KoulesSimulator::computeCollisionEvent(unsigned int i, unsigned int j)
}
t += time_;
if (t >= time_ && t <= endTime_)
- collisionEvents_.push(boost::make_tuple(t, i, j));
+ collisionEvents_.push(std::make_tuple(t, i, j));
}
void KoulesSimulator::elasticCollision(unsigned int i, unsigned int j)
@@ -329,8 +329,8 @@ void KoulesSimulator::step(const ob::State *start, const oc::Control* control,
while (!collisionEvents_.empty())
{
CollisionEvent event = collisionEvents_.top();
- double ct = event.get<0>();
- unsigned int i = event.get<1>(), j = event.get<2>();
+ double ct = std::get<0>(event);
+ unsigned int i = std::get<1>(event), j = std::get<2>(event);
collisionEvents_.pop();
advance(ct);
diff --git a/demos/Koules/KoulesSimulator.h b/demos/Koules/KoulesSimulator.h
index ba18e64..7d6f369 100644
--- a/demos/Koules/KoulesSimulator.h
+++ b/demos/Koules/KoulesSimulator.h
@@ -39,8 +39,7 @@
#include "KoulesConfig.h"
#include <ompl/control/StatePropagator.h>
-#include <boost/tuple/tuple.hpp>
-#include <boost/tuple/tuple_comparison.hpp>
+#include <tuple>
#include <queue>
// State propagator for KoulesSetup.
@@ -55,7 +54,7 @@ public:
protected:
// A tuple containing the time and id's of two objects colliding
- typedef boost::tuple<double, unsigned int, unsigned int> CollisionEvent;
+ typedef std::tuple<double, unsigned int, unsigned int> CollisionEvent;
// A priority queue of events, s.t. the top element is the collision
// that will happen first.
typedef std::priority_queue<CollisionEvent, std::vector<CollisionEvent>,
diff --git a/demos/Koules/KoulesStateSpace.cpp b/demos/Koules/KoulesStateSpace.cpp
index 2b2fc30..564a209 100644
--- a/demos/Koules/KoulesStateSpace.cpp
+++ b/demos/Koules/KoulesStateSpace.cpp
@@ -46,7 +46,7 @@ KoulesStateSpace::KoulesStateSpace(unsigned int numKoules)
{
mass_[0] = shipMass;
radius_[0] = shipRadius;
- setName("Koules" + boost::lexical_cast<std::string>(numKoules) + getName());
+ setName("Koules" + std::to_string(numKoules) + getName());
// layout: (x_s y_s vx_s vy_s theta_s ... x_i y_i vx_i vy_i ... ),
// where (x_i, y_i) is the position of koule i (i=1,..,numKoules),
// (vx_i, vy_i) its velocity, (x_s, y_s) the position of the ship,
diff --git a/demos/LTLWithTriangulation.cpp b/demos/LTLWithTriangulation.cpp
index 76d0912..d0b6608 100644
--- a/demos/LTLWithTriangulation.cpp
+++ b/demos/LTLWithTriangulation.cpp
@@ -77,9 +77,6 @@ public:
ob::SE2StateSpace::StateType* ws = s->as<ob::SE2StateSpace::StateType>();
ws->setXY(coord[0], coord[1]);
}
-
-private:
- ompl::RNG rng_;
};
void addObstaclesAndPropositions(oc::PropositionalTriangularDecomposition* decomp)
@@ -134,8 +131,8 @@ bool isStateValid(
return false;
const ob::SE2StateSpace::StateType* se2 = state->as<ob::SE2StateSpace::StateType>();
- double x = se2->getX();
- double y = se2->getY();
+ double x = se2->getX();
+ double y = se2->getY();
const std::vector<Polygon>& obstacles = decomp->getHoles();
typedef std::vector<Polygon>::const_iterator ObstacleIter;
for (ObstacleIter o = obstacles.begin(); o != obstacles.end(); ++o)
@@ -143,7 +140,7 @@ bool isStateValid(
if (polyContains(*o, x, y))
return false;
}
- return true;
+ return true;
}
void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
@@ -194,8 +191,8 @@ void plan(void)
cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
- si->setStateValidityChecker(boost::bind(&isStateValid, si.get(), ptd, _1));
- si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
+ si->setStateValidityChecker(std::bind(&isStateValid, si.get(), ptd, std::placeholders::_1));
+ si->setStatePropagator(propagate);
si->setPropagationStepSize(0.025);
//LTL co-safety sequencing formula: visit p2,p0 in that order
diff --git a/demos/OptimalPlanning.cpp b/demos/OptimalPlanning.cpp
index 9d4cfb2..7728bab 100644
--- a/demos/OptimalPlanning.cpp
+++ b/demos/OptimalPlanning.cpp
@@ -46,6 +46,7 @@
#include <ompl/geometric/planners/bitstar/BITstar.h>
#include <ompl/geometric/planners/cforest/CForest.h>
#include <ompl/geometric/planners/fmt/FMT.h>
+#include <ompl/geometric/planners/fmt/BFMT.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
#include <ompl/geometric/planners/prm/PRMstar.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
@@ -55,8 +56,8 @@
#include <boost/program_options.hpp>
// For string comparison (boost::iequals)
#include <boost/algorithm/string.hpp>
-// For boost::make_shared
-#include <boost/make_shared.hpp>
+// For std::make_shared
+#include <memory>
#include <fstream>
@@ -71,6 +72,7 @@ enum optimalPlanner
PLANNER_BITSTAR,
PLANNER_CFOREST,
PLANNER_FMTSTAR,
+ PLANNER_BFMTSTAR,
PLANNER_INF_RRTSTAR,
PLANNER_PRMSTAR,
PLANNER_RRTSTAR
@@ -142,32 +144,37 @@ ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner planne
{
case PLANNER_BITSTAR:
{
- return boost::make_shared<og::BITstar>(si);
+ return std::make_shared<og::BITstar>(si);
break;
}
case PLANNER_CFOREST:
{
- return boost::make_shared<og::CForest>(si);
+ return std::make_shared<og::CForest>(si);
break;
}
case PLANNER_FMTSTAR:
{
- return boost::make_shared<og::FMT>(si);
+ return std::make_shared<og::FMT>(si);
+ break;
+ }
+ case PLANNER_BFMTSTAR:
+ {
+ return std::make_shared<og::BFMT>(si);
break;
}
case PLANNER_INF_RRTSTAR:
{
- return boost::make_shared<og::InformedRRTstar>(si);
+ return std::make_shared<og::InformedRRTstar>(si);
break;
}
case PLANNER_PRMSTAR:
{
- return boost::make_shared<og::PRMstar>(si);
+ return std::make_shared<og::PRMstar>(si);
break;
}
case PLANNER_RRTSTAR:
{
- return boost::make_shared<og::RRTstar>(si);
+ return std::make_shared<og::RRTstar>(si);
break;
}
default:
@@ -267,7 +274,7 @@ void plan(double runTime, optimalPlanner plannerType, planningObjective objectiv
if (!outputFile.empty())
{
std::ofstream outFile(outputFile.c_str());
- boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
+ std::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
printAsMatrix(outFile);
outFile.close();
}
@@ -412,7 +419,7 @@ bool argParse(int argc, char** argv, double* runTimePtr, optimalPlanner *planner
desc.add_options()
("help,h", "produce help message")
("runtime,t", bpo::value<double>()->default_value(1.0), "(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.")
- ("planner,p", bpo::value<std::string>()->default_value("RRTstar"), "(Optional) Specify the optimal planner to use, defaults to RRTstar if not given. Valid options are BITstar, CForest, FMTstar, InformedRRTstar, PRMstar, and RRTstar.") //Alphabetical order
+ ("planner,p", bpo::value<std::string>()->default_value("RRTstar"), "(Optional) Specify the optimal planner to use, defaults to RRTstar if not given. Valid options are BITstar, CForest, FMTstar, BFMTstar, InformedRRTstar, PRMstar, and RRTstar.") //Alphabetical order
("objective,o", bpo::value<std::string>()->default_value("PathLength"), "(Optional) Specify the optimization objective, defaults to PathLength if not given. Valid options are PathClearance, PathLength, ThresholdPathLength, and WeightedLengthAndClearanceCombo.") //Alphabetical order
("file,f", bpo::value<std::string>()->default_value(""), "(Optional) Specify an output path for the found solution path.")
("info,i", bpo::value<unsigned int>()->default_value(0u), "(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG. Defaults to WARN.");
@@ -475,6 +482,10 @@ bool argParse(int argc, char** argv, double* runTimePtr, optimalPlanner *planner
{
*plannerPtr = PLANNER_FMTSTAR;
}
+ else if (boost::iequals("BFMTstar", plannerStr))
+ {
+ *plannerPtr = PLANNER_BFMTSTAR;
+ }
else if (boost::iequals("InformedRRTstar", plannerStr))
{
*plannerPtr = PLANNER_INF_RRTSTAR;
diff --git a/demos/OptimalPlanning.py b/demos/OptimalPlanning.py
index 1b0d51a..4979f09 100755
--- a/demos/OptimalPlanning.py
+++ b/demos/OptimalPlanning.py
@@ -170,6 +170,8 @@ def allocatePlanner(si, plannerType):
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
+ elif plannerType.lower() == "bfmtstar":
+ return og.BFMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
@@ -263,7 +265,7 @@ if __name__ == "__main__":
# Add a filename argument
parser.add_argument('-t', '--runtime', type=float, default=1.0, help='(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.')
- parser.add_argument('-p', '--planner', default='RRTstar', choices=['BITstar', 'FMTstar', 'InformedRRTstar', 'PRMstar', 'RRTstar'], help='(Optional) Specify the optimal planner to use, defaults to RRTstar if not given.') # Alphabetical order
+ parser.add_argument('-p', '--planner', default='RRTstar', choices=['BITstar', 'FMTstar', 'BFMTstar', 'InformedRRTstar', 'PRMstar', 'RRTstar'], help='(Optional) Specify the optimal planner to use, defaults to RRTstar if not given.') # Alphabetical order
parser.add_argument('-o', '--objective', default='PathLength', choices=['PathClearance', 'PathLength', 'ThresholdPathLength', 'WeightedLengthAndClearanceCombo'], help='(Optional) Specify the optimization objective, defaults to PathLength if not given.') # Alphabetical order
parser.add_argument('-f', '--file', default=None, help='(Optional) Specify an output path for the found solution path.')
parser.add_argument('-i', '--info', type=int, default=0, choices=[0, 1, 2], help='(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG. Defaults to WARN.')
diff --git a/demos/PlannerData.cpp b/demos/PlannerData.cpp
index 7e77e0f..f40ad8e 100644
--- a/demos/PlannerData.cpp
+++ b/demos/PlannerData.cpp
@@ -82,7 +82,7 @@ void planWithSimpleSetup(void)
og::SimpleSetup ss(space);
// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a random start state
ob::ScopedState<> start(space);
@@ -171,12 +171,12 @@ void readPlannerData(void)
goal.setState(data.getGoalVertex(0).getState());
ob::PlannerData::Graph::Vertex start = boost::vertex(data.getStartIndex(0), graph);
boost::astar_search(graph, start,
- boost::bind(&distanceHeuristic, _1, &goal, &opt, vertices),
+ std::bind(&distanceHeuristic, std::placeholders::_1, &goal, &opt, vertices),
boost::predecessor_map(prev).
- distance_compare(boost::bind(&ob::OptimizationObjective::
- isCostBetterThan, &opt, _1, _2)).
- distance_combine(boost::bind(&ob::OptimizationObjective::
- combineCosts, &opt, _1, _2)).
+ distance_compare(std::bind(&ob::OptimizationObjective::
+ isCostBetterThan, &opt, std::placeholders::_1, std::placeholders::_2)).
+ distance_combine(std::bind(&ob::OptimizationObjective::
+ combineCosts, &opt, std::placeholders::_1, std::placeholders::_2)).
distance_inf(opt.infiniteCost()).
distance_zero(opt.identityCost()));
diff --git a/demos/Point2DPlanning.cpp b/demos/Point2DPlanning.cpp
index 17f8ae2..c74d53b 100644
--- a/demos/Point2DPlanning.cpp
+++ b/demos/Point2DPlanning.cpp
@@ -75,7 +75,7 @@ public:
ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));
// set state validity checking for this space
- ss_->setStateValidityChecker(boost::bind(&Plane2DEnvironment::isStateValid, this, _1));
+ ss_->setStateValidityChecker(std::bind(&Plane2DEnvironment::isStateValid, this, std::placeholders::_1));
space->setup();
ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
// ss_->setPlanner(ob::PlannerPtr(new og::RRTConnect(ss_->getSpaceInformation())));
diff --git a/demos/Point2DPlanning.py b/demos/Point2DPlanning.py
index ad30536..ca382c0 100755
--- a/demos/Point2DPlanning.py
+++ b/demos/Point2DPlanning.py
@@ -43,12 +43,12 @@ try:
except:
# if the ompl module is not in the PYTHONPATH assume it is installed in a
# subdirectory of the parent directory called "py-bindings."
- from os.path import abspath, dirname, join
- import sys
sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
from ompl import util as ou
from ompl import base as ob
from ompl import geometric as og
+from os.path import abspath, dirname, join
+import sys
from functools import partial
class Plane2DEnvironment:
diff --git a/demos/RandomWalkPlanner.py b/demos/RandomWalkPlanner.py
index 24f74fc..0723f83 100755
--- a/demos/RandomWalkPlanner.py
+++ b/demos/RandomWalkPlanner.py
@@ -45,11 +45,10 @@ except:
# subdirectory of the parent directory called "py-bindings."
from os.path import abspath, dirname, join
import sys
- sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
+ sys.path.insert(0, join(dirname(dirname(abspath(__file__))), "py-bindings"))
from ompl import util as ou
from ompl import base as ob
from ompl import geometric as og
-from random import choice
## @cond IGNORE
diff --git a/demos/RigidBodyPlanning.cpp b/demos/RigidBodyPlanning.cpp
index 8015186..329c982 100644
--- a/demos/RigidBodyPlanning.cpp
+++ b/demos/RigidBodyPlanning.cpp
@@ -79,7 +79,7 @@ void plan(void)
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// set state validity checking for this space
- si->setStateValidityChecker(boost::bind(&isStateValid, _1));
+ si->setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a random start state
ob::ScopedState<> start(space);
@@ -144,7 +144,7 @@ void planWithSimpleSetup(void)
og::SimpleSetup ss(space);
// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a random start state
ob::ScopedState<> start(space);
diff --git a/demos/RigidBodyPlanningWithControls.cpp b/demos/RigidBodyPlanningWithControls.cpp
index d115386..80c0814 100644
--- a/demos/RigidBodyPlanningWithControls.cpp
+++ b/demos/RigidBodyPlanningWithControls.cpp
@@ -134,10 +134,11 @@ void plan(void)
oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
// set state validity checking for this space
- si->setStateValidityChecker(boost::bind(&isStateValid, si.get(), _1));
+ si->setStateValidityChecker(std::bind(&isStateValid, si.get(), std::placeholders::_1));
// set the state propagation routine
- si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
+ si->setStatePropagator(std::bind(&propagate, std::placeholders::_1,
+ std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
@@ -220,10 +221,12 @@ void planWithSimpleSetup(void)
oc::SimpleSetup ss(cspace);
// set the state propagation routine
- ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
+ ss.setStatePropagator(std::bind(&propagate, std::placeholders::_1,
+ std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid,
+ ss.getSpaceInformation().get(), std::placeholders::_1));
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
diff --git a/demos/RigidBodyPlanningWithIK.cpp b/demos/RigidBodyPlanningWithIK.cpp
index 33f87e5..ba38825 100644
--- a/demos/RigidBodyPlanningWithIK.cpp
+++ b/demos/RigidBodyPlanningWithIK.cpp
@@ -125,7 +125,9 @@ void planWithIK(void)
// bind a sampling function that fills its argument with a sampled state and returns true while it can produce new samples
// we don't need to check if new samples are different from ones previously computed as this is pefromed automatically by GoalLazySamples
- ob::GoalSamplingFn samplingFunction = boost::bind(®ionSamplingWithGS, ss.getSpaceInformation(), ss.getProblemDefinition(), ®ion, _1, _2);
+ ob::GoalSamplingFn samplingFunction = std::bind(®ionSamplingWithGS,
+ ss.getSpaceInformation(), ss.getProblemDefinition(), ®ion,
+ std::placeholders::_1, std::placeholders::_2);
// create an instance of GoalLazySamples:
ob::GoalPtr goal(new ob::GoalLazySamples(ss.getSpaceInformation(), samplingFunction));
diff --git a/demos/RigidBodyPlanningWithIntegrationAndControls.cpp b/demos/RigidBodyPlanningWithIntegrationAndControls.cpp
index 01403e4..a716b73 100644
--- a/demos/RigidBodyPlanningWithIntegrationAndControls.cpp
+++ b/demos/RigidBodyPlanningWithIntegrationAndControls.cpp
@@ -219,7 +219,7 @@ void planWithSimpleSetup(void)
oc::SimpleSetup ss(cspace);
/// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid, ss.getSpaceInformation().get(), std::placeholders::_1));
/// set the propagation routine for this space
ss.setStatePropagator(oc::StatePropagatorPtr(new DemoStatePropagator(ss.getSpaceInformation())));
diff --git a/demos/RigidBodyPlanningWithODESolverAndControls.cpp b/demos/RigidBodyPlanningWithODESolverAndControls.cpp
index ab039bf..35105e0 100644
--- a/demos/RigidBodyPlanningWithODESolverAndControls.cpp
+++ b/demos/RigidBodyPlanningWithODESolverAndControls.cpp
@@ -190,7 +190,7 @@ void planWithSimpleSetup(void)
oc::SimpleSetup ss(cspace);
// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid, ss.getSpaceInformation().get(), std::placeholders::_1));
// Setting the propagation routine for this space:
// KinematicCarModel does NOT use ODESolver
diff --git a/demos/StateSampling.cpp b/demos/StateSampling.cpp
index 4cad9b7..8865e1f 100644
--- a/demos/StateSampling.cpp
+++ b/demos/StateSampling.cpp
@@ -41,8 +41,8 @@
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/config.h>
-#include <boost/thread.hpp>
#include <iostream>
+#include <thread>
namespace ob = ompl::base;
namespace og = ompl::geometric;
@@ -112,7 +112,7 @@ bool isStateValid(const ob::State *state)
// Let's pretend that the validity check is computationally relatively
// expensive to emphasize the benefit of explicitly generating valid
// samples
- boost::this_thread::sleep(ompl::time::seconds(.0005));
+ std::this_thread::sleep_for(ompl::time::seconds(.0005));
// Valid states satisfy the following constraints:
// -1<= x,y,z <=1
// if .25 <= z <= .5, then |x|>.8 and |y|>.8
@@ -149,7 +149,7 @@ void plan(int samplerIndex)
og::SimpleSetup ss(space);
// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a start state
ob::ScopedState<> start(space);
diff --git a/demos/ThunderLightning.cpp b/demos/ThunderLightning.cpp
index e4d2660..6b65221 100644
--- a/demos/ThunderLightning.cpp
+++ b/demos/ThunderLightning.cpp
@@ -83,7 +83,7 @@ public:
expPlanner_->setFilePath("lightning.db");
}
// set state validity checking for this space
- expPlanner_->setStateValidityChecker(boost::bind(&Plane2DEnvironment::isStateValid, this, _1));
+ expPlanner_->setStateValidityChecker(std::bind(&Plane2DEnvironment::isStateValid, this, std::placeholders::_1));
space->setup();
expPlanner_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
vss_ = expPlanner_->getSpaceInformation()->allocValidStateSampler();
@@ -91,7 +91,7 @@ public:
// DTC
//experience_setup_->setPlanner(ob::PlannerPtr(new og::RRTConnect( si_ )));
// Set the repair planner
- // boost::shared_ptr<og::RRTConnect> repair_planner( new og::RRTConnect( si_ ) );
+ // std::shared_ptr<og::RRTConnect> repair_planner( new og::RRTConnect( si_ ) );
// experience_setup_->setRepairPlanner(ob::PlannerPtr( repair_planner ));
}
}
diff --git a/demos/TriangulationDemo.cpp b/demos/TriangulationDemo.cpp
index 8cdfa38..7a0ced9 100644
--- a/demos/TriangulationDemo.cpp
+++ b/demos/TriangulationDemo.cpp
@@ -134,10 +134,10 @@ void planWithSimpleSetup(void)
oc::SimpleSetup ss(cspace);
// set the state propagation routine
- ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
+ ss.setStatePropagator(std::bind(&propagate, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
// set state validity checking for this space
- ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
+ ss.setStateValidityChecker(std::bind(&isStateValid, ss.getSpaceInformation().get(), std::placeholders::_1));
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
diff --git a/demos/VFRRT/VectorFieldConservative.cpp b/demos/VFRRT/VectorFieldConservative.cpp
new file mode 100644
index 0000000..900de93
--- /dev/null
+++ b/demos/VFRRT/VectorFieldConservative.cpp
@@ -0,0 +1,175 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Caleb Voss and Wilson Beebe
+* 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.
+*********************************************************************/
+
+/* Authors: Caleb Voss, Wilson Beebe */
+
+#include <fstream>
+
+#include <ompl/base/StateSpace.h>
+#include <ompl/base/objectives/VFMechanicalWorkOptimizationObjective.h>
+#include <ompl/base/objectives/VFUpstreamCriterionOptimizationObjective.h>
+#include <ompl/base/spaces/RealVectorStateSpace.h>
+#include <ompl/geometric/planners/rrt/RRTstar.h>
+#include <ompl/geometric/planners/rrt/TRRT.h>
+#include <ompl/geometric/planners/rrt/VFRRT.h>
+#include <ompl/geometric/SimpleSetup.h>
+
+namespace ob = ompl::base;
+namespace og = ompl::geometric;
+
+enum PlannerType { VFRRT = 0, TRRT, RRTSTAR };
+
+/** Gradient of the potential function 1 + sin(x[0]) * sin(x[1]). */
+Eigen::VectorXd field(const ob::State *state)
+{
+ const ob::RealVectorStateSpace::StateType &x = *state->as<ob::RealVectorStateSpace::StateType>();
+ Eigen::VectorXd v(2);
+ v[0] = std::cos(x[0]) * std::sin(x[1]);
+ v[1] = std::sin(x[0]) * std::cos(x[1]);
+ return -v;
+}
+
+/** Make a problem using a conservative vector field. */
+og::SimpleSetupPtr setupProblem(PlannerType plannerType)
+{
+ // construct the state space we are planning in
+ ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
+ ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
+
+ ob::RealVectorBounds bounds(2);
+ bounds.setLow(-10);
+ bounds.setHigh(10);
+
+ space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
+
+ // define a simple setup class
+ og::SimpleSetupPtr ss(new og::SimpleSetup(space));
+
+ // set state validity checking for this space
+ ss->setStateValidityChecker(ob::StateValidityCheckerPtr(
+ new ob::AllValidStateValidityChecker(si)));
+
+ // create a start state
+ ob::ScopedState<> start(space);
+ start[0] = -5;
+ start[1] = -2;
+
+ // create a goal state
+ ob::ScopedState<> goal(space);
+ goal[0] = 5;
+ goal[1] = 3;
+
+ // set the start and goal states
+ ss->setStartAndGoalStates(start, goal, 0.1);
+
+ // make the optimization objectives for TRRT and RRT*, and set the planner
+ if (plannerType == TRRT)
+ {
+ ss->setOptimizationObjective(ob::OptimizationObjectivePtr(
+ new ob::VFMechanicalWorkOptimizationObjective(si, field)));
+ ss->setPlanner(ob::PlannerPtr(new og::TRRT(ss->getSpaceInformation())));
+ }
+ else if (plannerType == RRTSTAR)
+ {
+ ss->setOptimizationObjective(ob::OptimizationObjectivePtr(
+ new ob::VFUpstreamCriterionOptimizationObjective(si, field)));
+ ss->setPlanner(ob::PlannerPtr(new og::RRTstar(ss->getSpaceInformation())));
+ }
+ else if (plannerType == VFRRT)
+ {
+ double explorationSetting = 0.7;
+ double lambda = 1;
+ unsigned int update_freq = 100;
+ ss->setPlanner(ob::PlannerPtr(new og::VFRRT(ss->getSpaceInformation(), field, explorationSetting, lambda, update_freq)));
+ }
+ else
+ {
+ std::cout << "Bad problem number.\n";
+ exit(-1);
+ }
+
+ ss->setup();
+
+ return ss;
+}
+
+/** Get the filename to write the path to. */
+std::string problemName(PlannerType plannerType)
+{
+ if (plannerType == VFRRT)
+ return std::string("vfrrt-conservative.path");
+ else if (plannerType == TRRT)
+ return std::string("trrt-conservative.path");
+ else if (plannerType == RRTSTAR)
+ return std::string("rrtstar-conservative.path");
+ else
+ {
+ std::cout << "Bad problem number.\n";
+ exit(-1);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ // Run all three problems
+ for (unsigned int n = 0; n < 3; n++)
+ {
+ // initialize the planner
+ og::SimpleSetupPtr ss = setupProblem(PlannerType(n));
+
+ // attempt to solve the problem
+ ob::PlannerStatus solved = ss->solve(10.0);
+
+ if (solved)
+ {
+ if (solved == ob::PlannerStatus::EXACT_SOLUTION)
+ std::cout << "Found solution.\n";
+ else
+ std::cout << "Found approximate solution.\n";
+
+ // Set up to write the path
+ std::ofstream f(problemName(PlannerType(n)).c_str());
+ ompl::geometric::PathGeometric p = ss->getSolutionPath();
+ p.interpolate();
+ ob::OptimizationObjectivePtr upstream(new ob::VFUpstreamCriterionOptimizationObjective(
+ ss->getSpaceInformation(), field));
+ p.printAsMatrix(f);
+ std::cout << "Total upstream cost: " << p.cost(upstream) << "\n";
+ }
+ else
+ std::cout << "No solution found.\n";
+ }
+
+ return 0;
+}
diff --git a/demos/VFRRT/VectorFieldNonconservative.cpp b/demos/VFRRT/VectorFieldNonconservative.cpp
new file mode 100644
index 0000000..83e49c6
--- /dev/null
+++ b/demos/VFRRT/VectorFieldNonconservative.cpp
@@ -0,0 +1,123 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Caleb Voss and Wilson Beebe
+* 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.
+*********************************************************************/
+
+/* Authors: Caleb Voss, Wilson Beebe */
+
+#include <fstream>
+
+#include <ompl/base/StateSpace.h>
+#include <ompl/base/objectives/VFUpstreamCriterionOptimizationObjective.h>
+#include <ompl/base/spaces/RealVectorStateSpace.h>
+#include <ompl/geometric/planners/rrt/RRTstar.h>
+#include <ompl/geometric/planners/rrt/VFRRT.h>
+#include <ompl/geometric/SimpleSetup.h>
+
+namespace ob = ompl::base;
+namespace og = ompl::geometric;
+
+
+/** Basic unit-norm rotation field. */
+Eigen::VectorXd field(const ob::State *state)
+{
+ const ob::RealVectorStateSpace::StateType &x = *state->as<ob::RealVectorStateSpace::StateType>();
+ Eigen::VectorXd v(2);
+ v[0] = x[1];
+ v[1] = -x[0];
+ v.normalize();
+ return v;
+}
+
+int main(int argc, char **argv)
+{
+ // construct the state space we are planning in
+ ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
+ ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
+
+ ob::RealVectorBounds bounds(2);
+ bounds.setLow(-10);
+ bounds.setHigh(10);
+
+ space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
+
+ // define a simple setup class
+ og::SimpleSetup ss(space);
+
+ // set state validity checking for this space
+ ss.setStateValidityChecker(ob::StateValidityCheckerPtr(
+ new ob::AllValidStateValidityChecker(si)));
+
+ // create a start state
+ ob::ScopedState<> start(space);
+ start[0] = -5;
+ start[1] = -2;
+
+ // create a goal state
+ ob::ScopedState<> goal(space);
+ goal[0] = 5;
+ goal[1] = 2;
+
+ // set the start and goal states
+ ss.setStartAndGoalStates(start, goal, 0.1);
+
+ // initialize the planner
+ double explorationSetting = 0.7;
+ double lambda = 1;
+ unsigned int update_freq = 100;
+ ss.setPlanner(ob::PlannerPtr(new og::VFRRT(ss.getSpaceInformation(), field, explorationSetting, lambda, update_freq)));
+ ss.setup();
+
+ // attempt to solve the problem
+ ob::PlannerStatus solved = ss.solve(10.0);
+
+ if (solved)
+ {
+ if (solved == ob::PlannerStatus::EXACT_SOLUTION)
+ std::cout << "Found solution.\n";
+ else
+ std::cout << "Found approximate solution.\n";
+
+ // Set up to write the path
+ std::ofstream f("vfrrt-nonconservative.path");
+ ompl::geometric::PathGeometric p = ss.getSolutionPath();
+ p.interpolate();
+ ob::OptimizationObjectivePtr upstream(new ob::VFUpstreamCriterionOptimizationObjective(
+ ss.getSpaceInformation(), field));
+ p.printAsMatrix(f);
+ std::cout << "Total upstream cost: " << p.cost(upstream) << "\n";
+ }
+ else
+ std::cout << "No solution found.\n";
+
+ return 0;
+}
diff --git a/demos/VFRRT/plotConservative.py b/demos/VFRRT/plotConservative.py
new file mode 100755
index 0000000..8e3ebbf
--- /dev/null
+++ b/demos/VFRRT/plotConservative.py
@@ -0,0 +1,69 @@
+#!/usr/bin/env python
+
+######################################################################
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2015, Caleb Voss and Wilson Beebe
+# 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 Rice University 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.
+######################################################################
+
+# Authors: Caleb Voss, Wilson Beebe
+
+
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from pylab import *
+from matplotlib import cm
+
+def potential(x, y):
+ return 1 + np.sin(x) * np.sin(y)
+
+def potentialSurface():
+ X = np.arange(-8, 8, 0.25)
+ Y = np.arange(-8, 8, 0.25)
+ X, Y = np.meshgrid(X, Y)
+ Z = potential(X, Y)
+ return X, Y, Z
+
+fig = plt.figure()
+ax = fig.gca(projection='3d', aspect='equal')
+X, Y, Z = potentialSurface()
+ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.coolwarm, linewidth=0)
+
+x = np.loadtxt("vfrrt-conservative.path")
+ax.plot(x[:,0], x[:,1], potential(x[:,0], x[:,1]), color='b')
+
+x = np.loadtxt("trrt-conservative.path")
+ax.plot(x[:,0], x[:,1], potential(x[:,0], x[:,1]), color='r')
+
+x = np.loadtxt("rrtstar-conservative.path")
+ax.plot(x[:,0], x[:,1], potential(x[:,0], x[:,1]), color='g')
+
+plt.show()
diff --git a/demos/VFRRT/plotNonconservative.py b/demos/VFRRT/plotNonconservative.py
new file mode 100755
index 0000000..f0c6f14
--- /dev/null
+++ b/demos/VFRRT/plotNonconservative.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+
+######################################################################
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2015, Caleb Voss and Wilson Beebe
+# 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 Rice University 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.
+######################################################################
+
+# Authors: Caleb Voss, Wilson Beebe
+
+import matplotlib.pyplot as plt
+from pylab import *
+from matplotlib import cm
+
+def makeVectorField(f, xmin, xmax, ymin, ymax, step):
+ X,Y = meshgrid(arange(xmin,xmax,step),arange(ymin,ymax,step))
+ U,V = zip(*map(lambda xx: f(*xx), zip(X,Y)))
+ Q = quiver( X, Y, U, V, units='width')
+ quiverkey(Q, 0, 0, 4, '', coordinates='figure', labelpos='W')
+
+fig = plt.figure()
+ax = fig.gca(aspect='equal')
+x = np.loadtxt("vfrrt-nonconservative.path")
+makeVectorField(lambda x,y: (y/sqrt(x*x+y*y),-x/sqrt(x*x+y*y)), -6, 6, -6, 6, 0.5)
+ax.plot(x[:,0], x[:,1])
+plt.show()
diff --git a/doc/Doxyfile b/doc/Doxyfile
index 8a92c33..ddbd422 100644
--- a/doc/Doxyfile
+++ b/doc/Doxyfile
@@ -1996,7 +1996,7 @@ TAGFILES =
# tag file that is based on the input files it reads. See section "Linking to
# external documentation" for more information about the usage of tag files.
-GENERATE_TAGFILE =
+GENERATE_TAGFILE = html/ompl.tag
# If the ALLEXTERNALS tag is set to YES all external class will be listed in the
# class index. If set to NO only the inherited external classes will be listed.
diff --git a/doc/css/ompl.css b/doc/css/ompl.css
index 4df35ea..e87013e 100644
--- a/doc/css/ompl.css
+++ b/doc/css/ompl.css
@@ -75,6 +75,7 @@ div.contents div.textblock { margin-left: 20px; }
.navbar .nav > li > a { color: #ddd; padding: 10px 10px; }
.navbar { min-height: 40px; margin-bottom: 10px; }
.navbar-inverse .navbar-brand { height: 40px; padding: 10px 10px; color: #ddd; }
+.nav-tabs + .tab-content { margin-top: 20px; }
input[type="text"]:focus, select:focus {
-webkit-box-shadow: none;
@@ -172,3 +173,6 @@ div.plannerlist > dl.attention > dd::first-line {
top: -42px;
visibility: hidden;
}
+
+.btn-group a:visited { color: #333; }
+.modal-body pre { font-size: 12px; }
diff --git a/doc/header.html b/doc/header.html
index c43817f..4cc4ffd 100644
--- a/doc/header.html
+++ b/doc/header.html
@@ -46,8 +46,7 @@
<li><a href="installation.html">Installation</a></li>
<li><a href="tutorials.html">Tutorials</a></li>
<li><a href="group__demos.html">Demos</a></li>
- <li><a href="gui.html">OMPL.app GUI</a></li><li><a href="webapp.html">OMPL web app</a></li>
- <li><a href="benchmark.html">Benchmarking</a></li>
+
<li><a href="python.html">Python Bindings</a></li>
<li><a href="planners.html">Available Planners</a></li>
<li><a href="benchmark.html">Benchmarking Planners</a></li>
diff --git a/doc/header.html.in b/doc/header.html.in
index 06d1b6c..c7afcd4 100644
--- a/doc/header.html.in
+++ b/doc/header.html.in
@@ -47,7 +47,6 @@
<li><a href="tutorials.html">Tutorials</a></li>
<li><a href="group__demos.html">Demos</a></li>
@OMPLAPP_MENU@
- <li><a href="benchmark.html">Benchmarking</a></li>
<li><a href="python.html">Python Bindings</a></li>
<li><a href="planners.html">Available Planners</a></li>
<li><a href="benchmark.html">Benchmarking Planners</a></li>
diff --git a/doc/markdown/CForest.md b/doc/markdown/CForest.md
index cccf0ec..81e7c0d 100644
--- a/doc/markdown/CForest.md
+++ b/doc/markdown/CForest.md
@@ -78,8 +78,8 @@ planner->as<ompl::geometric::CForest>()->setNumThreads(6);
When implementing CForest, the focus was to modify the underlying planner as little as possible. Although the main idea of CForest remains, the actual implementation differs from the one proposed in the paper:
-- No message passing is used. Instead, shared memory and boost::threads are employed.
-- The paper creates two different versions: sequential (many trees expanding in the same CPU) and parallel (1 tree per CPU). Since boost::threads are used, the trees/CPU division is done by the scheduler.
+- No message passing is used. Instead, shared memory and std::threads are employed.
+- The paper creates two different versions: sequential (many trees expanding in the same CPU) and parallel (1 tree per CPU). Since std::threads are used, the trees/CPU division is done by the scheduler.
- In the paper, shared states are treated in a slightly different way than randomly sampled states. Due to the CForestStateSampler encapsulation, all states are treated the same way. Under some circumstances shared states are not included in other trees. However, tests showed that this does not have a high impact on performance.
- Sampling bounds are not explicitly set. When samples are created, we check whether they can lead to a better solution.
- Start and goal states are not included in the shared paths in order to keep code simpler.
@@ -1220,7 +1220,7 @@ ompl::geoemtric::MyPlanner solve(const base::PlannerTerminationCondition &ptc)
...
// Searching for improved solutions.
...
- if (updatedSolution)
+ if (updatedSolution)
{
...
if (intermediateSolutionCallback)
@@ -1321,7 +1321,7 @@ ompl::geometric::MyPlanner solve(const base::PlannerTerminationCondition &ptc)
}
// Searching for improved solutions.
...
- if (updatedSolution)
+ if (updatedSolution)
{
...
if (prune_) // Modification 3 - tree pruning.
diff --git a/doc/markdown/CForest.md.in b/doc/markdown/CForest.md.in
index cc0a6c8..35a8e51 100644
--- a/doc/markdown/CForest.md.in
+++ b/doc/markdown/CForest.md.in
@@ -78,8 +78,8 @@ planner->as<ompl::geometric::CForest>()->setNumThreads(6);
When implementing CForest, the focus was to modify the underlying planner as little as possible. Although the main idea of CForest remains, the actual implementation differs from the one proposed in the paper:
-- No message passing is used. Instead, shared memory and boost::threads are employed.
-- The paper creates two different versions: sequential (many trees expanding in the same CPU) and parallel (1 tree per CPU). Since boost::threads are used, the trees/CPU division is done by the scheduler.
+- No message passing is used. Instead, shared memory and std::threads are employed.
+- The paper creates two different versions: sequential (many trees expanding in the same CPU) and parallel (1 tree per CPU). Since std::threads are used, the trees/CPU division is done by the scheduler.
- In the paper, shared states are treated in a slightly different way than randomly sampled states. Due to the CForestStateSampler encapsulation, all states are treated the same way. Under some circumstances shared states are not included in other trees. However, tests showed that this does not have a high impact on performance.
- Sampling bounds are not explicitly set. When samples are created, we check whether they can lead to a better solution.
- Start and goal states are not included in the shared paths in order to keep code simpler.
@@ -223,7 +223,7 @@ ompl::geoemtric::MyPlanner solve(const base::PlannerTerminationCondition &ptc)
...
// Searching for improved solutions.
...
- if (updatedSolution)
+ if (updatedSolution)
{
...
if (intermediateSolutionCallback)
@@ -324,7 +324,7 @@ ompl::geometric::MyPlanner solve(const base::PlannerTerminationCondition &ptc)
}
// Searching for improved solutions.
...
- if (updatedSolution)
+ if (updatedSolution)
{
...
if (prune_) // Modification 3 - tree pruning.
diff --git a/doc/markdown/FAQ.md b/doc/markdown/FAQ.md
index ab17287..c842725 100644
--- a/doc/markdown/FAQ.md
+++ b/doc/markdown/FAQ.md
@@ -35,7 +35,7 @@
...
ompl::base::StateSpacePtr space(...);
ompl::base::SpaceInformationPtr si(new ompl::base::SpaceInformation(space));
- si->setValidStateSamplerAllocator(boost::bind(&allocValidStateSampler, _1));
+ si->setValidStateSamplerAllocator(std::bind(&allocValidStateSampler, std::placeholders::_1));
// for simplified calls, you can also use:
// si->setValidStateSamplerAllocator(&allocValidStateSampler);
~~~
diff --git a/doc/markdown/FindOMPL.cmake b/doc/markdown/FindOMPL.cmake
index 4a0003c..bc160ec 100644
--- a/doc/markdown/FindOMPL.cmake
+++ b/doc/markdown/FindOMPL.cmake
@@ -4,7 +4,7 @@
# OMPL_FOUND - OMPL was found
# OMPL_INCLUDE_DIRS - The OMPL include directory
# OMPL_LIBRARIES - The OMPL library
-# OMPLAPP_LIBRARIES - The OMPL.app library
+# OMPLAPP_LIBRARIES - The OMPL.app libraries
# OMPL_VERSION - The OMPL version in the form <major>.<minor>.<patchlevel>
# OMPL_MAJOR_VERSION - Major version
# OMPL_MINOR_VERSION - Minor version
@@ -55,17 +55,23 @@ if (OMPL_LIBRARY)
endif()
set(OMPL_LIBRARIES "${OMPL_LIBRARY}" CACHE FILEPATH "Path to OMPL library")
endif()
-# find the OMPL.app library
+# find the OMPL.app libraries
+find_library(OMPLAPPBASE_LIBRARY ompl_app_base
+ PATHS ${OMPL_LIB_PATH}
+ PATH_SUFFIXES lib build/lib)
find_library(OMPLAPP_LIBRARY ompl_app
PATHS ${OMPL_LIB_PATH}
PATH_SUFFIXES lib build/lib)
-if (OMPLAPP_LIBRARY)
+if (OMPLAPPBASE_LIBRARY AND OMPLAPP_LIBRARY)
if (OMPL_FIND_VERSION)
+ get_filename_component(libpath ${OMPLAPPBASE_LIBRARY} PATH)
+ file(GLOB OMPLAPPBASE_LIBS "${libpath}/libompl_app_base.${OMPL_FIND_VERSION}.*")
+ list(GET OMPLAPPBASE_LIBS -1 OMPLAPPBASE_LIBRARY)
get_filename_component(libpath ${OMPLAPP_LIBRARY} PATH)
file(GLOB OMPLAPP_LIBS "${libpath}/libompl_app.${OMPL_FIND_VERSION}.*")
list(GET OMPLAPP_LIBS -1 OMPLAPP_LIBRARY)
endif()
- set(OMPLAPP_LIBRARIES "${OMPLAPP_LIBRARY}" CACHE FILEPATH "Path to OMPL.app library")
+ set(OMPLAPP_LIBRARIES "${OMPLAPPBASE_LIBRARY};${OMPLAPP_LIBRARY}" CACHE STRING "Paths to OMPL.app libraries")
endif()
# find include path
diff --git a/doc/markdown/api_overview.md b/doc/markdown/api_overview.md
index 5981917..401f14b 100644
--- a/doc/markdown/api_overview.md
+++ b/doc/markdown/api_overview.md
@@ -282,7 +282,7 @@
\endhtmlonly
-The class ownership diagram above shows the relationship between the essential base classes in OMPL. For example, __SpaceInformation__ owns a __StateSpace__; __Planner__ does _not_ own __SpaceInformation__, although a __Planner__ does know about the __SpaceInformation__, and uses provided functionality. Users are encouraged to use the __SimpleSetup__ class (ompl::geometric::SimpleSetup or ompl::control::SimpleSetup). With this class, it is only necessary to instantiate a ompl::base::Stat [...]
+The class ownership diagram above shows the relationship between the essential base classes in OMPL. For example, __SpaceInformation__ owns a __StateSpace__; __Planner__ does _not_ own __SpaceInformation__, although a __Planner__ does know about the __SpaceInformation__, and uses provided functionality. Users are encouraged to use the __SimpleSetup__ class (ompl::geometric::SimpleSetup or ompl::control::SimpleSetup). With this class, it is only necessary to instantiate a ompl::base::Stat [...]
The ompl::base::StateValidityChecker is problem-specific, so no default implementation is available. See [this document](stateValidation.html) for more information on state validity checking. For more advanced definitions of goals, see [this document](goalRepresentation.html).
@@ -296,11 +296,11 @@ All static, non-member or const member functions are thread safe. Calling member
#### Memory management
-For all base classes __Class__, a __ClassPtr__ type is defined as well. __ClassPtr__ is in fact a [boost shared pointer](http://wiki.inkscape.org/wiki/index.php/Boost_shared_pointers) for __Class__:
+For all base classes __Class__, a __ClassPtr__ type is defined as well. __ClassPtr__ is in fact a [shared pointer](http://en.cppreference.com/w/cpp/memory/shared_ptr) for __Class__:
~~~{.cpp}
class Class;
-typedef boost::shared_ptr<Class> ClassPtr;
+typedef std::shared_ptr<Class> ClassPtr;
~~~
The code above is generated by the OMPL_CLASS_FORWARD macro defined in ompl/util/ClassForward.h:
diff --git a/doc/markdown/api_overview.md.in b/doc/markdown/api_overview.md.in
index b336aeb..fac5383 100644
--- a/doc/markdown/api_overview.md.in
+++ b/doc/markdown/api_overview.md.in
@@ -4,7 +4,7 @@
@OMPLSVG@
\endhtmlonly
-The class ownership diagram above shows the relationship between the essential base classes in OMPL. For example, __SpaceInformation__ owns a __StateSpace__; __Planner__ does _not_ own __SpaceInformation__, although a __Planner__ does know about the __SpaceInformation__, and uses provided functionality. Users are encouraged to use the __SimpleSetup__ class (ompl::geometric::SimpleSetup or ompl::control::SimpleSetup). With this class, it is only necessary to instantiate a ompl::base::Stat [...]
+The class ownership diagram above shows the relationship between the essential base classes in OMPL. For example, __SpaceInformation__ owns a __StateSpace__; __Planner__ does _not_ own __SpaceInformation__, although a __Planner__ does know about the __SpaceInformation__, and uses provided functionality. Users are encouraged to use the __SimpleSetup__ class (ompl::geometric::SimpleSetup or ompl::control::SimpleSetup). With this class, it is only necessary to instantiate a ompl::base::Stat [...]
The ompl::base::StateValidityChecker is problem-specific, so no default implementation is available. See [this document](stateValidation.html) for more information on state validity checking. For more advanced definitions of goals, see [this document](goalRepresentation.html).
@@ -18,11 +18,11 @@ All static, non-member or const member functions are thread safe. Calling member
#### Memory management
-For all base classes __Class__, a __ClassPtr__ type is defined as well. __ClassPtr__ is in fact a [boost shared pointer](http://wiki.inkscape.org/wiki/index.php/Boost_shared_pointers) for __Class__:
+For all base classes __Class__, a __ClassPtr__ type is defined as well. __ClassPtr__ is in fact a [shared pointer](http://en.cppreference.com/w/cpp/memory/shared_ptr) for __Class__:
~~~{.cpp}
class Class;
-typedef boost::shared_ptr<Class> ClassPtr;
+typedef std::shared_ptr<Class> ClassPtr;
~~~
The code above is generated by the OMPL_CLASS_FORWARD macro defined in ompl/util/ClassForward.h:
diff --git a/doc/markdown/benchmark.md b/doc/markdown/benchmark.md
index 9e52331..deee5e8 100644
--- a/doc/markdown/benchmark.md
+++ b/doc/markdown/benchmark.md
@@ -163,7 +163,7 @@ b.addPlanner(base::PlannerPtr(new geometric::LBKPIECE1(ss.getSpaceInformation())
// For planners that we want to configure in specific ways,
// the ompl::base::PlannerAllocator should be used:
-b.addPlannerAllocator(boost::bind(&myConfiguredPlanner, _1));
+b.addPlannerAllocator(std::bind(&myConfiguredPlanner, std::placeholders::_1));
// etc.
// Now we can benchmark: 5 second time limit for each plan computation,
@@ -208,8 +208,8 @@ void optionalPostRunEvent(const base::PlannerPtr &planner, tools::Benchmark::Run
}
// After the Benchmark class is defined, the events can be optionally registered:
-b.setPreRunEvent(boost::bind(&optionalPreRunEvent, _1));
-b.setPostRunEvent(boost::bind(&optionalPostRunEvent, _1, _2));
+b.setPreRunEvent(std::bind(&optionalPreRunEvent, std::placeholders::_1));
+b.setPostRunEvent(std::bind(&optionalPostRunEvent, std::placeholders::_1, std::placeholders::_2));
~~~
@@ -269,7 +269,7 @@ Collected benchmark data for each planner execution:
Planning algorithms can also register callback functions that the Benchmark class will use to measure progress properties at regular intervals during a run of the planning algorithm. Currently only RRT* uses this functionality. The RRT* constructor registers, among others, a function that returns the cost of the best path found so far:
- addPlannerProgressProperty("best cost REAL", boost::bind(&RRTstar::getBestCost, this));
+ addPlannerProgressProperty("best cost REAL", std::bind(&RRTstar::getBestCost, this));
With the Benchmark class one can thus measure how the cost is decreasing over time. The ompl_benchmark_statistics.py script will automatically generate plots of progress properties as a function of time.
diff --git a/doc/markdown/boost.md b/doc/markdown/boost.md
deleted file mode 100644
index a3dbcdd..0000000
--- a/doc/markdown/boost.md
+++ /dev/null
@@ -1,192 +0,0 @@
-# Boost Structures Used in OMPL
-
-[Boost](http://www.boost.org) provides an extension to the C++ standard template library, supplying the user with many generic, cross-platform concepts that are easily included in the developer's program. For users that are unfamiliar with Boost, this page will briefly describe the constructs from the Boost library used in OMPL. This tutorial makes no attempt to fully educate the user on the usage and API of the Boost library, only to give a high level overview of these components and wh [...]
-
-\attention
-OMPL requires Boost __version 1.48__ or greater.
-
-
-# Shared Pointer (boost/shared_ptr.hpp)
-
-The [shared pointer from Boost](http://www.boost.org/libs/smart_ptr/shared_ptr.htm) provides intelligent memory
-management for dynamically allocated objects created with the _new_ command. In short, the templated shared_ptr object will
-delete (free) the allocated memory when the pointer goes out of scope. Since these pointers utilize [reference counting](http://en.wikipedia.org/wiki/Reference_counting), it is safe to pass them into functions or copy them;
-the object referenced by the pointer will only be destroyed when the reference count goes to zero.
-
-~~~{.cpp}
-class MyClass
-{
- /* ... */
- void myClassMethod (void) { /* ... */ }
- /* ... */
-};
-
-void performComputation ()
-{
- // Create an instance of MyClass using a shared_ptr
- boost::shared_ptr <MyClass> myClassPtr (new MyClass ());
-
- // Invoke methods on the object as normal
- myClassPtr->myClassMethod ();
-
-} // myClassPtr goes out of scope here. The instance of MyClass will be deallocated at this point.
-~~~
-
-In the example above, the function performComputation creates a new instance of MyClass but instantiates it using a shared_ptr. Once the object is created, the pointer can be used like normal (using -> or the * operators). When the function has finished execution, the object does not have to be specifically deleted; the reference count on myClassPtr will decrement to zero when the shared_ptr goes out of scope, triggering the automatic destruction the MyClass object.
-
-The shared_ptr is used in OMPL in nearly all instances where an object is created from the heap in order to mitigate memory leaks. Most classes in OMPL already have a typedef for a shared_ptr object using the _OMPL_CLASS_FORWARD_ macro. For example, ompl::base::Planner utilizes the macro just before the class declaration:
-
-~~~{.cpp}
-OMPL_CLASS_FORWARD(Planner);
-~~~
-
-which when expanded creates a forward declaration of the class _Planner_, and defines the type _PlannerPtr_:
-
-~~~{.cpp}
-class Planner;
-typedef boost::shared_ptr<Planner> PlannerPtr;
-~~~
-
-
-# Function and Bind (boost/function.hpp and boost/bind.hpp)
-
-[Boost's Function library](http://www.boost.org/libs/function) provides a templated wrapper for function pointers and a generalized framework for [callbacks](http://en.wikipedia.org/wiki/Callback_%28computer_programming%29). It is very easy to encapsulate a function pointer into a single object using Boost. Function objects are templated with the first argument being the return type for the function, and subsequent arguments corresponding to the function's arguments. For example, assume [...]
-
-~~~{.cpp}
-int computeMin (int x, int y)
-{
- return x < y ? x : y;
-}
-~~~
-
-We could create a function pointer called _minFuncPtr_ to the _computeMin_ function using C++ syntax like this:
-
-~~~{.cpp}
-int (*minFuncPtr)(int, int) = computeMin;
-~~~
-
-The same object type using Boost looks like this:
-
-~~~{.cpp}
-boost::function<int(int,int)> minFuncPtr(computeMin);
-~~~
-
-In addition to improved readability and flexibility, we can also take advantage of the [Bind library](http://www.boost.org/libs/bind) from Boost to create function pointers to class members (not nearly as easy with standard C++ syntax), and fix one or more variables of our function pointer. In a more complex example, assume we have the class Math:
-
-~~~{.cpp}
-class Math
-{
- /* ... */
-
- // Return the minimum of x and y. If x or y is below a lower bound,
- // return the lower bound instead.
- int boundedMin (int x, int y, int lowerBound = 0)
- {
- if (x > lowerBound && y > lowerBound)
- return (x < y) ? x : y;
- else
- return lowerBound;
- }
-
- /* ... */
-};
-~~~
-
-If we wanted to create standard C++ function pointer to _Math::boundedMin_, the code gets a little crazy:
-
-~~~{.cpp}
-// Create a function pointer called minFuncPtr
-int (Math::*minFuncPtr)(int, int, int);
-
-// Assign the function pointer to the boundedMin method in the Math class.
-minFuncPtr = &Math::boundedMin;
-~~~
-
-Calling the boundedMin function through the function pointer we just created is equally as obfuscated:
-
-~~~{.cpp}
-// Create the Math object so that we have a method to invoke
-Math math;
-
-// Wait, what?
-int theMin = (math.*minFuncPtr)(1, 2, 0);
-~~~
-
-Note that the function pointer and the corresponding invocation had to include ALL arguments to the boundedMin method, even the default arguments. Since the function pointer is a type, all of the arguments supplied to the function are part of the type and we cannot use the default value of 0 for the lowerBound argument. We can get around all of this by using Boost Functions in conjunction with Boost's Bind library:
-
-~~~{.cpp}
-// Create the Math object so that we have a method to invoke
-Math math;
-
-// Setup the Boost function pointer as a function that returns an int, and takes two integer arguments
-boost::function<int(int,int)> minFuncPtr;
-
-// Initializing the function pointer to the boundedMin method of the math
-// instance, and binding the lowerBound argument to zero.
-minFuncPtr = boost::bind (&Math::boundedMin, math, _1, _2, 0);
-
-// Invoking the function. Much better.
-int theMin = minFuncPtr (1, 2);
-~~~
-
-In a nutshell, boost::bind returns a function pointer to the method given in the first argument, using the instance (“math”) supplied in the second argument. _1 and _2 indicate that the first and second arguments of the boundedMin function will be supplied when the function is called. The zero is permanently bound to the function pointer as the third argument (the default value for “lowerBound”).
-
-[boost::function](http://www.boost.org/libs/function) and [boost::bind](http://www.boost.org/libs/bind) are used in OMPL whenever a callback function is used to invoke methods on a global function or specific instance. These are used to improve readability of the code, as well as to simplify the use of callback functions across object barriers. Also, if an uninitialized Boost function pointer is accessed, a _bad_function_call_ exception is thrown, which is far more desirable than attempt [...]
-
-
-# NonCopyable (boost/noncopyable.hpp)
-
-Boost provides a base class called [noncopyable](http://www.boost.org/libs/utility/utility.htm) that classes can derive from in order to prevent them from being copied. noncopyable has a private copy constructor and assignment operator. Therefore, classes deriving from noncopyable are prohibited from invoking these copy operations. Fully utilizing noncopyable is as simple as:
-
-~~~{.cpp}
-class MyClass : boost::noncopyable
-{
- ...
-};
-~~~
-
-OMPL derives several classes from boost::noncopyable. The rationale behind
-this is that these classes should never be copied anyway, either because
-the copy mechanism is highly non-trivial, copying such an object would be
-prohibitive in terms of memory, or both.
-
-
-# Thread (boost/thread.hpp)
-
-OMPL is designed to be thread-safe, meaning that most of the common API is reentrant (in general, any function marked as _const_ is thread-safe). However, since OMPL is not limited to a single operating system, the creation and management of threads poses a complication since operating systems are free to choose how a developer creates and manages threads. Thankfully, Boost provides a cross-platform [Threading library](http://www.boost.org/libs/thread) to take care of the operating syste [...]
-
-It is very easy to create Threads in boost. Simply create an object of type boost::thread, and supply it the handle to a "callable" object, or function:
-
-~~~{.cpp}
-void threadFunction (void) { ... }
-
-// Threads start automatically on creation
-boost::thread myThread (threadFunction);
-// Wait until the thread has finished execution
-myThread.join ();
-~~~
-
-~~~{.cpp}
-void threadFunction (int someNumber, const char *aString) { ... }
-
-// Create a thread for threadFunction, and pass in some parameters
-boost::thread myThread (threadFunction, 42, "A good number");
-~~~
-
-~~~{.cpp}
-// Create an object for the Thread to operate in. Thread calls operator () to start.
-struct ThreadStruct
-{
- void operator () (int param1, const char *param2, const std::vector<double> ¶m3)
- {
- ...
- }
-};
-
-// Create threaded object.
-ThreadStruct tStruct;
-// Create Thread. Pass in three parameters.
-boost::thread myThread (tStruct, 1, "Two", std::vector <double> ());
-~~~
-
-By using [Boost thread](http://www.boost.org/libs/thread), OMPL is able to remain operating system independent in multi-threaded applications, as long as the operating system employed by the user is supported by Boost.
diff --git a/doc/markdown/buildOptions.md b/doc/markdown/buildOptions.md
index c8833cd..7378fef 100644
--- a/doc/markdown/buildOptions.md
+++ b/doc/markdown/buildOptions.md
@@ -1,6 +1,6 @@
# Build Options
-If you are building OMPL from source, there are several options that you can use to configure how OMPL is compiled. The options can be set via the command line like so: `cmake -D<option>=ON ../..`. If previously ran cmake in your build directory, it is safest to remove the CMakeCache.txt before re-running cmake with different options. Below is a list of all options.
+If you are building OMPL from source, there are several options that you can use to configure how OMPL is compiled. The options can be set via the command line like so: `cmake -D<option>=ON ../..`. If you previously ran cmake in your build directory, it is safest to remove the CMakeCache.txt before re-running cmake with different options. Below is a list of all options.
Option | Default value | Description
------------------------------|---------------|-----------------------------------------------------------------
@@ -8,7 +8,6 @@ OMPLAPP_PQP | ON | Enable support for the [PQP](htt
OMPL_BUILD_DEMOS | ON | Compile the OMPL demo programs. (The binaries are never installed.)
OMPL_BUILD_PYBINDINGS | ON | Whether to compile the Python bindings (requires Py++).
OMPL_BUILD_PYTESTS | ON | Whether the Python tests should be added to the `test` target.
-OMPL_LOCAL_PYPLUSPLUS_INSTALL | OFF | Whether `make installpyplusplus` installs Py++ in your build directory instead of `/usr/local`.
+OMPL_BUILD_TESTS | ON | Wether to compile the C++ unit tests
OMPL_REGISTRATION | ON | Whether the registration page is shown. (Disabling it might be useful for build bots.)
-OMPL_TESTS_TEAMCITY | OFF | Whether the output from the unit tests should be reformatted for the TeamCity Continuous Integration system.
OMPL_VERSIONED_INSTALL | OFF | Whether directories and executables created by `make install` have a version suffix. Useful if you want to install multiple versions of OMPL in the same directory.
diff --git a/doc/markdown/citations.md b/doc/markdown/citations.md
index 62dad05..2d47363 100644
--- a/doc/markdown/citations.md
+++ b/doc/markdown/citations.md
@@ -1,11 +1,16 @@
-# Citation
+# Citations
If you use OMPL in your research, we kindly ask you to include the following citation in your publications:
-- Ioan A. Șucan, Mark Moll, Lydia E. Kavraki, The Open Motion Planning Library, _IEEE Robotics & Automation Magazine,_ 19(4):72--82, December 2012. DOI: [10.1109/MRA.2012.2205651](http://dx.doi.org/10.1109/MRA.2012.2205651) [[PDF]](ieee-ram-2012-ompl.pdf) http://ompl.kavrakilab.org
-
-~~~
- at article{sucan2012the-open-motion-planning-library,
+<ul>
+<li> Ioan A. Șucan, Mark Moll, Lydia E. Kavraki, The Open Motion Planning Library, _IEEE Robotics & Automation Magazine,_ 19(4):72--82, December 2012. http://ompl.kavrakilab.org
+\htmlonly
+<br><div class="btn-group btn-group-xs">
+ <a class="btn btn-default" href="ieee-ram-2012-ompl.pdf"> pdf </a>
+ <a class="btn btn-default" href="http://dx.doi.org/10.1109/MRA.2012.2205651"> publisher </a>
+ <button class="btn btn-default" type="button" data-toggle="modal" data-target="#sucan2012ompl"> bibtex </button>
+</div>
+<div class="modal fade" tabindex="-1" id="sucan2012ompl"><div class="modal-dialog"><div class="modal-content"><div class="modal-header"><button type="button" class="close" data-dismiss="modal" aria-hidden="true">×</button><h4 class="modal-title">BibTeX</h4></div><div class="modal-body"><pre>@article{sucan2012the-open-motion-planning-library,
Author = {Ioan A. {\c{S}}ucan and Mark Moll and Lydia E. Kavraki},
Doi = {10.1109/MRA.2012.2205651},
Journal = {{IEEE} Robotics \& Automation Magazine},
@@ -16,5 +21,29 @@ If you use OMPL in your research, we kindly ask you to include the following cit
Note = {\url{http://ompl.kavrakilab.org}},
Volume = {19},
Year = {2012}
-}
-~~~
+}</pre></div></div></div></div>
+\endhtmlonly
+</ul>
+
+If you use the OMPL [benchmarking facilities](benchmark.html) or [Planner Arena](http://plannerarena.org), then we kindly ask you to include the following citation in your publications:
+<ul>
+<li> Mark Moll, Ioan A. Șucan, Lydia E. Kavraki, Benchmarking Motion Planning Algorithms: An Extensible Infrastructure for Analysis and Visualization, _IEEE Robotics & Automation Magazine,_ 22(3):96--102, September 2015.
+\htmlonly
+<br><div class="btn-group btn-group-xs">
+ <a class="btn btn-default" href="http://www.cs.rice.edu/~mmoll/publications/moll2015benchmarking-motion-planning-algorithms.pdf"> pdf </a>
+ <a class="btn btn-default" href="http://dx.doi.org/10.1109/MRA.2015.2448276"> publisher </a>
+ <button class="btn btn-default" type="button" data-toggle="modal" data-target="#moll2015benchmarking"> bibtex </button>
+</div>
+<div class="modal fade" tabindex="-1" id="moll2015benchmarking"><div class="modal-dialog"><div class="modal-content"><div class="modal-header"><button type="button" class="close" data-dismiss="modal" aria-hidden="true">×</button><h4 class="modal-title">BibTeX</h4></div><div class="modal-body"><pre>@article{sucan2012the-open-motion-planning-library,
+ Author = {Mark Moll and Ioan A. {\c{S}}ucan and Lydia E. Kavraki},
+ Doi = {10.1109/MRA.2015.2448276},
+ Journal = {{IEEE} Robotics \& Automation Magazine},
+ Month = {September},
+ Number = {3},
+ Pages = {96--102},
+ Title = {Benchmarking Motion Planning Algorithms: An Extensible Infrastructure for Analysis and Visualization},
+ Volume = {22},
+ Year = {2015}
+}</pre></div></div></div></div>
+\endhtmlonly
+</ul>
diff --git a/doc/markdown/code/svc.cpp b/doc/markdown/code/svc.cpp
index 536499b..fc03856 100644
--- a/doc/markdown/code/svc.cpp
+++ b/doc/markdown/code/svc.cpp
@@ -22,6 +22,6 @@ base::SpaceInformationPtr si(space);
// either this call:
si->setStateValidityChecker(base::StateValidityCheckerPtr(new myStateValidityCheckerClass(si)));
// or this call:
-si->setStateValidityChecker(boost::bind(&myStateValidityCheckerFunction, _1));
+si->setStateValidityChecker(std::bind(&myStateValidityCheckerFunction, std::placeholders::_1));
si->setStateValidityCheckingResolution(0.03); // 3%
si->setup();
diff --git a/doc/markdown/contact.md b/doc/markdown/contact.md
index 02251e8..406acb1 100644
--- a/doc/markdown/contact.md
+++ b/doc/markdown/contact.md
@@ -26,6 +26,11 @@ To report a bug, please click [here](https://bitbucket.org/ompl/ompl/issues?stat
</div>
<input type="hidden" name="_next" value="thank-you.html" />
<input type="hidden" name="_subject" value="OMPL contact form submission" />
+ <input type="hidden" name="IP" id="IP">
+ <script type="application/javascript">
+ window.onload = function () {
+ $.getJSON("https://api.ipify.org?format=jsonp&callback=?",function(json){$("#IP").val(json.ip);});};
+ </script>
<input type="text" name="_gotcha" style="display:none" />
<input type="submit" value="Send" name='submit' class="btn btn-primary" />
</form>
diff --git a/doc/markdown/contrib.md b/doc/markdown/contrib.md
index eef25e9..2066e7c 100644
--- a/doc/markdown/contrib.md
+++ b/doc/markdown/contrib.md
@@ -20,7 +20,7 @@ We welcome external contributions. If you would like us to list your contributio
3. In the unlikely event that the submitted code is of poor quality, we will try to suggest the changes necessary to include the contribution in OMPL, and postpone merging until the necessary changes are made.
- In almost all cases there will be minor issues. [Bitbucket](https://bitbucket.org/ompl/ompl) makes it very easy to have a two-way conversation with the OMPL developers about the code. Common issues that are typically easy to fix include: [const-correctnes](http://en.wikipedia.org/wiki/Const-correctness) of methods and their arguments, missing/incorrect documentation, [memory leaks](http://en.wikipedia.org/wiki/Memory_leak), and gross deviations form the [OMPL style guide](styleGuide.html).
+ In almost all cases there will be minor issues. [Bitbucket](https://bitbucket.org/ompl/ompl) makes it very easy to have a two-way conversation with the OMPL developers about the code. Common issues that are typically easy to fix include: [const-correctness](http://en.wikipedia.org/wiki/Const-correctness) of methods and their arguments, missing/incorrect documentation, [memory leaks](http://en.wikipedia.org/wiki/Memory_leak), and gross deviations form the [OMPL style guide](styleGuide.html).
4. All source code files must have a BSD license at the top of each file plus the names of the authors. Also, the code should contain doxygen-style documentation, so that the API documentation can be automatically generated and included in the OMPL web site. Finally, the code should be formatted according to the [OMPL style guide](styleGuide.html).
diff --git a/doc/markdown/developers.md b/doc/markdown/developers.md
index 4fd67af..8a2868e 100644
--- a/doc/markdown/developers.md
+++ b/doc/markdown/developers.md
@@ -1,10 +1,10 @@
# Developers
-OMPL is developed and maintained by the [Physical and Biological Computing Group](http://www.kavrakilab.org) at Rice University, led by [Dr. Lydia Kavraki](http://www.cs.rice.edu/~kavraki). The development is coordinated by [Dr. Mark Moll](http://www.cs.rice.edu/~mmoll), [Dr. Lydia Kavraki](http://www.cs.rice.edu/~kavraki) (Rice) and [Dr. Ioan Șucan](http://ioan.sucan.ro) (Google\[X\], formerly Rice). Many others have contributed to OMPL as well, as can be seen below.
+OMPL is developed and maintained by the [Physical and Biological Computing Group](http://www.kavrakilab.org) at Rice University, led by [Dr. Lydia Kavraki](http://www.cs.rice.edu/~kavraki). The development is coordinated by [Dr. Mark Moll](http://mmoll.rice.edu), [Dr. Lydia Kavraki](http://www.cs.rice.edu/~kavraki) (Rice) and [Dr. Ioan Șucan](http://ioan.sucan.ro) (Google\[X\], formerly Rice). Many others have contributed to OMPL as well, as can be seen below.
# Core developers:
-- [Mark Moll](http://www.cs.rice.edu/~mmoll), Rice University
+- [Mark Moll](http://mmoll.rice.edu), Rice University
- [Ioan Șucan](http://ioan.sucan.ro), Google\[X\] (formerly Rice University)
- [Ryan Luna](http://www.ryanluna.com), Rice University
- [Luis Torres](http://luis.web.unc.edu), Ron Alterovitz' [Computational Robotics Group](http://robotics.cs.unc.edu), University of North Carolina at Chapel Hill
diff --git a/doc/markdown/download.md.in b/doc/markdown/download.md.in
index 7fd516d..203fd01 100644
--- a/doc/markdown/download.md.in
+++ b/doc/markdown/download.md.in
@@ -16,6 +16,7 @@
.
Note that these package managers may not always have the latest release.
+- [Installation script for Ubuntu 14.04, 15.10, and 16.04](install-ompl-ubuntu.sh)
- [Installation instructions.](installation.html)
- [Older releases.](https://bitbucket.org/ompl/ompl/downloads) See the [release notes](releaseNotes.html) for a brief a description of changes for each release.
diff --git a/doc/markdown/education.md b/doc/markdown/education.md
index 29bf202..70fed2a 100644
--- a/doc/markdown/education.md
+++ b/doc/markdown/education.md
@@ -15,7 +15,7 @@ As part of this effort, we have developed a collection of assignments that can b
- Centralized multi-robot planning
- Protein folding
-We are looking for educational partners to use and further develop the material. If you are interested in the educational material, please contact [Mark Moll](http://www.cs.rice.edu/~mmoll) and [Lydia Kavraki](http://www.cs.rice.edu/~kavraki).
+We are looking for educational partners to use and further develop the material. If you are interested in the educational material, please contact [Mark Moll](http://mmoll.rice.edu) and [Lydia Kavraki](http://www.cs.rice.edu/~kavraki).
\htmlonly
diff --git a/doc/markdown/installPyPlusPlus.md b/doc/markdown/installPyPlusPlus.md
index fdb1427..b39e1de 100644
--- a/doc/markdown/installPyPlusPlus.md
+++ b/doc/markdown/installPyPlusPlus.md
@@ -1,12 +1,19 @@
# Installation of Py++
-We have included a script to download and install [Py++](https://bitbucket.org/ompl/pyplusplus) and its dependencies ([pygccxml](https://bitbucket.org/ompl/pygccxml) and [GCC-XML](https://github.com/gccxml/gccxml)). The script is located at ompl/src/external/installPyPlusPlus.{sh,bat}. You can run this script (after running cmake) like so:
+[Py++](https://bitbucket.org/ompl/pyplusplus) depends on [pygccxml](https://github.com/gccxml/pygccxml), which in turn depends on [CastXML](https://github.com/CastXML/CastXML). You can install these packages from source yourself, but you can also use package managers. For CastXML there are binaries available at <https://midas3.kitware.com/midas/folder/13152>.
- make installpyplusplus
+## Ubuntu
-The script may ask for your password so that it can install these programs. If the script completes successfully, run cmake again and you should be able to generate the Python bindings by typing “make update_bindings”.
+For Ubuntu 15.10 and newer you can install `castxml` with `sudo apt-get install castxml`.
-If you are using OS X and MacPorts, it is recommended you install Py++ like so:
+The pygccxml and Py++ packages can be installed with `pip` (a package manager specifically for Python packages):
- sudo port sync
- sudo port install py27-pyplusplus-devel
+ sudo apt-get install pip
+ sudo pip install pygccxml https://bitbucket.org/ompl/pyplusplus/get/1.6.tar.gz
+
+## OS X
+
+If you use MacPorts on OS X, then Py++ and its dependencies can be installed like so:
+
+ sudo port install py27-pyplusplus # if you use python2.7
+ sudo port install py35-pyplusplus # if you use python3.5
diff --git a/doc/markdown/installation.md b/doc/markdown/installation.md
index b95d11e..361c2ff 100644
--- a/doc/markdown/installation.md
+++ b/doc/markdown/installation.md
@@ -1,207 +1,108 @@
# Installation
-If you use Linux or OS X, then all dependencies can be installed either through a package manager or by OMPL's build system. In other words, you probably don't have to compile dependencies from source.
-
-To compile OMPL the following two packages are required:
-- [Boost], version 1.48 or higher, and
-- [CMake], version 2.8.7 or higher.
-
-The build system includes a [number of options](buildOptions.html) that you can enable or disable. To be able to generate python bindings you need to install the [Python] library and header files.
-
-Below are more detailed installation instructions for [Linux](#install_linux), [OS X](#install_osx), and [Windows](#install_windows).
-
-\note If you are using [ROS], please see [MoveIt!][moveit].
-
\htmlonly
-<div class="btn-group">
- <a class="btn btn-default" href="#install_linux">Linux</a>
- <a class="btn btn-default" href="#install_osx">OS X</a>
- <a class="btn btn-default" href="#install_windows">Windows</a>
+<div class="panel panel-default">
+ <div class="panel-body">
+ <h2>Select your operating system:</h2>
+ <!-- Nav tabs -->
+ <ul class="nav nav-pills" role="tablist">
+ <li role="presentation" class="active"><a href="#ubuntu" aria-controls="ubuntu" role="tab" data-toggle="pill">Ubuntu</a></li>
+ <li role="presentation"><a href="#fedora" aria-controls="fedora" role="tab" data-toggle="pill">Fedora</a></li>
+ <li role="presentation"><a href="#linux" aria-controls="linux" role="tab" data-toggle="pill">Linux (generic)</a></li>
+ <li role="presentation"><a href="#osx" aria-controls="osx" role="tab" data-toggle="pill">OS X</a></li>
+ <li role="presentation"><a href="#windows" aria-controls="windows" role="tab" data-toggle="pill">MS Windows</a></li>
+ </ul>
+ </div>
</div>
-\endhtmlonly
-
-
-# Installation on Linux {#install_linux}
-
-## Ubuntu Linux {#install_linux_ubuntu}
-
-### Installation from repositories
-
-Starting with Ubuntu 14.04, you can install OMPL like so
-
- apt-get install libompl-dev ompl-demos
-
-Debian packages for the latest version of OMPL are also found in ROS distributions. All you need to do is add the ROS repository to your list of sources (you probably have added this already if you are using ROS):
-
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list'
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-
-and install OMPL:
-
- sudo apt-get update
- sudo apt-get install ros-`rosversion -d`-ompl
-
-
-### Installation from source
-
-- Install Boost, CMake, and optional dependencies
-
- sudo apt-get install libboost-all-dev cmake doxygen graphviz python-dev libeigen3-dev libode-dev
-
-- Create a build directory and run cmake:
-
- cd ompl
- mkdir -p build/Release
- cd build/Release
- cmake ../..
-
-- If you want Python bindings, type the following two commands:
-
- make installpyplusplus && cmake . # download & install Py++
- make -j 4 update_bindings
-
-- Compile OMPL by typing `make -j 4`.
-- Optionally, run the test programs by typing `make test`.
-- Optionally, generate documentation by typing `make doc`.
-- If you need to install the library, you can type `sudo make install`. The install location is specified by `CMAKE_INSTALL_PREFIX`. If you install in a non-standard location, you have to set the environment variable PYTHONPATH to the directory where the OMPL python module is installed (e.g., $HOME/lib/python2.7/site-packages).
-
-### Installation from source for use with ROS
-
-If you'd like to use your own build of OMPL with ROS, follow the following steps:
-- [Create a normal ROS catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace)
-- [Clone OMPL into the src/ folder](\ref download_repos)
-- Add the package.xml file:
-
- wget https://raw.githubusercontent.com/ros-gbp/ompl-release/debian/`rosversion -d`/`lsb_release -cs`/ompl/package.xml
-
-- To build, you must run catkin_make_isolated instead of the normal catkin_make since OMPL is not a normal catkin package.
-- When sourcing this workspace, be sure to source devel_isolated/setup.bash.
-
-## Fedora Linux
-
-Thanks to Rich Mattes, OMPL core is available as a package for Fedora:
-
- sudo yum install ompl
-
-This package may not be the latest version, though. The source installation instructions for Fedora Linux are mostly the same as for Ubuntu Linux, although the packages have slightly different names. On Fedora, you can install the dependencies like so:
-
- sudo yum install boost-devel cmake python-devel eigen3 ode-devel doxygen graphviz
-
-The build steps are the same as for Ubuntu Linux.
-
-
-## Debian Linux
-
-Thanks to Leopold Palomo-Avellaneda, OMPL core is available as a collection of packages for Debian:
-
- apt-get install libompl-dev ompl-demos
-
-
-# Installation on Mac OS X {#install_osx}
-
-It is easiest to install OMPL through [MacPorts], a package manager for OS X. However, if you feel adventurous, it is possible to install OMPL's dependencies with [HomeBrew](#install_homebrew) and compile OMPL yourself.
+<!-- Tab panes -->
+<div class="tab-content">
+ <div role="tabpanel" class="tab-pane active" id="ubuntu">
+ <h2>Ubuntu</h2>
+ <ul class="nav nav-tabs" role="tablist">
+ <li role="presentation" class="active"><a href="#ubuntusource" aria-controls="ubuntusource" role="tab" data-toggle="tab">From source</a></li>
+ <li role="presentation"><a href="#ubuntubinary" aria-controls="ubuntubinary" role="tab" data-toggle="tab">Binary</a></li>
+ <li role="presentation"><a href="#ubunturos" aria-controls="ubunturos" role="tab" data-toggle="tab">ROS</a></li>
+ </ul>
+ <div class="tab-content">
+ <div role="tabpanel" class="tab-pane active" id="ubuntusource">
+ <a href="install-ompl-ubuntu.sh">Download the OMPL installation script</a>. First, make the script executable:
+ <pre class="fragment">chmod u+x install-ompl-ubuntu.sh</pre>
+ Next, there are three ways to run this script:
+ <ul>
+ <li><code>./install-ompl-ubuntu.sh</code> will install OMPL without Python bindings</li>
+ <li><code>./install-ompl-ubuntu.sh --python</code> will install OMPL with Python bindings</li>
+ <li><code>./install-ompl-ubuntu.sh --app</code> will install OMPL.app with Python bindings</li>
+ </ul>
+ The script downloads and installs OMPL and all dependencies via <code>apt-get</code> & <code>pip</code> and from source. It will ask for your password to install things. The script has been tested on vanilla installs of Ubuntu 14.04 (Trusty), 15.10 (Wily), and 16.04 (Xenial).
+ </div>
+ <div role="tabpanel" class="tab-pane" id="ubuntubinary">
+ Simply type:
+ <pre class="fragment">apt-get install libompl-dev ompl-demos</pre>
+ Note that this package does not include Python bindings.
+ </div>
+ <div role="tabpanel" class="tab-pane" id="ubunturos">
+ Debian packages for OMPL are also found in ROS distributions. Note that these packages do not include Python bindings. To install the ROS version of OMPL you need to add the ROS repository to your list of sources (you have probably have done this already if you are using ROS):
+ <pre class="fragment">sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list'
+wget http://packages.ros.org/ros.key -O - | sudo apt-key add -</pre>
+ and install OMPL:
+ <pre class="fragment">sudo apt-get update
+sudo apt-get install ros-`rosversion -d`-ompl</pre>
+ Please see <a href="http://moveit.ros.org">MoveIt!</a> for further information.
+ </div>
+ </div>
+ </div>
+
+ <!-- Fedora -->
+ <div role="tabpanel" class="tab-pane" id="fedora">
+ <h2>Fedora</h2>
+ Simply type:
+ <pre class="fragment">sudo yum install ompl</pre>
+ Note that this package does not include Python bindings.
+ </div>
+
+ <!-- Linux (generic) -->
+ <div role="tabpanel" class="tab-pane" id="linux">
+ <h2>Linux (generic)</h2>
+ <p>OMPL requires <a href="http://www.boost.org">Boost</a> (version 1.54 or higher) and <a href="http://www.cmake.org">CMake</a> (version 2.8.7 or higher). Some additional features are available if <a href="http://eigen.tuxfamily.org/index.php?title=Main_Page">Eigen 3</a> or <a href="http://www.ode.org">ODE</a> are installed.
+ To be able to generate python bindings you need to install the <a href="http://www.python.org">Python</a> library and header files and <a href="installPyPlusPlus.html">Py++</a>.
+ <p>Once the dependencies are installed, OMPL can then be compiled like so:</p>
+ <ul>
+ <li>Create a build directory and run cmake: <pre class="fragment">cd ompl
+mkdir -p build/Release
+cd build/Release
+cmake ../..</pre></li>
+ <li>Optionally, generate the Python bindings with <code>make -j 4 update_bindings</code>.</li>
+ <li>Optionally, run the test programs by typing <code>make test</code>.</li>
+ <li>Optionally, generate the documentation (i.e., a local copy of this web site) by typing <code>make doc</code> (requires <a href="http://www.doxygen.org">Doxygen</a> and <a href="http://www.graphviz.org">Graphviz</a> to be installed).</li>
+ </ul>
+ <p>The build system includes a <a href="buildOptions.html">number of options</a> that you can enable or disable.</p>
+ </div>
+
+ <!-- OS X -->
+ <div role="tabpanel" class="tab-pane" id="osx">
+ <h2>OS X</h2>
+ <ul class="nav nav-tabs" role="tablist">
+ <li role="presentation" class="active"><a href="#osxmacports" aria-controls="osxmacports" role="tab" data-toggle="tab">MacPorts</a></li>
+ <li role="presentation"><a href="#osxhomebrew" aria-controls="osxhomebrew" role="tab" data-toggle="tab">Homebrew</a></li>
+ </ul>
+ <div class="tab-content">
+ <div role="tabpanel" class="tab-pane active" id="osxmacports">
+ Install <a href="http://www.macports.org">MacPorts</a> and type:<pre class="fragment">sudo port sync \; install ompl</pre>
+ </div>
+ <div role="tabpanel" class="tab-pane" id="osxhomebrew">
+ Install <a href="http://brew.sh">Homebrew</a> and type:
+ <pre class="fragment">brew install ompl</pre>
+ Note that the <a href="http://braumeister.org/formula/ompl">Homebrew formula</a> does not include Python bindings.
+ </div>
+ </div>
+ </div>
+
+ <!-- Windows -->
+ <div role="tabpanel" class="tab-pane" id="windows">
+ <h2>MS Windows</h2>
+ Installation on Windows is possible, but not supported at this time. Our configuration files for compiling <a href="https://bitbucket.org/ompl/ompl/src/tip/.appveyor.yml">OMPL</a> using AppVeyor's Windows Continuous Integration servers might serve as a useful starting point.
+ </div>
+</div>
+\endhtmlonly
-## MacPorts {#install_macports}
-
-- Install [MacPorts].
-- If you do not need to modify or see the source code of OMPL, then the easiest way to install OMPL is with the MacPorts `port` command:
-
- sudo port sync \; install ompl
-
- This is it. You are done. Demo programs can be found in `/opt/local/share/ompl`.
-- If you downloaded the source distribution of OMPL, then you need to install the dependencies Boost, CMake, and optional dependencies. If you have MacPorts installed, type the following:
-
- sudo port sync
- sudo port install boost cmake ode py27-pyplusplus eigen3 graphviz doxygen
-
-- It is __very__ important that you use the same installed version of Python for all dependencies and OMPL. If you are using MacPorts, then you __must__ use the MacPorts version of python 2.7 (most likely installed in `/opt/local/bin`). To make this version the default python version, make sure `/opt/local/bin` appears before `/usr/bin` in your PATH. You can add a line like this to your `${HOME}/.bash_profile`:
-
- export PATH=/opt/local/bin:/opt/local/sbin:$PATH
-
- Next, execute the following command:
-
- sudo port select python python27
-
-- The build steps are the same as for Ubuntu Linux.
-
-
-## Homebrew {#install_homebrew}
-
-_Thanks to [Andrew Dobson](https://plus.google.com/104214233559576935970/about) for these instructions!_ __These instructions are somewhat experimental, however, and we haven't tested them ourselves.__ Email us if you have suggestions to improve these instructions.
-
-- Install [Homebrew].
-- Run `brew doctor` to make sure that everything is ready to go. If not, follow its instructions until it is ready.
-- Type the following commands:
-
- brew install boost cmake assimp ode eigen doxygen graphviz
-
-- The build steps are the same as for Ubuntu Linux.
-
-
-# Installation on Windows {#install_windows}
-
-\note It is possible to run OMPL natively on Windows, although it must be stressed that __extensive testing on Windows is not performed__ at this time, and running OMPL on Windows is considered _highly_ experimental. It is _much_ easier to install [VirtualBox], create an Ubuntu virtual machine, and follow the Ubuntu installation directions above.
-
-For best performance, the [MinGW] compiler is recommended. Visual Studio can also be used to build the core OMPL library, but currently it is not possible to generate the python bindings for OMPL with this compiler. However, if the bindings are generated with MinGW, the bindings can be compiled by Visual Studio with some minor tweaks to the code (not recommended, unless you are an experienced Windows developer).
-
-
-## Required Dependencies
-
-- [CMake], version 2.8.7 or higher,
-- [MinGW][] (recommended) or Visual Studio compiler, and
-- [Boost], version 1.48 or greater.
-
- It is recommended to make a complete Boost compilation from source. If using Visual Studio, this process can be automated using the [BoostPro](http://www.boostpro.com/download) installer. Once complete, set the environment variables `BOOST_ROOT` and `BOOST_LIBRARYDIR` to the locations where Boost and its libraries are installed. The default locations are `C:\\Boost` and `C:\\Boost\\lib`. Ensure that `BOOST_LIBRARYDIR` is also in the system PATH so that any necessary Boost dlls are l [...]
-
-
-## Optional Dependencies (for Python bindings)
-
-- A __32-bit__ version of [Python] 2.7. Ensure that this is installed __before building Boost__ so that Boost.Python is properly compiled.
-- Ensure that Python is added to the system `PATH`.
-- Py++: To generate the Python bindings, Py++ and its dependencies must be installed. A batch file has been included to automate this process (analogous to the Linux/Mac installation) that can be executed via cmake. Instructions can be found [here](installPyPlusPlus.html). Note that this process assumes the MinGW compiler, and installs gccxml to `C:\\gccxml`. You will need to be in a shell with administrator privileges to execute this batch file. Once installed, it is recommended that [...]
-
-
-## Build
-
-- Once the dependencies are installed, CMake can be used to generate MinGW makefiles or a Visual Studio solution
-by specifying a specific GENERATOR:
-
- cd ompl
- mkdir build
- cd build
- mkdir Release
- cd Release
- cmake -G "GENERATOR" ../.. [-DCMAKE_INSTALL_PREFIX=/path/to/install]
-
-The CMAKE_INSTALL_PREFIX variable is set to `C:\\Program Files (x86)\\ompl` by default.
-
-### MinGW
-- The CMake generator for MinGW is `"MinGW Makefiles"`
-- To generate the python bindings (optional), execute the update_bindings make command before compiling:
-
- mingw32-make update_bindings
-
- __Note:__ `update_bindings` is _never_ run automatically. If you change any of the OMPL header files, you need to regenerate the bindings for the changes to be reflected in the Python modules. See also the [more detailed documentation on generating python bindings](\ref updating_python_bindings).
-- Use `mingw32-make` to build OMPL.
-- If you wish to install OMPL, use the install command to copy the binaries, demo code, and other resources to the cmake install prefix. Note that you will need to be in a shell with administrator privileges to install to the default directory.
-
- mingw32-make install
-
-- Make sure to add the install path's \\lib subdirectory to the PATH so that the DLLs are found when code is loaded.
-
-
-### Visual Studio
-- The CMake generator for Visual Studio depends on the version of Visual Studio to generate a solution for. The generator for VS 2010 is `"Visual Studio 10"`, and the generator for VS 2008 is `"Visual Studio 9 2008"`. Consult the CMake documentation for other generators.
-- Open ompl.sln and build the solution. A static library will be created (ompl.lib) to link your code against, as well as several demo programs.
-- You can install OMPL by building the "INSTALL" project inside of the solution. Note that this will attempt to copy files to `C:\\Program Files (x86)\\ompl` (the default). The installation will fail unless Visual Studio is opened with administrator privileges, or a non-system install prefix is specified when cmake is run.
-
-[boost]: http://www.boost.org
-[cmake]: http://www.cmake.org
-[python]: http://www.python.org
-[ros]: http://www.ros.org
-[moveit]: http://moveit.ros.org
-[macports]: http://www.macports.org
-[homebrew]: http://mxcl.github.com/homebrew
-[mingw]: http://www.mingw.org
-[virtualbox]: http://www.virtualbox.org
diff --git a/doc/markdown/integration.md b/doc/markdown/integration.md
index 9f922e3..77b1e4a 100644
--- a/doc/markdown/integration.md
+++ b/doc/markdown/integration.md
@@ -6,8 +6,11 @@ OMPL provides a high level of abstraction to make it easier to integrate it into
<div class="btn-group">
<a class="btn btn-default" href="#integration_moveit">MoveIt!</a>
<a class="btn btn-default" href="#integration_openrave">OpenRAVE</a>
+ <a class="btn btn-default" href="#integration_vrep">V-REP</a>
<a class="btn btn-default" href="#integration_morse">MORSE</a>
<a class="btn btn-default" href="#integration_kautham">Kautham</a>
+ <a class="btn btn-default" href="#integration_verosim">VEROSIM</a>
+ <a class="btn btn-default" href="#integration_rl">Robotics Library</a>
<a class="btn btn-default" href="#integration_sims">SIMS</a>
<a class="btn btn-default" href="#integration_omplapp">OMPL.app</a>
</div>
@@ -40,6 +43,20 @@ OMPL provides a high level of abstraction to make it easier to integrate it into
</div>
+# V-REP {#integration_vrep}
+
+<div class="row">
+ <div class="col-lg-7 col-md-6 col-sm-5">
+ [The V-REP platform](http://coppeliarobotics.com) is a modular, generic and general purpose robot simulation framework that offers various tools related to robotics (4 physics engines, collision detection, minimum distance calculation, proximity sensor simulation, vision sensor simulation, full FK/IK kinematic solver, etc.), with various kinds of interfaces (ROS, remote API, plug-ins, add-ons) and language support: C/C++, Python, Java, Matlab, Octave, Lua. It is built on a distribute [...]
+ </div>
+ <div class="col-lg-5 col-md-6 col-sm-7">
+ <div class="embed-responsive embed-responsive-16by9">
+ \htmlonly<iframe src="http://www.youtube.com/embed/JAs2yciPjvM"></iframe>\endhtmlonly
+ </div>
+ </div>
+</div>
+
+
# MORSE {#integration_morse}
<div class="row">
@@ -66,6 +83,34 @@ OMPL provides a high level of abstraction to make it easier to integrate it into
</div>
+# VEROSIM {#integration_verosim}
+
+<div class="row">
+ <div class="col-lg-7 col-md-6 col-sm-5">
+ VEROSIM is a 3D simulation system which aims at implementing the core idea of “eRobotics,” i.e., to design, program, control and optimize complex automated systems in detailed 3D simulations of their prospective working environments (space, open landscapes, forests, cities, buildings, factories etc), before commissioning the real system. At VEROSIM co-developer MMI / RWTH Aachen University, Christian Schlette uses OMPL for conventional robot motion planning as well as examining the t [...]
+ </div>
+ <div class="col-lg-5 col-md-6 col-sm-7">
+ <div class="embed-responsive embed-responsive-16by9">
+ \htmlonly<iframe src="https://www.youtube.com/embed/CENLGo2WDAw"></iframe>\endhtmlonly
+ </div>
+ </div>
+</div>
+
+
+# Robotics Library {#integration_rl}
+
+<div class="row">
+ <div class="col-lg-7 col-md-6 col-sm-5">
+ [The Robotics Library](http://www.roboticslibrary.org) is a self-contained C++ library for robot kinematics, motion planning and control. It covers mathematics, kinematics and dynamics, hardware abstraction, motion planning, collision detection, and visualization. OMPL is not included by default, but we have created [a pull request on GitHub](https://github.com/roboticslibrary/rl/pull/2) to add OMPL support. Let us know if this works for you!
+ </div>
+ <div class="col-lg-5 col-md-6 col-sm-7">
+ <div class="embed-responsive embed-responsive-16by9">
+ \htmlonly<iframe src="https://www.youtube.com/embed/9JG3uY5M04A"></iframe>\endhtmlonly
+ </div>
+ </div>
+</div>
+
+
# SIMS {#integration_sims}
<div class="row">
diff --git a/doc/markdown/mainpage.md.in b/doc/markdown/mainpage.md.in
index 9d5e27b..7ca817f 100644
--- a/doc/markdown/mainpage.md.in
+++ b/doc/markdown/mainpage.md.in
@@ -106,9 +106,8 @@ OMPL is intended to be efficient, thread safe, easy to use, easily extensible an
\endif
- [Demos](group__demos.html) and [tutorials](tutorials.html).
- [Frequently Asked Questions.](FAQ.html)
-- Familiarize yourself with the [Boost structures](boost.html) used throughout OMPL.
- Learn how to integrate your own code with [OMPL's build system](buildSystem.html).
-- [Learn more about how OMPL is integrated within other systems](integration.html) (such as [MoveIt!](http://moveit.ros.org), [OpenRAVE](http://openrave.org), and [MORSE](https://www.openrobots.org/wiki/morse)).
+- [Learn more about how OMPL is integrated within other systems](integration.html) (such as [MoveIt!](http://moveit.ros.org), [OpenRAVE](http://openrave.org), [V-REP](http://coppeliarobotics.com), and [MORSE](https://www.openrobots.org/wiki/morse)).
- If interested in using Python, make sure to read [the documentation for the Python bindings](python.html).
\htmlonly</div><div class="col-md-4 col-sm-6">\endhtmlonly
diff --git a/doc/markdown/optimalPlanning.md b/doc/markdown/optimalPlanning.md
index e4b6ec5..89a3d3b 100644
--- a/doc/markdown/optimalPlanning.md
+++ b/doc/markdown/optimalPlanning.md
@@ -15,6 +15,8 @@ You can specify a path quality metric using the `ompl::base::OptimizationObjecti
- `ompl::geometric::InformedRRTstar`
- `ompl::geometric::BITstar`
- `ompl::geometric::FMT`
+- `ompl::geometric::BFMT`
+
You can find out more about asymptotic optimality in motion planning by checking out [this paper](http://sertac.scripts.mit.edu/web/wp-content/papercite-data/pdf/karaman.frazzoli-ijrr11.pdf) by Karaman and Frazzoli. The following planners also support `ompl::base::OptimizationObjective`, but do not provide theoretical guarantees on solution optimality:
diff --git a/doc/markdown/plannerarena.md b/doc/markdown/plannerarena.md
index ffe5341..c9fc962 100644
--- a/doc/markdown/plannerarena.md
+++ b/doc/markdown/plannerarena.md
@@ -6,11 +6,11 @@ The OMPL Planner Arena code allows you to easily create plots from a benchmark d
## Dependencies
- R 3.1.0 or higher
- \note The R packages on Ubuntu (and likely other Linux distributions) are too old. See the [CRAN Ubuntu page](https://cran.r-project.org/bin/linux/ubuntu/README) for details on how to get packages for the latest version of R.
+ \note The R packages on Ubuntu (and likely other Linux distributions) are too old. See the [CRAN Ubuntu page](https://cran.r-project.org/bin/linux/ubuntu/README.html) for details on how to get packages for the latest version of R.
- On Ubuntu you need to install libv8-dev.
- The following R packages: shiny, shinyjs, V8, ggplot2, Hmisc, RSQLite, and markdown. These packages can be installed like so:
- sudo R -e "install.packages(c('shiny', 'shinyjs', 'V8', 'ggplot2', 'Hmisc', 'RSQLite', 'markdown'), repos='http://cran.r-project.org')"
+ sudo R -e "install.packages(c('shiny', 'shinyjs', 'V8', 'ggplot2', 'Hmisc', 'dplyr', 'tidyr', 'RSQLite', 'markdown'), repos='http://cran.r-project.org')"
## Running
diff --git a/doc/markdown/planners.md b/doc/markdown/planners.md
index e2c4a82..a775812 100644
--- a/doc/markdown/planners.md
+++ b/doc/markdown/planners.md
@@ -36,14 +36,18 @@ Planners in this category only accounts for the geometric and kinematic constrai
An asymptotically optimal version of RRT: the algorithm converges on the optimal path as a function of time. This was the first provably asymptotically planner (together with PRM). Since its publication, several other algorithms have appeared that improve on RRT*'s convergence rate.
- [Lower Bound Tree RRT (LBTRRT)](\ref gLBTRRT) \[__experimental__\]<br>
LBTRRT is a asymptotically near-optimal version of RRT: it is guaranteed to converge to a solution that is within a constant factor of the optimal solution.
+ - [Sparse Stable RRT](\ref gSST)<br>
+ SST is an asymptotically near-optimal incremental version of RRT.
- [Transition-based RRT (T-RRT)](\ref gTRRT)<br>
T-RRT does not give any hard optimality guarantees, but tries to find short, low-cost paths.
+ - [Vector Field RRT](\gVFRRT)<br>
+ VF-RRT is a tree-based motion planner that tries to minimize the so-called upstream cost of a path. The upstream cost is defined by an integral over a user-defined vector field.
- [Parallel RRT (pRRT)](\ref gpRRT) \[__experimental__\]<br>
Many different parallelization schemes have been proposed for sampling-based planners, including RRT. In this implementation, several threads simultaneously add states to the same tree. Once a solution is found, all threads terminate.
- [Lazy RRT (LazyRRT)](\ref gLazyRRT)<br>
This planner performs lazy state validity checking (similar to LazyPRM). It is not experimental, but in our experience it does not seem to outperform other planners by a significant margin on any class of problems.
- [Expansive Space Trees (EST)](\ref gEST)<br>
- This planner was published around the same time as RRT. In our experience it is not as sensitive to having a good distance measure, which can be difficult to define for complex high-dimensional state spaces. On the other hand, in our implementation EST requires a low-dimensional projection to keep track of how the state space has been explored. Most of the time OMPL can automatically determine a reasonable projection. We have implemented a few planners that not necessarily simple var [...]
+ This planner was published around the same time as RRT. In our experience it is not as sensitive to having a good distance measure, which can be difficult to define for complex high-dimensional state spaces. There are actually three versions of EST: the [original version](\ref gEST) that is close to the first publication, [a bidirectional version](\ref gBiEST), and a [projection-based version](\ref gProjEST). The low-dimensional projection is used to keep track of how the state space [...]
- [Single-query Bi-directional Lazy collision checking planner (SBL)](\ref gSBL)<br>
This planner is essentially a bidirectional version of EST with lazy state validity checking.
- [Parallel Single-query Bi-directional Lazy collision checking planner (pSBL)](\ref gpSBL) \[__experimental__\]<br>
@@ -61,6 +65,8 @@ Planners in this category only accounts for the geometric and kinematic constrai
PDST is a planner that has entirely removed the dependency on a distance measure, which is useful in cases where a good distance metric is hard to define. PDST maintains a binary space partitioning such that motions are completely contained within one cell of the partition. The density of motions per cell is used to guide expansion of the tree.
- [Fast Marching Tree algorithm (FMT∗)](\ref gFMT) \[__experimental__\]<br>
The FMT∗ algorithm performs a “lazy” dynamic programming recursion on a set of probabilistically-drawn samples to grow a tree of paths, which moves outward in cost-to-come space. Unlike all other planners, the numbers of valid samples needs to be chosen beforehand.
+ - [Bidirectional Fast Marching Tree algorithm (BFMT∗)](\ref gBFMT) \[__experimental__\]<br>
+ Executes two FMT* trees, one from the start and another one from the goal resulting in a faster planner as it explores less space.
- **Optimizing planners**<br>
In recent years several sampling-based planning algorithms have been proposed that still provide some optimality guarantees. Typically, an optimal solution is assumed to be shortest path. In OMPL we have a more general framework for expressing the cost of states and paths that allows you to, e.g., maximize the minimum clearance along a path, minimize the mechanical work, or some arbitrary user-defined optimization criterion. See \ref optimalPlanning for more information. Some of the pl [...]
- [PRM*](\ref gPRMstar)<br> An asymptotically optimal version of PRM; _uses the general cost framework._
@@ -69,6 +75,7 @@ Planners in this category only accounts for the geometric and kinematic constrai
- [Informed RRT*](\ref gInformedRRTstar)<br> A variant of RRT* that uses heuristics to bound the search for optimal solutions. _It uses the general cost framework._
- [Batch Informed Trees (BIT*)](\ref gBITstar)<br> An anytime asymptotically optimal algorithm that uses heuristics to order and bound the search for optimal solutions. _It uses the general cost framework._
- [Lower Bound Tree RRT (LBTRRT)](\ref gLBTRRT) \[__experimental__\]<br> An asymptotically near-optimal version of RRT.
+ - [Sparse Stable RRT](\ref gSST)<br> SST is an asymptotically near-optimal incremental version of RRT.
- [Transition-based RRT (T-RRT)](\ref gTRRT)<br> T-RRT does not give any hard optimality guarantees, but tries to find short, low-cost paths. _It uses the general cost framework._
- [SPARS](\ref gSPARS)<br> An asymptotically near-optimal roadmap-based planner.
- [SPARS2](\ref gSPARStwo)<br> An asymptotically near-optimal roadmap-based planner.
@@ -93,6 +100,7 @@ If you use the ompl::geometric::SimpleSetup class (highly recommended) to define
<div class="plannerlist">
If the system under consideration is subject to differential constraints, then a control-based planner is used. These planners rely on [state propagation](\ref ompl::control::StatePropagator) rather than simple interpolation to generate motions. These planners do not require [a steering function](\ref ompl::control::StatePropagator::steer), but all of them (except KPIECE) will use it if the user implements it. The first two planners below are kinodynamic adaptations of the corresponding [...]
- [Rapidly-exploring Random Trees (RRT)](\ref cRRT)
+- [Sparse Stable RRT](\ref cSST)<br> SST is an asymptotically near-optimal incremental version of RRT.
- [Expansive Space Trees (EST)](\ref cEST)
- [Kinodynamic Planning by Interior-Exterior Cell Exploration (KPIECE)](\ref cKPIECE1)<br>
As the name suggest, the control-based version of KPIECE came first, and the geometric versions were derived from it.
diff --git a/doc/markdown/python.md b/doc/markdown/python.md
index 74a987c..241e1ee 100644
--- a/doc/markdown/python.md
+++ b/doc/markdown/python.md
@@ -13,7 +13,7 @@ dir(base), dir(control), dir(geometric), dir(tools), dir(util)
- \ref cpp_py_diffs
- \ref py_api_diffs
- \ref py_example
-- \ref pyfunction_to_boostfunction
+- \ref pyfunction_to_stdfunction
- \ref updating_python_bindings
@@ -29,7 +29,7 @@ Although almost all C++ functionality is exposed to Python, there are some cavea
# Important differences between C++ and Python {#cpp_py_diffs}
- There are no templates in Python, so templated C++ classes and functions need to be fully instantiated to allow them to be exposed to python.
-- There are no C-style pointers in python, and no “new” or “delete” operators. This could be a problem, but can be dealt with mostly by using Boost [shared_ptr](http://www.boost.org/doc/libs/release/libs/smart_ptr/shared_ptr.htm)'s. If a C++ function takes pointer input/output parameters, _usually_ a reference to the object is passed in the python bindings. In other words, you should be able to get and set the current value of an object through its methods. In some cases odd side effects [...]
+- There are no C-style pointers in python, and no “new” or “delete” operators. This could be a problem, but can be dealt with mostly by using [shared_ptr](http://en.cppreference.com/w/cpp/memory/shared_ptr)'s. If a C++ function takes pointer input/output parameters, _usually_ a reference to the object is passed in the python bindings. In other words, you should be able to get and set the current value of an object through its methods. In some cases odd side effects may occur if you pass [...]
# Differences between the C++ and Python API's {#py_api_diffs}
@@ -113,9 +113,9 @@ if __name__ == "__main__":
~~~
-# Creating boost::function objects from Python functions {#pyfunction_to_boostfunction}
+# Creating boost::function objects from Python functions {#pyfunction_to_stdfunction}
-OMPL relies heavily on boost::function objects for callback functions. To specify a Python function as a callback function, that function needs to be cast to the right function type. The simple example above already showed how to do this for a state validity checker function:
+OMPL relies heavily on std::function objects for callback functions. To specify a Python function as a callback function, that function needs to be cast to the right function type. The simple example above already showed how to do this for a state validity checker function:
~~~{.py}
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
diff --git a/doc/markdown/register.md b/doc/markdown/register.md
index 853e85d..c1f4c32 100644
--- a/doc/markdown/register.md
+++ b/doc/markdown/register.md
@@ -21,6 +21,11 @@ Registration for OMPL is completely voluntary. We will not spam you with emails.
</div>
<input type="hidden" name="_next" value="thank-you.html" />
<input type="hidden" name="_subject" value="=== OMPL registration ===" />
+ <input type="hidden" name="IP" id="IP">
+ <script type="application/javascript">
+ window.onload = function () {
+ $.getJSON("https://api.ipify.org?format=jsonp&callback=?",function(json){$("#IP").val(json.ip);});};
+ </script>
<input type="text" name="_gotcha" style="display:none" />
<input type="submit" value="Send" name='submit' class="btn btn-primary" />
</form>
diff --git a/doc/markdown/releaseNotes.md b/doc/markdown/releaseNotes.md
index 2a0bbee..39205b9 100644
--- a/doc/markdown/releaseNotes.md
+++ b/doc/markdown/releaseNotes.md
@@ -1,6 +1,24 @@
# Release Notes
+# OMPL 1.2.1 (July 1, 2016)
+
+- New simplified installation instructions. There is now also a [installation script](http://ompl.kavrakilab.org/install-ompl-ubuntu.sh) that will download and install OMPL and all its dependencies on Ubuntu 14.04, 15.10, and 16.04.
+- Fixed python bindings for gcc5. Python bindings still take a [very long time](https://github.com/gccxml/pygccxml/issues/56) to generate.
+- Misc. small bug fixes.
+
+
+# OMPL 1.2.0 (June 20, 2016)
+
+- C++11 is now **required**. A lot of Boost usage in older versions of OMPL has been replaced with C++11 STL equivalents.
+- Added several new planners and improved existing planners:
+ - Stable Sparse RRT, both a [geometric](\ref gSST) and a [control-based](\ref cSST) version. SST is an asymptotically near-optimal incremental version of RRT.
+ - [Vector Field RRT](\ref gVFRRT), a tree-based motion planner that tries to minimize the so-called upstream cost of a path. The upstream cost is defined by an integral over a user-defined vector field.
+ - [A bidirectional version of FMT](\ref gBFMT), small fixes in FMT.
+ - A new version of [Expansive Space Trees](\ref gEST) that does not rely on projections to estimate sampling density. Instead, it estimates density directly in the configuration space. This is closer to the original paper and actually works really well! There is now also a [bidirectional version of EST](\ref gBiEST). The previous implementation of EST has been renamed to [ProjEST](\ref gProjEST).
+- Minimum Boost version is now 1.54.
+
+
# OMPL 1.1.0 (October 28, 2015)
- Added several new and updated planners:
diff --git a/doc/markdown/thirdparty.md b/doc/markdown/thirdparty.md
index 278cb5d..b1d41d2 100644
--- a/doc/markdown/thirdparty.md
+++ b/doc/markdown/thirdparty.md
@@ -26,7 +26,14 @@ DOI: [10.1109/TRO.2013.2240176](http://dx.doi.org/10.1109/TRO.2013.2240176)<br>
### Authors: Ashley Clark and Wolfgang Pointner (Stanford University)
[FMT∗](\ref gFMT) is a new asymptotically optimal algorithm contributed by Marco Pavone's Autonomous Systems Laboratory. The FMT∗ algorithm performs a “lazy” dynamic programming recursion on a set of probabilistically-drawn samples to grow a tree of paths, which moves outward in cost-to-come space. The algorithm is described in:
-- L. Janson, A. Clark, and M. Pavone, Fast Marching Trees: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions, International Journal on Robotics Research, 2014. Submitted. http://arxiv.org/pdf/1306.3532v3.pdf
+- L. Janson, E. Schmerling, A. Clark, M. Pavone. Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. The International Journal of Robotics Research, 34(7):883-921, 2015. DOI: [10.1177/0278364915577958](http://dx.doi.org/10.1177/0278364915577958) http://arxiv.org/pdf/1306.3532.pdf
+
+# Bidirectional Fast Marching Tree algorithm (BFMT∗)
+
+### Authors: Joseph Starek (Stanford University) and Javier V Gomez (Universidad Carlos III de Madrid)
+
+[BFMT∗](\ref gBFMT) is a new asymptotically optimal algorithm contributed by Marco Pavone's Autonomous Systems Laboratory. The BFMT∗ algorithm expands two Fast Marching Trees, one from the initial state and the other from the goal, resulting in faster convergence rates are less space is explored by the trees. The algorithm is described in:
+- J. A. Starek, J. V. Gomez, E. Schmerling, L. Janson, L. Moreno, and M. Pavone, An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional Motion Planning, in IEEE/RSJ International Conference on Intelligent Robots Systems, 2015. http://arxiv.org/pdf/1507.07602.pdf
# Generalized Optimal Path Planning Framework
diff --git a/doc/mkdocs.sh b/doc/mkdocs.sh
index d79106e..ba02f3d 100755
--- a/doc/mkdocs.sh
+++ b/doc/mkdocs.sh
@@ -1,7 +1,7 @@
#!/bin/sh
rm -rf html
doxygen Doxyfile
-cp -r css fonts images js ieee-ram-2012-ompl.pdf html
+cp -r css fonts images js ieee-ram-2012-ompl.pdf ../install-ompl-ubuntu.sh html
cd html
for f in md_doc_markdown_*; do mv $f `echo $f | cut -c17-1000`; done
diff --git a/install-ompl-ubuntu.sh b/install-ompl-ubuntu.sh
new file mode 100755
index 0000000..666a07f
--- /dev/null
+++ b/install-ompl-ubuntu.sh
@@ -0,0 +1,111 @@
+#!/bin/bash
+
+if [ `lsb_release -rs` == "14.04" ]; then
+ export TRUSTY=1
+fi
+
+install_common_dependencies()
+{
+ # install most dependencies via apt-get
+ sudo apt-get -y update
+ sudo apt-get -y upgrade
+ # On Ubuntu 14.04 we need to add a PPA to get a recent compiler (g++-4.8 is too old).
+ # We also need to specify a boost version, since the default Boost is too old.
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install cmake libboost-all-dev libeigen3-dev libode-dev
+ else
+ # needed for the add-apt-repository command, which was not part of early Trusty releases
+ sudo apt-get -y install software-properties-common
+ sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test
+ sudo apt-get -y update
+ sudo apt-get -y install g++-5 cmake libboost1.55-all-dev libeigen3-dev libode-dev
+ export CXX=g++-5
+ fi
+ export MAKEFLAGS="-j `nproc`"
+}
+
+install_python_binding_dependencies()
+{
+ sudo apt-get -y install python-dev python-pip
+ # install additional python dependencies via pip
+ sudo -H pip -v install pygccxml https://bitbucket.org/ompl/pyplusplus/get/1.6.tar.gz
+ # install castxml
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install castxml
+ else
+ wget -O - https://midas3.kitware.com/midas/download/item/318227/castxml-linux.tar.gz | tar zxf - -C $HOME
+ export PATH=$HOME/castxml/bin:$PATH
+ fi
+}
+
+install_app_dependencies()
+{
+ # We prefer PyQt5, but PyQt4 also still works.
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install python-pyqt5.qtopengl
+ else
+ sudo apt-get -y install python-qt4-dev python-qt4-gl
+ fi
+ sudo apt-get -y install freeglut3-dev libassimp-dev python-opengl python-flask python-celery
+ # install additional python dependencies via pip
+ sudo -H pip -v install PyOpenGL-accelerate
+ # install libccd
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install libccd-dev
+ else
+ wget -O - https://github.com/danfis/libccd/archive/v2.0.tar.gz | tar zxf -
+ cd libccd-2.0; cmake .; sudo -E make install; cd ..
+ fi
+ # install fcl
+ wget -O - https://github.com/flexible-collision-library/fcl/archive/0.4.0.tar.gz | tar zxf -
+ cd fcl-0.4.0; cmake .; sudo -E make install; cd ..
+}
+
+install_ompl()
+{
+ if [ -z $2 ]; then
+ OMPL="ompl"
+ else
+ OMPL="omplapp"
+ fi
+ wget -O - https://bitbucket.org/ompl/ompl/downloads/$OMPL-1.2.1-Source.tar.gz | tar zxf -
+ cd $OMPL-1.2.1-Source
+ mkdir -p build/Release
+ cd build/Release
+ cmake ../..
+ if [ ! -z $1 ]; then
+ make update_bindings
+ fi
+ make
+ sudo make install
+}
+
+for i in "$@"
+do
+case $i in
+ -a|--app)
+ APP=1
+ PYTHON=1
+ shift
+ ;;
+ -p|--python)
+ PYTHON=1
+ shift
+ ;;
+ *)
+ # unknown option -> show help
+ echo "Usage: `basename $0` [-p] [-a]"
+ echo " -p: enable Python bindings"
+ echo " -a: enable OMPL.app (implies '-p')"
+ ;;
+esac
+done
+
+install_common_dependencies
+if [ ! -z $PYTHON ]; then
+ install_python_binding_dependencies
+fi
+if [ ! -z $APP ]; then
+ install_app_dependencies
+fi
+install_ompl $PYTHON $APP
diff --git a/install-ompl-ubuntu.sh.in b/install-ompl-ubuntu.sh.in
new file mode 100755
index 0000000..0367a4b
--- /dev/null
+++ b/install-ompl-ubuntu.sh.in
@@ -0,0 +1,111 @@
+#!/bin/bash
+
+if [ `lsb_release -rs` == "14.04" ]; then
+ export TRUSTY=1
+fi
+
+install_common_dependencies()
+{
+ # install most dependencies via apt-get
+ sudo apt-get -y update
+ sudo apt-get -y upgrade
+ # On Ubuntu 14.04 we need to add a PPA to get a recent compiler (g++-4.8 is too old).
+ # We also need to specify a boost version, since the default Boost is too old.
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install cmake libboost-all-dev libeigen3-dev libode-dev
+ else
+ # needed for the add-apt-repository command, which was not part of early Trusty releases
+ sudo apt-get -y install software-properties-common
+ sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test
+ sudo apt-get -y update
+ sudo apt-get -y install g++-5 cmake libboost1.55-all-dev libeigen3-dev libode-dev
+ export CXX=g++-5
+ fi
+ export MAKEFLAGS="-j `nproc`"
+}
+
+install_python_binding_dependencies()
+{
+ sudo apt-get -y install python-dev python-pip
+ # install additional python dependencies via pip
+ sudo -H pip -v install pygccxml https://bitbucket.org/ompl/pyplusplus/get/1.6.tar.gz
+ # install castxml
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install castxml
+ else
+ wget -O - https://midas3.kitware.com/midas/download/item/318227/castxml-linux.tar.gz | tar zxf - -C $HOME
+ export PATH=$HOME/castxml/bin:$PATH
+ fi
+}
+
+install_app_dependencies()
+{
+ # We prefer PyQt5, but PyQt4 also still works.
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install python-pyqt5.qtopengl
+ else
+ sudo apt-get -y install python-qt4-dev python-qt4-gl
+ fi
+ sudo apt-get -y install freeglut3-dev libassimp-dev python-opengl python-flask python-celery
+ # install additional python dependencies via pip
+ sudo -H pip -v install PyOpenGL-accelerate
+ # install libccd
+ if [ -z $TRUSTY ]; then
+ sudo apt-get -y install libccd-dev
+ else
+ wget -O - https://github.com/danfis/libccd/archive/v2.0.tar.gz | tar zxf -
+ cd libccd-2.0; cmake .; sudo -E make install; cd ..
+ fi
+ # install fcl
+ wget -O - https://github.com/flexible-collision-library/fcl/archive/0.4.0.tar.gz | tar zxf -
+ cd fcl-0.4.0; cmake .; sudo -E make install; cd ..
+}
+
+install_ompl()
+{
+ if [ -z $2 ]; then
+ OMPL="ompl"
+ else
+ OMPL="omplapp"
+ fi
+ wget -O - https://bitbucket.org/ompl/ompl/downloads/$OMPL-@OMPL_VERSION@-Source.tar.gz | tar zxf -
+ cd $OMPL- at OMPL_VERSION@-Source
+ mkdir -p build/Release
+ cd build/Release
+ cmake ../..
+ if [ ! -z $1 ]; then
+ make update_bindings
+ fi
+ make
+ sudo make install
+}
+
+for i in "$@"
+do
+case $i in
+ -a|--app)
+ APP=1
+ PYTHON=1
+ shift
+ ;;
+ -p|--python)
+ PYTHON=1
+ shift
+ ;;
+ *)
+ # unknown option -> show help
+ echo "Usage: `basename $0` [-p] [-a]"
+ echo " -p: enable Python bindings"
+ echo " -a: enable OMPL.app (implies '-p')"
+ ;;
+esac
+done
+
+install_common_dependencies
+if [ ! -z $PYTHON ]; then
+ install_python_binding_dependencies
+fi
+if [ ! -z $APP ]; then
+ install_app_dependencies
+fi
+install_ompl $PYTHON $APP
diff --git a/py-bindings/CMakeLists.txt b/py-bindings/CMakeLists.txt
index d4f4ef1..1e3aa36 100644
--- a/py-bindings/CMakeLists.txt
+++ b/py-bindings/CMakeLists.txt
@@ -23,18 +23,18 @@ if (OMPL_BUILD_PYBINDINGS)
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/bindings_generator.py.in"
"${CMAKE_CURRENT_SOURCE_DIR}/ompl/bindings_generator.py" @ONLY)
- create_module_generation_targets(util "${CMAKE_CURRENT_SOURCE_DIR}")
+ create_module_generation_targets(util)
if(OMPL_HAVE_EIGEN3)
- create_module_generation_targets(base "${CMAKE_CURRENT_SOURCE_DIR}")
- create_module_generation_targets(geometric "${CMAKE_CURRENT_SOURCE_DIR}")
+ create_module_generation_targets(base)
+ create_module_generation_targets(geometric)
else()
- create_module_generation_targets(base "${CMAKE_CURRENT_SOURCE_DIR}" PathLengthDirectInfSampler)
- create_module_generation_targets(geometric "${CMAKE_CURRENT_SOURCE_DIR}" bitstar)
+ create_module_generation_targets(base PathLengthDirectInfSampler)
+ create_module_generation_targets(geometric bitstar)
endif()
- create_module_generation_targets(control "${CMAKE_CURRENT_SOURCE_DIR}")
- create_module_generation_targets(tools "${CMAKE_CURRENT_SOURCE_DIR}")
+ create_module_generation_targets(control)
+ create_module_generation_targets(tools)
if(OMPL_EXTENSION_MORSE)
- create_module_generation_targets(morse "${CMAKE_CURRENT_SOURCE_DIR}")
+ create_module_generation_targets(morse)
endif()
add_dependencies(update_base_bindings update_util_bindings)
@@ -48,9 +48,9 @@ if (OMPL_BUILD_PYBINDINGS)
endif()
endif(PY_OMPL_GENERATE)
- if(PY_OMPL_COMPILE AND EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/bindings")
+ if(PY_OMPL_COMPILE AND EXISTS "${CMAKE_CURRENT_BINARY_DIR}/bindings")
foreach(module ${OMPL_MODULES})
- create_module_target(${module} "${CMAKE_CURRENT_SOURCE_DIR}")
+ create_module_target(${module})
endforeach(module)
install(DIRECTORY ompl DESTINATION "${OMPL_PYTHON_INSTALL_DIR}"
@@ -89,6 +89,6 @@ if (OMPL_BUILD_PYBINDINGS)
endif(OMPL_BUILD_PYBINDINGS)
add_custom_target(clean_bindings
- "${CMAKE_COMMAND}" -E remove_directory "${CMAKE_CURRENT_SOURCE_DIR}/bindings"
+ "${CMAKE_COMMAND}" -E remove_directory "${CMAKE_CURRENT_BINARY_DIR}/bindings"
COMMAND "${CMAKE_COMMAND}" -E remove -f pyplusplus_{base,control,geometric,tools,util}.{cache,log}
WORKING_DIRECTORY "${PROJECT_BINARY_DIR}")
diff --git a/py-bindings/bindings_generator.py.in b/py-bindings/bindings_generator.py.in
index 74e0861..d068e6e 100644
--- a/py-bindings/bindings_generator.py.in
+++ b/py-bindings/bindings_generator.py.in
@@ -43,7 +43,8 @@ from os.path import exists, join, isdir
from os import getenv
import subprocess
from sys import platform
-from pygccxml import declarations
+from pygccxml import declarations, parser
+from pygccxml.utils import xml_generator
from pyplusplus import module_builder, messages
from pyplusplus.module_builder import call_policies
from pyplusplus.decl_wrappers import print_declarations
@@ -98,6 +99,10 @@ void __setitem(%s* obj, unsigned int i, %s val)
}
""")
+# impl_conv_code = """
+# boost::python::implicitly_convertible< std::shared_ptr< %(from)s >, std::shared_ptr< %(to)s > >();
+# """
+
def returns_reference(decl):
"""Return True iff the declaration returns a pointer or reference."""
c = decl.return_type.decl_string[-1]
@@ -111,40 +116,11 @@ class code_generator_t(object):
@name name of the python module
@dep name of another module this module depends on"""
module_builder.set_logger_level( logging.INFO )
- candidate_include_paths = [ "@OMPL_INCLUDE_DIR@", "@OMPLAPP_INCLUDE_DIR@",
- "@PYTHON_INCLUDE_DIRS@", "@Boost_INCLUDE_DIR@", "@ASSIMP_INCLUDE_DIRS@", "@ODEINT_INCLUDE_DIR@", "@Eigen_INCLUDE_DIRS@"]
-
- # Adding standard windows headers
- if platform == 'win32':
- compiler = getenv('GCCXML_COMPILER')
- # MinGW
- if compiler != None and (compiler.lower().endswith('g++') or compiler.lower().endswith('c++')):
- version = subprocess.Popen([compiler, '-dumpversion'],
- stdout=subprocess.PIPE).communicate()[0].strip()
- # Find whole path to MinGW
- compiler_path = ""
- for path in getenv('PATH').split(';'):
- if exists(join(path, compiler + '.exe')):
- compiler_path = path
- break
-
- if compiler_path is not "":
- # Adding in necessary include paths
- candidate_include_paths.append (join(compiler_path, "..", "include"))
- candidate_include_paths.append (join(compiler_path, "..", "lib", "gcc", "mingw32", version, "include"))
- candidate_include_paths.append (join(compiler_path, "..", "lib", "gcc", "mingw32", version, "include", "c++"))
- candidate_include_paths.append (join(compiler_path, "..", "lib", "gcc", "mingw32", version, "include", "c++", "mingw32"))
-
- include_paths = []
- for path in candidate_include_paths:
- if len(path)>0 and exists(path):
- include_paths.append(path)
+ xml_generator_config = parser.load_xml_generator_configuration("@CASTXMLCONFIGPATH@")
self.mb = module_builder.module_builder_t(
files = [ 'bindings/' + name + '.h' ],
cache = '@CMAKE_BINARY_DIR@/pyplusplus_'+name+'.cache',
- gccxml_path = "@GCCXML@",
- include_paths = include_paths,
- cflags="@PYOMPL_EXTRA_CFLAGS@",
+ xml_generator_config = xml_generator_config,
indexing_suite_version = indexing_suite_version )
self.replacement = {} if replacement_dict==None else replacement_dict
self.mb.classes().always_expose_using_scope = True
@@ -178,7 +154,10 @@ class code_generator_t(object):
def filter_declarations(self):
"""Remove some classes and functions from the std namespace"""
- self.std_ns.class_('ios_base').exclude()
+ try:
+ self.std_ns.class_('ios_base').exclude()
+ except:
+ pass
self.std_ns.free_functions().exclude()
self.std_ns.operators().exclude()
@@ -213,6 +192,26 @@ class code_generator_t(object):
cls.add_registration_code(reg)
cls.add_declaration_code(wrapper % (cls.decl_string, rettype))
- def add_boost_function(self, FT, func_name, func_doc):
+ def add_function_wrapper(self, FT, func_name, func_doc):
self.mb.add_declaration_code('PYDECLARE_FUNCTION(%s,%s)' % (FT, func_name))
self.mb.add_registration_code('PYREGISTER_FUNCTION(%s,%s,"%s")' % (FT, func_name, func_doc))
+
+ # Boost.Python has special code to handle boost::shared_ptr conversions,
+ # but support for std::shared_ptr is still missing (see
+ # https://github.com/boostorg/python/issues/29). This function attempts
+ # to (partially) fix this for specific classes.
+
+ # We currently use a different workaround: copy the special bits of Boost
+ # code that handle boost::shared_ptr's and modify them to use
+ # std::shared_ptr's instead. Might not be portable across a wide range of
+ # Boost.Python versions....
+
+ # def add_implicit_conversions(self, cls):
+ # cls.held_type = 'std::shared_ptr< %s >' % cls.wrapper_alias
+ # cls.add_registration_code(impl_conv_code % {'from': cls.wrapper_alias, 'to': cls.decl_string}, False)
+ # for base in cls.recursive_bases:
+ # if base.access_type == 'public':
+ # # from class to its base
+ # cls.add_registration_code(impl_conv_code % {'from': cls.decl_string, 'to': base.related_class.decl_string}, False)
+ # # from wrapper to clas base class
+ # cls.add_registration_code(impl_conv_code % {'from': cls.wrapper_alias, 'to': base.related_class.decl_string}, False)
diff --git a/py-bindings/generate_bindings.py b/py-bindings/generate_bindings.py
index 5232fd0..6536a17 100755
--- a/py-bindings/generate_bindings.py
+++ b/py-bindings/generate_bindings.py
@@ -36,12 +36,12 @@
# Author: Mark Moll
+from os.path import join, dirname
from sys import argv, setrecursionlimit
from pygccxml import declarations
from pyplusplus.module_builder import call_policies
from ompl.bindings_generator import code_generator_t, default_replacement
-
class ompl_base_generator_t(code_generator_t):
"""Class for generating the ompl.base python module."""
@@ -145,16 +145,17 @@ class ompl_base_generator_t(code_generator_t):
self.ompl_ns.class_('Path').include()
code_generator_t.filter_declarations(self)
# rename STL vectors of certain types
- self.std_ns.class_('map< std::string, boost::shared_ptr< ompl::base::ProjectionEvaluator > >').rename('mapStringToProjectionEvaluator')
- self.std_ns.class_('vector< ompl::base::State* >').rename('vectorState')
+ self.std_ns.class_('map< std::string, std::shared_ptr< ompl::base::ProjectionEvaluator > >').rename('mapStringToProjectionEvaluator')
+ self.std_ns.class_('vector< ompl::base::State * >').rename('vectorState')
try:
self.std_ns.class_('vector< ompl::base::State const* >').rename('vectorConstState')
except: pass
- self.std_ns.class_('vector< boost::shared_ptr<ompl::base::StateSpace> >').rename('vectorStateSpacePtr')
+ self.std_ns.class_('vector< std::shared_ptr<ompl::base::StateSpace> >').rename('vectorStateSpacePtr')
#self.std_ns.class_('vector< <ompl::base::PlannerSolution> >').rename('vectorPlannerSolution')
- self.std_ns.class_('map< std::string, boost::shared_ptr<ompl::base::GenericParam> >').rename('mapStringToGenericParam')
+ self.std_ns.class_('map< std::string, std::shared_ptr<ompl::base::GenericParam> >').rename('mapStringToGenericParam')
self.std_ns.class_('map< std::string, ompl::base::StateSpace::SubstateLocation >').rename('mapStringToSubstateLocation')
self.std_ns.class_('vector<ompl::base::PlannerSolution>').rename('vectorPlannerSolution')
+ self.ompl_ns.member_functions('maybeWrapBool').exclude()
# rename some templated types
self.ompl_ns.class_('SpecificParam< bool >').rename('SpecificParamBool')
self.ompl_ns.class_('SpecificParam< char >').rename('SpecificParamChar')
@@ -162,7 +163,7 @@ class ompl_base_generator_t(code_generator_t):
self.ompl_ns.class_('SpecificParam< unsigned int >').rename('SpecificParamUint')
self.ompl_ns.class_('SpecificParam< float >').rename('SpecificParamFloat')
self.ompl_ns.class_('SpecificParam< double >').rename('SpecificParamDouble')
- self.ompl_ns.class_('SpecificParam< std::string >').rename('SpecificParamString')
+ self.ompl_ns.class_('SpecificParam< std::basic_string<char> >').rename('SpecificParamString')
for cls in self.ompl_ns.classes(lambda decl: decl.name.startswith('SpecificParam')):
cls.constructors().exclude()
# don't export variables that need a wrapper
@@ -205,6 +206,7 @@ class ompl_base_generator_t(code_generator_t):
'def(bp::init<ompl::base::ScopedState<ompl::base::%sStateSpace> const &>(( bp::arg("other") )))' % stype)
# add array access to double components of state
self.add_array_access(state,'double')
+
# I don't know how to export a C-style array of an enum type
for stype in ['Dubins', 'ReedsShepp']:
self.ompl_ns.enumeration(stype + 'PathSegmentType').exclude()
@@ -241,10 +243,15 @@ class ompl_base_generator_t(code_generator_t):
cls.add_registration_code(
'def("__bool__", &ompl::base::PlannerStatus::operator bool)')
+ # Using nullptr as a default value in method arguments causes
+ # problems with Boost.Python.
+ # See https://github.com/boostorg/python/issues/60
+ self.ompl_ns.class_('ProblemDefinition').add_declaration_code('#define nullptr NULL\n')
+
# Exclude PlannerData::getEdges function that returns a map of PlannerDataEdge* for now
#self.ompl_ns.class_('PlannerData').member_functions('getEdges').exclude()
#self.std_ns.class_('map< unsigned int, ompl::base::PlannerDataEdge const*>').include()
- mapUintToPlannerDataEdge_cls = self.std_ns.class_('map< unsigned int, ompl::base::PlannerDataEdge const*>')
+ mapUintToPlannerDataEdge_cls = self.std_ns.class_('map< unsigned int, const ompl::base::PlannerDataEdge *>')
mapUintToPlannerDataEdge_cls.rename('mapUintToPlannerDataEdge')
mapUintToPlannerDataEdge_cls.indexing_suite.call_policies = \
call_policies.return_value_policy(call_policies.reference_existing_object)
@@ -253,6 +260,9 @@ class ompl_base_generator_t(code_generator_t):
# Make PlannerData printable
self.replace_member_function(self.ompl_ns.class_('PlannerData').member_function('printGraphviz'))
self.replace_member_function(self.ompl_ns.class_('PlannerData').member_function('printGraphML'))
+ # serialize passes archive by reference which causes problems
+ self.ompl_ns.class_('PlannerDataVertex').member_functions('serialize').exclude()
+ self.ompl_ns.class_('PlannerDataEdge').member_functions('serialize').exclude()
# add array indexing to the RealVectorState
self.add_array_access(self.ompl_ns.class_('RealVectorStateSpace').class_('StateType'))
@@ -283,26 +293,26 @@ class ompl_base_generator_t(code_generator_t):
self.replace_member_function(self.ompl_ns.class_('StateSpace').member_function('Diagram'))
# make state space list printable
self.replace_member_function(self.ompl_ns.class_('StateSpace').member_function('List'))
- # add wrappers for boost::function types
- self.add_boost_function('bool(const ompl::base::GoalLazySamples*, ompl::base::State*)',
+ # add wrappers for std::function types
+ self.add_function_wrapper('bool(const ompl::base::GoalLazySamples*, ompl::base::State*)',
'GoalSamplingFn', 'Goal sampling function')
- self.add_boost_function('void(const ompl::base::State*)',
+ self.add_function_wrapper('void(const ompl::base::State*)',
'NewStateCallbackFn', 'New state callback function')
- self.add_boost_function('ompl::base::PlannerPtr(const ompl::base::SpaceInformationPtr&)',
+ self.add_function_wrapper('ompl::base::PlannerPtr(const ompl::base::SpaceInformationPtr&)',
'PlannerAllocator', 'Planner allocator')
- self.add_boost_function('bool()',
+ self.add_function_wrapper('bool()',
'PlannerTerminationConditionFn','Planner termination condition function')
- self.add_boost_function('bool(const ompl::base::State*)',
+ self.add_function_wrapper('bool(const ompl::base::State*)',
'StateValidityCheckerFn', 'State validity checker function')
- self.add_boost_function('ompl::base::StateSamplerPtr(const ompl::base::StateSpace*)',
+ self.add_function_wrapper('ompl::base::StateSamplerPtr(const ompl::base::StateSpace*)',
'StateSamplerAllocator', 'State sampler allocator')
- self.add_boost_function('ompl::base::ValidStateSamplerPtr(ompl::base::SpaceInformation const*)',
+ self.add_function_wrapper('ompl::base::ValidStateSamplerPtr(ompl::base::SpaceInformation const*)',
'ValidStateSamplerAllocator', 'Valid state allocator function')
- self.add_boost_function('double(const ompl::base::PlannerDataVertex&, const ompl::base::PlannerDataVertex&, const ompl::base::PlannerDataEdge&)',
+ self.add_function_wrapper('double(const ompl::base::PlannerDataVertex&, const ompl::base::PlannerDataVertex&, const ompl::base::PlannerDataEdge&)',
'EdgeWeightFn', 'Edge weight function')
- self.add_boost_function('ompl::base::Cost(const ompl::base::State*, const ompl::base::Goal*)',
+ self.add_function_wrapper('ompl::base::Cost(const ompl::base::State*, const ompl::base::Goal*)',
'CostToGoHeuristic', 'Cost-to-go heuristic for optimizing planners')
- self.add_boost_function('std::string()', 'PlannerProgressProperty',
+ self.add_function_wrapper('std::string()', 'PlannerProgressProperty',
'Function that returns stringified value of a property while a planner is running')
# rename SamplerSelectors
@@ -356,7 +366,7 @@ class ompl_control_generator_t(code_generator_t):
def filter_declarations(self):
code_generator_t.filter_declarations(self)
# rename STL vectors of certain types
- self.std_ns.class_('vector< ompl::control::Control* >').rename('vectorControlPtr')
+ self.std_ns.class_('vector< ompl::control::Control * >').rename('vectorControlPtr')
# don't export variables that need a wrapper
self.ompl_ns.variables(lambda decl: decl.is_wrapper_needed()).exclude()
# force ControlSpace::allocControl to be exported.
@@ -382,19 +392,14 @@ class ompl_control_generator_t(code_generator_t):
self.replace_member_functions(self.ompl_ns.member_functions('printControl'))
# print paths as matrices
self.replace_member_functions(self.ompl_ns.member_functions('printAsMatrix'))
- try:
- # export ODESolver-derived classes that use Boost.OdeInt
- for odesolver in ['ODEBasicSolver', 'ODEErrorSolver', 'ODEAdaptiveSolver']:
- self.ompl_ns.class_(lambda cls: cls.name.startswith(odesolver)).rename(odesolver)
- # Somehow, Py++ changes the type of the ODE's first argument. Weird...
- self.add_boost_function('void(ompl::control::ODESolver::StateType, const ompl::control::Control*, ompl::control::ODESolver::StateType &)',
- 'ODE','Ordinary differential equation')
- # workaround for default argument for PostPropagationEvent
- self.replace_member_function(self.ompl_ns.class_('ODESolver').member_function(
- 'getStatePropagator'))
- except declarations.matcher.declaration_not_found_t:
- # not available for boost < 1.44, so ignore this
- pass
+ # export ODESolver-derived classes that use Boost.OdeInt
+ for odesolver in ['ODEBasicSolver', 'ODEErrorSolver', 'ODEAdaptiveSolver']:
+ self.ompl_ns.class_(lambda cls: cls.name.startswith(odesolver)).rename(odesolver)
+ self.add_function_wrapper('void(const ompl::control::ODESolver::StateType &, const ompl::control::Control*, ompl::control::ODESolver::StateType &)',
+ 'ODE','Ordinary differential equation')
+ # workaround for default argument for PostPropagationEvent
+ self.replace_member_function(self.ompl_ns.class_('ODESolver').member_function(
+ 'getStatePropagator'))
# LLVM's clang++ compiler doesn't like exporting this method because
# the argument type (Grid::Cell) is protected
self.ompl_ns.member_functions('computeImportance').exclude()
@@ -416,22 +421,22 @@ class ompl_control_generator_t(code_generator_t):
# code doesn't compile (don't know why)
syclop.class_('Defaults').exclude()
- # add wrappers for boost::function types
- self.add_boost_function('ompl::control::ControlSamplerPtr(const ompl::control::ControlSpace*)',
+ # add wrappers for std::function types
+ self.add_function_wrapper('ompl::control::ControlSamplerPtr(const ompl::control::ControlSpace*)',
'ControlSamplerAllocator', 'Control sampler allocator')
- self.add_boost_function('ompl::control::DirectedControlSamplerPtr(const ompl::control::SpaceInformation*)',
+ self.add_function_wrapper('ompl::control::DirectedControlSamplerPtr(const ompl::control::SpaceInformation*)',
'DirectedControlSamplerAllocator','Directed control sampler allocator')
# same type as StatePropagatorFn, so no need to export this. Instead, we just define a type alias in the python module.
- #self.add_boost_function('void(const ompl::base::State*, const ompl::control::Control*, const double, ompl::base::State*)',
+ #self.add_function_wrapper('void(const ompl::base::State*, const ompl::control::Control*, const double, ompl::base::State*)',
# 'PostPropagationEvent','Post-propagation event')
- self.add_boost_function('void(const ompl::base::State*, const ompl::control::Control*, const double, ompl::base::State*)',
+ self.add_function_wrapper('void(const ompl::base::State*, const ompl::control::Control*, const double, ompl::base::State*)',
'StatePropagatorFn','State propagator function')
- self.add_boost_function('double(int, int)','EdgeCostFactorFn',
+ self.add_function_wrapper('double(int, int)','EdgeCostFactorFn',
'Syclop edge cost factor function')
- self.add_boost_function('void(int, int, std::vector<int>&)','LeadComputeFn',
+ self.add_function_wrapper('void(int, int, std::vector<int>&)','LeadComputeFn',
'Syclop lead compute function')
- # code generation fails because of same bug in gxxcml that requires us
- # to patch the generated code with workaround_for_gccxml_bug.cmake
+ # code generation fails to compile, most likely because of a bug in
+ # Py++'s generation of exposed_decl.pypp.txt.
self.ompl_ns.member_functions('getPlannerAllocator').exclude()
self.ompl_ns.member_functions('setPlannerAllocator').exclude()
self.ompl_ns.namespace('control').class_('SimpleSetup').add_registration_code(
@@ -453,8 +458,8 @@ class ompl_control_generator_t(code_generator_t):
# solution.
# do this for all planners
- for planner in ['KPIECE1', 'PDST', 'RRT', 'EST', 'Syclop', 'SyclopEST', 'SyclopRRT']:
- # many planners exist with the same name in another namespace
+ for planner in ['KPIECE1', 'PDST', 'RRT', 'EST', 'Syclop', 'SyclopEST', 'SyclopRRT','SST']:
+ # many planners exist with the same name in another namespace
self.ompl_ns.namespace('control').class_(planner).wrapper_alias = 'Control%s_wrapper' % planner
self.ompl_ns.class_(planner).add_registration_code("""
def("solve", (::ompl::base::PlannerStatus(::ompl::base::Planner::*)( double ))(&::ompl::base::Planner::solve), (bp::arg("solveTime")) )""")
@@ -488,6 +493,7 @@ class ompl_geometric_generator_t(code_generator_t):
def filter_declarations(self):
code_generator_t.filter_declarations(self)
+ #self.ompl_ns.namespace('util').exclude()
# don't export variables that need a wrapper
self.ompl_ns.variables(lambda decl: decl.is_wrapper_needed()).exclude()
# make objects printable that have a print function
@@ -507,15 +513,15 @@ class ompl_geometric_generator_t(code_generator_t):
# LLVM's clang++ compiler doesn't like exporting this method because
# the argument type (Grid::Cell) is protected
self.ompl_ns.member_functions('computeImportance').exclude()
- # add wrappers for boost::function types
- self.add_boost_function('unsigned int()',
+ # add wrappers for std::function types
+ self.add_function_wrapper('unsigned int()',
'NumNeighborsFn', 'Number of neighbors function')
- # self.add_boost_function('std::vector<ompl::geometric::PRM::Vertex>&(const ompl::geometric::PRM::Vertex)',
+ # self.add_function_wrapper('std::vector<ompl::geometric::PRM::Vertex>&(const ompl::geometric::PRM::Vertex)',
# 'ConnectionStrategy', 'Connection strategy')
- self.add_boost_function('bool(const ompl::geometric::PRM::Vertex&, const ompl::geometric::PRM::Vertex&)',
+ self.add_function_wrapper('bool(const ompl::geometric::PRM::Vertex&, const ompl::geometric::PRM::Vertex&)',
'ConnectionFilter', 'Connection filter')
- # code generation fails because of same bug in gxxcml that requires us
- # to patch the generated code with workaround_for_gccxml_bug.cmake
+ # code generation fails to compile, most likely because of a bug in
+ # Py++'s generation of exposed_decl.pypp.txt.
self.ompl_ns.member_functions('getPlannerAllocator').exclude()
self.ompl_ns.member_functions('setPlannerAllocator').exclude()
self.ompl_ns.namespace('geometric').class_('SimpleSetup').add_registration_code(
@@ -523,8 +529,8 @@ class ompl_geometric_generator_t(code_generator_t):
self.ompl_ns.namespace('geometric').class_('SimpleSetup').add_registration_code(
'def("getPlannerAllocator", &ompl::geometric::SimpleSetup::getPlannerAllocator, bp::return_value_policy< bp::copy_const_reference >())')
# rename to something simpler
- self.std_ns.class_('vector< boost::shared_ptr<ompl::geometric::BITstar::Vertex> >').rename('vectorBITstarVertexPtr')
- self.std_ns.class_('vector<ompl::base::State const*>').exclude()
+ self.std_ns.class_('vector< std::shared_ptr<ompl::geometric::BITstar::Vertex> >').rename('vectorBITstarVertexPtr')
+ self.std_ns.class_('vector<const ompl::base::State *>').exclude()
# Py++ seems to get confused by some methods declared in one module
# that are *not* overridden in a derived class in another module. The
@@ -535,7 +541,7 @@ class ompl_geometric_generator_t(code_generator_t):
# solution.
# do this for all planners
- for planner in ['EST', 'KPIECE1', 'BKPIECE1', 'LBKPIECE1', 'PRM', 'LazyPRM', 'LazyPRMstar', 'PDST', 'LazyRRT', 'RRT', 'RRTConnect', 'TRRT', 'RRTstar', 'LBTRRT', 'SBL', 'SPARS', 'SPARStwo', 'STRIDE', 'FMT', 'BITstar']:
+ for planner in ['EST', 'BiEST', 'ProjEST', 'KPIECE1', 'BKPIECE1', 'LBKPIECE1', 'PRM', 'LazyPRM', 'LazyPRMstar', 'PDST', 'LazyRRT', 'RRT', 'RRTConnect', 'TRRT', 'RRTstar', 'LBTRRT', 'SBL', 'SPARS', 'SPARStwo', 'STRIDE', 'FMT', 'BFMT', 'BITstar', 'SST']:
try:
cls = self.ompl_ns.class_(planner)
except:
@@ -569,7 +575,8 @@ class ompl_geometric_generator_t(code_generator_t):
::ompl::base::PlannerStatus default_solve( ::ompl::base::PlannerTerminationCondition const & ptc );
""")
- PRM_cls.add_declaration_code(open('PRM.SingleThreadSolve.cpp','r').read())
+ PRM_cls.add_declaration_code(open(join(dirname(__file__),
+ 'PRM.SingleThreadSolve.cpp'),'r').read())
# This needs to be the last registration code added to the PRM_cls to the ugly hack below.
PRM_cls.add_registration_code("""def("solve",
(::ompl::base::PlannerStatus(::ompl::geometric::PRM::*)( ::ompl::base::PlannerTerminationCondition const &))(&PRM_wrapper::solve),
@@ -610,7 +617,7 @@ class ompl_geometric_generator_t(code_generator_t):
""".format(planner))
# used in SPARS
- self.std_ns.class_('deque<ompl::base::State*>').rename('dequeState')
+ self.std_ns.class_('deque<ompl::base::State *>').rename('dequeState')
# needed to able to set connection strategy for PRM
# the PRM::Vertex type is typedef-ed to boost::graph_traits<Graph>::vertex_descriptor. This can
@@ -622,7 +629,7 @@ class ompl_geometric_generator_t(code_generator_t):
self.ompl_ns.class_('KStrategy<unsigned long>').rename('KStrategy')
self.ompl_ns.class_('KStarStrategy<unsigned long>').rename('KStarStrategy')
# used in SPARStwo
- self.std_ns.class_('map<unsigned long, ompl::base::State*>').rename('mapVertexToState')
+ self.std_ns.class_('map<unsigned long, ompl::base::State *>').rename('mapVertexToState')
except:
self.ompl_ns.class_('NearestNeighbors<unsigned int>').include()
self.ompl_ns.class_('NearestNeighbors<unsigned int>').rename('NearestNeighbors')
@@ -630,7 +637,7 @@ class ompl_geometric_generator_t(code_generator_t):
self.ompl_ns.class_('KStrategy<unsigned int>').rename('KStrategy')
self.ompl_ns.class_('KStarStrategy<unsigned int>').rename('KStarStrategy')
# used in SPARStwo
- self.std_ns.class_('map<unsigned int, ompl::base::State*>').rename('mapVertexToState')
+ self.std_ns.class_('map<unsigned int, ompl::base::State *>').rename('mapVertexToState')
try:
# Exclude some functions from BIT* that cause some Py++ compilation problems:
@@ -703,8 +710,6 @@ class ompl_tools_generator_t(code_generator_t):
, bp::wrapper< ompl::tools::Benchmark >(){}""")
# don't want to export iostream
benchmark_cls.member_function('saveResultsToStream').exclude()
- # code generation fails because of same bug in gxxcml that requires us
- # to patch the generated code with workaround_for_gccxml_bug.cmake
self.ompl_ns.member_functions('addPlannerAllocator').exclude()
benchmark_cls.member_functions(lambda method: method.name.startswith('set') and method.name.endswith('Event')).exclude()
benchmark_cls.add_registration_code(
@@ -717,9 +722,9 @@ class ompl_tools_generator_t(code_generator_t):
'def("setPreRunEvent", &ompl::tools::Benchmark::setPreRunEvent)')
benchmark_cls.add_registration_code(
'def("setPostRunEvent", &ompl::tools::Benchmark::setPostRunEvent)')
- self.add_boost_function('void(const ompl::base::PlannerPtr&)',
+ self.add_function_wrapper('void(const ompl::base::PlannerPtr&)',
'PreSetupEvent', 'Pre-setup event')
- self.add_boost_function('void(const ompl::base::PlannerPtr&, ompl::tools::Benchmark::RunProperties&)',
+ self.add_function_wrapper('void(const ompl::base::PlannerPtr&, ompl::tools::Benchmark::RunProperties&)',
'PostSetupEvent', 'Post-setup event')
benchmark_cls.class_('Request').no_init = False
diff --git a/py-bindings/headers_base.txt b/py-bindings/headers_base.txt
index 390c7a2..832b368 100644
--- a/py-bindings/headers_base.txt
+++ b/py-bindings/headers_base.txt
@@ -1,58 +1,58 @@
-../src/ompl/util/ClassForward.h
-../src/ompl/base/GenericParam.h
-../src/ompl/base/StateSpaceTypes.h
-../src/ompl/base/State.h
-../src/ompl/base/StateSampler.h
-../src/ompl/base/ValidStateSampler.h
-../src/ompl/base/PrecomputedStateSampler.h
-../src/ompl/base/spaces/RealVectorBounds.h
-../src/ompl/base/ProjectionEvaluator.h
-../src/ompl/base/StateSpace.h
-../src/ompl/base/StateStorage.h
-../src/ompl/base/StateValidityChecker.h
-../src/ompl/base/MotionValidator.h
-../src/ompl/base/SpaceInformation.h
-../src/ompl/base/StateSamplerArray.h
-../src/ompl/base/ScopedState.h
-../src/ompl/base/Cost.h
-../src/ompl/base/Path.h
-../src/ompl/base/GoalTypes.h
-../src/ompl/base/Goal.h
-../src/ompl/base/goals/GoalRegion.h
-../src/ompl/base/goals/GoalSampleableRegion.h
-../src/ompl/base/goals/GoalState.h
-../src/ompl/base/goals/GoalStates.h
-../src/ompl/base/goals/GoalLazySamples.h
-../src/ompl/base/DiscreteMotionValidator.h
-../src/ompl/base/OptimizationObjective.h
-../src/ompl/base/objectives/MinimaxObjective.h
-../src/ompl/base/objectives/MaximizeMinClearanceObjective.h
-../src/ompl/base/objectives/MechanicalWorkOptimizationObjective.h
-../src/ompl/base/objectives/PathLengthOptimizationObjective.h
-../src/ompl/base/objectives/StateCostIntegralObjective.h
-../src/ompl/base/ProblemDefinition.h
-../src/ompl/base/PlannerTerminationCondition.h
-../src/ompl/base/PlannerData.h
-../src/ompl/base/PlannerDataStorage.h
-../src/ompl/base/PlannerStatus.h
-../src/ompl/base/Planner.h
-../src/ompl/base/SolutionNonExistenceProof.h
-../src/ompl/base/samplers/UniformValidStateSampler.h
-../src/ompl/base/spaces/RealVectorStateSpace.h
-../src/ompl/base/spaces/RealVectorStateProjections.h
-../src/ompl/base/spaces/SO2StateSpace.h
-../src/ompl/base/spaces/SO3StateSpace.h
-../src/ompl/base/spaces/SE2StateSpace.h
-../src/ompl/base/spaces/SE3StateSpace.h
-../src/ompl/base/spaces/DiscreteStateSpace.h
-../src/ompl/base/spaces/TimeStateSpace.h
-../src/ompl/base/spaces/DubinsStateSpace.h
-../src/ompl/base/spaces/ReedsSheppStateSpace.h
-../src/ompl/base/samplers/GaussianValidStateSampler.h
-../src/ompl/base/samplers/MaximizeClearanceValidStateSampler.h
-../src/ompl/base/samplers/ObstacleBasedValidStateSampler.h
-../src/ompl/base/samplers/UniformValidStateSampler.h
-../src/ompl/base/samplers/InformedStateSampler.h
-../src/ompl/base/samplers/informed/PathLengthDirectInfSampler.h
-../src/ompl/base/samplers/informed/RejectionInfSampler.h
-ompl_py_base.h
+src/ompl/util/ClassForward.h
+src/ompl/base/GenericParam.h
+src/ompl/base/StateSpaceTypes.h
+src/ompl/base/State.h
+src/ompl/base/StateSampler.h
+src/ompl/base/ValidStateSampler.h
+src/ompl/base/PrecomputedStateSampler.h
+src/ompl/base/spaces/RealVectorBounds.h
+src/ompl/base/ProjectionEvaluator.h
+src/ompl/base/StateSpace.h
+src/ompl/base/StateStorage.h
+src/ompl/base/StateValidityChecker.h
+src/ompl/base/MotionValidator.h
+src/ompl/base/SpaceInformation.h
+src/ompl/base/StateSamplerArray.h
+src/ompl/base/ScopedState.h
+src/ompl/base/Cost.h
+src/ompl/base/Path.h
+src/ompl/base/GoalTypes.h
+src/ompl/base/Goal.h
+src/ompl/base/goals/GoalRegion.h
+src/ompl/base/goals/GoalSampleableRegion.h
+src/ompl/base/goals/GoalState.h
+src/ompl/base/goals/GoalStates.h
+src/ompl/base/goals/GoalLazySamples.h
+src/ompl/base/DiscreteMotionValidator.h
+src/ompl/base/OptimizationObjective.h
+src/ompl/base/objectives/MinimaxObjective.h
+src/ompl/base/objectives/MaximizeMinClearanceObjective.h
+src/ompl/base/objectives/MechanicalWorkOptimizationObjective.h
+src/ompl/base/objectives/PathLengthOptimizationObjective.h
+src/ompl/base/objectives/StateCostIntegralObjective.h
+src/ompl/base/ProblemDefinition.h
+src/ompl/base/PlannerTerminationCondition.h
+src/ompl/base/PlannerData.h
+src/ompl/base/PlannerDataStorage.h
+src/ompl/base/PlannerStatus.h
+src/ompl/base/Planner.h
+src/ompl/base/SolutionNonExistenceProof.h
+src/ompl/base/samplers/UniformValidStateSampler.h
+src/ompl/base/spaces/RealVectorStateSpace.h
+src/ompl/base/spaces/RealVectorStateProjections.h
+src/ompl/base/spaces/SO2StateSpace.h
+src/ompl/base/spaces/SO3StateSpace.h
+src/ompl/base/spaces/SE2StateSpace.h
+src/ompl/base/spaces/SE3StateSpace.h
+src/ompl/base/spaces/DiscreteStateSpace.h
+src/ompl/base/spaces/TimeStateSpace.h
+src/ompl/base/spaces/DubinsStateSpace.h
+src/ompl/base/spaces/ReedsSheppStateSpace.h
+src/ompl/base/samplers/GaussianValidStateSampler.h
+src/ompl/base/samplers/MaximizeClearanceValidStateSampler.h
+src/ompl/base/samplers/ObstacleBasedValidStateSampler.h
+src/ompl/base/samplers/UniformValidStateSampler.h
+src/ompl/base/samplers/InformedStateSampler.h
+src/ompl/base/samplers/informed/PathLengthDirectInfSampler.h
+src/ompl/base/samplers/informed/RejectionInfSampler.h
+py-bindings/ompl_py_base.h
diff --git a/py-bindings/headers_control.txt b/py-bindings/headers_control.txt
index c2de435..8413a15 100644
--- a/py-bindings/headers_control.txt
+++ b/py-bindings/headers_control.txt
@@ -1,25 +1,26 @@
-../src/ompl/control/Control.h
-../src/ompl/control/ControlSampler.h
-../src/ompl/control/DirectedControlSampler.h
-../src/ompl/control/SimpleDirectedControlSampler.h
-../src/ompl/control/ControlSpaceTypes.h
-../src/ompl/control/ControlSpace.h
-../src/ompl/control/StatePropagator.h
-../src/ompl/control/SpaceInformation.h
-../src/ompl/control/ODESolver.h
-../src/ompl/control/PathControl.h
-../src/ompl/control/SimpleSetup.h
-../src/ompl/control/spaces/DiscreteControlSpace.h
-../src/ompl/control/spaces/RealVectorControlSpace.h
-../src/ompl/control/PlannerData.h
-../src/ompl/control/PlannerDataStorage.h
-../src/ompl/control/planners/est/EST.h
-../src/ompl/control/planners/kpiece/KPIECE1.h
-../src/ompl/control/planners/pdst/PDST.h
-../src/ompl/control/planners/rrt/RRT.h
-../src/ompl/control/planners/syclop/Decomposition.h
-../src/ompl/control/planners/syclop/GridDecomposition.h
-../src/ompl/control/planners/syclop/Syclop.h
-../src/ompl/control/planners/syclop/SyclopEST.h
-../src/ompl/control/planners/syclop/SyclopRRT.h
-ompl_py_control.h
+src/ompl/control/Control.h
+src/ompl/control/ControlSampler.h
+src/ompl/control/DirectedControlSampler.h
+src/ompl/control/SimpleDirectedControlSampler.h
+src/ompl/control/ControlSpaceTypes.h
+src/ompl/control/ControlSpace.h
+src/ompl/control/StatePropagator.h
+src/ompl/control/SpaceInformation.h
+src/ompl/control/ODESolver.h
+src/ompl/control/PathControl.h
+src/ompl/control/SimpleSetup.h
+src/ompl/control/spaces/DiscreteControlSpace.h
+src/ompl/control/spaces/RealVectorControlSpace.h
+src/ompl/control/PlannerData.h
+src/ompl/control/PlannerDataStorage.h
+src/ompl/control/planners/est/EST.h
+src/ompl/control/planners/kpiece/KPIECE1.h
+src/ompl/control/planners/pdst/PDST.h
+src/ompl/control/planners/rrt/RRT.h
+src/ompl/control/planners/syclop/Decomposition.h
+src/ompl/control/planners/syclop/GridDecomposition.h
+src/ompl/control/planners/syclop/Syclop.h
+src/ompl/control/planners/syclop/SyclopEST.h
+src/ompl/control/planners/syclop/SyclopRRT.h
+src/ompl/control/planners/sst/SST.h
+py-bindings/ompl_py_control.h
diff --git a/py-bindings/headers_geometric.txt b/py-bindings/headers_geometric.txt
index 2bddd22..f872f4e 100644
--- a/py-bindings/headers_geometric.txt
+++ b/py-bindings/headers_geometric.txt
@@ -1,35 +1,39 @@
-../src/ompl/geometric/PathGeometric.h
-../src/ompl/geometric/PathSimplifier.h
-../src/ompl/geometric/PathHybridization.h
-../src/ompl/geometric/SimpleSetup.h
-../src/ompl/geometric/planners/prm/ConnectionStrategy.h
-../src/ompl/geometric/planners/prm/PRM.h
-../src/ompl/geometric/planners/prm/LazyPRM.h
-../src/ompl/geometric/planners/prm/LazyPRMstar.h
-../src/ompl/geometric/planners/prm/SPARS.h
-../src/ompl/geometric/planners/prm/SPARStwo.h
-../src/ompl/geometric/planners/est/EST.h
-../src/ompl/geometric/planners/kpiece/KPIECE1.h
-../src/ompl/geometric/planners/kpiece/BKPIECE1.h
-../src/ompl/geometric/planners/kpiece/LBKPIECE1.h
-../src/ompl/geometric/planners/pdst/PDST.h
-../src/ompl/geometric/planners/rrt/RRT.h
-../src/ompl/geometric/planners/rrt/RRTConnect.h
-../src/ompl/geometric/planners/rrt/LazyRRT.h
-../src/ompl/geometric/planners/rrt/TRRT.h
-../src/ompl/geometric/planners/rrt/RRTstar.h
-../src/ompl/geometric/planners/rrt/LBTRRT.h
-../src/ompl/geometric/planners/rrt/LazyLBTRRT.h
-../src/ompl/geometric/planners/rrt/InformedRRTstar.h
-../src/ompl/geometric/planners/bitstar/BITstar.h
-../src/ompl/geometric/planners/bitstar/datastructures/Vertex.h
-../src/ompl/geometric/planners/sbl/SBL.h
-../src/ompl/geometric/planners/stride/STRIDE.h
-../src/ompl/geometric/planners/fmt/FMT.h
-../src/ompl/datastructures/NearestNeighbors.h
-../src/ompl/datastructures/NearestNeighborsLinear.h
-../src/ompl/geometric/planners/bitstar/BITstar.h
-../src/ompl/geometric/planners/bitstar/datastructures/IdGenerator.h
-../src/ompl/geometric/planners/bitstar/datastructures/Vertex.h
-../src/ompl/geometric/planners/bitstar/datastructures/IntegratedQueue.h
-ompl_py_geometric.h
+src/ompl/geometric/PathGeometric.h
+src/ompl/geometric/PathSimplifier.h
+src/ompl/geometric/PathHybridization.h
+src/ompl/geometric/SimpleSetup.h
+src/ompl/geometric/planners/prm/ConnectionStrategy.h
+src/ompl/geometric/planners/prm/PRM.h
+src/ompl/geometric/planners/prm/LazyPRM.h
+src/ompl/geometric/planners/prm/LazyPRMstar.h
+src/ompl/geometric/planners/prm/SPARS.h
+src/ompl/geometric/planners/prm/SPARStwo.h
+src/ompl/geometric/planners/est/EST.h
+src/ompl/geometric/planners/est/BiEST.h
+src/ompl/geometric/planners/est/ProjEST.h
+src/ompl/geometric/planners/kpiece/KPIECE1.h
+src/ompl/geometric/planners/kpiece/BKPIECE1.h
+src/ompl/geometric/planners/kpiece/LBKPIECE1.h
+src/ompl/geometric/planners/pdst/PDST.h
+src/ompl/geometric/planners/rrt/RRT.h
+src/ompl/geometric/planners/rrt/RRTConnect.h
+src/ompl/geometric/planners/rrt/LazyRRT.h
+src/ompl/geometric/planners/rrt/TRRT.h
+src/ompl/geometric/planners/rrt/RRTstar.h
+src/ompl/geometric/planners/rrt/LBTRRT.h
+src/ompl/geometric/planners/rrt/LazyLBTRRT.h
+src/ompl/geometric/planners/rrt/InformedRRTstar.h
+src/ompl/geometric/planners/bitstar/BITstar.h
+src/ompl/geometric/planners/bitstar/datastructures/Vertex.h
+src/ompl/geometric/planners/sbl/SBL.h
+src/ompl/geometric/planners/stride/STRIDE.h
+src/ompl/geometric/planners/fmt/FMT.h
+src/ompl/geometric/planners/fmt/BFMT.h
+src/ompl/datastructures/NearestNeighbors.h
+src/ompl/datastructures/NearestNeighborsLinear.h
+src/ompl/geometric/planners/bitstar/BITstar.h
+src/ompl/geometric/planners/bitstar/datastructures/IdGenerator.h
+src/ompl/geometric/planners/bitstar/datastructures/Vertex.h
+src/ompl/geometric/planners/bitstar/datastructures/IntegratedQueue.h
+src/ompl/geometric/planners/sst/SST.h
+py-bindings/ompl_py_geometric.h
diff --git a/py-bindings/headers_morse.txt b/py-bindings/headers_morse.txt
index fb1ad36..e1dd005 100644
--- a/py-bindings/headers_morse.txt
+++ b/py-bindings/headers_morse.txt
@@ -1,10 +1,10 @@
-../src/ompl/extensions/morse/MorseEnvironment.h
-../src/ompl/extensions/morse/MorseTerminationCondition.h
-../src/ompl/extensions/morse/MorseGoal.h
-../src/ompl/extensions/morse/MorseStateSpace.h
-../src/ompl/extensions/morse/MorseControlSpace.h
-../src/ompl/extensions/morse/MorseStatePropagator.h
-../src/ompl/extensions/morse/MorseStateValidityChecker.h
-../src/ompl/extensions/morse/MorseProjection.h
-../src/ompl/extensions/morse/MorseSimpleSetup.h
-ompl_py_morse.h
+src/ompl/extensions/morse/MorseEnvironment.h
+src/ompl/extensions/morse/MorseTerminationCondition.h
+src/ompl/extensions/morse/MorseGoal.h
+src/ompl/extensions/morse/MorseStateSpace.h
+src/ompl/extensions/morse/MorseControlSpace.h
+src/ompl/extensions/morse/MorseStatePropagator.h
+src/ompl/extensions/morse/MorseStateValidityChecker.h
+src/ompl/extensions/morse/MorseProjection.h
+src/ompl/extensions/morse/MorseSimpleSetup.h
+py-bindings/ompl_py_morse.h
diff --git a/py-bindings/headers_tools.txt b/py-bindings/headers_tools.txt
index 0d96112..b380e25 100644
--- a/py-bindings/headers_tools.txt
+++ b/py-bindings/headers_tools.txt
@@ -1,8 +1,8 @@
-../src/ompl/tools/benchmark/Benchmark.h
-../src/ompl/tools/benchmark/MachineSpecs.h
-../src/ompl/tools/config/MagicConstants.h
-../src/ompl/tools/config/SelfConfig.h
-../src/ompl/tools/lightning/DynamicTimeWarp.h
-../src/ompl/tools/multiplan/ParallelPlan.h
-../src/ompl/tools/multiplan/OptimizePlan.h
-ompl_py_tools.h
+src/ompl/tools/benchmark/Benchmark.h
+src/ompl/tools/benchmark/MachineSpecs.h
+src/ompl/tools/config/MagicConstants.h
+src/ompl/tools/config/SelfConfig.h
+src/ompl/tools/lightning/DynamicTimeWarp.h
+src/ompl/tools/multiplan/ParallelPlan.h
+src/ompl/tools/multiplan/OptimizePlan.h
+py-bindings/ompl_py_tools.h
diff --git a/py-bindings/headers_util.txt b/py-bindings/headers_util.txt
index be54b5c..0fb6be5 100644
--- a/py-bindings/headers_util.txt
+++ b/py-bindings/headers_util.txt
@@ -1,7 +1,7 @@
-../src/ompl/util/Exception.h
-../src/ompl/util/RandomNumbers.h
-../src/ompl/util/Console.h
-../src/ompl/util/PPM.h
-../src/ompl/util/GeometricEquations.h
-../src/ompl/util/ProlateHyperspheroid.h
-ompl_py_util.h
+src/ompl/util/Exception.h
+src/ompl/util/RandomNumbers.h
+src/ompl/util/Console.h
+src/ompl/util/PPM.h
+src/ompl/util/GeometricEquations.h
+src/ompl/util/ProlateHyperspheroid.h
+py-bindings/ompl_py_util.h
diff --git a/py-bindings/ompl/__init__.py b/py-bindings/ompl/__init__.py
index 2de03c4..2583b91 100644
--- a/py-bindings/ompl/__init__.py
+++ b/py-bindings/ompl/__init__.py
@@ -41,7 +41,7 @@ class PlanningAlgorithms(object):
plannerObject = eval("""%s(ompl.control.SpaceInformation(
ompl.base.RealVectorStateSpace(1),
ompl.control.RealVectorControlSpace(
- ompl.base.RealVectorStateSpace(1),1)))""" % planner)
+ ompl.base.RealVectorStateSpace(1),1)))""" % planner)
params = plannerObject.params()
except:
try:
@@ -65,27 +65,27 @@ class PlanningAlgorithms(object):
if rangeSuggestion == '': continue
rangeSuggestion = rangeSuggestion.split(':')
defaultValue = p.getValue()
- if len(rangeSuggestion)==1:
+ if len(rangeSuggestion) == 1:
rangeSuggestion = rangeSuggestion[0].split(',')
- if len(rangeSuggestion)==1:
+ if len(rangeSuggestion) == 1:
raise Exception('Cannot parse range suggestion')
- elif len(rangeSuggestion)==2:
+ elif len(rangeSuggestion) == 2:
rangeType = self.BOOL
- defaultValue = 0 if defaultValue==rangeSuggestion[0] else 1
+ defaultValue = 0 if defaultValue == rangeSuggestion[0] else 1
rangeSuggestion = ''
else:
rangeType = self.ENUM
- defaultValue = 0 if defaultValue=='' else rangeSuggestion.index(defaultValue)
+ defaultValue = 0 if defaultValue == '' else rangeSuggestion.index(defaultValue)
else:
if ('.' in rangeSuggestion[0] or '.' in rangeSuggestion[-1]):
rangeType = self.DOUBLE
rangeSuggestion = [float(r) for r in rangeSuggestion]
- defaultValue = 0. if defaultValue=='' else float(defaultValue)
+ defaultValue = 0. if defaultValue == '' else float(defaultValue)
else:
rangeType = self.INT
rangeSuggestion = [int(r) for r in rangeSuggestion]
- defaultValue = 0 if defaultValue=='' else int(defaultValue)
- if len(rangeSuggestion)==2:
+ defaultValue = 0 if defaultValue == '' else int(defaultValue)
+ if len(rangeSuggestion) == 2:
rangeSuggestion = [rangeSuggestion[0], 1, rangeSuggestion[1]]
name = p.getName()
displayName = name.replace('_', ' ').capitalize()
diff --git a/py-bindings/ompl/bindings_generator.py b/py-bindings/ompl/bindings_generator.py
index 6625c7a..3391106 100644
--- a/py-bindings/ompl/bindings_generator.py
+++ b/py-bindings/ompl/bindings_generator.py
@@ -43,7 +43,8 @@ from os.path import exists, join, isdir
from os import getenv
import subprocess
from sys import platform
-from pygccxml import declarations
+from pygccxml import declarations, parser
+from pygccxml.utils import xml_generator
from pyplusplus import module_builder, messages
from pyplusplus.module_builder import call_policies
from pyplusplus.decl_wrappers import print_declarations
@@ -98,6 +99,10 @@ void __setitem(%s* obj, unsigned int i, %s val)
}
""")
+# impl_conv_code = """
+# boost::python::implicitly_convertible< std::shared_ptr< %(from)s >, std::shared_ptr< %(to)s > >();
+# """
+
def returns_reference(decl):
"""Return True iff the declaration returns a pointer or reference."""
c = decl.return_type.decl_string[-1]
@@ -111,40 +116,11 @@ class code_generator_t(object):
@name name of the python module
@dep name of another module this module depends on"""
module_builder.set_logger_level( logging.INFO )
- candidate_include_paths = [ "/Users/mmoll/omplapp/ompl/src", "/Users/mmoll/omplapp/src",
- "/opt/local/Library/Frameworks/Python.framework/Versions/3.4/include/python3.4m", "/opt/local/include", "/opt/local/include", "", ""]
-
- # Adding standard windows headers
- if platform == 'win32':
- compiler = getenv('GCCXML_COMPILER')
- # MinGW
- if compiler != None and (compiler.lower().endswith('g++') or compiler.lower().endswith('c++')):
- version = subprocess.Popen([compiler, '-dumpversion'],
- stdout=subprocess.PIPE).communicate()[0].strip()
- # Find whole path to MinGW
- compiler_path = ""
- for path in getenv('PATH').split(';'):
- if exists(join(path, compiler + '.exe')):
- compiler_path = path
- break
-
- if compiler_path is not "":
- # Adding in necessary include paths
- candidate_include_paths.append (join(compiler_path, "..", "include"))
- candidate_include_paths.append (join(compiler_path, "..", "lib", "gcc", "mingw32", version, "include"))
- candidate_include_paths.append (join(compiler_path, "..", "lib", "gcc", "mingw32", version, "include", "c++"))
- candidate_include_paths.append (join(compiler_path, "..", "lib", "gcc", "mingw32", version, "include", "c++", "mingw32"))
-
- include_paths = []
- for path in candidate_include_paths:
- if len(path)>0 and exists(path):
- include_paths.append(path)
+ xml_generator_config = parser.load_xml_generator_configuration("/Users/mmoll/build/omplapp/Release/castxml.cfg")
self.mb = module_builder.module_builder_t(
files = [ 'bindings/' + name + '.h' ],
cache = '/Users/mmoll/build/omplapp/Release/pyplusplus_'+name+'.cache',
- gccxml_path = "/opt/local/bin/gccxml",
- include_paths = include_paths,
- cflags="-m64",
+ xml_generator_config = xml_generator_config,
indexing_suite_version = indexing_suite_version )
self.replacement = {} if replacement_dict==None else replacement_dict
self.mb.classes().always_expose_using_scope = True
@@ -178,7 +154,10 @@ class code_generator_t(object):
def filter_declarations(self):
"""Remove some classes and functions from the std namespace"""
- self.std_ns.class_('ios_base').exclude()
+ try:
+ self.std_ns.class_('ios_base').exclude()
+ except:
+ pass
self.std_ns.free_functions().exclude()
self.std_ns.operators().exclude()
@@ -213,6 +192,26 @@ class code_generator_t(object):
cls.add_registration_code(reg)
cls.add_declaration_code(wrapper % (cls.decl_string, rettype))
- def add_boost_function(self, FT, func_name, func_doc):
+ def add_function_wrapper(self, FT, func_name, func_doc):
self.mb.add_declaration_code('PYDECLARE_FUNCTION(%s,%s)' % (FT, func_name))
self.mb.add_registration_code('PYREGISTER_FUNCTION(%s,%s,"%s")' % (FT, func_name, func_doc))
+
+ # Boost.Python has special code to handle boost::shared_ptr conversions,
+ # but support for std::shared_ptr is still missing (see
+ # https://github.com/boostorg/python/issues/29). This function attempts
+ # to (partially) fix this for specific classes.
+
+ # We currently use a different workaround: copy the special bits of Boost
+ # code that handle boost::shared_ptr's and modify them to use
+ # std::shared_ptr's instead. Might not be portable across a wide range of
+ # Boost.Python versions....
+
+ # def add_implicit_conversions(self, cls):
+ # cls.held_type = 'std::shared_ptr< %s >' % cls.wrapper_alias
+ # cls.add_registration_code(impl_conv_code % {'from': cls.wrapper_alias, 'to': cls.decl_string}, False)
+ # for base in cls.recursive_bases:
+ # if base.access_type == 'public':
+ # # from class to its base
+ # cls.add_registration_code(impl_conv_code % {'from': cls.decl_string, 'to': base.related_class.decl_string}, False)
+ # # from wrapper to clas base class
+ # cls.add_registration_code(impl_conv_code % {'from': cls.wrapper_alias, 'to': base.related_class.decl_string}, False)
diff --git a/py-bindings/ompl/control/__init__.py b/py-bindings/ompl/control/__init__.py
index 4d51eac..6157204 100644
--- a/py-bindings/ompl/control/__init__.py
+++ b/py-bindings/ompl/control/__init__.py
@@ -6,5 +6,5 @@ from ompl.control._control import *
# parameter info
planners = None
-# type alias for boost::function<void(const State*, const Control*, const double, State*)>
+# type alias for std::function<void(const State*, const Control*, const double, State*)>
PostPropagationEvent = StatePropagatorFn
diff --git a/py-bindings/ompl_py_base.h b/py-bindings/ompl_py_base.h
index 3e5998c..d16475b 100644
--- a/py-bindings/ompl_py_base.h
+++ b/py-bindings/ompl_py_base.h
@@ -46,7 +46,7 @@
#include "ompl/base/spaces/ReedsSheppStateSpace.h"
#include "ompl/base/Goal.h"
#include "ompl/base/PlannerData.h"
-#include "py_boost_function.hpp"
+#include "py_std_function.hpp"
#define DeclareStateType(T) \
inline int __dummy##T() \
diff --git a/py-bindings/ompl_py_control.h b/py-bindings/ompl_py_control.h
index dbd3d28..ecc3033 100644
--- a/py-bindings/ompl_py_control.h
+++ b/py-bindings/ompl_py_control.h
@@ -38,20 +38,17 @@
#define PY_BINDINGS_OMPL_PY_CONTROL_
#include "ompl/control/ODESolver.h"
-#include "py_boost_function.hpp"
+#include "py_std_function.hpp"
namespace ompl
{
namespace control
{
-// Boost.OdeInt needs Boost version >= 1.44
-#if BOOST_VERSION >= 104400
inline int dummyODESolverSize()
{
return sizeof(ODEBasicSolver<>) + sizeof(ODEErrorSolver<>) + sizeof(ODEAdaptiveSolver<>);
}
-#endif
}
}
diff --git a/py-bindings/ompl_py_geometric.h b/py-bindings/ompl_py_geometric.h
index 4634aa8..24f605c 100644
--- a/py-bindings/ompl_py_geometric.h
+++ b/py-bindings/ompl_py_geometric.h
@@ -44,7 +44,7 @@
#include <deque>
#include <map>
#include <boost/graph/adjacency_list.hpp>
-#include "py_boost_function.hpp"
+#include "py_std_function.hpp"
namespace ompl
@@ -55,7 +55,7 @@ namespace ompl
inline int dummyConnectionStrategy()
{
NearestNeighborsLinear<PRM::Vertex> nn;
- boost::shared_ptr<NearestNeighbors<PRM::Vertex> > nnPtr(&nn);
+ std::shared_ptr<NearestNeighbors<PRM::Vertex> > nnPtr(&nn);
return sizeof(KStrategy<PRM::Vertex>(1, nnPtr)) + sizeof(KStarStrategy<PRM::Vertex>(dummyFn, nnPtr, 1)) + sizeof(nn);
}
inline int dummySTLContainerSize()
@@ -63,7 +63,7 @@ namespace ompl
return sizeof(std::deque<ompl::base::State*>) +
sizeof(std::map<boost::adjacency_list<>::vertex_descriptor, ompl::base::State*>) +
sizeof(std::vector<const ompl::base::State*>) +
- sizeof(std::vector< boost::shared_ptr<ompl::geometric::BITstar::Vertex> >);
+ sizeof(std::vector< std::shared_ptr<ompl::geometric::BITstar::Vertex> >);
}
}
}
diff --git a/py-bindings/ompl_py_morse.h b/py-bindings/ompl_py_morse.h
index f659c4d..e067c34 100644
--- a/py-bindings/ompl_py_morse.h
+++ b/py-bindings/ompl_py_morse.h
@@ -3,7 +3,7 @@
#include "ompl/base/ScopedState.h"
#include "ompl/extensions/morse/MorseStateSpace.h"
-#include "py_boost_function.hpp"
+#include "py_std_function.hpp"
#define DeclareStateType(T) \
inline int __dummy##T() \
diff --git a/py-bindings/ompl_py_tools.h b/py-bindings/ompl_py_tools.h
index af95393..5f17907 100644
--- a/py-bindings/ompl_py_tools.h
+++ b/py-bindings/ompl_py_tools.h
@@ -37,6 +37,6 @@
#ifndef PY_BINDINGS_OMPL_PY_TOOLS_
#define PY_BINDINGS_OMPL_PY_TOOLS_
-#include "py_boost_function.hpp"
+#include "py_std_function.hpp"
#endif
diff --git a/py-bindings/py_boost_function.hpp b/py-bindings/py_std_function.hpp
similarity index 95%
rename from py-bindings/py_boost_function.hpp
rename to py-bindings/py_std_function.hpp
index 63bfe79..909b6d8 100644
--- a/py-bindings/py_boost_function.hpp
+++ b/py-bindings/py_std_function.hpp
@@ -36,7 +36,7 @@
/******************************************************************************
- * How to wrap boost.function objects with boost.python and use them both from
+ * How to wrap C++11 std::function objects with boost.python and use them both from
* C++ and Python.
*
* This is a significantly enhanced version of the original version by
@@ -49,9 +49,9 @@
* For the boost.python C++ module:
*
* ...
- * #include <boost/python.hpp>
+ * #include <functional>
* ...
- * #include "py_boost_function.hpp"
+ * #include "py_std_function.hpp"
* ...
*
* void module_greeter_f(std::string const& origin)
@@ -92,16 +92,15 @@
-#ifndef PY_BINDINGS_PY_BOOST_FUNCTION_
-#define PY_BINDINGS_PY_BOOST_FUNCTION_
+#ifndef PY_BINDINGS_PY_STD_FUNCTION_
+#define PY_BINDINGS_PY_STD_FUNCTION_
#include <boost/python.hpp>
-#include <boost/function.hpp>
-#include <boost/function_types/function_type.hpp>
-#include <boost/function_types/result_type.hpp>
-#include <boost/function_types/is_function.hpp>
+#include <functional>
+#include <boost/function_types/parameter_types.hpp>
+#include <boost/function_types/is_function.hpp>
#include <boost/type_traits/function_traits.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/is_fundamental.hpp>
@@ -234,7 +233,7 @@ void def_function(const char* func_name, const char* func_doc)
{
BOOST_STATIC_ASSERT( boost::function_types::is_function< FT >::value ) ;
namespace bp = boost::python;
- typedef boost::function<FT> function_t;
+ typedef std::function<FT> function_t;
bp::class_< function_t >
(func_name, func_doc, bp::no_init)
.def("__call__", &function_t::operator() )
@@ -244,7 +243,7 @@ void def_function(const char* func_name, const char* func_doc)
#define PYDECLARE_FUNCTION(FT, func_name) \
namespace detail \
{ \
- boost::function<FT> func_name(boost::python::object o) \
+ std::function<FT> func_name(boost::python::object o) \
{ \
return detail::pyobject_invoker<FT, boost::function_traits<FT>::result_type, \
boost::function_traits< FT >::arity>(o); \
diff --git a/scripts/ompl_benchmark_statistics.py b/scripts/ompl_benchmark_statistics.py
index 61483a4..34ccab7 100755
--- a/scripts/ompl_benchmark_statistics.py
+++ b/scripts/ompl_benchmark_statistics.py
@@ -41,13 +41,18 @@ from os.path import basename, splitext, exists
import os
import sqlite3
import datetime
-import matplotlib
-matplotlib.use('pdf')
-from matplotlib import __version__ as matplotlibversion
-from matplotlib.backends.backend_pdf import PdfPages
-import matplotlib.pyplot as plt
-import numpy as np
-from math import floor
+plottingEnabled=True
+try:
+ import matplotlib
+ matplotlib.use('pdf')
+ from matplotlib import __version__ as matplotlibversion
+ from matplotlib.backends.backend_pdf import PdfPages
+ import matplotlib.pyplot as plt
+ import numpy as np
+ from math import floor
+except:
+ print('Matplotlib or Numpy was not found; disabling plotting capabilities...')
+ plottingEnabled=False
from optparse import OptionParser, OptionGroup
# Given a text line, split it into tokens (by space) and return the token
@@ -563,15 +568,16 @@ if __name__ == "__main__":
help="Append data to database (as opposed to overwriting an existing database)")
parser.add_option("-v", "--view", action="store_true", dest="view", default=False,
help="Compute the views for best planner configurations")
- parser.add_option("-p", "--plot", dest="plot", default=None,
- help="Create a PDF of plots")
+ if plottingEnabled:
+ parser.add_option("-p", "--plot", dest="plot", default=None,
+ help="Create a PDF of plots")
parser.add_option("-m", "--mysql", dest="mysqldb", default=None,
help="Save SQLite3 database as a MySQL dump file")
parser.add_option("--moveit", action="store_true", dest="moveit", default=False,
help="Log files are produced by MoveIt!")
(options, args) = parser.parse_args()
- if not options.append and exists(options.dbname):
+ if not options.append and exists(options.dbname) and len(args)>0:
os.remove(options.dbname)
if len(args)>0:
diff --git a/scripts/plannerarena/global.R b/scripts/plannerarena/global.R
new file mode 100644
index 0000000..e5fc95d
--- /dev/null
+++ b/scripts/plannerarena/global.R
@@ -0,0 +1,73 @@
+######################################################################
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2016, Rice University
+# 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 Rice University 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: Mark Moll
+
+library(shiny)
+library(shinyjs, warn.conflicts=FALSE)
+library(dplyr, warn.conflicts=FALSE)
+library(tidyr)
+library(ggplot2)
+
+# set default max upload size to 50MB
+options(shiny.maxRequestSize = getOption("shiny.maxRequestSize", 50000000))
+options(shiny.reactlog = TRUE)
+
+# see https://groups.google.com/d/msg/shiny-discuss/uSetp4TtW-s/Jktu3fS60RAJ
+disable <- function(x) {
+ if (inherits(x, 'shiny.tag')) {
+ if (x$name %in% c('input', 'select', 'label'))
+ x$attribs$disabled <- 'disabled'
+ x$children <- disable(x$children)
+ }
+ else if (is.list(x) && length(x) > 0) {
+ for (i in 1:length(x))
+ x[[i]] <- disable(x[[i]])
+ }
+ x
+}
+conditionalDisable <- function(widget, condition) {
+ if (condition)
+ disable(widget)
+ else
+ widget
+}
+
+# see http://stackoverflow.com/questions/29948876/adding-prefix-or-suffix-to-most-data-frame-variable-names-in-piped-r-workflow
+tbl.renamer <- function(tbl, prefix="x", suffix=NULL, index=seq_along(tbl_vars(tbl))) {
+ newnames <- tbl_vars(tbl) # Get old variable names
+ names(newnames) <- newnames
+ names(newnames)[index] <- paste0(prefix,".",newnames,suffix)[index] # create a named vector for .dots
+ rename_(tbl,.dots=newnames) # rename the variables
+}
diff --git a/scripts/plannerarena/plannerarena b/scripts/plannerarena/plannerarena
index 92b0a03..a9beb10 100755
--- a/scripts/plannerarena/plannerarena
+++ b/scripts/plannerarena/plannerarena
@@ -1,10 +1,10 @@
#!/usr/bin/env Rscript
-packages <- c('shiny', 'shinyjs', 'V8', 'ggplot2', 'Hmisc', 'RSQLite', 'markdown')
+packages <- c('shiny', 'shinyjs', 'V8', 'dplyr', 'tidyr', 'ggplot2', 'Hmisc', 'RSQLite', 'markdown')
missing.packages <- setdiff(packages, rownames(installed.packages()))
if (length(missing.packages) > 0) {
cat(c('The following packages will be installed:\n\t', missing.packages, '\n'))
- install.packages(missing.packages, repos="http://cran.r-project.org")
+ install.packages(missing.packages, repos="http://cran.r-project.org")
}
library(methods)
@@ -51,8 +51,8 @@ if (file.exists(paste0(script.dir,"/app.R"))) {
cfg <- parse.cfg(cfg.file)
options(shiny.maxRequestSize = strtoi(cfg$plannerarena$max_upload_size),
shiny.port = strtoi(cfg$plannerarena$plannerarena_port),
- warn=-1)
+ warn = -1)
# open plannerarena in the browser
launch.browser <- strtoi(cfg$plannerarena$launch_browser)
-runApp(appDir, launch.browser=launch.browser)
+runApp(appDir, launch.browser = launch.browser)
diff --git a/scripts/plannerarena/app.R b/scripts/plannerarena/server.R
similarity index 52%
rename from scripts/plannerarena/app.R
rename to scripts/plannerarena/server.R
index 0122575..8ab3d30 100644
--- a/scripts/plannerarena/app.R
+++ b/scripts/plannerarena/server.R
@@ -1,93 +1,105 @@
-library(DBI)
-library(shiny)
-library(shinyjs, warn.conflicts=FALSE)
-library(ggplot2)
-library(RSQLite)
-library(reshape2)
-
+######################################################################
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2016, Rice University
+# 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 Rice University 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: Mark Moll
defaultDatabase <- "www/benchmark.db"
-
noDatabaseText <- "No database loaded yet. Upload one by clicking on “Change database”."
-
notReadyText <- "The benchmarking results are not available yet, check back later."
-
-sessionsFolder = "/tmp/omplweb_sessions"
-
+sessionsFolder <- "/tmp/omplweb_sessions"
problemParamsAggregateText <- "all (aggregate)"
problemParamsSeparateText <- "all (separate)"
-disable <- function(x) {
- if (inherits(x, 'shiny.tag')) {
- if (x$name %in% c('input', 'select', 'label'))
- x$attribs$disabled <- 'disabled'
- x$children <- disable(x$children)
- }
- else if (is.list(x) && length(x) > 0) {
- for (i in 1:length(x))
- x[[i]] <- disable(x[[i]])
- }
- x
-}
-conditionalDisable <- function(widget, condition) {
- if (condition)
- disable(widget)
- else
- widget
+problemSelectWidget <- function(problems, name) {
+ widget <- selectInput(name,
+ label = h4("Motion planning problem"),
+ choices = problems)
+ conditionalDisable(widget, length(problems) < 2)
}
-sqlProblemParamSelect <- function(param, val) {
+problemParamSelect <- function(param, val) {
if (val == problemParamsAggregateText || val == problemParamsSeparateText)
# select all
- "1"
+ 1
else
- # select specific parameter value
- sprintf('experiments.%s = "%s"', param, val)
+ # select specific parameter value.
+ # Use fuzzy matching when comparing numbers because precision is lost
+ # when real-valued parameter values are converted to strings for
+ # parameter selection widget.
+ if (regexpr('[-+]?\\d*\\.\\d+|\\d+', val)[1] == -1)
+ sprintf('experiment.%s == %s', param, val)
+ else
+ sprintf('abs(experiment.%s - %s) < 0.0000001', param, val)
}
-sqlPlannerSelect <- function(name) sprintf('plannerConfigs.name = "%s"', name)
-sqlVersionSelect <- function(version) sprintf('experiments.version = "%s"', version)
-problemSelectWidget <- function(con, name) {
- problems <- dbGetQuery(con, "SELECT DISTINCT name FROM experiments")
- problems <- problems$name
- widget <- selectInput(name,
- label = h4("Motion planning problem"),
- choices = problems)
- conditionalDisable(widget, length(problems) < 2)
-}
-problemParams <- function(con) {
- params <- dbGetQuery(con, "PRAGMA table_info(experiments)")
- numParams <- length(params$name)
+# return parameters of parametrized benchmarks if they exist, NULL otherwise
+problemParams <- function(experiments) {
+ params <- tbl_vars(experiments)
+ numParams <- length(params)
if (numParams > 12)
- paramNames <- params[13:numParams,]
+ paramNames <- params[13:numParams]
else
paramNames <- NULL
}
-problemParamValue <- function(prefix, param, input) {
- eval(parse(text=sprintf("input$%s", paste0(prefix, "problemParam", param))))
-}
-problemParamValues <- function(con, prefix, input) {
- params <- problemParams(con)
- values <- lapply(params$name, problemParamValue, prefix=prefix, input=input)
- names(values) <- params$name
+# return values of benchmark parameters
+problemParamValues <- function(experiments, prefix, input) {
+ params <- problemParams(experiments)
+ values <- lapply(params,
+ function(p) eval(parse(text=sprintf("input$%s", paste0(prefix, "problemParam", p)))))
+ names(values) <- params
return(values)
}
+# determine whether a performance attribute should be grouped by a benchmark parameter value
problemParamGroupBy <- function(values) {
grouping <- match(problemParamsSeparateText, values)
if (is.na(grouping))
NULL
else
- names(values)[grouping]
+ sprintf("experiment.%s", names(values)[grouping])
}
-problemParamSelectWidget <- function(name, con, prefix, problem, version) {
- query <- sprintf("SELECT DISTINCT %s AS \"values\" FROM experiments WHERE name=\"%s\" AND version=\"%s\";", name, problem, version)
- values <- dbGetQuery(con, query)
- values <- values$values
+# create a widget for a given benchmark parameter
+problemParamSelectWidget <- function(name, experiments, prefix, problem, version) {
+ values <- (experiments %>%
+ filter(name == problem & version == version) %>%
+ select_(name) %>% distinct() %>% collect())[[name]]
dispName <- gsub("_", " ", name)
internalName <- paste0(prefix, "problemParam", name)
if (length(values)==1)
{
# don't show any widget for parameter if the only value is NA
+ # (this means that the given benchmark parameter is not applicable to the
+ # currently selected benchmark problem)
if (!is.na(values[1]))
# disable selection if there is only value for parameter
disable(selectInput(internalName, label = h6(dispName), choices = values))
@@ -96,37 +108,25 @@ problemParamSelectWidget <- function(name, con, prefix, problem, version) {
selectInput(internalName, label = h6(dispName), choices = append(values,
c(problemParamsAggregateText, problemParamsSeparateText), 0))
}
-
-problemParamSelectWidgets <- function(con, prefix, problem, version) {
- params <- problemParams(con)
+# create widgets for all benchmark parameters
+problemParamSelectWidgets <- function(experiments, prefix, problem, version) {
+ params <- problemParams(experiments)
if (!is.null(params))
- {
- paramWidgets <- lapply(params$name, problemParamSelectWidget,
- con = con, prefix = prefix, problem = problem, version = version)
+ # eparams <- paste0("experiment.", params)
+ # names(eparams) <- gsub("_", " ", params)
div(class="well well-light",
h5("Problem parameters"),
- paramWidgets
+ lapply(params, problemParamSelectWidget,
+ experiments = experiments, prefix = prefix, problem = problem, version = version)
)
- }
-}
-
-numVersions <- function(con) {
- dbGetQuery(con, "SELECT COUNT(DISTINCT version) FROM experiments")
}
-stripLibnamePrefix <- function(str) {
- # assume the version number is the last "word" in the string
- tail(strsplit(str, " ")[[1]], n=1)
-}
-versionSelectWidget <- function(con, name, checkbox) {
- versions <- dbGetQuery(con, "SELECT DISTINCT version FROM experiments")
- versions <- versions$version
+versionSelectWidget <- function(experiments, name, checkbox) {
+ versions <- (experiments %>% select(version) %>% distinct() %>% collect())$version
if (checkbox)
- {
widget <- checkboxGroupInput(name, label = h4("Selected versions"),
choices = versions,
selected = versions)
- }
else
widget <- selectInput(name, label = h4("Version"),
choices = versions,
@@ -138,105 +138,133 @@ versionSelectWidget <- function(con, name, checkbox) {
plannerNameMapping <- function(fullname) {
sub("control_", " ", sub("geometric_", "", fullname))
}
-plannerSelectWidget <- function(con, name, problem, version) {
- query <- sprintf("SELECT DISTINCT plannerConfigs.name AS name FROM plannerConfigs INNER JOIN runs ON plannerConfigs.id = runs.plannerid INNER JOIN experiments ON experiments.id = runs.experimentid WHERE experiments.name=\"%s\" AND experiments.version=\"%s\";", problem, version)
- planners <- dbGetQuery(con, query)
- planners <- unique(unlist(planners$name))
- names(planners) <- sapply(planners, plannerNameMapping)
+plannerSelectWidget <- function(performance, name) {
+ planners <- (performance %>% select(planner.name) %>% distinct() %>% collect())$planner.name
+ names(planners) <- plannerNameMapping(planners)
# select first 4 planners (or all if there are less than 4)
if (length(planners) < 4)
selection <- planners
else
selection <- planners[1:4]
- conditionalDisable(checkboxGroupInput(name, label = h4("Selected planners"),
- choices = planners, selected = selection), length(planners) < 2)
+ #conditionalDisable(
+ checkboxGroupInput(name, label = h4("Selected planners"),
+ choices = planners, selected = selection)#, length(planners) < 2)
}
-perfAttrs <- function(con) {
- dbGetQuery(con, "PRAGMA table_info(runs)")
-}
-perfAttrSelectWidget <- function(con, name) {
- attrs <- perfAttrs(con)
- # strip off first 3 names, which correspond to internal id's
- attrNames <- gsub("_", " ", attrs$name[4:length(attrs$name)])
- if ('time' %in% attrNames)
- selection <- 'time'
+perfAttrSelectWidget <- function(runs, name) {
+ attrs <- tbl_vars(runs)
+ names(attrs) <- gsub("_", " ", gsub("^run\\.", "", attrs))
+ if ('run.time' %in% attrs)
+ selection <- 'run.time'
else
selection <- NULL
+ # strip off first 3 names, which correspond to internal id's
selectInput(name, label = h4("Benchmark attribute"),
- choices = attrNames, selected = selection)
-}
-
-hasProgressData <- function(con) {
- count <- dbGetQuery(con, "SELECT COUNT(*) FROM progress")
- count > 0
+ choices = attrs[4:length(attrs)], selected = selection)
}
-server <- function(input, output, session) {
- con <- reactive({
+shinyServer(function(input, output, session) {
+ # Create a connection to a database. There are three possibilities:
+ # 1. The user is using the default database.
+ # 2. The user has upload their own database.
+ # 3. The user has submitted a benchmark job via the OMPL.app web app and
+ # the user wants to look at the database generated by this job.
+ db <- reactive({
query <- parseQueryString(session$clientData$url_search)
if (is.null(query$user) || is.null(query$job)) {
if (is.null(input$database) || is.null(input$database$datapath))
+ # case 1
database <- defaultDatabase
else
+ # case 2
database <- input$database$datapath
- } else {
+ } else
+ # case 3
database <- paste(sessionsFolder, query$user, query$job, sep="/")
- }
-
if (file.exists(database)) {
- dbConnection <- dbConnect(dbDriver("SQLite"), database)
- ready <- dbExistsTable(dbConnection, "experiments")
- if (!ready) {
- js$refresh()
- } else {
+ db <- src_sqlite(database)
+ # benchmark job may not yet be finished so check that "experiments"
+ # table exists.
+ if ("experiments" %in% src_tbls(db))
+ {
updateTabsetPanel(session, "navbar", selected = "performance")
+ db
+ } else {
+ js$refresh()
+ NULL
}
-
- # TODO: For some reason, have to do this line again, gives error otherwise.
- dbConnection <- dbConnect(dbDriver("SQLite"), database)
- }
- else
+ } else
NULL
})
+ # create variables for tables in database
+ experiments <- reactive({tbl(db(), "experiments")})
+ plannerConfigs <- reactive({tbl(db(), "plannerConfigs")})
+ runs <- reactive({tbl(db(), "runs") %>% tbl.renamer("run")})
+ attrs.names <- reactive({
+ rnames <- tbl_vars(runs())
+ names(rnames) <- gsub("_", " ", gsub("^run\\.", "", rnames))
+ rnames
+ })
+ progress <- reactive({tbl(db(), "progress") %>% tbl.renamer("progress")})
+ progAttrs.names <- reactive({
+ rnames <- tbl_vars(progress())
+ names(rnames) <- gsub("_", " ", gsub("^progress\\.", "", rnames))
+ rnames
+ })
+ enums <- reactive({tbl(db(), "enums")})
+ runs_ext <- reactive({plannerConfigs() %>% tbl.renamer("planner") %>%
+ inner_join(runs(), c("planner.id" = "run.plannerid")) %>%
+ inner_join(experiments() %>% tbl.renamer("experiment"), c("run.experimentid" = "experiment.id"))
+ })
+ performance <- reactive({runs_ext() %>%
+ filter(experiment.name == input$perfProblem & experiment.version == input$perfVersion)
+ })
+ progPerf <- reactive({runs_ext() %>%
+ inner_join(progress(), c("run.id" = "progress.runid")) %>%
+ filter(experiment.name == input$progProblem & experiment.version == input$progVersion)
+ })
+ regrPerf <- reactive({runs_ext() %>%
+ filter(experiment.name == input$regrProblem & experiment.version %in% input$regrVersions)
+ })
+
# Go straight to the database upload page if there is no default database
observe({
- if (is.null(con()))
+ if (is.null(db()))
updateTabsetPanel(session, "navbar", selected = "database")
})
-
- output$perfProblemSelect <- renderUI({ problemSelectWidget(con(), "perfProblem") })
- output$progProblemSelect <- renderUI({ problemSelectWidget(con(), "progProblem") })
- output$regrProblemSelect <- renderUI({ problemSelectWidget(con(), "regrProblem") })
+ problemNames <- reactive({ (experiments() %>% select(name) %>% distinct() %>% collect())$name })
+ output$perfProblemSelect <- renderUI({ problemSelectWidget(problemNames(), "perfProblem") })
+ output$progProblemSelect <- renderUI({ problemSelectWidget(problemNames(), "progProblem") })
+ output$regrProblemSelect <- renderUI({ problemSelectWidget(problemNames(), "regrProblem") })
output$perfProblemParamSelect <- renderUI({
validate(
need(input$perfProblem, 'Select a problem'),
need(input$perfVersion, 'Select a version')
)
- problemParamSelectWidgets(con(), "perf", input$perfProblem, input$perfVersion)
+ problemParamSelectWidgets(experiments(), "perf", input$perfProblem, input$perfVersion)
})
output$progProblemParamSelect <- renderUI({
validate(
need(input$progProblem, 'Select a problem'),
need(input$progVersion, 'Select a version')
)
- problemParamSelectWidgets(con(), "prog", input$progProblem, input$progVersion)
+ problemParamSelectWidgets(experiments(), "prog", input$progProblem, input$progVersion)
})
output$regrProblemParamSelect <- renderUI({
validate(
need(input$regrProblem, 'Select a problem'),
need(input$regrVersions, 'Select a version')
)
- problemParamSelectWidgets(con(), "regr", input$regrProblem, tail(input$regrVersions, n=1))
+ problemParamSelectWidgets(experiments(), "regr", input$regrProblem, tail(input$regrVersions, n=1))
})
output$perfAttrSelect <- renderUI({
- list(
- perfAttrSelectWidget(con(), "perfAttr"),
+ tagList(
+ perfAttrSelectWidget(runs(), "perfAttr"),
checkboxInput('perfShowAdvOptions', 'Show advanced options', FALSE),
conditionalPanel(condition = 'input.perfShowAdvOptions',
div(class="well well-light",
@@ -247,62 +275,68 @@ server <- function(input, output, session) {
)
)
})
- output$regrAttrSelect <- renderUI({ perfAttrSelectWidget(con(), "regrAttr") })
+ output$regrAttrSelect <- renderUI({
+ perfAttrSelectWidget(runs(), "regrAttr")
+ })
output$progAttrSelect <- renderUI({
- progressAttrs <- dbGetQuery(con(), "PRAGMA table_info(progress)")
+ progressAttrs <- tbl_vars(progress())
+ names(progressAttrs) <- gsub("_", " ", gsub("^progress\\.", "", progressAttrs))
# strip off first 2 names, which correspond to an internal id and time
- attrs <- gsub("_", " ", progressAttrs$name[3:length(progressAttrs$name)])
- list(
+ attrs <- progressAttrs[3:length(progressAttrs)]
+ tagList(
conditionalDisable(selectInput("progress", label = h4("Progress attribute"),
choices = attrs
), length(attrs) < 2),
- div(class="well well-light",
- checkboxInput("progressShowMeasurements", label = "Show individual measurements"),
- sliderInput("progressOpacity", label = "Measurement opacity", 0, 100, 50)
+ checkboxInput('progShowAdvOptions', 'Show advanced options', FALSE),
+ conditionalPanel(condition = 'input.progShowAdvOptions',
+ div(class="well well-light",
+ checkboxInput("progressShowMeasurements", label = "Show individual measurements"),
+ sliderInput("progressOpacity", label = "Measurement opacity", 0, 100, 50)
+ )
)
)
})
- output$perfVersionSelect <- renderUI({ versionSelectWidget(con(), "perfVersion", FALSE) })
- output$progVersionSelect <- renderUI({ versionSelectWidget(con(), "progVersion", FALSE) })
- output$regrVersionSelect <- renderUI({ versionSelectWidget(con(), "regrVersions", TRUE) })
+ output$perfVersionSelect <- renderUI({ versionSelectWidget(experiments(), "perfVersion", FALSE) })
+ output$progVersionSelect <- renderUI({ versionSelectWidget(experiments(), "progVersion", FALSE) })
+ output$regrVersionSelect <- renderUI({ versionSelectWidget(experiments(), "regrVersions", TRUE) })
output$perfPlannerSelect <- renderUI({
validate(
need(input$perfProblem, 'Select a problem'),
need(input$perfVersion, 'Select a version')
)
- plannerSelectWidget(con(), "perfPlanners", input$perfProblem, input$perfVersion)
+ plannerSelectWidget(performance(), "perfPlanners")
})
output$progPlannerSelect <- renderUI({
validate(
need(input$progProblem, 'Select a problem'),
need(input$progVersion, 'Select a version')
)
- plannerSelectWidget(con(), "progPlanners", input$progProblem, input$progVersion)
+ plannerSelectWidget(progPerf(), "progPlanners")
})
output$regrPlannerSelect <- renderUI({
validate(
need(input$regrProblem, 'Select a problem'),
need(input$regrVersions, 'Select a version')
)
- plannerSelectWidget(con(), "regrPlanners", input$regrProblem, tail(input$regrVersions, n=1))
+ plannerSelectWidget(regrPerf(), "regrPlanners")
})
output$benchmarkInfo <- renderTable({
- validate(need(con(), noDatabaseText))
+ validate(need(experiments(), noDatabaseText))
validate(need(input$perfVersion, "Select a version on the “Overall performance” page"))
- query <- sprintf("SELECT * FROM experiments WHERE name=\"%s\" AND version=\"%s\"",
- input$perfProblem, input$perfVersion)
- data <- dbGetQuery(con(), query)
- t(data)
+ experiments() %>%
+ filter(name == input$perfProblem & version == input$perfVersion) %>%
+ collect() %>% t()
}, include.colnames=FALSE)
output$plannerConfigs <- renderTable({
- query <- sprintf("SELECT DISTINCT plannerConfigs.name, plannerConfigs.settings FROM plannerConfigs INNER JOIN runs ON plannerConfigs.id = runs.plannerid INNER JOIN experiments ON experiments.id = runs.experimentid WHERE experiments.name=\"%s\" AND experiments.version=\"%s\";",
- input$perfProblem, input$perfVersion)
- dbGetQuery(con(), query)
- }, include.rownames=FALSE)
+ performance() %>%
+ select(planner.name, planner.settings) %>%
+ distinct() %>%
+ collect()
+ }, include.rownames=FALSE, include.colnames=FALSE)
# font selection
fontSelection <- reactive({
@@ -311,41 +345,47 @@ server <- function(input, output, session) {
# plot of overall performance
perfPlot <- reactive({
- paramValues <- problemParamValues(con(), "perf", input)
+ # performance results for parametrized benchmarks can be grouped by parameter values
+ paramValues <- problemParamValues(experiments(), "perf", input)
grouping <- problemParamGroupBy(paramValues)
-
- attribs <- perfAttrs(con())
- attr <- gsub(" ", "_", input$perfAttr)
- simplifiedAttr <- paste("simplified", attr, sep="_")
- includeSimplifiedAttr <- input$perfShowSimplified && simplifiedAttr %in% attribs$name
- # build up query
- query <- sprintf("SELECT plannerConfigs.name AS planner, runs.%s AS attr", attr)
+ # for certain performance metrics we can also include the results after path simplification
+ attribs <- tbl_vars(runs())
+ attr <- input$perfAttr
+ dispAttr <- names(attrs.names()[attrs.names() == attr])
+ simplifiedAttr <- gsub("^run\\.", "run.simplified_", attr)
+ includeSimplifiedAttr <- input$perfShowSimplified && simplifiedAttr %in% attribs
+ # compute the selection of columns and their new names
+ selection <- c("planner" = "planner.name", "attr" = attr)
if (includeSimplifiedAttr)
- query <- sprintf("%s, runs.%s AS simplifiedAttr", query, simplifiedAttr)
+ selection <- c(selection, "simplifiedAttr" = simplifiedAttr)
if (!is.null(grouping))
- query <- sprintf("%s, experiments.%s as \"grouping\"", query, grouping)
- query <- sprintf("%s FROM plannerConfigs INNER JOIN runs ON plannerConfigs.id = runs.plannerid INNER JOIN experiments ON experiments.id = runs.experimentid WHERE experiments.name=\"%s\" AND experiments.version=\"%s\" AND (%s)",
- query,
- input$perfProblem,
- input$perfVersion,
- paste(sapply(input$perfPlanners, sqlPlannerSelect), collapse=" OR "))
+ selection <- c(selection, "grouping" = grouping)
+ # compute selection of rows (add empty string to work around bug if there is only one planner selected)
+ filter_expr <- c(~ planner.name %in% c(input$perfPlanners,''))
+ # for parametrized benchmarks we want only the data that matches all parameters exactly
if (length(paramValues) > 0)
- query <- sprintf("%s AND %s", query,
- paste(mapply(sqlProblemParamSelect, names(paramValues), paramValues), collapse=" AND "))
- data <- dbGetQuery(con(), query)
- data$planner <- factor(data$planner, unique(data$planner), labels = sapply(unique(data$planner), plannerNameMapping))
+ filter_expr <- c(filter_expr, paste(mapply(
+ problemParamSelect, names(paramValues), paramValues), collapse= " & "))
+ # extract the data to be plotted
+ data <- performance() %>%
+ filter_(.dots = filter_expr) %>%
+ select_(.dots = selection) %>%
+ collect()
+ # turn the planner and grouping columns into factors ordered in the same way that they occur in the database.
+ data$planner <- factor(data$planner, unique(data$planner),
+ labels = sapply(unique(data$planner), plannerNameMapping))
if (!is.null(grouping))
data$grouping <- factor(data$grouping)
- if (attribs$type[match(attr, attribs$name)] == "ENUM")
+ # use bar charts for enum types
+ attr2 <- gsub("^run\\.", "", attr)
+ enum <- enums() %>% filter(name == attr2) %>% collect()
+ if (nrow(enum) > 0)
{
- query <- sprintf("SELECT * FROM enums WHERE name=\"%s\";", attr)
- enum <- dbGetQuery(con(), query)
val <- enum$value
names(val) <- enum$description
- attrAsFactor <- factor(data[,match("attr", colnames(data))],
- levels=enum$value)
+ attrAsFactor <- factor(data$attr, levels=val)
levels(attrAsFactor) <- enum$description
- p <- qplot(planner, data=data, geom="histogram", fill=attrAsFactor) +
+ p <- qplot(planner, data = data, geom = "bar", fill = attrAsFactor) +
# labels
theme(legend.title = element_blank(), text = fontSelection())
if (!is.null(grouping))
@@ -356,10 +396,12 @@ server <- function(input, output, session) {
if (includeSimplifiedAttr)
{
# the "all (separate)" case is not handled here
- data <- melt(data, id.vars='planner', measure.vars=c("attr", "simplifiedAttr"))
+ data <- data %>%
+ collect() %>%
+ gather(key, value, c(attr, simplifiedAttr), factor_key=TRUE)
if (input$perfShowAsCDF)
p <- ggplot(data, aes(x = value, color = planner,
- group = interaction(planner, variable), linetype=variable)) +
+ group = interaction(planner, key), linetype=key)) +
# labels
xlab(input$perfAttr) +
ylab('cumulative probability') +
@@ -368,9 +410,9 @@ server <- function(input, output, session) {
stat_ecdf(size = 1) +
scale_linetype_discrete(name = "", labels = c("before simplification", "after simplification"))
else
- p <- ggplot(data, aes(x=planner, y=value, color=variable, fill=variable)) +
+ p <- ggplot(data, aes(x=planner, y=value, color=key, fill=key)) +
# labels
- ylab(input$perfAttr) +
+ ylab(dispAttr) +
theme(legend.title = element_blank(), text = fontSelection()) +
geom_boxplot() +
scale_fill_manual(values = c("#99c9eb", "#ebc999"),
@@ -398,7 +440,7 @@ server <- function(input, output, session) {
if (input$perfShowParameterizedBoxPlots || is.null(grouping)) {
p <- ggplot(data, aes(x = planner, y = attr, group = planner)) +
# labels
- ylab(input$perfAttr) +
+ ylab(dispAttr) +
theme(legend.position = "none", text = fontSelection()) +
# box plots for boolean, integer, and real-valued attributes
geom_boxplot(color = I("#3073ba"), fill = I("#99c9eb"))
@@ -408,7 +450,7 @@ server <- function(input, output, session) {
p <- ggplot(data, aes(x = grouping, y = attr, group = planner, color = planner, fill = planner)) +
# labels
xlab(gsub('_', ' ', grouping)) +
- ylab(input$perfAttr) +
+ ylab(dispAttr) +
theme(legend.title = element_blank(), text = fontSelection()) +
geom_smooth(method = "loess") +
scale_x_discrete(expand=c(0.05,0))
@@ -436,8 +478,7 @@ server <- function(input, output, session) {
)
output$perfDownloadRdata <- downloadHandler(filename = 'perfplot.RData',
content = function(file) {
- perfplot <- perfPlot()
- save(perfplot, file = file)
+ save(perfplot(), file = file)
}
)
output$perfMissingDataTable <- renderTable({
@@ -447,28 +488,24 @@ server <- function(input, output, session) {
need(input$perfAttr, 'Select a benchmark attribute'),
need(input$perfPlanners, 'Select some planners')
)
- attr <- gsub(" ", "_", input$perfAttr)
- paramValues <- problemParamValues(con(), "perf", input)
+ attr <- input$perfAttr
+ # performance results for parametrized benchmarks can be grouped by parameter values
+ paramValues <- problemParamValues(experiments(), "perf", input)
grouping <- problemParamGroupBy(paramValues)
- query <- sprintf("SELECT plannerConfigs.name AS planner")
if (!is.null(grouping))
- query <- sprintf("%s, experiments.%s as \"%s\"", query, grouping, grouping)
- query <- sprintf("%s, SUM(runs.%s IS NULL) AS missing, COUNT(*) AS total FROM plannerConfigs INNER JOIN runs ON plannerConfigs.id = runs.plannerid INNER JOIN experiments ON experiments.id = runs.experimentid WHERE experiments.name=\"%s\" AND experiments.version=\"%s\" AND (%s)",
- query,
- attr,
- input$perfProblem,
- input$perfVersion,
- paste(sapply(input$perfPlanners, sqlPlannerSelect), collapse=" OR "))
- if (length(paramValues) > 0)
- query <- sprintf("%s AND %s", query,
- paste(mapply(sqlProblemParamSelect, names(paramValues), paramValues), collapse=" AND "))
- if (is.null(grouping))
- query <- sprintf("%s GROUP BY plannerConfigs.name;", query)
+ group_order <- c("planner.name", grouping)
else
- query <- sprintf("%s GROUP BY plannerConfigs.name, experiments.%s;", query, grouping)
- data <- dbGetQuery(con(), query)
- data$planner <- factor(data$planner, unique(data$planner), labels = sapply(unique(data$planner), plannerNameMapping))
- data
+ group_order <- c("planner.name")
+ data <- performance() %>%
+ # add empty string to work around bug if there is only one planner selected
+ filter_(.dots = ~ planner.name %in% c(input$perfPlanners,'')) %>%
+ group_by_(.dots = group_order) %>%
+ select_(.dots = c("attr" = attr)) %>%
+ mutate_(missing = ~ is.na(attr)) %>%
+ summarise(missing = sum(missing), total = n()) %>%
+ collect()
+ data$planner.name <- factor(data$planner.name, unique(data$planner.name), labels = sapply(unique(data$planner.name), plannerNameMapping))
+ data %>% rename(planner=planner.name)
}, include.rownames=FALSE)
# progress plot
@@ -479,30 +516,34 @@ server <- function(input, output, session) {
need(input$progress, 'Select a benchmark attribute'),
need(input$progPlanners, 'Select some planners')
)
- paramValues <- problemParamValues(con(), "prog", input)
+ # performance results for parametrized benchmarks can be grouped by parameter values
+ paramValues <- problemParamValues(experiments(), "prog", input)
grouping <- problemParamGroupBy(paramValues)
attr <- gsub(" ", "_", input$progress)
- # build up query
- query <- sprintf("SELECT plannerConfigs.name AS planner, progress.time, progress.%s AS attr", attr)
+ selection <- c("planner" = "planner.name", "time" = "progress.time", "attr" = attr)
if (!is.null(grouping))
- query <- sprintf("%s, experiments.%s as \"grouping\"", query, grouping)
- query <- sprintf("%s FROM plannerConfigs INNER JOIN runs ON plannerConfigs.id = runs.plannerid INNER JOIN experiments ON experiments.id = runs.experimentid INNER JOIN progress ON progress.runid = runs.id WHERE experiments.name=\"%s\" AND experiments.version=\"%s\" AND progress.%s IS NOT NULL AND (%s)",
- query,
- input$progProblem,
- input$progVersion,
- attr,
- paste(sapply(input$progPlanners, sqlPlannerSelect), collapse=" OR "))
+ selection <- c(selection, "grouping" = grouping)
+ # compute selection of rows (add empty string to work around bug if there is only one planner selected)
+ filter_expr <- c(~ planner.name %in% c(input$progPlanners,''), ~ !is.na(attr))
if (length(paramValues) > 0)
- query <- sprintf("%s AND %s", query,
- paste(mapply(sqlProblemParamSelect, names(paramValues), paramValues), collapse=" AND "))
- data <- dbGetQuery(con(), query)
- data$planner <- factor(data$planner, unique(data$planner), labels = sapply(unique(data$planner), plannerNameMapping))
+ filter_expr <- c(filter_expr, paste(mapply(
+ problemParamSelect, names(paramValues), paramValues), collapse= " & "))
+ # extract the data to be plotted
+ data <- progPerf() %>%
+ filter_(.dots = filter_expr) %>%
+ select_(.dots = selection) %>%
+ collect()
+
+ # turn the planner and grouping columns into factors ordered in the same way that they occur in the database.
+ data$planner <- factor(data$planner, unique(data$planner),
+ labels = sapply(unique(data$planner), plannerNameMapping))
if (!is.null(grouping))
data$grouping <- factor(data$grouping)
list(data = data, grouping = grouping)
})
progPlot <- reactive({
attr <- gsub(" ", "_", input$progress)
+ dispAttr <- names(progAttrs.names()[progAttrs.names() == attr])
progdata <- progPlotData()
data <- progdata$data
grouping <- progdata$grouping
@@ -510,7 +551,7 @@ server <- function(input, output, session) {
p <- ggplot(data, aes(x = time, y = attr, group = planner, color = planner, fill = planner)) +
# labels
xlab('time (s)') +
- ylab(input$progress) +
+ ylab(dispAttr) +
theme(text = fontSelection()) +
# smooth interpolating curve
geom_smooth(method = "gam") +
@@ -524,6 +565,8 @@ server <- function(input, output, session) {
})
output$progPlot <- renderPlot({ progPlot() })
progNumMeasurementsPlot <- reactive({
+ attr <- gsub(" ", "_", input$progress)
+ dispAttr <- names(progAttrs.names()[progAttrs.names() == attr])
progdata <- progPlotData()
data <- progdata$data
grouping <- progdata$grouping
@@ -532,7 +575,7 @@ server <- function(input, output, session) {
p <- ggplot(data, aes(x = time, group = planner, color = planner)) +
# labels
xlab('time (s)') +
- ylab(sprintf("# measurements for %s", input$progress)) +
+ ylab(sprintf("# measurements for %s", dispAttr)) +
theme(text = fontSelection()) +
geom_freqpoly(binwidth=1) +
coord_cartesian(xlim = c(0, trunc(max(data$time))))
@@ -560,36 +603,51 @@ server <- function(input, output, session) {
# regression plot
regrPlot <- reactive({
- regrParamValues <- problemParamValues(con(), "regr", input)
- grouping <- problemParamGroupBy(regrParamValues)
- attr <- gsub(" ", "_", input$regrAttr)
- # build up query
- query <- sprintf("SELECT plannerConfigs.name AS name, runs.%s AS attr, experiments.version", attr)
+ paramValues <- problemParamValues(experiments(), "regr", input)
+ grouping <- problemParamGroupBy(paramValues)
+ attr <- input$regrAttr
+ dispAttr <- names(attrs.names()[attrs.names() == attr])
+ # compute the selection of columns and their new names
+ selection <- c("planner" = "planner.name",
+ "attr" = attr,
+ "version" = "experiment.version")
if (!is.null(grouping))
- query <- sprintf("%s, experiments.%s as \"grouping\"", query, grouping)
- query <- sprintf("%s FROM plannerConfigs INNER JOIN runs ON plannerConfigs.id = runs.plannerid INNER JOIN experiments ON experiments.id = runs.experimentid WHERE experiments.name=\"%s\" AND (%s) AND (%s)",
- query,
- input$regrProblem,
- paste(sapply(input$regrPlanners, sqlPlannerSelect), collapse=" OR "),
- paste(sapply(input$regrVersions, sqlVersionSelect), collapse=" OR "))
+ selection <- c(selection, "grouping" = grouping)
+ # compute selection of rows (add empty string to work around bug if there is only one planner selected)
+ filter_expr <- c(~ planner.name %in% c(input$regrPlanners,''),
+ ~ experiment.version %in% c(input$regrVersions,''))
+ # for parametrized benchmarks we want only the data that matches all parameters exactly
if (length(paramValues) > 0)
- query <- sprintf("%s AND %s", query,
- paste(mapply(sqlProblemParamSelect, names(paramValues), paramValues), collapse=" AND "))
- data <- dbGetQuery(con(), query)
+ filter_expr <- c(filter_expr, paste(mapply(
+ problemParamSelect, names(paramValues), paramValues), collapse= " & "))
+ # extract the data to be plotted
+ data <- regrPerf() %>%
+ filter_(.dots = filter_expr) %>%
+ select_(.dots = selection) %>%
+ collect()
+ # turn the planner and grouping columns into factors ordered in the same way that they occur in the database.
+ data$planner <- factor(data$planner, unique(data$planner),
+ labels = sapply(unique(data$planner), plannerNameMapping))
+ if (!is.null(grouping))
+ data$grouping <- factor(data$grouping)
# strip "OMPL " prefix, so we can fit more labels on the X-axis
- data$version <- sapply(data$version, stripLibnamePrefix)
+ data$version <- sapply(data$version,
+ function(str) {
+ # assume the version number is the last "word" in the string
+ tail(strsplit(str, " ")[[1]], n=1)
+ })
# order by order listed in data frame (i.e., "0.9.*" before "0.10.*")
data$version <- factor(data$version, unique(data$version))
- data$name <- factor(data$name, unique(data$name), labels = sapply(unique(data$name), plannerNameMapping))
- p <- ggplot(data, aes(x = version, y = attr, fill = name, group = name)) +
+ p <- ggplot(data, aes(x = version, y = attr, fill = planner, group = planner)) +
# labels
- ylab(input$regrAttr) +
+ ylab(dispAttr) +
theme(legend.title = element_blank(), text = fontSelection()) +
# plot mean and error bars
stat_summary(fun.data = "mean_cl_boot", geom="bar", position = position_dodge()) +
stat_summary(fun.data = "mean_cl_boot", geom="errorbar", position = position_dodge())
if (!is.null(grouping))
p <- p + facet_grid(grouping ~ .)
+ list(plot = p, query = query)
})
output$regrPlot <- renderPlot({
validate(
@@ -598,24 +656,24 @@ server <- function(input, output, session) {
need(input$regrAttr, 'Select a benchmark attribute'),
need(input$regrPlanners, 'Select some planners')
)
- print(regrPlot())
+ print(regrPlot()$plot)
})
output$regrDownloadPlot <- downloadHandler(filename = 'regrplot.pdf',
content = function(file) {
pdf(file=file, width=input$paperWidth, height=input$paperHeight)
- print(regrPlot())
+ print(regrPlot()$plot)
dev.off()
}
)
output$regrDownloadRdata <- downloadHandler(filename = 'regrplot.RData',
content = function(file) {
- regrplot <- regrPlot()
+ regrplot <- regrPlot()$plot
save(regrplot, file = file)
}
)
output$performancePage <- renderUI({
- validate(need(con(), noDatabaseText))
+ validate(need(performance(), noDatabaseText))
sidebarLayout(
sidebarPanel(
uiOutput("perfProblemSelect"),
@@ -634,8 +692,8 @@ server <- function(input, output, session) {
)
})
output$progressPage <- renderUI({
- validate(need(con(), noDatabaseText))
- validate(need(hasProgressData(con()), "There is no progress data in this database."))
+ validate(need(db(), noDatabaseText))
+ validate(need(progress() %>% tally() %>% collect() > 0, "There is no progress data in this database."))
sidebarLayout(
sidebarPanel(
uiOutput("progProblemSelect"),
@@ -654,8 +712,9 @@ server <- function(input, output, session) {
})
output$regressionPage <- renderUI({
- validate(need(con(), noDatabaseText))
- validate(need(numVersions(con())>1, "Only one version of OMPL was used for the benchmarks."))
+ validate(need(db(), noDatabaseText))
+ validate(need(experiments() %>% select(version) %>% distinct() %>% tally() %>% collect() > 1,
+ "Only one version of OMPL was used for the benchmarks."))
sidebarLayout(
sidebarPanel(
uiOutput("regrProblemSelect"),
@@ -673,89 +732,12 @@ server <- function(input, output, session) {
})
output$dbinfoPage <- renderUI({
- validate(need(con(), noDatabaseText))
+ validate(need(db(), noDatabaseText))
tabsetPanel(
tabPanel("Benchmark setup", tableOutput("benchmarkInfo")),
tabPanel("Planner Configurations", tableOutput("plannerConfigs"))
)
})
-}
+})
-ui <- navbarPage("Planner Arena",
- useShinyjs(),
- extendShinyjs(script = "www/plannerarena.js"),
- tabPanel("Overall performance",
- uiOutput("performancePage"),
- value="performance",
- icon=icon("bar-chart")),
- tabPanel("Progress",
- uiOutput("progressPage"),
- value="progress",
- icon=icon("area-chart")),
- tabPanel("Regression",
- uiOutput("regressionPage"),
- value="regression",
- icon=icon("bar-chart")),
- tabPanel("Database info",
- uiOutput('dbinfoPage'),
- value="dbinfo",
- icon=icon("info-circle")),
- tabPanel("Change database",
- div(class="row",
- div(class="col-sm-10 col-sm-offset-1",
- fileInput("database",
- label = h2("Upload benchmark database"),
- accept = c("application/x-sqlite3", ".db")
- ),
- h2("Default benchmark database"),
- tags$ul(
- tags$li(a(href="javascript:history.go(0)", "Reset to default database")),
- tags$li(a(href="benchmark.db", "Download default database"))
- )
- )
- ),
- value="database",
- icon=icon("database")),
- tabPanel("Help",
- div(class="row",
- div(class="col-sm-10 col-sm-offset-1",
- includeMarkdown("www/help.md")
- )
- ),
- value="help",
- icon=icon("question-circle")),
- tabPanel("Settings",
- div(class="row",
- div(class="col-sm-10 col-sm-offset-1",
- h2("Plot settings"),
- h3("Font settings"),
- selectInput("fontFamily",
- label = "Font family",
- choices = c("Courier", "Helvetica", "Palatino", "Times"),
- selected ="Helvetica"),
- numericInput("fontSize", "Font size", 20, min=1, max=100),
- h3("PDF export paper size (in inches)"),
- numericInput("paperWidth", "Width", 12, min=1, max=50),
- numericInput("paperHeight", "Height", 8, min=1, max=50)
- )
- ),
- value="settings",
- icon=icon("gear")),
- id = "navbar",
- header = tags$link(rel="stylesheet", type="text/css", href="plannerarena.css"),
- footer = div(class="footer",
- div(class="container",
- p(
- a(href="http://www.kavrakilab.org", "Physical and Biological Computing Group"),
- "•",
- a(href="http://www.cs.rice.edu", "Department of Computer Science"),
- "•",
- a(href="http://www.rice.edu", "Rice University")
- )
- ),
- includeScript('www/ga.js')
- ),
- inverse = TRUE
- )
-shinyApp(ui = ui, server = server)
diff --git a/scripts/plannerarena/ui.R b/scripts/plannerarena/ui.R
new file mode 100644
index 0000000..674c674
--- /dev/null
+++ b/scripts/plannerarena/ui.R
@@ -0,0 +1,112 @@
+######################################################################
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2016, Rice University
+# 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 Rice University 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: Mark Moll
+
+shinyUI(navbarPage("Planner Arena",
+ useShinyjs(),
+ extendShinyjs(script = "www/plannerarena.js"),
+ tabPanel("Overall performance",
+ uiOutput("performancePage"),
+ value="performance",
+ icon=icon("bar-chart")),
+ tabPanel("Progress",
+ uiOutput("progressPage"),
+ value="progress",
+ icon=icon("area-chart")),
+ tabPanel("Regression",
+ uiOutput("regressionPage"),
+ value="regression",
+ icon=icon("bar-chart")),
+ tabPanel("Database info",
+ uiOutput('dbinfoPage'),
+ value="dbinfo",
+ icon=icon("info-circle")),
+ tabPanel("Change database",
+ div(class="row",
+ div(class="col-sm-10 col-sm-offset-1",
+ fileInput("database",
+ label = h2("Upload benchmark database"),
+ accept = c("application/x-sqlite3", ".db")
+ ),
+ h2("Default benchmark database"),
+ tags$ul(
+ tags$li(a(href="javascript:history.go(0)", "Reset to default database")),
+ tags$li(a(href="benchmark.db", "Download default database"))
+ )
+ )
+ ),
+ value="database",
+ icon=icon("database")),
+ tabPanel("Help",
+ div(class="row",
+ div(class="col-sm-10 col-sm-offset-1",
+ includeMarkdown("www/help.md")
+ )
+ ),
+ value="help",
+ icon=icon("question-circle")),
+ tabPanel("Settings",
+ div(class="row",
+ div(class="col-sm-10 col-sm-offset-1",
+ h2("Plot settings"),
+ h3("Font settings"),
+ selectInput("fontFamily",
+ label = "Font family",
+ choices = c("Courier", "Helvetica", "Palatino", "Times"),
+ selected ="Helvetica"),
+ numericInput("fontSize", "Font size", 20, min=1, max=100),
+ h3("PDF export paper size (in inches)"),
+ numericInput("paperWidth", "Width", 12, min=1, max=50),
+ numericInput("paperHeight", "Height", 8, min=1, max=50)
+ )
+ ),
+ value="settings",
+ icon=icon("gear")),
+ id = "navbar",
+ header = tags$link(rel="stylesheet", type="text/css", href="plannerarena.css"),
+ footer = div(class="footer",
+ div(class="container",
+ p(
+ a(href="http://www.kavrakilab.org", "Physical and Biological Computing Group"),
+ "•",
+ a(href="http://www.cs.rice.edu", "Department of Computer Science"),
+ "•",
+ a(href="http://www.rice.edu", "Rice University")
+ )
+ ),
+ includeScript('www/ga.js')
+ ),
+ inverse = TRUE
+))
diff --git a/scripts/plannerarena/www/plannerarena.css b/scripts/plannerarena/www/plannerarena.css
index e85a5b9..95a3f67 100644
--- a/scripts/plannerarena/www/plannerarena.css
+++ b/scripts/plannerarena/www/plannerarena.css
@@ -33,7 +33,7 @@ div.checkbox {
}
.table-bordered { border: none; }
.table-bordered th, .table-bordered td { border-left: none; }
-.table tr:first-child > td, .table tr:first-child > th {
+.table tr:first-child > th {
background-color: #666;
color: #fff;
}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4cf8ab7..bf91d64 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -7,38 +7,3 @@ install(DIRECTORY ompl/ DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/ompl${OMPL_INST
REGEX "/src$" EXCLUDE
REGEX "/doc$" EXCLUDE
REGEX "/tests$" EXCLUDE)
-
-if(${Boost_VERSION} LESS 105300)
- install(DIRECTORY external/omplext_odeint/
- DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/omplext_odeint${OMPL_INSTALL_SUFFIX}"
- COMPONENT ompl)
-endif()
-
-find_program(CURL curl)
-if(CURL)
- set(DOWNLOAD_CMD "${CURL} --location-trusted")
-else()
- find_program(WGET wget)
- if (WGET)
- set(DOWNLOAD_CMD "${WGET} --no-check-certificate -O -")
- else()
- message(WARNING "Neither curl nor wget is installed. Install either one of those programs for \"make installpyplusplus\" to succeed.")
- endif()
-endif()
-
-# Add target to install Py++ and its dependencies pygccxml and gccxml
-if (APPLE)
- # need to compile gccxml with llvm-gcc-4.2 instead of clang
- set(CMAKE_GCCXML_ARGS "${CMAKE_GCCXML_ARGS} -DCMAKE_C_COMPILER=llvm-gcc-4.2 -DCMAKE_CXX_COMPILER=llvm-g++-4.2")
-endif()
-if(WIN32)
- configure_file("${CMAKE_CURRENT_SOURCE_DIR}/external/installPyPlusPlus.bat.in"
- "${CMAKE_CURRENT_SOURCE_DIR}/external/installPyPlusPlus.bat" @ONLY)
- add_custom_target(installpyplusplus
- COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/external/installPyPlusPlus.bat" VERBATIM)
-else(WIN32)
- configure_file("${CMAKE_CURRENT_SOURCE_DIR}/external/installPyPlusPlus.sh.in"
- "${CMAKE_CURRENT_SOURCE_DIR}/external/installPyPlusPlus.sh" @ONLY)
- add_custom_target(installpyplusplus
- COMMAND "sh" "${CMAKE_CURRENT_SOURCE_DIR}/external/installPyPlusPlus.sh")
-endif(WIN32)
diff --git a/src/boost/python/converter/registered.hpp b/src/boost/python/converter/registered.hpp
new file mode 100644
index 0000000..8039187
--- /dev/null
+++ b/src/boost/python/converter/registered.hpp
@@ -0,0 +1,104 @@
+// Copyright David Abrahams 2002.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef REGISTERED_DWA2002710_HPP
+# define REGISTERED_DWA2002710_HPP
+# include <boost/python/type_id.hpp>
+# include <boost/python/converter/registry.hpp>
+# include <boost/python/converter/registrations.hpp>
+# include <boost/type_traits/transform_traits.hpp>
+# include <boost/type_traits/cv_traits.hpp>
+# include <boost/type_traits/is_void.hpp>
+# include <boost/detail/workaround.hpp>
+# include <boost/python/type_id.hpp>
+# include <boost/type.hpp>
+# include <memory>
+
+namespace boost { namespace python { namespace converter {
+
+struct registration;
+
+namespace detail
+{
+ template <class T>
+ struct registered_base
+ {
+ static registration const& converters;
+ };
+}
+
+template <class T>
+struct registered
+ : detail::registered_base<
+ typename add_reference<
+ typename add_cv<T>::type
+ >::type
+ >
+{
+};
+
+# if !BOOST_WORKAROUND(BOOST_MSVC, BOOST_TESTED_AT(1310))
+// collapses a few more types to the same static instance. MSVC7.1
+// fails to strip cv-qualification from array types in typeid. For
+// some reason we can't use this collapse there or array converters
+// will not be found.
+template <class T>
+struct registered<T&>
+ : registered<T> {};
+# endif
+
+//
+// implementations
+//
+namespace detail
+{
+ inline void
+ register_shared_ptr0(...)
+ {
+ }
+
+ template <class T>
+ inline void
+ register_shared_ptr0(std::shared_ptr<T>*)
+ {
+ registry::lookup_shared_ptr(type_id<std::shared_ptr<T> >());
+ }
+
+ template <class T>
+ inline void
+ register_shared_ptr1(T const volatile*)
+ {
+ detail::register_shared_ptr0((T*)0);
+ }
+
+ template <class T>
+ inline registration const&
+ registry_lookup2(T&(*)())
+ {
+ detail::register_shared_ptr1((T*)0);
+ return registry::lookup(type_id<T&>());
+ }
+
+ template <class T>
+ inline registration const&
+ registry_lookup1(type<T>)
+ {
+ return registry_lookup2((T(*)())0);
+ }
+
+ inline registration const&
+ registry_lookup1(type<const volatile void>)
+ {
+ detail::register_shared_ptr1((void*)0);
+ return registry::lookup(type_id<void>());
+ }
+
+ template <class T>
+ registration const& registered_base<T>::converters = detail::registry_lookup1(type<T>());
+
+}
+
+}}} // namespace boost::python::converter
+
+#endif // REGISTERED_DWA2002710_HPP
diff --git a/src/boost/python/converter/shared_ptr_from_python.hpp b/src/boost/python/converter/shared_ptr_from_python.hpp
new file mode 100644
index 0000000..2262ead
--- /dev/null
+++ b/src/boost/python/converter/shared_ptr_from_python.hpp
@@ -0,0 +1,63 @@
+// Copyright David Abrahams 2002.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef SHARED_PTR_FROM_PYTHON_DWA20021130_HPP
+# define SHARED_PTR_FROM_PYTHON_DWA20021130_HPP
+
+# include <boost/python/handle.hpp>
+# include <boost/python/converter/shared_ptr_deleter.hpp>
+# include <boost/python/converter/from_python.hpp>
+# include <boost/python/converter/rvalue_from_python_data.hpp>
+# include <boost/python/converter/registered.hpp>
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+# include <boost/python/converter/pytype_function.hpp>
+#endif
+# include <memory>
+
+namespace boost { namespace python { namespace converter {
+
+template <class T>
+struct shared_ptr_from_python
+{
+ shared_ptr_from_python()
+ {
+ converter::registry::insert(&convertible, &construct, type_id<std::shared_ptr<T> >()
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+ , &converter::expected_from_python_type_direct<T>::get_pytype
+#endif
+ );
+ }
+
+ private:
+ static void* convertible(PyObject* p)
+ {
+ if (p == Py_None)
+ return p;
+
+ return converter::get_lvalue_from_python(p, registered<T>::converters);
+ }
+
+ static void construct(PyObject* source, rvalue_from_python_stage1_data* data)
+ {
+ void* const storage = ((converter::rvalue_from_python_storage<std::shared_ptr<T> >*)data)->storage.bytes;
+ // Deal with the "None" case.
+ if (data->convertible == source)
+ new (storage) std::shared_ptr<T>();
+ else
+ {
+ std::shared_ptr<void> hold_convertible_ref_count(
+ (void*)0, shared_ptr_deleter(handle<>(borrowed(source))) );
+ // use aliasing constructor
+ new (storage) std::shared_ptr<T>(
+ hold_convertible_ref_count,
+ static_cast<T*>(data->convertible));
+ }
+
+ data->convertible = storage;
+ }
+};
+
+}}} // namespace boost::python::converter
+
+#endif // SHARED_PTR_FROM_PYTHON_DWA20021130_HPP
diff --git a/src/boost/python/converter/shared_ptr_to_python.hpp b/src/boost/python/converter/shared_ptr_to_python.hpp
new file mode 100644
index 0000000..bed71bd
--- /dev/null
+++ b/src/boost/python/converter/shared_ptr_to_python.hpp
@@ -0,0 +1,28 @@
+// Copyright David Abrahams 2003.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef SHARED_PTR_TO_PYTHON_DWA2003224_HPP
+# define SHARED_PTR_TO_PYTHON_DWA2003224_HPP
+
+# include <boost/python/refcount.hpp>
+# include <boost/python/converter/shared_ptr_deleter.hpp>
+# include <memory>
+# include <boost/get_pointer.hpp>
+
+namespace boost { namespace python { namespace converter {
+
+template <class T>
+PyObject* shared_ptr_to_python(std::shared_ptr<T> const& x)
+{
+ if (!x)
+ return python::detail::none();
+ else if (shared_ptr_deleter* d = std::get_deleter<shared_ptr_deleter>(x))
+ return incref( get_pointer( d->owner ) );
+ else
+ return converter::registered<std::shared_ptr<T> const&>::converters.to_python(&x);
+}
+
+}}} // namespace boost::python::converter
+
+#endif // SHARED_PTR_TO_PYTHON_DWA2003224_HPP
diff --git a/src/boost/python/detail/is_shared_ptr.hpp b/src/boost/python/detail/is_shared_ptr.hpp
new file mode 100755
index 0000000..6bc2673
--- /dev/null
+++ b/src/boost/python/detail/is_shared_ptr.hpp
@@ -0,0 +1,17 @@
+// Copyright David Abrahams 2003.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef IS_SHARED_PTR_DWA2003224_HPP
+# define IS_SHARED_PTR_DWA2003224_HPP
+
+# include <boost/python/detail/is_xxx.hpp>
+# include <memory>
+
+namespace boost { namespace python { namespace detail {
+
+BOOST_PYTHON_IS_XXX_DEF(shared_ptr, std::shared_ptr, 1)
+
+}}} // namespace boost::python::detail
+
+#endif // IS_SHARED_PTR_DWA2003224_HPP
diff --git a/src/boost/python/detail/is_xxx.hpp b/src/boost/python/detail/is_xxx.hpp
new file mode 100644
index 0000000..9ddfafd
--- /dev/null
+++ b/src/boost/python/detail/is_xxx.hpp
@@ -0,0 +1,13 @@
+// Copyright David Abrahams 2005.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef IS_XXX_DWA2003224_HPP
+# define IS_XXX_DWA2003224_HPP
+
+# include <boost/detail/is_xxx.hpp>
+
+# define BOOST_PYTHON_IS_XXX_DEF(name, qualified_name, nargs) \
+ BOOST_DETAIL_IS_XXX_DEF(name, qualified_name, nargs)
+
+#endif // IS_XXX_DWA2003224_HPP
diff --git a/src/boost/python/detail/value_is_shared_ptr.hpp b/src/boost/python/detail/value_is_shared_ptr.hpp
new file mode 100644
index 0000000..5fa44c1
--- /dev/null
+++ b/src/boost/python/detail/value_is_shared_ptr.hpp
@@ -0,0 +1,17 @@
+// Copyright David Abrahams 2003.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef VALUE_IS_SHARED_PTR_DWA2003224_HPP
+# define VALUE_IS_SHARED_PTR_DWA2003224_HPP
+
+# include <boost/python/detail/value_is_xxx.hpp>
+# include <memory>
+
+namespace boost { namespace python { namespace detail {
+
+BOOST_PYTHON_VALUE_IS_XXX_DEF(shared_ptr, std::shared_ptr, 1)
+
+}}} // namespace boost::python::detail
+
+#endif // VALUE_IS_SHARED_PTR_DWA2003224_HPP
diff --git a/src/boost/python/detail/value_is_xxx.hpp b/src/boost/python/detail/value_is_xxx.hpp
new file mode 100644
index 0000000..c3d77ef
--- /dev/null
+++ b/src/boost/python/detail/value_is_xxx.hpp
@@ -0,0 +1,32 @@
+// Copyright David Abrahams 2003.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef VALUE_IS_XXX_DWA2003224_HPP
+# define VALUE_IS_XXX_DWA2003224_HPP
+
+# include <boost/config.hpp>
+# include <boost/mpl/bool.hpp>
+# include <boost/preprocessor/enum_params.hpp>
+
+
+# include <boost/type_traits/remove_reference.hpp>
+# include <boost/type_traits/remove_cv.hpp>
+# include <boost/python/detail/is_xxx.hpp>
+
+# define BOOST_PYTHON_VALUE_IS_XXX_DEF(name, qualified_name, nargs) \
+template <class X_> \
+struct value_is_##name \
+{ \
+ BOOST_PYTHON_IS_XXX_DEF(name,qualified_name,nargs) \
+ BOOST_STATIC_CONSTANT(bool, value = is_##name< \
+ typename remove_cv< \
+ typename remove_reference<X_>::type \
+ >::type \
+ >::value); \
+ typedef mpl::bool_<value> type; \
+ \
+};
+
+
+#endif // VALUE_IS_XXX_DWA2003224_HPP
diff --git a/src/boost/python/object/inheritance.hpp b/src/boost/python/object/inheritance.hpp
new file mode 100644
index 0000000..883bcdc
--- /dev/null
+++ b/src/boost/python/object/inheritance.hpp
@@ -0,0 +1,132 @@
+// Copyright David Abrahams 2002.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef INHERITANCE_DWA200216_HPP
+# define INHERITANCE_DWA200216_HPP
+
+# include <boost/python/type_id.hpp>
+# include <memory>
+# include <boost/mpl/if.hpp>
+# include <boost/type_traits/is_polymorphic.hpp>
+# include <boost/type_traits/is_base_and_derived.hpp>
+# include <boost/detail/workaround.hpp>
+
+namespace boost { namespace python { namespace objects {
+
+typedef type_info class_id;
+using python::type_id;
+
+// Types used to get address and id of most derived type
+typedef std::pair<void*,class_id> dynamic_id_t;
+typedef dynamic_id_t (*dynamic_id_function)(void*);
+
+BOOST_PYTHON_DECL void register_dynamic_id_aux(
+ class_id static_id, dynamic_id_function get_dynamic_id);
+
+BOOST_PYTHON_DECL void add_cast(
+ class_id src_t, class_id dst_t, void* (*cast)(void*), bool is_downcast);
+
+//
+// a generator with an execute() function which, given a source type
+// and a pointer to an object of that type, returns its most-derived
+// /reachable/ type identifier and object pointer.
+//
+
+// first, the case where T has virtual functions
+template <class T>
+struct polymorphic_id_generator
+{
+ static dynamic_id_t execute(void* p_)
+ {
+ T* p = static_cast<T*>(p_);
+ return std::make_pair(dynamic_cast<void*>(p), class_id(typeid(*p)));
+ }
+};
+
+// now, the non-polymorphic case.
+template <class T>
+struct non_polymorphic_id_generator
+{
+ static dynamic_id_t execute(void* p_)
+ {
+ return std::make_pair(p_, python::type_id<T>());
+ }
+};
+
+// Now the generalized selector
+template <class T>
+struct dynamic_id_generator
+ : mpl::if_<
+ boost::is_polymorphic<T>
+ , boost::python::objects::polymorphic_id_generator<T>
+ , boost::python::objects::non_polymorphic_id_generator<T>
+ >
+{};
+
+// Register the dynamic id function for T with the type-conversion
+// system.
+template <class T>
+void register_dynamic_id(T* = 0)
+{
+ typedef typename dynamic_id_generator<T>::type generator;
+ register_dynamic_id_aux(
+ python::type_id<T>(), &generator::execute);
+}
+
+//
+// a generator with an execute() function which, given a void*
+// pointing to an object of type Source will attempt to convert it to
+// an object of type Target.
+//
+
+template <class Source, class Target>
+struct dynamic_cast_generator
+{
+ static void* execute(void* source)
+ {
+ return dynamic_cast<Target*>(
+ static_cast<Source*>(source));
+ }
+
+};
+
+template <class Source, class Target>
+struct implicit_cast_generator
+{
+ static void* execute(void* source)
+ {
+ Target* result = static_cast<Source*>(source);
+ return result;
+ }
+};
+
+template <class Source, class Target>
+struct cast_generator
+ : mpl::if_<
+ is_base_and_derived<Target,Source>
+ , implicit_cast_generator<Source,Target>
+ , dynamic_cast_generator<Source,Target>
+ >
+{
+};
+
+template <class Source, class Target>
+inline void register_conversion(
+ bool is_downcast = ::boost::is_base_and_derived<Source,Target>::value
+ // These parameters shouldn't be used; they're an MSVC bug workaround
+ , Source* = 0, Target* = 0)
+{
+ typedef typename cast_generator<Source,Target>::type generator;
+
+ add_cast(
+ python::type_id<Source>()
+ , python::type_id<Target>()
+ , &generator::execute
+ , is_downcast
+ );
+}
+
+}}} // namespace boost::python::object
+
+#endif // INHERITANCE_DWA200216_HPP
diff --git a/src/boost/python/to_python_indirect.hpp b/src/boost/python/to_python_indirect.hpp
new file mode 100644
index 0000000..406d7aa
--- /dev/null
+++ b/src/boost/python/to_python_indirect.hpp
@@ -0,0 +1,109 @@
+// Copyright David Abrahams 2002.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef TO_PYTHON_INDIRECT_DWA200221_HPP
+# define TO_PYTHON_INDIRECT_DWA200221_HPP
+
+# include <boost/python/detail/prefix.hpp>
+
+# include <boost/python/object/pointer_holder.hpp>
+# include <boost/python/object/make_ptr_instance.hpp>
+
+# include <boost/python/detail/none.hpp>
+
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+# include <boost/python/converter/pytype_function.hpp>
+#endif
+
+# include <boost/python/refcount.hpp>
+
+# include <boost/type_traits/is_pointer.hpp>
+# include <boost/type_traits/is_polymorphic.hpp>
+
+# include <boost/mpl/bool.hpp>
+
+# include <memory>
+
+namespace boost { namespace python {
+
+template <class T, class MakeHolder>
+struct to_python_indirect
+{
+ template <class U>
+ inline PyObject*
+ operator()(U const& ref) const
+ {
+ return this->execute(const_cast<U&>(ref), is_pointer<U>());
+ }
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+ inline PyTypeObject const*
+ get_pytype()const
+ {
+ return converter::registered_pytype<T>::get_pytype();
+ }
+#endif
+ private:
+ template <class U>
+ inline PyObject* execute(U* ptr, mpl::true_) const
+ {
+ // No special nullptr treatment for references
+ if (ptr == 0)
+ return python::detail::none();
+ else
+ return this->execute(*ptr, mpl::false_());
+ }
+
+ template <class U>
+ inline PyObject* execute(U const& x, mpl::false_) const
+ {
+ U* const p = &const_cast<U&>(x);
+ if (is_polymorphic<U>::value)
+ {
+ if (PyObject* o = detail::wrapper_base_::owner(p))
+ return incref(o);
+ }
+ return MakeHolder::execute(p);
+ }
+};
+
+//
+// implementations
+//
+namespace detail
+{
+ struct make_owning_holder
+ {
+ template <class T>
+ static PyObject* execute(T* p)
+ {
+ // can't use auto_ptr with Intel 5 and VC6 Dinkum library
+ // for some reason. We get link errors against the auto_ptr
+ // copy constructor.
+# if defined(__ICL) && __ICL < 600
+ typedef std::shared_ptr<T> smart_pointer;
+# else
+ typedef std::auto_ptr<T> smart_pointer;
+# endif
+ typedef objects::pointer_holder<smart_pointer, T> holder_t;
+
+ smart_pointer ptr(const_cast<T*>(p));
+ return objects::make_ptr_instance<T, holder_t>::execute(ptr);
+ }
+ };
+
+ struct make_reference_holder
+ {
+ template <class T>
+ static PyObject* execute(T* p)
+ {
+ typedef objects::pointer_holder<T*, T> holder_t;
+ T* q = const_cast<T*>(p);
+ return objects::make_ptr_instance<T, holder_t>::execute(q);
+ }
+ };
+}
+
+}} // namespace boost::python
+
+#endif // TO_PYTHON_INDIRECT_DWA200221_HPP
diff --git a/src/boost/python/to_python_value.hpp b/src/boost/python/to_python_value.hpp
new file mode 100644
index 0000000..e283fbc
--- /dev/null
+++ b/src/boost/python/to_python_value.hpp
@@ -0,0 +1,171 @@
+// Copyright David Abrahams 2002.
+// Distributed under the Boost Software License, Version 1.0. (See
+// accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#ifndef TO_PYTHON_VALUE_DWA200221_HPP
+# define TO_PYTHON_VALUE_DWA200221_HPP
+
+# include <boost/python/detail/prefix.hpp>
+
+# include <boost/python/refcount.hpp>
+# include <boost/python/tag.hpp>
+# include <boost/python/handle.hpp>
+
+# include <boost/python/converter/registry.hpp>
+# include <boost/python/converter/registered.hpp>
+# include <boost/python/converter/builtin_converters.hpp>
+# include <boost/python/converter/object_manager.hpp>
+# include <boost/python/converter/shared_ptr_to_python.hpp>
+
+# include <boost/python/detail/value_is_shared_ptr.hpp>
+# include <boost/python/detail/value_arg.hpp>
+
+# include <boost/type_traits/transform_traits.hpp>
+
+# include <boost/mpl/if.hpp>
+# include <boost/mpl/or.hpp>
+# include <boost/type_traits/is_const.hpp>
+
+namespace boost { namespace python {
+
+namespace detail
+{
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+
+template <bool is_const_ref>
+struct object_manager_get_pytype
+{
+ template <class U>
+ static PyTypeObject const* get( U& (*)() =0)
+ {
+ return converter::object_manager_traits<U>::get_pytype();
+ }
+};
+
+template <>
+struct object_manager_get_pytype<true>
+{
+ template <class U>
+ static PyTypeObject const* get( U const& (*)() =0)
+ {
+ return converter::object_manager_traits<U>::get_pytype();
+ }
+};
+
+#endif
+
+ template <class T>
+ struct object_manager_to_python_value
+ {
+ typedef typename value_arg<T>::type argument_type;
+
+ PyObject* operator()(argument_type) const;
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+ typedef boost::mpl::bool_<is_handle<T>::value> is_t_handle;
+ typedef boost::detail::indirect_traits::is_reference_to_const<T> is_t_const;
+ PyTypeObject const* get_pytype() const {
+ return get_pytype_aux((is_t_handle*)0);
+ }
+
+ inline static PyTypeObject const* get_pytype_aux(mpl::true_*) {return converter::object_manager_traits<T>::get_pytype();}
+
+ inline static PyTypeObject const* get_pytype_aux(mpl::false_* )
+ {
+ return object_manager_get_pytype<is_t_const::value>::get((T(*)())0);
+ }
+
+#endif
+
+ // This information helps make_getter() decide whether to try to
+ // return an internal reference or not. I don't like it much,
+ // but it will have to serve for now.
+ BOOST_STATIC_CONSTANT(bool, uses_registry = false);
+ };
+
+
+ template <class T>
+ struct registry_to_python_value
+ {
+ typedef typename value_arg<T>::type argument_type;
+
+ PyObject* operator()(argument_type) const;
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+ PyTypeObject const* get_pytype() const {return converter::registered<T>::converters.to_python_target_type();}
+#endif
+
+ // This information helps make_getter() decide whether to try to
+ // return an internal reference or not. I don't like it much,
+ // but it will have to serve for now.
+ BOOST_STATIC_CONSTANT(bool, uses_registry = true);
+ };
+
+ template <class T>
+ struct shared_ptr_to_python_value
+ {
+ typedef typename value_arg<T>::type argument_type;
+
+ PyObject* operator()(argument_type) const;
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+ PyTypeObject const* get_pytype() const {return get_pytype((boost::type<argument_type>*)0);}
+#endif
+ // This information helps make_getter() decide whether to try to
+ // return an internal reference or not. I don't like it much,
+ // but it will have to serve for now.
+ BOOST_STATIC_CONSTANT(bool, uses_registry = false);
+ private:
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+ template <class U>
+ PyTypeObject const* get_pytype(boost::type<std::shared_ptr<U> &> *) const {return converter::registered<U>::converters.to_python_target_type();}
+ template <class U>
+ PyTypeObject const* get_pytype(boost::type<const std::shared_ptr<U> &> *) const {return converter::registered<U>::converters.to_python_target_type();}
+#endif
+ };
+}
+
+template <class T>
+struct to_python_value
+ : mpl::if_<
+ detail::value_is_shared_ptr<T>
+ , detail::shared_ptr_to_python_value<T>
+ , typename mpl::if_<
+ mpl::or_<
+ converter::is_object_manager<T>
+ , converter::is_reference_to_object_manager<T>
+ >
+ , detail::object_manager_to_python_value<T>
+ , detail::registry_to_python_value<T>
+ >::type
+ >::type
+{
+};
+
+//
+// implementation
+//
+namespace detail
+{
+ template <class T>
+ inline PyObject* registry_to_python_value<T>::operator()(argument_type x) const
+ {
+ return converter::registered<argument_type>::converters.to_python(&x);
+ }
+
+ template <class T>
+ inline PyObject* object_manager_to_python_value<T>::operator()(argument_type x) const
+ {
+ return python::upcast<PyObject>(
+ python::xincref(
+ get_managed_object(x, tag))
+ );
+ }
+
+ template <class T>
+ inline PyObject* shared_ptr_to_python_value<T>::operator()(argument_type x) const
+ {
+ return converter::shared_ptr_to_python(x);
+ }
+}
+
+}} // namespace boost::python
+
+#endif // TO_PYTHON_VALUE_DWA200221_HPP
diff --git a/src/ompl/CMakeLists.txt b/src/ompl/CMakeLists.txt
index 229b6b5..75e14db 100644
--- a/src/ompl/CMakeLists.txt
+++ b/src/ompl/CMakeLists.txt
@@ -15,7 +15,9 @@ file(GLOB_RECURSE OMPL_SOURCE_CODE datastructures/*.cpp util/*.cpp base/*.cpp ge
if(NOT OMPL_HAVE_EIGEN3)
list(REMOVE_ITEM OMPL_SOURCE_CODE
"${CMAKE_CURRENT_SOURCE_DIR}/util/src/ProlateHyperspheroid.cpp"
- "${CMAKE_CURRENT_SOURCE_DIR}/base/samplers/informed/src/PathLengthDirectInfSampler.cpp")
+ "${CMAKE_CURRENT_SOURCE_DIR}/base/samplers/informed/src/PathLengthDirectInfSampler.cpp"
+ "${CMAKE_CURRENT_SOURCE_DIR}/geometric/planners/rrt/src/VFRRT.cpp"
+ )
endif()
#############################################
diff --git a/src/ompl/base/GenericParam.h b/src/ompl/base/GenericParam.h
index 856e733..f5c4b25 100644
--- a/src/ompl/base/GenericParam.h
+++ b/src/ompl/base/GenericParam.h
@@ -39,9 +39,8 @@
#include "ompl/util/Console.h"
#include "ompl/util/ClassForward.h"
-#include <boost/function.hpp>
+#include <functional>
#include <boost/lexical_cast.hpp>
-#include <boost/type_traits.hpp>
#include <iostream>
#include <string>
#include <vector>
@@ -161,10 +160,10 @@ namespace ompl
public:
/** \brief The type for the 'setter' function for this parameter */
- typedef boost::function<void(T)> SetterFn;
+ typedef std::function<void(T)> SetterFn;
/** \brief The type for the 'getter' function for this parameter */
- typedef boost::function<T()> GetterFn;
+ typedef std::function<T()> GetterFn;
/** \brief An explicit instantiation of a parameter \e name requires the \e setter function and optionally the \e
getter function. */
diff --git a/src/ompl/base/Goal.h b/src/ompl/base/Goal.h
index f8056c5..800eabe 100644
--- a/src/ompl/base/Goal.h
+++ b/src/ompl/base/Goal.h
@@ -43,7 +43,6 @@
#include "ompl/base/GoalTypes.h"
#include "ompl/util/Console.h"
#include <iostream>
-#include <boost/noncopyable.hpp>
#include <boost/concept_check.hpp>
#include <vector>
@@ -57,12 +56,15 @@ namespace ompl
/// @endcond
/** \class ompl::base::GoalPtr
- \brief A boost shared pointer wrapper for ompl::base::Goal */
+ \brief A shared pointer wrapper for ompl::base::Goal */
/** \brief Abstract definition of goals.*/
- class Goal : private boost::noncopyable
+ class Goal
{
public:
+ // non-copyable
+ Goal(const Goal&) = delete;
+ Goal& operator=(const Goal&) = delete;
/** \brief Constructor. The goal must always know the space information it is part of */
Goal(const SpaceInformationPtr &si);
diff --git a/src/ompl/base/MotionValidator.h b/src/ompl/base/MotionValidator.h
index 263c9d2..9ba19f0 100644
--- a/src/ompl/base/MotionValidator.h
+++ b/src/ompl/base/MotionValidator.h
@@ -56,7 +56,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::MotionValidatorPtr
- \brief A boost shared pointer wrapper for ompl::base::MotionValidator */
+ \brief A shared pointer wrapper for ompl::base::MotionValidator */
/** \brief Abstract definition for a class checking the
validity of motions -- path segments between states. This
@@ -90,7 +90,7 @@ namespace ompl
0 and \e s2 being at t = 1. This function assumes \e s1 is valid.
\param s1 start state of the motion to be checked (assumed to be valid)
\param s2 final state of the motion to be checked
- \param lastValid first: storage for the last valid state (may be NULL, if the user does not care about the exact state); this need not be different from \e s1 or \e s2. second: the time (between 0 and 1) of the last valid state, on the motion from \e s1 to \e s2. If the function returns false, \e lastValid.first must be set to a valid state, even if that implies copying \e s1 to \e lastValid.first (in case \e lastValid.second = 0). If the function returns true, \e lastVal [...]
+ \param lastValid first: storage for the last valid state (may be nullptr, if the user does not care about the exact state); this need not be different from \e s1 or \e s2. second: the time (between 0 and 1) of the last valid state, on the motion from \e s1 to \e s2. If the function returns false, \e lastValid.first must be set to a valid state, even if that implies copying \e s1 to \e lastValid.first (in case \e lastValid.second = 0). If the function returns true, \e last [...]
\note This function updates the number of valid and invalid segments. */
virtual bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const = 0;
diff --git a/src/ompl/base/OptimizationObjective.h b/src/ompl/base/OptimizationObjective.h
index ebc92c0..2488de3 100644
--- a/src/ompl/base/OptimizationObjective.h
+++ b/src/ompl/base/OptimizationObjective.h
@@ -42,8 +42,6 @@
#include "ompl/util/ClassForward.h"
#include "ompl/base/ProblemDefinition.h"
#include "ompl/base/samplers/InformedStateSampler.h"
-#include <boost/noncopyable.hpp>
-#include <boost/concept_check.hpp>
#include <iostream>
@@ -54,7 +52,7 @@ namespace ompl
class Goal;
/** \brief The definition of a function which returns an admissible estimate of the optimal path cost from a given state to a goal. */
- typedef boost::function<Cost (const State*, const Goal*)> CostToGoHeuristic;
+ typedef std::function<Cost (const State*, const Goal*)> CostToGoHeuristic;
/// @cond IGNORE
/** \brief Forward declaration of ompl::base::OptimizationObjective */
@@ -62,14 +60,18 @@ namespace ompl
/// @endcond
/** \class ompl::base::OptimizationObjectivePtr
- \brief A boost shared pointer wrapper for ompl::base::OptimizationObjective */
+ \brief A shared pointer wrapper for ompl::base::OptimizationObjective */
/** \brief Abstract definition of optimization objectives.
\note This implementation has greatly benefited from discussions with Kris Hauser */
- class OptimizationObjective : private boost::noncopyable
+ class OptimizationObjective
{
public:
+ // non-copyable
+ OptimizationObjective(const OptimizationObjective&) = delete;
+ OptimizationObjective& operator=(const OptimizationObjective&) = delete;
+
/** \brief Constructor. The objective must always know the space information it is part of. The cost threshold for objective satisfaction defaults to 0.0. */
OptimizationObjective(const SpaceInformationPtr &si);
diff --git a/src/ompl/base/Path.h b/src/ompl/base/Path.h
index e559bd5..421154e 100644
--- a/src/ompl/base/Path.h
+++ b/src/ompl/base/Path.h
@@ -41,7 +41,6 @@
#include "ompl/base/Cost.h"
#include <iostream>
#include <boost/concept_check.hpp>
-#include <boost/noncopyable.hpp>
namespace ompl
{
@@ -62,12 +61,15 @@ namespace ompl
/// @endcond
/** \class ompl::base::PathPtr
- \brief A boost shared pointer wrapper for ompl::base::Path */
+ \brief A shared pointer wrapper for ompl::base::Path */
/** \brief Abstract definition of a path */
- class Path : private boost::noncopyable
+ class Path
{
public:
+ // non-copyable
+ Path(const Path&) = delete;
+ Path& operator=(const Path&) = delete;
/** \brief Constructor. A path must always know the space information it is part of */
Path(const SpaceInformationPtr &si) : si_(si)
diff --git a/src/ompl/base/Planner.h b/src/ompl/base/Planner.h
index 5f1d105..8383c3b 100644
--- a/src/ompl/base/Planner.h
+++ b/src/ompl/base/Planner.h
@@ -47,10 +47,8 @@
#include "ompl/util/Time.h"
#include "ompl/util/ClassForward.h"
#include "ompl/util/Deprecation.h"
-#include <boost/function.hpp>
+#include <functional>
#include <boost/concept_check.hpp>
-#include <boost/noncopyable.hpp>
-#include <boost/lexical_cast.hpp>
#include <string>
#include <map>
@@ -66,7 +64,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::PlannerPtr
- \brief A boost shared pointer wrapper for ompl::base::Planner */
+ \brief A shared pointer wrapper for ompl::base::Planner */
/** \brief Helper class to extract valid start & goal
@@ -87,23 +85,23 @@ namespace ompl
/** \brief Default constructor. No work is performed. */
PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
{
- tempState_ = NULL;
+ tempState_ = nullptr;
update();
}
/** \brief Default constructor. No work is performed. */
PlannerInputStates(const Planner *planner) : planner_(planner)
{
- tempState_ = NULL;
+ tempState_ = nullptr;
update();
}
/** \brief Default constructor. No work is performed. A
call to use() needs to be made, before making any
calls to nextStart() or nextGoal(). */
- PlannerInputStates() : planner_(NULL)
+ PlannerInputStates() : planner_(nullptr)
{
- tempState_ = NULL;
+ tempState_ = nullptr;
clear();
}
@@ -146,11 +144,11 @@ namespace ompl
state are available and goal was set */
void checkValidity() const;
- /** \brief Return the next valid start state or NULL if no
+ /** \brief Return the next valid start state or nullptr if no
more valid start states are available. */
const State* nextStart();
- /** \brief Return the next valid goal state or NULL if no
+ /** \brief Return the next valid goal state or nullptr if no
more valid goal states are available. Because
sampling of goal states may also produce invalid
goals, this function takes an argument that specifies
@@ -229,10 +227,13 @@ namespace ompl
};
/** \brief Base class for a planner */
- class Planner : private boost::noncopyable
+ class Planner
{
public:
+ // non-copyable
+ Planner(const Planner&) = delete;
+ Planner& operator=(const Planner&) = delete;
/** \brief Constructor */
Planner(const SpaceInformationPtr &si, const std::string &name);
@@ -350,7 +351,7 @@ namespace ompl
}
/** \brief Definition of a function which returns a property about the planner's progress that can be queried by a benchmarking routine */
- typedef boost::function<std::string ()> PlannerProgressProperty;
+ typedef std::function<std::string ()> PlannerProgressProperty;
/** \brief A dictionary which maps the name of a progress property to the function to be used for querying that property */
typedef std::map<std::string, PlannerProgressProperty> PlannerProgressProperties;
@@ -373,7 +374,7 @@ namespace ompl
template<typename T, typename PlannerType, typename SetterType, typename GetterType>
void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const GetterType& getter, const std::string &rangeSuggestion = "")
{
- params_.declareParam<T>(name, boost::bind(setter, planner, _1), boost::bind(getter, planner));
+ params_.declareParam<T>(name, std::bind(setter, planner, std::placeholders::_1), std::bind(getter, planner));
if (!rangeSuggestion.empty())
params_[name].setRangeSuggestion(rangeSuggestion);
}
@@ -382,7 +383,7 @@ namespace ompl
template<typename T, typename PlannerType, typename SetterType>
void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const std::string &rangeSuggestion = "")
{
- params_.declareParam<T>(name, boost::bind(setter, planner, _1));
+ params_.declareParam<T>(name, std::bind(setter, planner, std::placeholders::_1));
if (!rangeSuggestion.empty())
params_[name].setRangeSuggestion(rangeSuggestion);
}
@@ -419,7 +420,7 @@ namespace ompl
};
/** \brief Definition of a function that can allocate a planner */
- typedef boost::function<PlannerPtr(const SpaceInformationPtr&)> PlannerAllocator;
+ typedef std::function<PlannerPtr(const SpaceInformationPtr&)> PlannerAllocator;
}
}
diff --git a/src/ompl/base/PlannerData.h b/src/ompl/base/PlannerData.h
index 72bdba9..a818b7e 100644
--- a/src/ompl/base/PlannerData.h
+++ b/src/ompl/base/PlannerData.h
@@ -45,8 +45,7 @@
#include "ompl/base/Cost.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/ClassForward.h"
-#include <boost/noncopyable.hpp>
-#include <boost/function.hpp>
+#include <functional>
#include <boost/serialization/access.hpp>
namespace ompl
@@ -153,7 +152,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::PlannerDataPtr
- \brief A boost shared pointer wrapper for ompl::base::PlannerData */
+ \brief A shared pointer wrapper for ompl::base::PlannerData */
/// \brief Object containing planner generated vertex and edge data. It
@@ -161,7 +160,7 @@ namespace ompl
/// edge connects two vertices.
/// \note The storage for states this class maintains belongs to the planner
/// instance that filled the data (by default; see PlannerData::decoupleFromPlanner())
- class PlannerData : boost::noncopyable
+ class PlannerData
{
public:
class Graph;
@@ -173,6 +172,10 @@ namespace ompl
/// \brief Representation of an invalid vertex index
static const unsigned int INVALID_INDEX;
+ // non-copyable
+ PlannerData(const PlannerData&) = delete;
+ PlannerData& operator=(const PlannerData&) = delete;
+
/// \brief Constructor. Accepts a SpaceInformationPtr for the space planned in.
PlannerData(const SpaceInformationPtr &si);
/// \brief Destructor.
diff --git a/src/ompl/base/PlannerTerminationCondition.h b/src/ompl/base/PlannerTerminationCondition.h
index b96708e..418f8df 100644
--- a/src/ompl/base/PlannerTerminationCondition.h
+++ b/src/ompl/base/PlannerTerminationCondition.h
@@ -37,8 +37,8 @@
#ifndef OMPL_BASE_PLANNER_TERMINATION_CONDITION_
#define OMPL_BASE_PLANNER_TERMINATION_CONDITION_
-#include <boost/function.hpp>
-#include <boost/shared_ptr.hpp>
+#include <functional>
+#include <memory>
#include <ompl/base/ProblemDefinition.h>
#include <ompl/util/Time.h>
@@ -55,7 +55,7 @@ namespace ompl
signaled to terminate its computation. Otherwise,
computation continues while this function returns false,
until a solution is found. */
- typedef boost::function<bool()> PlannerTerminationConditionFn;
+ typedef std::function<bool()> PlannerTerminationConditionFn;
/** \brief Encapsulate a termination condition for a motion
planner. Planners will call operator() to decide whether
@@ -102,7 +102,7 @@ namespace ompl
private:
class PlannerTerminationConditionImpl;
- boost::shared_ptr<PlannerTerminationConditionImpl> impl_;
+ std::shared_ptr<PlannerTerminationConditionImpl> impl_;
};
/** \brief Simple termination condition that always returns false. The termination condition will never be met */
diff --git a/src/ompl/base/ProblemDefinition.h b/src/ompl/base/ProblemDefinition.h
index ea6817a..a4f23f2 100644
--- a/src/ompl/base/ProblemDefinition.h
+++ b/src/ompl/base/ProblemDefinition.h
@@ -52,8 +52,6 @@
#include <iostream>
#include <limits>
-#include <boost/noncopyable.hpp>
-
namespace ompl
{
namespace base
@@ -66,7 +64,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::ProblemDefinitionPtr
- \brief A boost shared pointer wrapper for ompl::base::ProblemDefinition */
+ \brief A shared pointer wrapper for ompl::base::ProblemDefinition */
/** \brief Representation of a solution to a planning problem */
struct PlannerSolution
@@ -141,16 +139,19 @@ namespace ompl
/** \brief When a planner has an intermediate solution (e.g., optimizing planners), a function with this signature can be called
to report the states of that solution. */
- typedef boost::function<void(const Planner*, const std::vector<const base::State*> &, const Cost)> ReportIntermediateSolutionFn;
+ typedef std::function<void(const Planner*, const std::vector<const base::State*> &, const Cost)> ReportIntermediateSolutionFn;
OMPL_CLASS_FORWARD(OptimizationObjective);
/** \brief Definition of a problem to be solved. This includes
the start state(s) for the system and a goal specification.
Will contain solutions, if found. */
- class ProblemDefinition : private boost::noncopyable
+ class ProblemDefinition
{
public:
+ // non-copyable
+ ProblemDefinition(const ProblemDefinition&) = delete;
+ ProblemDefinition& operator=(const ProblemDefinition&) = delete;
/** \brief Create a problem definition given the SpaceInformation it is part of */
ProblemDefinition(const SpaceInformationPtr &si);
@@ -181,7 +182,7 @@ namespace ompl
/** \brief Check whether a specified starting state is
already included in the problem definition and
optionally return the index of that starting state */
- bool hasStartState(const State *state, unsigned int *startIndex = NULL);
+ bool hasStartState(const State *state, unsigned int *startIndex = nullptr) const;
/** \brief Clear all start states (memory is freed) */
void clearStartStates()
@@ -292,7 +293,7 @@ namespace ompl
will be set to the index of the starting state that
satisfies the goal. The distance to the goal can
optionally be returned as well. */
- bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
+ bool isTrivial(unsigned int *startIndex = nullptr, double *distance = nullptr) const;
/** \brief Check if a straight line path is valid. If it
is, return an instance of a path that represents the
diff --git a/src/ompl/base/ProjectionEvaluator.h b/src/ompl/base/ProjectionEvaluator.h
index 0f3dcf4..681cd76 100644
--- a/src/ompl/base/ProjectionEvaluator.h
+++ b/src/ompl/base/ProjectionEvaluator.h
@@ -46,7 +46,6 @@
#include <vector>
#include <valarray>
#include <iostream>
-#include <boost/noncopyable.hpp>
#include <boost/numeric/ublas/matrix.hpp>
namespace ompl
@@ -127,7 +126,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::ProjectionEvaluatorPtr
- \brief A boost shared pointer wrapper for ompl::base::ProjectionEvaluator */
+ \brief A shared pointer wrapper for ompl::base::ProjectionEvaluator */
/** \brief Abstract definition for a class computing
projections to R<sup>n</sup>. Implicit integer grids are
@@ -135,9 +134,12 @@ namespace ompl
sizes. Before use, the user must supply cell sizes
for the integer grid (setCellSizes()). The
implementation of this class is thread safe. */
- class ProjectionEvaluator : private boost::noncopyable
+ class ProjectionEvaluator
{
public:
+ // non-copyable
+ ProjectionEvaluator(const ProjectionEvaluator&) = delete;
+ ProjectionEvaluator& operator=(const ProjectionEvaluator&) = delete;
/** \brief Construct a projection evaluator for a specific state space */
ProjectionEvaluator(const StateSpace *space);
diff --git a/src/ompl/base/ScopedState.h b/src/ompl/base/ScopedState.h
index 2e0144d..259ac91 100644
--- a/src/ompl/base/ScopedState.h
+++ b/src/ompl/base/ScopedState.h
@@ -554,7 +554,7 @@ namespace ompl
}
/** \brief Shared pointer to a ScopedState<> */
- typedef boost::shared_ptr< ScopedState<> > ScopedStatePtr;
+ typedef std::shared_ptr< ScopedState<> > ScopedStatePtr;
}
}
diff --git a/src/ompl/base/SolutionNonExistenceProof.h b/src/ompl/base/SolutionNonExistenceProof.h
index 968924d..0238657 100644
--- a/src/ompl/base/SolutionNonExistenceProof.h
+++ b/src/ompl/base/SolutionNonExistenceProof.h
@@ -50,7 +50,7 @@ namespace ompl
/// @endcond
/// \class ompl::base::SolutionNonExistenceProofPtr
- /// \brief A boost shared pointer wrapper for ompl::base::SolutionNonExistenceProof
+ /// \brief A shared pointer wrapper for ompl::base::SolutionNonExistenceProof
/// \brief Abstract definition of a proof for the non-existence of a solution to a problem
class SolutionNonExistenceProof
diff --git a/src/ompl/base/SpaceInformation.h b/src/ompl/base/SpaceInformation.h
index 744dacc..0cee019 100644
--- a/src/ompl/base/SpaceInformation.h
+++ b/src/ompl/base/SpaceInformation.h
@@ -47,10 +47,7 @@
#include "ompl/util/Console.h"
#include "ompl/util/Exception.h"
-#include <boost/noncopyable.hpp>
-#include <boost/function.hpp>
-#include <boost/bind.hpp>
-
+#include <functional>
#include <utility>
#include <cstdlib>
#include <vector>
@@ -72,20 +69,23 @@ namespace ompl
/// @endcond
/** \class ompl::base::SpaceInformationPtr
- \brief A boost shared pointer wrapper for ompl::base::SpaceInformation */
+ \brief A shared pointer wrapper for ompl::base::SpaceInformation */
/** \brief If no state validity checking class is specified
- (StateValidityChecker), a boost function can be specified
+ (StateValidityChecker), a std::function can be specified
instead */
- typedef boost::function<bool(const State*)> StateValidityCheckerFn;
+ typedef std::function<bool(const State*)> StateValidityCheckerFn;
/** \brief The base class for space information. This contains
all the information about the space planning is done in.
setup() needs to be called as well, before use */
- class SpaceInformation : private boost::noncopyable
+ class SpaceInformation
{
public:
+ // non-copyable
+ SpaceInformation(const SpaceInformation&) = delete;
+ SpaceInformation& operator=(const SpaceInformation&) = delete;
/** \brief Constructor. Sets the instance of the state space to plan with. */
SpaceInformation(const StateSpacePtr &space);
@@ -154,7 +154,7 @@ namespace ompl
}
/** \brief If no state validity checking class is
- specified (StateValidityChecker), a boost function can
+ specified (StateValidityChecker), a function can
be specified instead. This version however incurs a
small additional overhead when calling the function,
since there is one more level of indirection */
@@ -255,9 +255,7 @@ namespace ompl
/** \brief Clone a state */
State* cloneState(const State *source) const
{
- State *copy = stateSpace_->allocState();
- stateSpace_->copyState(copy, source);
- return copy;
+ return stateSpace_->cloneState(source);
}
/** @} */
@@ -328,7 +326,7 @@ namespace ompl
0 and s2 being at t = 1. This function assumes s1 is valid.
\param s1 start state of the motion to be checked (assumed to be valid)
\param s2 final state of the motion to be checked
- \param lastValid first: storage for the last valid state (may be NULL); this need not be different from \e s1 or \e s2. second: the time (between 0 and 1) of the last valid state, on the motion from \e s1 to \e s2 */
+ \param lastValid first: storage for the last valid state (may be nullptr); this need not be different from \e s1 or \e s2. second: the time (between 0 and 1) of the last valid state, on the motion from \e s1 to \e s2 */
bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
{
return motionValidator_->checkMotion(s1, s2, lastValid);
diff --git a/src/ompl/base/State.h b/src/ompl/base/State.h
index 6ef0060..113f379 100644
--- a/src/ompl/base/State.h
+++ b/src/ompl/base/State.h
@@ -96,7 +96,7 @@ namespace ompl
{
public:
- CompoundState() : State(), components(NULL)
+ CompoundState() : State(), components(nullptr)
{
}
diff --git a/src/ompl/base/StateSampler.h b/src/ompl/base/StateSampler.h
index d3b8696..66a0812 100644
--- a/src/ompl/base/StateSampler.h
+++ b/src/ompl/base/StateSampler.h
@@ -42,8 +42,7 @@
#include "ompl/util/ClassForward.h"
#include <vector>
#include <string>
-#include <boost/function.hpp>
-#include <boost/noncopyable.hpp>
+#include <functional>
namespace ompl
{
@@ -60,12 +59,15 @@ namespace ompl
/// @endcond
/** \class ompl::base::StateSamplerPtr
- \brief A boost shared pointer wrapper for ompl::base::StateSampler */
+ \brief A shared pointer wrapper for ompl::base::StateSampler */
/** \brief Abstract definition of a state space sampler. */
- class StateSampler : private boost::noncopyable
+ class StateSampler
{
public:
+ // non-copyable
+ StateSampler(const StateSampler&) = delete;
+ StateSampler& operator=(const StateSampler&) = delete;
/** \brief Constructor */
StateSampler(const StateSpace *space) : space_(space)
@@ -79,10 +81,24 @@ namespace ompl
/** \brief Sample a state */
virtual void sampleUniform(State *state) = 0;
- /** \brief Sample a state near another, within specified distance */
+ /** \brief Sample a state near another, within a neighborhood controlled by a distance parameter.
+
+ Typically, StateSampler-derived classes will return in `state` a
+ state that is uniformly distributed within a ball with radius
+ `distance` defined by the distance function from the corresponding
+ state space. However, this is not guaranteed. For example, the
+ default state sampler for the RealVectorStateSpace returns samples
+ uniformly distributed using L_inf distance, while the default
+ distance function is L_2 distance.
+ */
virtual void sampleUniformNear(State *state, const State *near, const double distance) = 0;
- /** \brief Sample a state using a Gaussian distribution with given \e mean and standard deviation (\e stdDev) */
+ /** \brief Sample a state using a Gaussian distribution with given \e mean and standard deviation (\e stdDev).
+
+ As with sampleUniform, the implementation of sampleGaussian is
+ specific to the derived class and few assumptions can be made
+ about the distance between `state` and `mean`.
+ */
virtual void sampleGaussian(State *state, const State *mean, const double stdDev) = 0;
protected:
@@ -181,7 +197,7 @@ namespace ompl
};
/** \brief Definition of a function that can allocate a state sampler */
- typedef boost::function<StateSamplerPtr(const StateSpace*)> StateSamplerAllocator;
+ typedef std::function<StateSamplerPtr(const StateSpace*)> StateSamplerAllocator;
}
}
diff --git a/src/ompl/base/StateSpace.h b/src/ompl/base/StateSpace.h
index a570e24..b968c4b 100644
--- a/src/ompl/base/StateSpace.h
+++ b/src/ompl/base/StateSpace.h
@@ -45,7 +45,6 @@
#include "ompl/util/Console.h"
#include "ompl/util/ClassForward.h"
#include <boost/concept_check.hpp>
-#include <boost/noncopyable.hpp>
#include <iostream>
#include <vector>
#include <string>
@@ -62,7 +61,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::StateSpacePtr
- \brief A boost shared pointer wrapper for ompl::base::StateSpace */
+ \brief A shared pointer wrapper for ompl::base::StateSpace */
/** \brief Representation of a space in which planning can be
@@ -70,9 +69,12 @@ namespace ompl
are defined.
See \ref implementingStateSpaces. */
- class StateSpace : private boost::noncopyable
+ class StateSpace
{
public:
+ // non-copyable
+ StateSpace(const StateSpace&) = delete;
+ StateSpace& operator=(const StateSpace&) = delete;
/** \brief Define the type of state allocated by this space */
typedef State StateType;
@@ -296,6 +298,9 @@ namespace ompl
/** \brief Copy a state to another. The memory of source and destination should NOT overlap.
\note For more advanced state copying methods (partial copy, for example), see \ref advancedStateCopy. */
virtual void copyState(State *destination, const State *source) const = 0;
+
+ /** \brief Clone a state*/
+ State* cloneState(const State* source) const;
/** \brief Computes distance between two states. This function satisfies the properties of a
metric if isMetricSpace() is true, and its return value will always be between 0 and getMaximumExtent() */
@@ -346,7 +351,7 @@ namespace ompl
/** \brief Many states contain a number of double values. This function provides a means to get the
memory address of a double value from state \e state located at position \e index. The first double value
is returned for \e index = 0. If \e index is too large (does not point to any double values in the state),
- the return value is NULL.
+ the return value is nullptr.
\note This function does @b not map a state to an
array of doubles. There may be components of a state
diff --git a/src/ompl/base/StateStorage.h b/src/ompl/base/StateStorage.h
index 026ffe1..bd536b6 100644
--- a/src/ompl/base/StateStorage.h
+++ b/src/ompl/base/StateStorage.h
@@ -41,7 +41,7 @@
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/vector.hpp>
-#include <boost/function.hpp>
+#include <functional>
#include <iostream>
namespace ompl
@@ -128,7 +128,7 @@ namespace ompl
/** \brief Sort the states according to the less-equal operator \e op. Metadata is NOT sorted;
if metadata was added, the index values of the metadata will not match after the sort. */
- void sort(const boost::function<bool(const State*, const State*)> &op);
+ void sort(const std::function<bool(const State*, const State*)> &op);
/** \brief Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled
states are actually from this storage structure. */
@@ -277,7 +277,7 @@ namespace ompl
/** \brief Storage of states where the metadata is a vector of indices. This is is typically used to store a graph */
typedef StateStorageWithMetadata<std::vector<std::size_t> > GraphStateStorage;
- typedef boost::shared_ptr<GraphStateStorage> GraphStateStoragePtr;
+ typedef std::shared_ptr<GraphStateStorage> GraphStateStoragePtr;
}
}
#endif
diff --git a/src/ompl/base/StateValidityChecker.h b/src/ompl/base/StateValidityChecker.h
index 6ee9114..0576c22 100644
--- a/src/ompl/base/StateValidityChecker.h
+++ b/src/ompl/base/StateValidityChecker.h
@@ -56,7 +56,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::StateValidityCheckerPtr
- \brief A boost shared pointer wrapper for ompl::base::StateValidityChecker */
+ \brief A shared pointer wrapper for ompl::base::StateValidityChecker */
/** \brief Properties that a state validity checker may have */
struct StateValidityCheckerSpecs
diff --git a/src/ompl/base/TypedSpaceInformation.h b/src/ompl/base/TypedSpaceInformation.h
new file mode 100644
index 0000000..2d3985a
--- /dev/null
+++ b/src/ompl/base/TypedSpaceInformation.h
@@ -0,0 +1,145 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Rice University
+* 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 Rice University 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: Neil T. Dantam */
+
+#ifndef OMPL_BASE_TYPED_SPACE_INFORMATION_
+#define OMPL_BASE_TYPED_SPACE_INFORMATION_
+
+#include "ompl/base/SpaceInformation.h"
+#include "ompl/base/ScopedState.h"
+
+namespace ompl
+{
+ namespace base
+ {
+
+ template<typename SpaceType_>
+ class TypedSpaceInformation : public SpaceInformation
+ {
+ public:
+
+ /*--- Type Definitions ---*/
+
+ /** The actual type of the state space */
+ typedef SpaceType_ SpaceType;
+
+ /** The actual type of states in the space. */
+ typedef typename SpaceType::StateType StateType;
+
+ /** The actual type for a Scoped State. */
+ typedef ScopedState<SpaceType> ScopedStateType;
+
+ /** Shared Pointer to the actual type of the space. */
+ typedef std::shared_ptr<SpaceType> SpacePtr;
+
+ /** Shared pointer to the typed space. */
+ typedef std::shared_ptr< TypedSpaceInformation<SpaceType> > Ptr;
+
+
+ /*--- Constructor ---*/
+
+ /** Construct from shared pointer to the actual space. */
+ TypedSpaceInformation(const SpacePtr &space) : SpaceInformation(space)
+ {
+ }
+
+ /*--- Space Accessors ---*/
+
+ /** Get space pointer of the proper type, const. */
+ const SpaceType* getTypedStateSpace() const
+ {
+ return getStateSpace()->template as<SpaceType>();
+ }
+
+ /** Get space pointer of the proper type. */
+ SpaceType* getTypedStateSpace()
+ {
+ return getStateSpace()->template as<SpaceType>();
+ }
+
+ /*--- State Memory Management ---*/
+
+ /** Allocate a state of the proper type. */
+ StateType* allocTypedState() const
+ {
+ return this->allocState()->template as<StateType>();
+ }
+
+ /** Allocate memory for typed states in array */
+ void allocTypedStates(std::vector<StateType *> &states) const
+ {
+ allocStates(states);
+ }
+
+ /** Free a state of the proper type. */
+ void freeTypedState(StateType *state) const
+ {
+ freeState(state);
+ }
+
+ /** Free typed states in array */
+ void freeTypedStates(std::vector<StateType*> &states) const
+ {
+ freeStates(states);
+ }
+
+ /** Copy a state of the proper type. */
+ void copyTypedState(StateType *destination, const StateType *source) const
+ {
+ copyState(destination, source);
+ }
+
+ /** Clone a state of the proper type. */
+ StateType* cloneTypedState(const StateType *source) const {
+ return this->cloneState()->template as<StateType>();
+ }
+
+ static StateType* state_as(ompl::base::State *s)
+ {
+ return s->template as<StateType>();
+ }
+
+ static const StateType* state_as(const ompl::base::State *s)
+ {
+ return s->template as<StateType>();
+ }
+
+ };
+
+ }
+
+}
+
+#endif
diff --git a/src/ompl/extensions/morse/MorseTerminationCondition.h b/src/ompl/base/TypedStateValidityChecker.h
similarity index 59%
copy from src/ompl/extensions/morse/MorseTerminationCondition.h
copy to src/ompl/base/TypedStateValidityChecker.h
index 3f48b6e..3032901 100644
--- a/src/ompl/extensions/morse/MorseTerminationCondition.h
+++ b/src/ompl/base/TypedStateValidityChecker.h
@@ -1,7 +1,7 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
-* Copyright (c) 2010, Rice University
+* Copyright (c) 2015, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -32,42 +32,58 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/* Author: Caleb Voss */
+/* Author: Neil T. Dantam */
-#ifndef OMPL_EXTENSION_MORSE_TERMINATION_CONDITION_
-#define OMPL_EXTENSION_MORSE_TERMINATION_CONDITION_
+#ifndef OMPL_BASE_TYPED_STATE_VALIDITY_CHECKER_
+#define OMPL_BASE_TYPED_STATE_VALIDITY_CHECKER_
-#include "ompl/base/PlannerTerminationCondition.h"
-#include "ompl/extensions/morse/MorseEnvironment.h"
+#include "ompl/base/SpaceInformation.h"
+#include "ompl/base/StateValidityChecker.h"
+#include "ompl/base/TypedSpaceInformation.h"
namespace ompl
{
namespace base
{
-
- /** \brief This class represents a termination condition for the planner that
- only terminates if the user shuts down the MORSE simulation */
- class MorseTerminationCondition : public PlannerTerminationCondition
+
+ template <typename SpaceType_>
+ class TypedStateValidityChecker : public StateValidityChecker
{
public:
-
- /** \brief The representation of the MORSE simulation environment */
- const MorseEnvironmentPtr env_;
-
- MorseTerminationCondition(const MorseEnvironmentPtr env) :
- PlannerTerminationCondition(NULL), env_(env)
+
+ typedef SpaceType_ SpaceType;
+ typedef TypedSpaceInformation<SpaceType> SpaceInformationType;
+
+ TypedStateValidityChecker(SpaceInformationType *si) : StateValidityChecker(si)
+ {
+ }
+
+ TypedStateValidityChecker(const typename SpaceInformationType::Ptr &si) : StateValidityChecker(si)
+ {
+ }
+
+ SpaceInformationType* getTypedSpaceInformation() const
+ {
+ return static_cast<SpaceInformationType*>(si_);
+ }
+
+ SpaceType* getTypedStateSpace() const
+ {
+ return getTypedSpaceInformation()->getTypedStateSpace();
+ }
+
+ static typename SpaceType::StateType* state_as(State *s)
{
+ return SpaceInformationType::state_as(s);
}
-
- ~MorseTerminationCondition()
+
+ static const typename SpaceType::StateType* state_as(const State *s)
{
+ return SpaceInformationType::state_as(s);
}
-
- /** \brief Return true if the simulation is still running */
- bool eval() const;
};
+
}
-}
+}
#endif
-
diff --git a/src/ompl/base/ValidStateSampler.h b/src/ompl/base/ValidStateSampler.h
index fb7f1e3..789852d 100644
--- a/src/ompl/base/ValidStateSampler.h
+++ b/src/ompl/base/ValidStateSampler.h
@@ -40,8 +40,7 @@
#include "ompl/base/State.h"
#include "ompl/util/ClassForward.h"
#include "ompl/base/GenericParam.h"
-#include <boost/function.hpp>
-#include <boost/noncopyable.hpp>
+#include <functional>
#include <string>
namespace ompl
@@ -59,12 +58,15 @@ namespace ompl
/// @endcond
/** \class ompl::base::ValidStateSamplerPtr
- \brief A boost shared pointer wrapper for ompl::base::ValidStateSampler */
+ \brief A shared pointer wrapper for ompl::base::ValidStateSampler */
/** \brief Abstract definition of a state sampler. */
- class ValidStateSampler : private boost::noncopyable
+ class ValidStateSampler
{
public:
+ // non-copyable
+ ValidStateSampler(const ValidStateSampler&) = delete;
+ ValidStateSampler& operator=(const ValidStateSampler&) = delete;
/** \brief Constructor */
ValidStateSampler(const SpaceInformation *si);
@@ -132,7 +134,7 @@ namespace ompl
};
/** \brief Definition of a function that can allocate a valid state sampler */
- typedef boost::function<ValidStateSamplerPtr(const SpaceInformation*)> ValidStateSamplerAllocator;
+ typedef std::function<ValidStateSamplerPtr(const SpaceInformation*)> ValidStateSamplerAllocator;
}
}
diff --git a/src/ompl/base/goals/GoalLazySamples.h b/src/ompl/base/goals/GoalLazySamples.h
index 0101640..c6f4e4c 100644
--- a/src/ompl/base/goals/GoalLazySamples.h
+++ b/src/ompl/base/goals/GoalLazySamples.h
@@ -38,8 +38,9 @@
#define OMPL_BASE_GOALS_GOAL_LAZY_SAMPLES_
#include "ompl/base/goals/GoalStates.h"
-#include <boost/thread/thread.hpp>
-#include <boost/function.hpp>
+#include <thread>
+#include <mutex>
+#include <functional>
#include <limits>
namespace ompl
@@ -53,7 +54,7 @@ namespace ompl
/** \brief Goal sampling function. Returns false when no further calls should be made to it.
Fills its second argument (the state) with the sampled goal state. This function need not
be thread safe. */
- typedef boost::function<bool(const GoalLazySamples*, State*)> GoalSamplingFn;
+ typedef std::function<bool(const GoalLazySamples*, State*)> GoalSamplingFn;
/** \brief Definition of a goal region that can be sampled,
but the sampling process can be slow. This class allows
@@ -76,7 +77,7 @@ namespace ompl
/** \brief When new samples are generated and added to the
list of possible samples, a callback can be
called. This type specifies the signature of that callback */
- typedef boost::function<void(const base::State*)> NewStateCallbackFn;
+ typedef std::function<void(const base::State*)> NewStateCallbackFn;
/** \brief Create a goal region that can be sampled in a
lazy fashion. A function (\e samplerFunc) that
@@ -163,7 +164,7 @@ namespace ompl
void goalSamplingThread();
/** \brief Lock for updating the set of states */
- mutable boost::mutex lock_;
+ mutable std::mutex lock_;
/** \brief Function that produces samples */
GoalSamplingFn samplerFunc_;
@@ -172,7 +173,7 @@ namespace ompl
bool terminateSamplingThread_;
/** \brief Additional thread for sampling goal states */
- boost::thread *samplingThread_;
+ std::thread *samplingThread_;
/** \brief The number of times the sampling function was called and it returned true */
unsigned int samplingAttempts_;
diff --git a/src/ompl/base/goals/GoalRegion.h b/src/ompl/base/goals/GoalRegion.h
index 10bf998..767df49 100644
--- a/src/ompl/base/goals/GoalRegion.h
+++ b/src/ompl/base/goals/GoalRegion.h
@@ -58,7 +58,7 @@ namespace ompl
{
}
- /** \brief Equivalent to calling isSatisfied(const State *, double *) with a NULL second argument. */
+ /** \brief Equivalent to calling isSatisfied(const State *, double *) with a nullptr second argument. */
virtual bool isSatisfied(const State *st) const;
/** \brief Decide whether a given state is part of the
diff --git a/src/ompl/base/goals/GoalState.h b/src/ompl/base/goals/GoalState.h
index 0a5924e..10fc1f3 100644
--- a/src/ompl/base/goals/GoalState.h
+++ b/src/ompl/base/goals/GoalState.h
@@ -52,7 +52,7 @@ namespace ompl
public:
/** \brief Create a goal representation that is in fact a state */
- GoalState(const SpaceInformationPtr &si) : GoalSampleableRegion(si), state_(NULL)
+ GoalState(const SpaceInformationPtr &si) : GoalSampleableRegion(si), state_(nullptr)
{
type_ = GOAL_STATE;
}
diff --git a/src/ompl/base/goals/src/GoalLazySamples.cpp b/src/ompl/base/goals/src/GoalLazySamples.cpp
index 0b64adf..598f566 100644
--- a/src/ompl/base/goals/src/GoalLazySamples.cpp
+++ b/src/ompl/base/goals/src/GoalLazySamples.cpp
@@ -39,7 +39,7 @@
#include "ompl/util/Time.h"
ompl::base::GoalLazySamples::GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart, double minDist) :
- GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(NULL), samplingAttempts_(0), minDist_(minDist)
+ GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(nullptr), samplingAttempts_(0), minDist_(minDist)
{
type_ = GOAL_LAZY_SAMPLES;
if (autoStart)
@@ -53,11 +53,11 @@ ompl::base::GoalLazySamples::~GoalLazySamples()
void ompl::base::GoalLazySamples::startSampling()
{
- if (samplingThread_ == NULL)
+ if (samplingThread_ == nullptr)
{
OMPL_DEBUG("Starting goal sampling thread");
terminateSamplingThread_ = false;
- samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
+ samplingThread_ = new std::thread(&GoalLazySamples::goalSamplingThread, this);
}
}
@@ -69,14 +69,14 @@ void ompl::base::GoalLazySamples::stopSampling()
terminateSamplingThread_ = true;
samplingThread_->join();
delete samplingThread_;
- samplingThread_ = NULL;
+ samplingThread_ = nullptr;
}
else
if (samplingThread_)
{ // join a finished thread
samplingThread_->join();
delete samplingThread_;
- samplingThread_ = NULL;
+ samplingThread_ = nullptr;
}
}
@@ -87,7 +87,7 @@ void ompl::base::GoalLazySamples::goalSamplingThread()
OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
// wait for everything to be set up before performing computation
while (!terminateSamplingThread_ && !si_->isSetup())
- boost::this_thread::sleep(time::seconds(0.01));
+ std::this_thread::sleep_for(time::seconds(0.01));
}
unsigned int prevsa = samplingAttempts_;
if (!terminateSamplingThread_ && samplerFunc_)
@@ -110,7 +110,7 @@ void ompl::base::GoalLazySamples::goalSamplingThread()
bool ompl::base::GoalLazySamples::isSampling() const
{
- return terminateSamplingThread_ == false && samplingThread_ != NULL;
+ return terminateSamplingThread_ == false && samplingThread_ != nullptr;
}
bool ompl::base::GoalLazySamples::couldSample() const
@@ -120,19 +120,19 @@ bool ompl::base::GoalLazySamples::couldSample() const
void ompl::base::GoalLazySamples::clear()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
GoalStates::clear();
}
double ompl::base::GoalLazySamples::distanceGoal(const State *st) const
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
return GoalStates::distanceGoal(st);
}
void ompl::base::GoalLazySamples::sampleGoal(base::State *st) const
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
GoalStates::sampleGoal(st);
}
@@ -143,34 +143,34 @@ void ompl::base::GoalLazySamples::setNewStateCallback(const NewStateCallbackFn &
void ompl::base::GoalLazySamples::addState(const State *st)
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
GoalStates::addState(st);
}
const ompl::base::State* ompl::base::GoalLazySamples::getState(unsigned int index) const
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
return GoalStates::getState(index);
}
bool ompl::base::GoalLazySamples::hasStates() const
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
return GoalStates::hasStates();
}
std::size_t ompl::base::GoalLazySamples::getStateCount() const
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
return GoalStates::getStateCount();
}
bool ompl::base::GoalLazySamples::addStateIfDifferent(const State *st, double minDistance)
{
- const base::State *newState = NULL;
+ const base::State *newState = nullptr;
bool added = false;
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
if (GoalStates::distanceGoal(st) > minDistance)
{
GoalStates::addState(st);
diff --git a/src/ompl/base/goals/src/GoalRegion.cpp b/src/ompl/base/goals/src/GoalRegion.cpp
index 56038c9..e0a7919 100644
--- a/src/ompl/base/goals/src/GoalRegion.cpp
+++ b/src/ompl/base/goals/src/GoalRegion.cpp
@@ -45,7 +45,7 @@ ompl::base::GoalRegion::GoalRegion(const SpaceInformationPtr &si) : Goal(si), th
bool ompl::base::GoalRegion::isSatisfied(const State *st) const
{
- return isSatisfied(st, NULL);
+ return isSatisfied(st, nullptr);
}
bool ompl::base::GoalRegion::isSatisfied(const State *st, double *distance) const
diff --git a/src/ompl/base/goals/src/GoalStates.cpp b/src/ompl/base/goals/src/GoalStates.cpp
index 2cd597d..4adbd52 100644
--- a/src/ompl/base/goals/src/GoalStates.cpp
+++ b/src/ompl/base/goals/src/GoalStates.cpp
@@ -37,7 +37,6 @@
#include "ompl/base/goals/GoalStates.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/Exception.h"
-#include <boost/lexical_cast.hpp>
#include <limits>
ompl::base::GoalStates::~GoalStates()
@@ -110,8 +109,8 @@ void ompl::base::GoalStates::addState(const ScopedState<> &st)
const ompl::base::State* ompl::base::GoalStates::getState(unsigned int index) const
{
if (index >= states_.size())
- throw Exception("Index " + boost::lexical_cast<std::string>(index) + " out of range. Only " +
- boost::lexical_cast<std::string>(states_.size()) + " states are available");
+ throw Exception("Index " + std::to_string(index) + " out of range. Only " +
+ std::to_string(states_.size()) + " states are available");
return states_[index];
}
diff --git a/src/ompl/base/objectives/VFMechanicalWorkOptimizationObjective.h b/src/ompl/base/objectives/VFMechanicalWorkOptimizationObjective.h
new file mode 100644
index 0000000..c0d9887
--- /dev/null
+++ b/src/ompl/base/objectives/VFMechanicalWorkOptimizationObjective.h
@@ -0,0 +1,99 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Caleb Voss and Wilson Beebe
+* 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.
+*********************************************************************/
+
+/* Authors: Caleb Voss, Wilson Beebe */
+
+#ifndef OMPL_BASE_OBJECTIVES_VF_MECHANICAL_WORK_OPTIMIZATION_OBJECTIVE_
+#define OMPL_BASE_OBJECTIVES_VF_MECHANICAL_WORK_OPTIMIZATION_OBJECTIVE_
+
+#include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
+#include "ompl/geometric/planners/rrt/VFRRT.h"
+
+namespace ompl
+{
+ namespace base
+ {
+
+ /**
+ * Optimization objective that computes mechanical work between two states by following a vector field.
+ */
+ class VFMechanicalWorkOptimizationObjective : public ompl::base::MechanicalWorkOptimizationObjective
+ {
+
+ public:
+
+ /** Constructor. */
+ VFMechanicalWorkOptimizationObjective(const ompl::base::SpaceInformationPtr &si,
+ const geometric::VFRRT::VectorField& vf)
+ : ompl::base::MechanicalWorkOptimizationObjective(si), vf_(vf)
+ {
+ }
+
+ /** Assume we can always do better. */
+ bool isSatisfied(ompl::base::Cost c) const
+ {
+ return false;
+ }
+
+ /** Compute mechanical work between two states. */
+ ompl::base::Cost motionCost(const ompl::base::State *s1, const ompl::base::State *s2) const
+ {
+ const base::StateSpacePtr &space = si_->getStateSpace();
+ // Per equation 7 in the paper
+ Eigen::VectorXd f = vf_(s2);
+ unsigned int vfdim = f.size();
+ Eigen::VectorXd qprime(vfdim);
+
+ for (unsigned int i = 0; i < vfdim; i++)
+ qprime[i] = *space->getValueAddressAtIndex(s2, i)
+ - *space->getValueAddressAtIndex(s1, i);
+
+ // Don't included negative work
+ double positiveCostAccrued = std::max(-(f.dot(qprime)), 0.);
+ return ompl::base::Cost(positiveCostAccrued + pathLengthWeight_ * si_->distance(s1, s2));
+ }
+
+ bool isSymmetric(void) const
+ {
+ return false;
+ }
+
+ protected:
+ /** VectorField associated with the space. */
+ geometric::VFRRT::VectorField vf_;
+ };
+ }
+}
+
+#endif
diff --git a/src/ompl/base/objectives/VFUpstreamCriterionOptimizationObjective.h b/src/ompl/base/objectives/VFUpstreamCriterionOptimizationObjective.h
new file mode 100644
index 0000000..d24897d
--- /dev/null
+++ b/src/ompl/base/objectives/VFUpstreamCriterionOptimizationObjective.h
@@ -0,0 +1,115 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Caleb Voss and Wilson Beebe
+* 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.
+*********************************************************************/
+
+/* Authors: Caleb Voss, Wilson Beebe */
+
+#ifndef OMPL_BASE_OBJECTIVES_VF_UPSTREAM_CRITERION_OPTIMIZATION_OBJECTIVE_
+#define OMPL_BASE_OBJECTIVES_VF_UPSTREAM_CRITERION_OPTIMIZATION_OBJECTIVE_
+
+#include "ompl/base/OptimizationObjective.h"
+#include "ompl/geometric/planners/rrt/VFRRT.h"
+
+namespace ompl
+{
+ namespace base
+ {
+
+ /**
+ * Optimization objective that computes the upstream criterion between two states.
+ */
+ class VFUpstreamCriterionOptimizationObjective : public ompl::base::OptimizationObjective
+ {
+
+ public:
+
+ /** Constructor. */
+ VFUpstreamCriterionOptimizationObjective(const ompl::base::SpaceInformationPtr &si,
+ const geometric::VFRRT::VectorField &vf)
+ : ompl::base::OptimizationObjective(si), vf_(vf)
+ {
+ description_ = "Upstream Criterion";
+ }
+
+ /** Assume we can always do better. */
+ bool isSatisfied(ompl::base::Cost c) const
+ {
+ return false;
+ }
+
+ /** \brief Returns a cost with a value of 0. */
+ virtual Cost stateCost(const State *s) const
+ {
+ return Cost(0.);
+ }
+
+ /** Compute upstream criterion between two states. */
+ ompl::base::Cost motionCost(const State *s1, const State *s2) const
+ {
+ const base::StateSpacePtr &space = si_->getStateSpace();
+ // Per equation 1 in the paper, Riemann approximation on the left
+ unsigned int vfdim = space->getValueLocations().size();
+ Eigen::VectorXd qprime(vfdim);
+ unsigned int numSegments = space->validSegmentCount(s1, s2);
+ std::vector<ompl::base::State*> interp;
+
+ for (unsigned int i = 0; i < vfdim; i++)
+ qprime[i] = *space->getValueAddressAtIndex(s2, i)
+ - *space->getValueAddressAtIndex(s1, i);
+ qprime.normalize();
+ si_->getMotionStates(s1, s2, interp, numSegments - 1, true, true);
+ double cost = 0;
+ for (unsigned int i = 0; i < interp.size() - 1; i++)
+ {
+ Eigen::VectorXd f = vf_(interp[i]);
+ cost += si_->distance(interp[i], interp[i + 1]) * (f.norm() - f.dot(qprime));
+ si_->freeState(interp[i]);
+ }
+ si_->freeState(interp[interp.size()-1]);
+ return ompl::base::Cost(cost);
+ }
+
+ bool isSymmetric(void) const
+ {
+ return false;
+ }
+
+ protected:
+ /** VectorField associated with the space. */
+ geometric::VFRRT::VectorField vf_;
+ };
+
+ }
+}
+
+#endif
diff --git a/src/ompl/base/objectives/src/PathLengthOptimizationObjective.cpp b/src/ompl/base/objectives/src/PathLengthOptimizationObjective.cpp
index c678381..d525d27 100644
--- a/src/ompl/base/objectives/src/PathLengthOptimizationObjective.cpp
+++ b/src/ompl/base/objectives/src/PathLengthOptimizationObjective.cpp
@@ -35,7 +35,7 @@
/* Author: Luis G. Torres, Jonathan Gammell */
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
-#include <boost/make_shared.hpp>
+#include <memory>
#if OMPL_HAVE_EIGEN3
#include "ompl/base/samplers/informed/PathLengthDirectInfSampler.h"
#else
@@ -49,7 +49,8 @@ PathLengthOptimizationObjective(const SpaceInformationPtr &si) :
description_ = "Path Length";
//Setup a default cost-to-go heuristics:
- setCostToGoHeuristic(boost::bind(&base::goalRegionCostToGo, _1, _2));
+ setCostToGoHeuristic(std::bind(&base::goalRegionCostToGo,
+ std::placeholders::_1, std::placeholders::_2));
}
ompl::base::Cost ompl::base::PathLengthOptimizationObjective::stateCost(const State *s) const
@@ -71,7 +72,7 @@ ompl::base::InformedSamplerPtr ompl::base::PathLengthOptimizationObjective::allo
{
// Make the direct path-length informed sampler and return. If OMPL was compiled with Eigen, a direct version is available, if not a rejection-based technique can be used
#if OMPL_HAVE_EIGEN3
- return boost::make_shared<PathLengthDirectInfSampler>(probDefn, maxNumberCalls);
+ return std::make_shared<PathLengthDirectInfSampler>(probDefn, maxNumberCalls);
#else
throw Exception("Direct sampling of the path-length objective requires Eigen, but this version of OMPL was compiled without Eigen support. If possible, please install Eigen and recompile OMPL. If this is not possible, you can manually create an instantiation of RejectionInfSampler to approximate the behaviour of direct informed sampling.");
// Return a null pointer to avoid compiler warnings
diff --git a/src/ompl/base/samplers/InformedStateSampler.h b/src/ompl/base/samplers/InformedStateSampler.h
index 3210202..a37693f 100644
--- a/src/ompl/base/samplers/InformedStateSampler.h
+++ b/src/ompl/base/samplers/InformedStateSampler.h
@@ -44,10 +44,8 @@
// We use a pointer to the problem definition to access problem and solution data.
#include "ompl/base/ProblemDefinition.h"
-//For boost::function
-#include <boost/function.hpp>
-//For boost::noncopyable
-#include <boost/noncopyable.hpp>
+//For std::function
+#include <functional>
namespace ompl
{
@@ -60,9 +58,13 @@ namespace ompl
/** \brief An abstract class for the concept of using information about the state space
and the current solution cost to limit future search to a planning
subproblem that contains all possibly better solutions. */
- class InformedSampler : private boost::noncopyable
+ class InformedSampler
{
public:
+ // non-copyable
+ InformedSampler(const InformedSampler&) = delete;
+ InformedSampler& operator=(const InformedSampler&) = delete;
+
/** \brief Construct a sampler that only generates states with a heuristic solution estimate that is less than the cost of the current solution. Requires a function pointer to a method to query the cost of the current solution.
If iteration is required, only maxNumberCalls are attempted, to assure that the function returns. */
InformedSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls);
@@ -110,7 +112,7 @@ namespace ompl
public:
/** \brief The definition of a function pointer for querying the current solution cost. */
- typedef boost::function<Cost ()> GetCurrentCostFunc;
+ typedef std::function<Cost ()> GetCurrentCostFunc;
/** \brief Construct a sampler that only generates states with a heuristic solution estimate that is less than the cost of the current solution using the default informed sampler for the current optimization objective. Requires a function pointer to a method to query the cost of the current solution. */
InformedStateSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls, const GetCurrentCostFunc &costFunc);
diff --git a/src/ompl/base/samplers/MinimumClearanceValidStateSampler.h b/src/ompl/base/samplers/MinimumClearanceValidStateSampler.h
new file mode 100644
index 0000000..946f270
--- /dev/null
+++ b/src/ompl/base/samplers/MinimumClearanceValidStateSampler.h
@@ -0,0 +1,94 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016, University of Colorado, Boulder
+ * 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 Univ of CO, Boulder 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: Dave Coleman <dave at dav.ee>
+ Desc: Find a valid sample with a minimum distance to nearby obstacles
+ (clearance threshold)
+*/
+
+#ifndef OMPL_BASE_SAMPLERS_MINIMUM_CLEARANCE_VALID_STATE_SAMPLER_
+#define OMPL_BASE_SAMPLERS_MINIMUM_CLEARANCE_VALID_STATE_SAMPLER_
+
+#include "ompl/base/ValidStateSampler.h"
+#include "ompl/base/StateSampler.h"
+
+namespace ompl
+{
+ namespace base
+ {
+
+ /// @cond IGNORE
+ OMPL_CLASS_FORWARD(MinimumClearanceValidStateSampler);
+ /// @endcond
+
+ /** \brief Generate valid samples randomly with extra requirement of min for clearance to nearest obstacle */
+ class MinimumClearanceValidStateSampler : public ValidStateSampler
+ {
+ public:
+
+ /** \brief Constructor */
+ MinimumClearanceValidStateSampler(const SpaceInformation *si);
+
+ virtual ~MinimumClearanceValidStateSampler() {};
+
+ virtual bool sample(State *state);
+
+ virtual bool sampleNear(State *state, const State *near, const double distance);
+
+ /** \brief Set the minimum required distance of sample from nearest obstacle to be considered valid */
+ void setMinimumObstacleClearance(double clearance)
+ {
+ clearance_ = clearance;
+ }
+
+ /** \brief Get the minimum required distance of sample from nearest obstacle to be considered valid */
+ unsigned int getMinimumObstacleClearance() const
+ {
+ return clearance_;
+ }
+
+ protected:
+
+ /** \brief The sampler to build upon */
+ StateSamplerPtr sampler_;
+
+ /** \brief Minimum required distance of sample from nearest obstacle to be considered valid */
+ double clearance_;
+ };
+
+ }
+}
+
+
+#endif
diff --git a/src/ompl/base/samplers/informed/PathLengthDirectInfSampler.h b/src/ompl/base/samplers/informed/PathLengthDirectInfSampler.h
index e42902f..b715096 100644
--- a/src/ompl/base/samplers/informed/PathLengthDirectInfSampler.h
+++ b/src/ompl/base/samplers/informed/PathLengthDirectInfSampler.h
@@ -102,7 +102,7 @@ namespace ompl
private:
/** \brief A constant pointer to ProlateHyperspheroid */
- typedef boost::shared_ptr<const ompl::ProlateHyperspheroid> ProlateHyperspheroidCPtr;
+ typedef std::shared_ptr<const ompl::ProlateHyperspheroid> ProlateHyperspheroidCPtr;
// Helper functions:
// High level
@@ -134,7 +134,7 @@ namespace ompl
/** \brief Iterate through the list of PHSs and return true if the sample is in any of them */
bool isInAnyPhs(const std::vector<double>& informedVector) const;
- /** \brief Return true if the sample is in the specified PHS. Really just a wrapper to aid with boost::bind */
+ /** \brief Return true if the sample is in the specified PHS. Really just a wrapper to aid with std::bind */
bool isInPhs(const ProlateHyperspheroidCPtr &phsCPtr, const std::vector<double> &informedVector) const;
/** \brief Iterate through the list of PHSs and return the number of PHSs that the sample is in */
diff --git a/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp b/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp
index 30d1208..c244008 100644
--- a/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp
+++ b/src/ompl/base/samplers/informed/src/PathLengthDirectInfSampler.cpp
@@ -42,8 +42,8 @@
#include "ompl/base/StateSpace.h"
#include "ompl/base/spaces/RealVectorStateSpace.h"
-// For boost::make_shared
-#include <boost/make_shared.hpp>
+// For std::make_shared
+#include <memory>
// For std::vector
#include <vector>
@@ -207,7 +207,7 @@ namespace ompl
std::vector<double> goalFocusVector = getInformedSubstate(goalStates.at(j));
// Create the definition of the PHS
- listPhsPtrs_.push_back(boost::make_shared<ProlateHyperspheroid>(informedSubSpace_->getDimension(), &startFocusVector[0], &goalFocusVector[0]));
+ listPhsPtrs_.push_back(std::make_shared<ProlateHyperspheroid>(informedSubSpace_->getDimension(), &startFocusVector[0], &goalFocusVector[0]));
}
}
diff --git a/src/ompl/base/samplers/src/GaussianValidStateSampler.cpp b/src/ompl/base/samplers/src/GaussianValidStateSampler.cpp
index 2ede2d1..b8ea382 100644
--- a/src/ompl/base/samplers/src/GaussianValidStateSampler.cpp
+++ b/src/ompl/base/samplers/src/GaussianValidStateSampler.cpp
@@ -43,8 +43,8 @@ ompl::base::GaussianValidStateSampler::GaussianValidStateSampler(const SpaceInfo
{
name_ = "gaussian";
params_.declareParam<double>("standard_deviation",
- boost::bind(&GaussianValidStateSampler::setStdDev, this, _1),
- boost::bind(&GaussianValidStateSampler::getStdDev, this));
+ std::bind(&GaussianValidStateSampler::setStdDev, this, std::placeholders::_1),
+ std::bind(&GaussianValidStateSampler::getStdDev, this));
}
bool ompl::base::GaussianValidStateSampler::sample(State *state)
diff --git a/src/ompl/base/samplers/src/MaximizeClearanceValidStateSampler.cpp b/src/ompl/base/samplers/src/MaximizeClearanceValidStateSampler.cpp
index 1fe0f4a..d4df99c 100644
--- a/src/ompl/base/samplers/src/MaximizeClearanceValidStateSampler.cpp
+++ b/src/ompl/base/samplers/src/MaximizeClearanceValidStateSampler.cpp
@@ -42,8 +42,8 @@ ompl::base::MaximizeClearanceValidStateSampler::MaximizeClearanceValidStateSampl
{
name_ = "max_clearance";
params_.declareParam<unsigned int>("nr_improve_attempts",
- boost::bind(&MaximizeClearanceValidStateSampler::setNrImproveAttempts, this, _1),
- boost::bind(&MaximizeClearanceValidStateSampler::getNrImproveAttempts, this));
+ std::bind(&MaximizeClearanceValidStateSampler::setNrImproveAttempts, this, std::placeholders::_1),
+ std::bind(&MaximizeClearanceValidStateSampler::getNrImproveAttempts, this));
}
ompl::base::MaximizeClearanceValidStateSampler::~MaximizeClearanceValidStateSampler()
diff --git a/src/ompl/base/samplers/src/MinimumClearanceValidStateSampler.cpp b/src/ompl/base/samplers/src/MinimumClearanceValidStateSampler.cpp
new file mode 100644
index 0000000..385377f
--- /dev/null
+++ b/src/ompl/base/samplers/src/MinimumClearanceValidStateSampler.cpp
@@ -0,0 +1,94 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016, University of Colorado, Boulder
+ * 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 Univ of CO, Boulder 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: Dave Coleman <dave at dav.ee>
+ Desc: Find a valid sample with a minimum distance to nearby obstacles
+ (clearance threshold)
+*/
+
+#include "ompl/base/samplers/MinimumClearanceValidStateSampler.h"
+#include "ompl/base/SpaceInformation.h"
+
+ompl::base::MinimumClearanceValidStateSampler::MinimumClearanceValidStateSampler(const SpaceInformation *si) :
+ ValidStateSampler(si), sampler_(si->allocStateSampler()), clearance_(1)
+{
+ name_ = "min_clearance";
+ params_.declareParam<unsigned int>("min_obstacle_clearance",
+ std::bind(&MinimumClearanceValidStateSampler::setMinimumObstacleClearance, this, std::placeholders::_1),
+ std::bind(&MinimumClearanceValidStateSampler::getMinimumObstacleClearance, this));
+}
+
+bool ompl::base::MinimumClearanceValidStateSampler::sample(State *state)
+{
+ unsigned int attempts = 0;
+ bool valid = false;
+ double dist = 0.0;
+ do
+ {
+ sampler_->sampleUniform(state);
+ valid = si_->getStateValidityChecker()->isValid(state, dist);
+
+ // Also check for distance to nearest obstacle and invalidate if too close
+ if (dist < clearance_)
+ {
+ valid = false;
+ }
+
+ ++attempts;
+ } while (!valid && attempts < attempts_);
+
+ return valid;
+}
+
+bool ompl::base::MinimumClearanceValidStateSampler::sampleNear(State *state, const State *near, const double distance)
+{
+ unsigned int attempts = 0;
+ bool valid = false;
+ double dist = 0.0;
+ do
+ {
+ sampler_->sampleUniformNear(state, near, distance);
+ valid = si_->getStateValidityChecker()->isValid(state, dist);
+
+ // Also check for distance to nearest obstacle and invalidate if too close
+ if (dist < clearance_)
+ {
+ valid = false;
+ }
+
+ ++attempts;
+ } while (!valid && attempts < attempts_);
+
+ return valid;
+}
diff --git a/src/ompl/base/spaces/SO3StateSpace.h b/src/ompl/base/spaces/SO3StateSpace.h
index 1da39a7..89862f0 100644
--- a/src/ompl/base/spaces/SO3StateSpace.h
+++ b/src/ompl/base/spaces/SO3StateSpace.h
@@ -95,7 +95,7 @@ namespace ompl
{
public:
- /** \brief Set the quaternion from axis-angle representation */
+ /** \brief Set the quaternion from axis-angle representation. The angle is given in radians. */
void setAxisAngle(double ax, double ay, double az, double angle);
/** \brief Set the state to identity -- no rotation */
diff --git a/src/ompl/base/spaces/src/DiscreteStateSpace.cpp b/src/ompl/base/spaces/src/DiscreteStateSpace.cpp
index af959d6..ecfd1b7 100644
--- a/src/ompl/base/spaces/src/DiscreteStateSpace.cpp
+++ b/src/ompl/base/spaces/src/DiscreteStateSpace.cpp
@@ -193,7 +193,7 @@ void ompl::base::DiscreteStateSpace::printState(const State *state, std::ostream
if (state)
out << state->as<StateType>()->value;
else
- out << "NULL";
+ out << "nullptr";
out << ']' << std::endl;
}
diff --git a/src/ompl/base/spaces/src/RealVectorStateSpace.cpp b/src/ompl/base/spaces/src/RealVectorStateSpace.cpp
index f47114f..2a3303e 100644
--- a/src/ompl/base/spaces/src/RealVectorStateSpace.cpp
+++ b/src/ompl/base/spaces/src/RealVectorStateSpace.cpp
@@ -37,7 +37,6 @@
#include "ompl/base/spaces/RealVectorStateSpace.h"
#include "ompl/base/spaces/RealVectorStateProjections.h"
#include "ompl/util/Exception.h"
-#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <cstring>
#include <limits>
@@ -126,8 +125,8 @@ void ompl::base::RealVectorStateSpace::setBounds(const RealVectorBounds &bounds)
bounds.check();
if (bounds.low.size() != dimension_)
throw Exception("Bounds do not match dimension of state space: expected dimension " +
- boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
- boost::lexical_cast<std::string>(bounds.low.size()));
+ std::to_string(dimension_) + " but got dimension " +
+ std::to_string(bounds.low.size()));
bounds_ = bounds;
}
@@ -290,7 +289,7 @@ void ompl::base::RealVectorStateSpace::freeState(State *state) const
double* ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
{
- return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
+ return index < dimension_ ? static_cast<StateType*>(state)->values + index : nullptr;
}
void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
@@ -307,7 +306,7 @@ void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostre
}
}
else
- out << "NULL" << std::endl;
+ out << "nullptr" << std::endl;
out << ']' << std::endl;
}
diff --git a/src/ompl/base/spaces/src/SO2StateSpace.cpp b/src/ompl/base/spaces/src/SO2StateSpace.cpp
index e45166f..015b24d 100644
--- a/src/ompl/base/spaces/src/SO2StateSpace.cpp
+++ b/src/ompl/base/spaces/src/SO2StateSpace.cpp
@@ -205,7 +205,7 @@ void ompl::base::SO2StateSpace::registerProjections()
double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
{
- return index == 0 ? &(state->as<StateType>()->value) : NULL;
+ return index == 0 ? &(state->as<StateType>()->value) : nullptr;
}
void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
@@ -214,7 +214,7 @@ void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out
if (state)
out << state->as<StateType>()->value;
else
- out << "NULL";
+ out << "nullptr";
out << ']' << std::endl;
}
diff --git a/src/ompl/base/spaces/src/SO3StateSpace.cpp b/src/ompl/base/spaces/src/SO3StateSpace.cpp
index 9092876..6998d98 100644
--- a/src/ompl/base/spaces/src/SO3StateSpace.cpp
+++ b/src/ompl/base/spaces/src/SO3StateSpace.cpp
@@ -40,19 +40,8 @@
#include <cmath>
#include "ompl/tools/config/MagicConstants.h"
#include <boost/math/constants/constants.hpp>
-#include <boost/version.hpp>
#include <boost/assert.hpp>
-#ifndef BOOST_ASSERT_MSG
-#define BOOST_ASSERT_MSG(expr, msg) assert(expr)
-#endif
-
-#if BOOST_VERSION < 105000
-namespace boost{ namespace math{ namespace constants{
-BOOST_DEFINE_MATH_CONSTANT(root_three, 1.732050807568877293527446341505872366, 1.73205080756887729352744634150587236694280525381038062805580697945193301690880003708114618675724857567562614142, 0)
-}}}
-#endif
-
static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
/// @cond IGNORE
@@ -159,7 +148,7 @@ void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mea
SO3StateSpace::StateType q,
*qs = static_cast<SO3StateSpace::StateType*>(state);
const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
- double half_theta = theta / 2.0;
+ double half_theta = theta / 2.0;
double s = sin(half_theta) / theta;
q.w = cos(half_theta);
q.x = s * x;
@@ -380,7 +369,7 @@ void ompl::base::SO3StateSpace::registerProjections()
double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
{
- return index < 4 ? &(state->as<StateType>()->x) + index : NULL;
+ return index < 4 ? &(state->as<StateType>()->x) + index : nullptr;
}
void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
@@ -392,7 +381,7 @@ void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out
out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
}
else
- out << "NULL";
+ out << "nullptr";
out << ']' << std::endl;
}
diff --git a/src/ompl/base/spaces/src/TimeStateSpace.cpp b/src/ompl/base/spaces/src/TimeStateSpace.cpp
index a9fff12..e846ba6 100644
--- a/src/ompl/base/spaces/src/TimeStateSpace.cpp
+++ b/src/ompl/base/spaces/src/TimeStateSpace.cpp
@@ -197,7 +197,7 @@ void ompl::base::TimeStateSpace::registerProjections()
double* ompl::base::TimeStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
{
- return index == 0 ? &(state->as<StateType>()->position) : NULL;
+ return index == 0 ? &(state->as<StateType>()->position) : nullptr;
}
void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &out) const
@@ -206,7 +206,7 @@ void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &ou
if (state)
out << state->as<StateType>()->position;
else
- out << "NULL";
+ out << "nullptr";
out << ']' << std::endl;
}
diff --git a/src/ompl/base/src/Goal.cpp b/src/ompl/base/src/Goal.cpp
index 78807c6..f396b0e 100644
--- a/src/ompl/base/src/Goal.cpp
+++ b/src/ompl/base/src/Goal.cpp
@@ -44,7 +44,7 @@ ompl::base::Goal::Goal(const SpaceInformationPtr &si) :
bool ompl::base::Goal::isSatisfied(const State *st, double *distance) const
{
- if (distance != NULL)
+ if (distance != nullptr)
*distance = std::numeric_limits<double>::max();
return isSatisfied(st);
}
diff --git a/src/ompl/base/src/OptimizationObjective.cpp b/src/ompl/base/src/OptimizationObjective.cpp
index c6bc4ee..03f6424 100644
--- a/src/ompl/base/src/OptimizationObjective.cpp
+++ b/src/ompl/base/src/OptimizationObjective.cpp
@@ -39,8 +39,8 @@
#include "ompl/base/goals/GoalRegion.h"
#include "ompl/base/samplers/informed/RejectionInfSampler.h"
#include <limits>
-// For boost::make_shared
-#include <boost/make_shared.hpp>
+// For std::make_shared
+#include <memory>
ompl::base::OptimizationObjective::OptimizationObjective(const SpaceInformationPtr &si) :
si_(si),
@@ -167,7 +167,7 @@ const ompl::base::SpaceInformationPtr& ompl::base::OptimizationObjective::getSpa
ompl::base::InformedSamplerPtr ompl::base::OptimizationObjective::allocInformedStateSampler(const ProblemDefinitionPtr probDefn, unsigned int maxNumberCalls) const
{
OMPL_INFORM("%s: No direct informed sampling scheme is defined, defaulting to rejection sampling.", description_.c_str());
- return boost::make_shared<RejectionInfSampler>(probDefn, maxNumberCalls);
+ return std::make_shared<RejectionInfSampler>(probDefn, maxNumberCalls);
}
void ompl::base::OptimizationObjective::print(std::ostream &out) const
diff --git a/src/ompl/base/src/Planner.cpp b/src/ompl/base/src/Planner.cpp
index 5ae2df6..49d4fb0 100644
--- a/src/ompl/base/src/Planner.cpp
+++ b/src/ompl/base/src/Planner.cpp
@@ -38,7 +38,7 @@
#include "ompl/util/Exception.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include <sstream>
-#include <boost/thread.hpp>
+#include <thread>
ompl::base::Planner::Planner(const SpaceInformationPtr &si, const std::string &name) :
si_(si), pis_(this), name_(name), setup_(false)
@@ -159,12 +159,12 @@ void ompl::base::PlannerInputStates::clear()
if (tempState_)
{
si_->freeState(tempState_);
- tempState_ = NULL;
+ tempState_ = nullptr;
}
addedStartStates_ = 0;
sampledGoalsCount_ = 0;
- pdef_ = NULL;
- si_ = NULL;
+ pdef_ = nullptr;
+ si_ = nullptr;
}
void ompl::base::PlannerInputStates::restart()
@@ -229,7 +229,7 @@ bool ompl::base::PlannerInputStates::use(const ProblemDefinition *pdef)
const ompl::base::State* ompl::base::PlannerInputStates::nextStart()
{
- if (pdef_ == NULL || si_ == NULL)
+ if (pdef_ == nullptr || si_ == nullptr)
{
std::string error = "Missing space information or problem definition";
if (planner_)
@@ -258,7 +258,7 @@ const ompl::base::State* ompl::base::PlannerInputStates::nextStart()
ss.str().c_str());
}
}
- return NULL;
+ return nullptr;
}
const ompl::base::State* ompl::base::PlannerInputStates::nextGoal()
@@ -270,7 +270,7 @@ const ompl::base::State* ompl::base::PlannerInputStates::nextGoal()
const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
{
- if (pdef_ == NULL || si_ == NULL)
+ if (pdef_ == nullptr || si_ == nullptr)
{
std::string error = "Missing space information or problem definition";
if (planner_)
@@ -279,7 +279,7 @@ const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerT
throw Exception(error);
}
- const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL;
+ const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;
if (goal)
{
@@ -292,7 +292,7 @@ const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerT
if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
{
- if (tempState_ == NULL)
+ if (tempState_ == nullptr)
tempState_ = si_->allocState();
do
{
@@ -333,13 +333,13 @@ const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerT
OMPL_DEBUG("%s: Waiting for goal region samples ...",
planner_ ? planner_->getName().c_str() : "PlannerInputStates");
}
- boost::this_thread::sleep(time::seconds(0.01));
+ std::this_thread::sleep_for(time::seconds(0.01));
attempt = !ptc;
}
}
}
- return NULL;
+ return nullptr;
}
bool ompl::base::PlannerInputStates::haveMoreStartStates() const
diff --git a/src/ompl/base/src/PlannerData.cpp b/src/ompl/base/src/PlannerData.cpp
index 4266235..ee5b2aa 100644
--- a/src/ompl/base/src/PlannerData.cpp
+++ b/src/ompl/base/src/PlannerData.cpp
@@ -44,12 +44,7 @@
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
-#include <boost/version.hpp>
-#if BOOST_VERSION < 105100
-#warning Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML
-#else
#include <boost/property_map/function_property_map.hpp>
-#endif
// This is a convenient macro to cast the void* graph pointer as the
// Boost.Graph structure from PlannerDataGraph.h
@@ -71,7 +66,7 @@ ompl::base::PlannerData::~PlannerData ()
if (graph_)
{
delete graph_;
- graphRaw_ = NULL;
+ graphRaw_ = nullptr;
}
}
@@ -293,9 +288,6 @@ namespace
void ompl::base::PlannerData::printGraphML (std::ostream& out) const
{
-#if BOOST_VERSION < 105100
- OMPL_WARN("Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML");
-#else
// For some reason, make_function_property_map can't infer its
// template arguments corresponding to edgeWeightAsDouble's type
// signature. So, we have to use this horribly verbose
@@ -304,16 +296,16 @@ void ompl::base::PlannerData::printGraphML (std::ostream& out) const
// \todo Can we use make_function_property_map() here and have it
// infer the property template arguments?
boost::function_property_map<
- boost::function<double (ompl::base::PlannerData::Graph::Edge)>,
+ std::function<double (ompl::base::PlannerData::Graph::Edge)>,
ompl::base::PlannerData::Graph::Edge,
double>
- weightmap(boost::bind(&edgeWeightAsDouble, *graph_, _1));
+ weightmap(std::bind(&edgeWeightAsDouble, *graph_, std::placeholders::_1));
ompl::base::ScopedState<> s(si_);
boost::function_property_map<
- boost::function<std::string (ompl::base::PlannerData::Graph::Vertex)>,
+ std::function<std::string (ompl::base::PlannerData::Graph::Vertex)>,
ompl::base::PlannerData::Graph::Vertex,
std::string >
- coordsmap(boost::bind(&vertexCoords, *graph_, s, _1));
+ coordsmap(std::bind(&vertexCoords, *graph_, s, std::placeholders::_1));
// Not writing vertex or edge structures.
@@ -322,7 +314,6 @@ void ompl::base::PlannerData::printGraphML (std::ostream& out) const
dp.property("coords", coordsmap);
boost::write_graphml(out, *graph_, dp);
-#endif
}
unsigned int ompl::base::PlannerData::vertexIndex (const PlannerDataVertex &v) const
@@ -404,7 +395,7 @@ ompl::base::PlannerDataVertex& ompl::base::PlannerData::getGoalVertex (unsigned
unsigned int ompl::base::PlannerData::addVertex (const PlannerDataVertex &st)
{
// Do not add vertices with null states
- if (st.getState() == NULL)
+ if (st.getState() == nullptr)
return INVALID_INDEX;
unsigned int index = vertexIndex(st);
@@ -534,7 +525,7 @@ bool ompl::base::PlannerData::removeVertex (unsigned int vIndex)
{
decoupledStates_.erase(vtxState);
si_->freeState(vtxState);
- vtxState = NULL;
+ vtxState = nullptr;
}
// Slay the vertex
@@ -670,8 +661,9 @@ void ompl::base::PlannerData::extractMinimumSpanningTree (unsigned int v,
boost::dijkstra_shortest_paths
(*graph_, v,
boost::predecessor_map(&pred[0]).
- distance_compare(boost::bind(&base::OptimizationObjective::
- isCostBetterThan, &opt, _1, _2)).
+ distance_compare(std::bind(&base::OptimizationObjective::
+ isCostBetterThan, &opt,
+ std::placeholders::_1, std::placeholders::_2)).
distance_combine(&project2nd).
distance_inf(opt.infiniteCost()).
distance_zero(opt.identityCost()));
diff --git a/src/ompl/base/src/PlannerTerminationCondition.cpp b/src/ompl/base/src/PlannerTerminationCondition.cpp
index 606883e..02101ec 100644
--- a/src/ompl/base/src/PlannerTerminationCondition.cpp
+++ b/src/ompl/base/src/PlannerTerminationCondition.cpp
@@ -36,9 +36,8 @@
#include "ompl/base/PlannerTerminationCondition.h"
#include "ompl/util/Time.h"
-#include <boost/bind.hpp>
-#include <boost/thread.hpp>
-#include <boost/lambda/bind.hpp>
+#include <functional>
+#include <thread>
#include <utility>
namespace ompl
@@ -54,7 +53,7 @@ namespace ompl
fn_(fn),
period_(period),
terminate_(false),
- thread_(NULL),
+ thread_(nullptr),
evalValue_(false),
signalThreadStop_(false)
{
@@ -91,7 +90,7 @@ namespace ompl
{
signalThreadStop_ = false;
evalValue_ = false;
- thread_ = new boost::thread(boost::bind(&PlannerTerminationConditionImpl::periodicEval, this));
+ thread_ = new std::thread(std::bind(&PlannerTerminationConditionImpl::periodicEval, this));
}
}
@@ -103,7 +102,7 @@ namespace ompl
{
thread_->join();
delete thread_;
- thread_ = NULL;
+ thread_ = nullptr;
}
}
@@ -128,7 +127,7 @@ namespace ompl
{
if (terminate_ || signalThreadStop_)
break;
- boost::this_thread::sleep(s);
+ std::this_thread::sleep_for(s);
}
}
}
@@ -143,7 +142,7 @@ namespace ompl
mutable bool terminate_;
/** \brief Thread for periodicEval() */
- boost::thread *thread_;
+ std::thread *thread_;
/** \brief Cached value returned by fn_() */
bool evalValue_;
@@ -178,12 +177,12 @@ bool ompl::base::PlannerTerminationCondition::eval() const
ompl::base::PlannerTerminationCondition ompl::base::plannerNonTerminatingCondition()
{
- return PlannerTerminationCondition(boost::lambda::constant(false));
+ return PlannerTerminationCondition([] { return false; });
}
ompl::base::PlannerTerminationCondition ompl::base::plannerAlwaysTerminatingCondition()
{
- return PlannerTerminationCondition(boost::lambda::constant(true));
+ return PlannerTerminationCondition([] { return true; });
}
/// @cond IGNORE
@@ -212,12 +211,12 @@ namespace ompl
ompl::base::PlannerTerminationCondition ompl::base::plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
{
- return PlannerTerminationCondition(boost::bind(&plannerOrTerminationConditionAux, c1, c2));
+ return PlannerTerminationCondition(std::bind(&plannerOrTerminationConditionAux, c1, c2));
}
ompl::base::PlannerTerminationCondition ompl::base::plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
{
- return PlannerTerminationCondition(boost::bind(&plannerAndTerminationConditionAux, c1, c2));
+ return PlannerTerminationCondition(std::bind(&plannerAndTerminationConditionAux, c1, c2));
}
ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration)
@@ -227,19 +226,19 @@ ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondi
ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(time::duration duration)
{
- return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + duration));
+ return PlannerTerminationCondition(std::bind(&timePassed, time::now() + duration));
}
ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration, double interval)
{
if (interval > duration)
interval = duration;
- return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)), interval);
+ return PlannerTerminationCondition(std::bind(&timePassed, time::now() + time::seconds(duration)), interval);
}
ompl::base::PlannerTerminationCondition ompl::base::exactSolnPlannerTerminationCondition(ompl::base::ProblemDefinitionPtr pdef)
{
- return PlannerTerminationCondition(boost::bind(&ProblemDefinition::hasExactSolution, pdef));
+ return PlannerTerminationCondition(std::bind(&ProblemDefinition::hasExactSolution, pdef));
}
namespace ompl
@@ -266,7 +265,7 @@ namespace ompl
IterationTerminationCondition::operator PlannerTerminationCondition()
{
- return PlannerTerminationCondition( boost::bind(&IterationTerminationCondition::eval, this) );
+ return PlannerTerminationCondition( std::bind(&IterationTerminationCondition::eval, this) );
}
}
}
diff --git a/src/ompl/base/src/ProblemDefinition.cpp b/src/ompl/base/src/ProblemDefinition.cpp
index 9cddec8..ee07bd5 100644
--- a/src/ompl/base/src/ProblemDefinition.cpp
+++ b/src/ompl/base/src/ProblemDefinition.cpp
@@ -43,8 +43,7 @@
#include "ompl/tools/config/MagicConstants.h"
#include <sstream>
#include <algorithm>
-
-#include <boost/thread/mutex.hpp>
+#include <mutex>
/// @cond IGNORE
namespace ompl
@@ -62,7 +61,7 @@ namespace ompl
void add(const PlannerSolution &s)
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
int index = solutions_.size();
solutions_.push_back(s);
solutions_.back().index_ = index;
@@ -71,20 +70,20 @@ namespace ompl
void clear()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
solutions_.clear();
}
std::vector<PlannerSolution> getSolutions()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
std::vector<PlannerSolution> copy = solutions_;
return copy;
}
bool isApproximate()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
bool result = false;
if (!solutions_.empty())
result = solutions_[0].approximate_;
@@ -93,7 +92,7 @@ namespace ompl
bool isOptimized()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
bool result = false;
if (!solutions_.empty())
result = solutions_[0].optimized_;
@@ -102,7 +101,7 @@ namespace ompl
double getDifference()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
double diff = -1.0;
if (!solutions_.empty())
diff = solutions_[0].difference_;
@@ -111,7 +110,7 @@ namespace ompl
PathPtr getTopSolution()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
PathPtr copy;
if (!solutions_.empty())
copy = solutions_[0].path_;
@@ -120,7 +119,7 @@ namespace ompl
bool getTopSolution(PlannerSolution& solution)
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
if (!solutions_.empty())
{
@@ -135,7 +134,7 @@ namespace ompl
std::size_t getSolutionCount()
{
- boost::mutex::scoped_lock slock(lock_);
+ std::lock_guard<std::mutex> slock(lock_);
std::size_t result = solutions_.size();
return result;
}
@@ -143,7 +142,7 @@ namespace ompl
private:
std::vector<PlannerSolution> solutions_;
- boost::mutex lock_;
+ std::mutex lock_;
};
}
}
@@ -184,7 +183,7 @@ void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double
setGoal(GoalPtr(gs));
}
-bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex)
+bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex) const
{
for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
if (si_->equalStates(state, startStates_[i]))
@@ -280,10 +279,10 @@ void ompl::base::ProblemDefinition::getInputStates(std::vector<const State*> &st
ompl::base::PathPtr ompl::base::ProblemDefinition::isStraightLinePathValid() const
{
PathPtr path;
- if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
+ if (control::SpaceInformationPtr sic = std::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
{
unsigned int startIndex;
- if (isTrivial(&startIndex, NULL))
+ if (isTrivial(&startIndex, nullptr))
{
control::PathControl *pc = new control::PathControl(sic);
pc->append(startStates_[startIndex]);
@@ -471,14 +470,14 @@ void ompl::base::ProblemDefinition::print(std::ostream &out) const
if (goal_)
goal_->print(out);
else
- out << "Goal = NULL" << std::endl;
+ out << "Goal = nullptr" << std::endl;
if (optimizationObjective_)
{
optimizationObjective_->print(out);
out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
}
else
- out << "OptimizationObjective = NULL" << std::endl;
+ out << "OptimizationObjective = nullptr" << std::endl;
out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl;
}
diff --git a/src/ompl/base/src/ProjectionEvaluator.cpp b/src/ompl/base/src/ProjectionEvaluator.cpp
index f0b4c68..b45a286 100644
--- a/src/ompl/base/src/ProjectionEvaluator.cpp
+++ b/src/ompl/base/src/ProjectionEvaluator.cpp
@@ -43,15 +43,14 @@
#include "ompl/tools/config/MagicConstants.h"
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
-#include <boost/lexical_cast.hpp>
-#include <boost/bind.hpp>
+#include <functional>
#include <cmath>
#include <cstring>
#include <limits>
ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
{
- using namespace boost::numeric::ublas;
+ namespace nu = boost::numeric::ublas;
RNG rng;
Matrix projection(to, from);
@@ -59,7 +58,7 @@ ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom
for (unsigned int j = 0 ; j < from ; ++j)
{
if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
- boost::numeric::ublas::column(projection, j) = boost::numeric::ublas::zero_vector<double>(to);
+ nu::column(projection, j) = nu::zero_vector<double>(to);
else
for (unsigned int i = 0 ; i < to ; ++i)
projection(i, j) = rng.gaussian01();
@@ -67,10 +66,10 @@ ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom
for (unsigned int i = 0 ; i < to ; ++i)
{
- matrix_row<Matrix> row(projection, i);
+ nu::matrix_row<Matrix> row(projection, i);
for (unsigned int j = 0 ; j < i ; ++j)
{
- matrix_row<Matrix> prevRow(projection, j);
+ nu::matrix_row<Matrix> prevRow(projection, j);
// subtract projection
row -= inner_prod(row, prevRow) * prevRow;
}
@@ -87,7 +86,7 @@ ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom
if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
z++;
else
- boost::numeric::ublas::column(projection, i) /= scale[i];
+ nu::column(projection, i) /= scale[i];
}
if (z == from)
OMPL_WARN("Computed projection matrix is all 0s");
@@ -112,10 +111,10 @@ void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const
void ompl::base::ProjectionMatrix::project(const double *from, EuclideanProjection& to) const
{
- using namespace boost::numeric::ublas;
+ namespace nu = boost::numeric::ublas;
// create a temporary uBLAS vector from a C-style array without copying data
- shallow_array_adaptor<const double> tmp1(mat.size2(), from);
- vector<double, shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
+ nu::shallow_array_adaptor<const double> tmp1(mat.size2(), from);
+ nu::vector<double, nu::shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
to = prod(mat, tmp2);
}
@@ -129,7 +128,7 @@ ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space) :
bounds_(0), estimatedBounds_(0),
defaultCellSizes_(true), cellSizesWereInferred_(false)
{
- params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
+ params_.declareParam<double>("cellsize_factor", std::bind(&ProjectionEvaluator::mulCellSizes, this, std::placeholders::_1));
}
ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space) :
@@ -137,7 +136,7 @@ ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space)
bounds_(0), estimatedBounds_(0),
defaultCellSizes_(true), cellSizesWereInferred_(false)
{
- params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
+ params_.declareParam<double>("cellsize_factor", std::bind(&ProjectionEvaluator::mulCellSizes, this, std::placeholders::_1));
}
ompl::base::ProjectionEvaluator::~ProjectionEvaluator()
@@ -295,6 +294,9 @@ void ompl::base::ProjectionEvaluator::inferCellSizes()
void ompl::base::ProjectionEvaluator::setup()
{
+ typedef void(ProjectionEvaluator::*setCellSizesFunctionType)(unsigned int, double);
+ typedef double(ProjectionEvaluator::*getCellSizesFunctionType)(unsigned int) const;
+
if (defaultCellSizes_)
defaultCellSizes();
@@ -306,9 +308,9 @@ void ompl::base::ProjectionEvaluator::setup()
unsigned int dim = getDimension();
for (unsigned int i = 0 ; i < dim ; ++i)
- params_.declareParam<double>("cellsize." + boost::lexical_cast<std::string>(i),
- boost::bind(&ProjectionEvaluator::setCellSizes, this, i, _1),
- boost::bind(&ProjectionEvaluator::getCellSizes, this, i));
+ params_.declareParam<double>("cellsize." + std::to_string(i),
+ std::bind((setCellSizesFunctionType)&ProjectionEvaluator::setCellSizes, this, i, std::placeholders::_1),
+ std::bind((getCellSizesFunctionType)&ProjectionEvaluator::getCellSizes, this, i));
}
void ompl::base::ProjectionEvaluator::computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
@@ -350,7 +352,7 @@ ompl::base::SubspaceProjectionEvaluator::SubspaceProjectionEvaluator(const State
if (!space_->isCompound())
throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
if (space_->as<CompoundStateSpace>()->getSubspaceCount() <= index_)
- throw Exception("State space " + space_->getName() + " does not have a subspace at index " + boost::lexical_cast<std::string>(index_));
+ throw Exception("State space " + space_->getName() + " does not have a subspace at index " + std::to_string(index_));
}
void ompl::base::SubspaceProjectionEvaluator::setup()
@@ -360,7 +362,7 @@ void ompl::base::SubspaceProjectionEvaluator::setup()
else
proj_ = space_->as<CompoundStateSpace>()->getSubspace(index_)->getDefaultProjection();
if (!proj_)
- throw Exception("No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
+ throw Exception("No projection specified for subspace at index " + std::to_string(index_));
cellSizes_ = proj_->getCellSizes();
ProjectionEvaluator::setup();
diff --git a/src/ompl/base/src/SpaceInformation.cpp b/src/ompl/base/src/SpaceInformation.cpp
index dccf1e3..c4e1d66 100644
--- a/src/ompl/base/src/SpaceInformation.cpp
+++ b/src/ompl/base/src/SpaceInformation.cpp
@@ -306,6 +306,8 @@ bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states
if (!isValid(states[mid]))
return false;
+ pos.pop();
+
if (x.first < mid - 1)
pos.push(std::make_pair(x.first, mid));
if (x.second > mid + 1)
@@ -366,7 +368,7 @@ double ompl::base::SpaceInformation::averageValidMotionLength(unsigned int attem
State *s2 = allocState();
std::pair<State*, double> lastValid;
- lastValid.first = NULL;
+ lastValid.first = nullptr;
double d = 0.0;
unsigned int count = 0;
diff --git a/src/ompl/base/src/StateSpace.cpp b/src/ompl/base/src/StateSpace.cpp
index 9cfb5c5..a410151 100644
--- a/src/ompl/base/src/StateSpace.cpp
+++ b/src/ompl/base/src/StateSpace.cpp
@@ -36,11 +36,9 @@
#include "ompl/util/Exception.h"
#include "ompl/tools/config/MagicConstants.h"
#include "ompl/base/spaces/RealVectorStateSpace.h"
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/once.hpp>
-#include <boost/lexical_cast.hpp>
+#include <mutex>
#include <boost/scoped_ptr.hpp>
-#include <boost/bind.hpp>
+#include <functional>
#include <numeric>
#include <limits>
#include <queue>
@@ -63,12 +61,12 @@ namespace ompl
{
}
std::list<StateSpace*> list_;
- boost::mutex lock_;
+ std::mutex lock_;
unsigned int counter_;
};
static boost::scoped_ptr<AllocatedSpaces> g_allocatedSpaces;
- static boost::once_flag g_once = BOOST_ONCE_INIT;
+ static std::once_flag g_once;
void initAllocatedSpaces()
{
@@ -77,7 +75,7 @@ namespace ompl
AllocatedSpaces& getAllocatedSpaces()
{
- boost::call_once(&initAllocatedSpaces, g_once);
+ std::call_once(g_once, &initAllocatedSpaces);
return *g_allocatedSpaces;
}
} // namespace
@@ -88,10 +86,10 @@ namespace ompl
ompl::base::StateSpace::StateSpace()
{
AllocatedSpaces &as = getAllocatedSpaces();
- boost::mutex::scoped_lock smLock(as.lock_);
+ std::lock_guard<std::mutex> smLock(as.lock_);
// autocompute a unique name
- name_ = "Space" + boost::lexical_cast<std::string>(as.counter_++);
+ name_ = "Space" + std::to_string(as.counter_++);
longestValidSegment_ = 0.0;
longestValidSegmentFraction_ = 0.01; // 1%
@@ -102,19 +100,19 @@ ompl::base::StateSpace::StateSpace()
maxExtent_ = std::numeric_limits<double>::infinity();
params_.declareParam<double>("longest_valid_segment_fraction",
- boost::bind(&StateSpace::setLongestValidSegmentFraction, this, _1),
- boost::bind(&StateSpace::getLongestValidSegmentFraction, this));
+ std::bind(&StateSpace::setLongestValidSegmentFraction, this, std::placeholders::_1),
+ std::bind(&StateSpace::getLongestValidSegmentFraction, this));
params_.declareParam<unsigned int>("valid_segment_count_factor",
- boost::bind(&StateSpace::setValidSegmentCountFactor, this, _1),
- boost::bind(&StateSpace::getValidSegmentCountFactor, this));
+ std::bind(&StateSpace::setValidSegmentCountFactor, this, std::placeholders::_1),
+ std::bind(&StateSpace::getValidSegmentCountFactor, this));
as.list_.push_back(this);
}
ompl::base::StateSpace::~StateSpace()
{
AllocatedSpaces &as = getAllocatedSpaces();
- boost::mutex::scoped_lock smLock(as.lock_);
+ std::lock_guard<std::mutex> smLock(as.lock_);
as.list_.remove(this);
}
@@ -144,7 +142,7 @@ namespace ompl
loc.stateLocation.space = s;
substateMap[s->getName()] = loc.stateLocation;
State *test = s->allocState();
- if (s->getValueAddressAtIndex(test, 0) != NULL)
+ if (s->getValueAddressAtIndex(test, 0) != nullptr)
{
loc.index = 0;
locationsMap[s->getName()] = loc;
@@ -158,7 +156,7 @@ namespace ompl
locationsMap[name] = loc;
}
locationsArray.push_back(loc);
- while (s->getValueAddressAtIndex(test, ++loc.index) != NULL)
+ while (s->getValueAddressAtIndex(test, ++loc.index) != nullptr)
{
if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
{
@@ -224,6 +222,13 @@ void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
signature.insert(signature.begin(), signature.size());
}
+ompl::base::State* ompl::base::StateSpace::cloneState(const State* source) const
+{
+ State* copy = allocState();
+ copyState(copy, source);
+ return copy;
+}
+
void ompl::base::StateSpace::registerProjections()
{
}
@@ -297,7 +302,7 @@ const ompl::base::State* ompl::base::StateSpace::getSubstateAtLocation(const Sta
double* ompl::base::StateSpace::getValueAddressAtIndex(State* /*state*/, const unsigned int /*index*/) const
{
- return NULL;
+ return nullptr;
}
const double* ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
@@ -349,13 +354,13 @@ const double* ompl::base::StateSpace::getValueAddressAtLocation(const State *sta
double* ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
{
std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
- return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
+ return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
}
const double* ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
{
std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
- return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
+ return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
}
unsigned int ompl::base::StateSpace::getSerializationLength() const
@@ -515,7 +520,7 @@ void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::ve
void ompl::base::StateSpace::List(std::ostream &out)
{
AllocatedSpaces &as = getAllocatedSpaces();
- boost::mutex::scoped_lock smLock(as.lock_);
+ std::lock_guard<std::mutex> smLock(as.lock_);
for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
out << "@ " << *it << ": " << (*it)->getName() << std::endl;
}
@@ -557,7 +562,7 @@ void ompl::base::StateSpace::diagram(std::ostream &out) const
const StateSpace *s = m->as<CompoundStateSpace>()->getSubspace(i).get();
q.push(s);
out << '"' << m->getName() << "\" -> \"" << s->getName() << "\" [label=\"" <<
- boost::lexical_cast<std::string>(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << "\"];" << std::endl;
+ std::to_string(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << "\"];" << std::endl;
}
}
}
@@ -568,7 +573,7 @@ void ompl::base::StateSpace::diagram(std::ostream &out) const
void ompl::base::StateSpace::Diagram(std::ostream &out)
{
AllocatedSpaces &as = getAllocatedSpaces();
- boost::mutex::scoped_lock smLock(as.lock_);
+ std::lock_guard<std::mutex> smLock(as.lock_);
out << "digraph StateSpaces {" << std::endl;
for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
{
@@ -578,7 +583,7 @@ void ompl::base::StateSpace::Diagram(std::ostream &out)
{
if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubspace((*jt)->getName()))
out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
- boost::lexical_cast<std::string>((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
+ std::to_string((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
"\"];" << std::endl;
else
if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
@@ -602,7 +607,7 @@ void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int
State *s1 = allocState();
State *s2 = allocState();
StateSamplerPtr ss = allocStateSampler();
- char *serialization = NULL;
+ char *serialization = nullptr;
if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
serialization = new char[getSerializationLength()];
for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
@@ -640,13 +645,13 @@ void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int
double d21 = distance(s2, s1);
if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
throw Exception("The distance function should be symmetric (A->B=" +
- boost::lexical_cast<std::string>(d12) + ", B->A=" +
- boost::lexical_cast<std::string>(d21) + ", difference is " +
- boost::lexical_cast<std::string>(fabs(d12 - d21)) + ")");
+ std::to_string(d12) + ", B->A=" +
+ std::to_string(d21) + ", difference is " +
+ std::to_string(fabs(d12 - d21)) + ")");
if (flags & STATESPACE_DISTANCE_BOUND)
if (d12 > maxExt + zero)
throw Exception("The distance function should not report values larger than the maximum extent ("+
- boost::lexical_cast<std::string>(d12) + " > " + boost::lexical_cast<std::string>(maxExt) + ")");
+ std::to_string(d12) + " > " + std::to_string(maxExt) + ")");
}
}
if (serialization)
@@ -682,7 +687,7 @@ void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int
double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
- boost::lexical_cast<std::string>(diff) + " difference)");
+ std::to_string(diff) + " difference)");
interpolate(s3, s2, 0.5, s3);
interpolate(s1, s2, 0.75, s2);
@@ -1172,7 +1177,7 @@ double* ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, con
else
break;
}
- return NULL;
+ return nullptr;
}
void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
diff --git a/src/ompl/base/src/StateStorage.cpp b/src/ompl/base/src/StateStorage.cpp
index 645a740..0eb28ce 100644
--- a/src/ompl/base/src/StateStorage.cpp
+++ b/src/ompl/base/src/StateStorage.cpp
@@ -39,7 +39,7 @@
#include "ompl/util/Exception.h"
#include <fstream>
#include <algorithm>
-#include <boost/bind.hpp>
+#include <functional>
#include <boost/serialization/binary_object.hpp>
#include <boost/archive/archive_exception.hpp>
@@ -229,7 +229,7 @@ void ompl::base::StateStorage::clear()
states_.clear();
}
-void ompl::base::StateStorage::sort(const boost::function<bool(const State*, const State*)> &op)
+void ompl::base::StateStorage::sort(const std::function<bool(const State*, const State*)> &op)
{
std::sort(states_.begin(), states_.end(), op);
}
@@ -255,7 +255,7 @@ ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAlloc
throw Exception("Cannot allocate state sampler from empty state storage");
std::vector<int> sig;
space_->computeSignature(sig);
- return boost::bind(&allocPrecomputedStateSampler, _1, sig, &states_, from, to);
+ return std::bind(&allocPrecomputedStateSampler, std::placeholders::_1, sig, &states_, from, to);
}
void ompl::base::StateStorage::print(std::ostream &out) const
diff --git a/src/ompl/base/src/ValidStateSampler.cpp b/src/ompl/base/src/ValidStateSampler.cpp
index 2ee01de..08502c9 100644
--- a/src/ompl/base/src/ValidStateSampler.cpp
+++ b/src/ompl/base/src/ValidStateSampler.cpp
@@ -36,14 +36,14 @@
#include "ompl/base/ValidStateSampler.h"
#include "ompl/tools/config/MagicConstants.h"
-#include <boost/bind.hpp>
+#include <functional>
ompl::base::ValidStateSampler::ValidStateSampler(const SpaceInformation *si) :
si_(si), attempts_(magic::MAX_VALID_SAMPLE_ATTEMPTS), name_("not set")
{
params_.declareParam<unsigned int>("nr_attempts",
- boost::bind(&ValidStateSampler::setNrAttempts, this, _1),
- boost::bind(&ValidStateSampler::getNrAttempts, this));
+ std::bind(&ValidStateSampler::setNrAttempts, this, std::placeholders::_1),
+ std::bind(&ValidStateSampler::getNrAttempts, this));
}
ompl::base::ValidStateSampler::~ValidStateSampler()
diff --git a/src/ompl/contrib/rrt_star/RRTstar.h b/src/ompl/contrib/rrt_star/RRTstar.h
deleted file mode 100644
index dd84ff4..0000000
--- a/src/ompl/contrib/rrt_star/RRTstar.h
+++ /dev/null
@@ -1,2 +0,0 @@
-#warning "Please include <ompl/geometric/planners/rrt/RRTstar.h> directly"
-#include "ompl/geometric/planners/rrt/RRTstar.h"
diff --git a/src/ompl/control/ControlSampler.h b/src/ompl/control/ControlSampler.h
index cc9952b..46a35c3 100644
--- a/src/ompl/control/ControlSampler.h
+++ b/src/ompl/control/ControlSampler.h
@@ -42,8 +42,7 @@
#include "ompl/util/RandomNumbers.h"
#include "ompl/util/ClassForward.h"
#include <vector>
-#include <boost/function.hpp>
-#include <boost/noncopyable.hpp>
+#include <functional>
namespace ompl
{
@@ -60,16 +59,19 @@ namespace ompl
/// @endcond
/** \class ompl::control::ControlSamplerPtr
- \brief A boost shared pointer wrapper for ompl::control::ControlSampler */
+ \brief A shared pointer wrapper for ompl::control::ControlSampler */
/** \brief Abstract definition of a control sampler. Motion
planners that need to sample controls will call functions
from this class. Planners should call the versions of
sample() and sampleNext() with most arguments, whenever
this information is available. */
- class ControlSampler : private boost::noncopyable
+ class ControlSampler
{
public:
+ // non-copyable
+ ControlSampler(const ControlSampler&) = delete;
+ ControlSampler& operator=(const ControlSampler&) = delete;
/** \brief Constructor takes the state space to construct samples for as argument */
ControlSampler(const ControlSpace *space) : space_(space)
@@ -164,7 +166,7 @@ namespace ompl
};
/** \brief Definition of a function that can allocate a control sampler */
- typedef boost::function<ControlSamplerPtr(const ControlSpace*)> ControlSamplerAllocator;
+ typedef std::function<ControlSamplerPtr(const ControlSpace*)> ControlSamplerAllocator;
}
}
diff --git a/src/ompl/control/ControlSpace.h b/src/ompl/control/ControlSpace.h
index b397f32..2d505db 100644
--- a/src/ompl/control/ControlSpace.h
+++ b/src/ompl/control/ControlSpace.h
@@ -44,7 +44,6 @@
#include "ompl/util/Console.h"
#include "ompl/util/ClassForward.h"
#include <boost/concept_check.hpp>
-#include <boost/noncopyable.hpp>
#include <iostream>
#include <vector>
@@ -60,12 +59,15 @@ namespace ompl
/// @endcond
/** \class ompl::control::ControlSpacePtr
- \brief A boost shared pointer wrapper for ompl::control::ControlSpace */
+ \brief A shared pointer wrapper for ompl::control::ControlSpace */
/** \brief A control space representing the space of applicable controls */
- class ControlSpace : private boost::noncopyable
+ class ControlSpace
{
public:
+ // non-copyable
+ ControlSpace(const ControlSpace&) = delete;
+ ControlSpace& operator=(const ControlSpace&) = delete;
/** \brief Construct a control space, given the state space */
ControlSpace(const base::StateSpacePtr &stateSpace);
@@ -147,7 +149,7 @@ namespace ompl
/** \brief Many controls contain a number of double values. This function provides a means to get the
memory address of a double value from a control \e control located at position \e index. The first double value
is returned for \e index = 0. If \e index is too large (does not point to any double values in the control),
- the return value is NULL. */
+ the return value is nullptr. */
virtual double* getValueAddressAtIndex(Control *control, const unsigned int index) const;
/** \brief Print a control to a stream */
diff --git a/src/ompl/control/DirectedControlSampler.h b/src/ompl/control/DirectedControlSampler.h
index be40dfc..442740a 100644
--- a/src/ompl/control/DirectedControlSampler.h
+++ b/src/ompl/control/DirectedControlSampler.h
@@ -40,8 +40,7 @@
#include "ompl/base/State.h"
#include "ompl/control/Control.h"
#include "ompl/util/ClassForward.h"
-#include <boost/function.hpp>
-#include <boost/noncopyable.hpp>
+#include <functional>
namespace ompl
{
@@ -54,15 +53,18 @@ namespace ompl
/// @endcond
/** \class ompl::control::DirectedControlSamplerPtr
- \brief A boost shared pointer wrapper for ompl::control::DirectedControlSampler */
+ \brief A shared pointer wrapper for ompl::control::DirectedControlSampler */
/** \brief Abstract definition of a directed control sampler. Motion
planners that need to sample controls that take the system to a desired direction will call functions
from this class. Planners should call the versions of sampleTo() with most arguments, whenever this information is available.
If no direction information is available, the use of a ControlSampler is perhaps more appropriate. */
- class DirectedControlSampler : private boost::noncopyable
+ class DirectedControlSampler
{
public:
+ // non-copyable
+ DirectedControlSampler(const DirectedControlSampler&) = delete;
+ DirectedControlSampler& operator=(const DirectedControlSampler&) = delete;
/** \brief Constructor takes the state space to construct samples for as argument */
DirectedControlSampler(const SpaceInformation *si) : si_(si)
@@ -101,7 +103,7 @@ namespace ompl
};
/** \brief Definition of a function that can allocate a directed control sampler */
- typedef boost::function<DirectedControlSamplerPtr(const SpaceInformation*)> DirectedControlSamplerAllocator;
+ typedef std::function<DirectedControlSamplerPtr(const SpaceInformation*)> DirectedControlSamplerAllocator;
}
}
diff --git a/src/ompl/control/ODESolver.h b/src/ompl/control/ODESolver.h
index e6ecea2..51989d3 100644
--- a/src/ompl/control/ODESolver.h
+++ b/src/ompl/control/ODESolver.h
@@ -44,14 +44,9 @@
#include "ompl/util/ClassForward.h"
#include <boost/version.hpp>
-#if BOOST_VERSION >= 105300
#include <boost/numeric/odeint.hpp>
namespace odeint = boost::numeric::odeint;
-#else
-#include <omplext_odeint/boost/numeric/odeint.hpp>
-namespace odeint = boost::numeric::omplext_odeint;
-#endif
-#include <boost/function.hpp>
+#include <functional>
#include <cassert>
#include <vector>
@@ -66,7 +61,7 @@ namespace ompl
/// @endcond
/// \class ompl::control::ODESolverPtr
- /// \brief A boost shared pointer wrapper for ompl::control::ODESolver
+ /// \brief A shared pointer wrapper for ompl::control::ODESolver
/// \brief Abstract base class for an object that can solve ordinary differential
/// equations (ODE) of the type q' = f(q,u) using numerical integration. Classes
@@ -80,11 +75,11 @@ namespace ompl
/// \brief Callback function that defines the ODE. Accepts
/// the current state, input control, and output state.
- typedef boost::function<void(const StateType &, const Control*, StateType &)> ODE;
+ typedef std::function<void(const StateType &, const Control*, StateType &)> ODE;
/// \brief Callback function to perform an event at the end of numerical
/// integration. This functionality is optional.
- typedef boost::function<void(const base::State *state, const Control *control, const double duration, base::State *result)> PostPropagationEvent;
+ typedef std::function<void(const base::State *state, const Control *control, const double duration, base::State *result)> PostPropagationEvent;
/// \brief Parameterized constructor. Takes a reference to SpaceInformation,
/// an ODE to solve, and the integration step size.
@@ -127,7 +122,7 @@ namespace ompl
/// numerical integration is finished for further operations on the resulting
/// state.
static StatePropagatorPtr getStatePropagator (ODESolverPtr solver,
- const PostPropagationEvent &postEvent = NULL)
+ const PostPropagationEvent &postEvent = nullptr)
{
class ODESolverStatePropagator : public StatePropagator
{
diff --git a/src/ompl/control/PlannerData.h b/src/ompl/control/PlannerData.h
index 1663c97..6ed3971 100644
--- a/src/ompl/control/PlannerData.h
+++ b/src/ompl/control/PlannerData.h
@@ -91,7 +91,7 @@ namespace ompl
friend class PlannerDataStorage;
friend class PlannerData;
- PlannerDataEdgeControl() : PlannerDataEdge(), c_(NULL) {};
+ PlannerDataEdgeControl() : PlannerDataEdge(), c_(nullptr) {};
template <class Archive>
void serialize(Archive &ar, const unsigned int /*version*/)
diff --git a/src/ompl/control/SimpleSetup.h b/src/ompl/control/SimpleSetup.h
index 1e5ea7e..90d75e0 100644
--- a/src/ompl/control/SimpleSetup.h
+++ b/src/ompl/control/SimpleSetup.h
@@ -57,7 +57,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::SimpleSetupPtr
- \brief A boost shared pointer wrapper for ompl::control::SimpleSetup */
+ \brief A shared pointer wrapper for ompl::control::SimpleSetup */
/** \brief Create the set of classes typically needed to solve a
control problem */
diff --git a/src/ompl/control/SpaceInformation.h b/src/ompl/control/SpaceInformation.h
index 69471cd..a134db0 100644
--- a/src/ompl/control/SpaceInformation.h
+++ b/src/ompl/control/SpaceInformation.h
@@ -59,11 +59,11 @@ namespace ompl
/// @endcond
/** \class ompl::control::SpaceInformationPtr
- \brief A boost shared pointer wrapper for ompl::control::SpaceInformation */
+ \brief A shared pointer wrapper for ompl::control::SpaceInformation */
/** \brief A function that achieves state propagation.*/
- typedef boost::function<void(const base::State*, const Control*, const double, base::State*)> StatePropagatorFn;
+ typedef std::function<void(const base::State*, const Control*, const double, base::State*)> StatePropagatorFn;
/** \brief Space information containing necessary information for planning with controls. setup() needs to be called before use. */
class SpaceInformation : public base::SpaceInformation
diff --git a/src/ompl/control/StatePropagator.h b/src/ompl/control/StatePropagator.h
index 9b3ff8b..4808589 100644
--- a/src/ompl/control/StatePropagator.h
+++ b/src/ompl/control/StatePropagator.h
@@ -37,6 +37,7 @@
#ifndef OMPL_CONTROL_STATE_PROPAGATOR_
#define OMPL_CONTROL_STATE_PROPAGATOR_
+#include "ompl/base/State.h"
#include "ompl/control/Control.h"
#include "ompl/util/ClassForward.h"
@@ -56,7 +57,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::StatePropagatorPtr
- \brief A boost shared pointer wrapper for ompl::control::StatePropagator */
+ \brief A shared pointer wrapper for ompl::control::StatePropagator */
/** \brief Model the effect of controls on system states */
class StatePropagator
diff --git a/src/ompl/control/SteeredControlSampler.h b/src/ompl/control/SteeredControlSampler.h
index 1a24a02..9770b4c 100644
--- a/src/ompl/control/SteeredControlSampler.h
+++ b/src/ompl/control/SteeredControlSampler.h
@@ -39,6 +39,7 @@
#include "ompl/control/DirectedControlSampler.h"
#include "ompl/control/StatePropagator.h"
+#include "ompl/control/SpaceInformation.h"
#include <cmath>
namespace ompl
diff --git a/src/ompl/control/planners/est/EST.h b/src/ompl/control/planners/est/EST.h
index 48709d7..1cd1621 100644
--- a/src/ompl/control/planners/est/EST.h
+++ b/src/ompl/control/planners/est/EST.h
@@ -41,7 +41,7 @@
#include "ompl/control/planners/PlannerIncludes.h"
#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/datastructures/PDF.h"
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <vector>
namespace ompl
@@ -152,12 +152,12 @@ namespace ompl
{
public:
- Motion() : state(NULL), control(NULL), steps(0), parent(NULL)
+ Motion() : state(nullptr), control(nullptr), steps(0), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state and the control */
- Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
+ Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(nullptr)
{
}
diff --git a/src/ompl/control/planners/est/src/EST.cpp b/src/ompl/control/planners/est/src/EST.cpp
index 5c946e0..66717c9 100644
--- a/src/ompl/control/planners/est/src/EST.cpp
+++ b/src/ompl/control/planners/est/src/EST.cpp
@@ -46,7 +46,7 @@ ompl::control::EST::EST(const SpaceInformationPtr &si) : base::Planner(si, "EST"
goalBias_ = 0.05;
maxDistance_ = 0.0;
siC_ = si.get();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias, "0.:.05:1.");
@@ -76,7 +76,7 @@ void ompl::control::EST::clear()
tree_.grid.clear();
tree_.size = 0;
pdf_.clear ();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::control::EST::freeMemory()
@@ -123,8 +123,8 @@ ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminati
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
- Motion *solution = NULL;
- Motion *approxsol = NULL;
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
Motion *rmotion = new Motion(siC_);
bool solved = false;
@@ -179,19 +179,19 @@ ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminati
}
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
// Constructing the solution path
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -222,7 +222,7 @@ ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminati
ompl::control::EST::Motion* ompl::control::EST::selectMotion()
{
GridCell* cell = pdf_.sample(rng_.uniform01());
- return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
+ return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : nullptr;
}
void ompl::control::EST::addMotion(Motion *motion)
diff --git a/src/ompl/control/planners/kpiece/KPIECE1.h b/src/ompl/control/planners/kpiece/KPIECE1.h
index bd21ac4..3cace18 100644
--- a/src/ompl/control/planners/kpiece/KPIECE1.h
+++ b/src/ompl/control/planners/kpiece/KPIECE1.h
@@ -202,12 +202,12 @@ namespace ompl
/** \brief Representation of a motion for this algorithm */
struct Motion
{
- Motion() : state(NULL), control(NULL), steps(0), parent(NULL)
+ Motion() : state(nullptr), control(nullptr), steps(0), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state and the control */
- Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
+ Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(nullptr)
{
}
diff --git a/src/ompl/control/planners/kpiece/src/KPIECE1.cpp b/src/ompl/control/planners/kpiece/src/KPIECE1.cpp
index fcddadb..d6d21dc 100644
--- a/src/ompl/control/planners/kpiece/src/KPIECE1.cpp
+++ b/src/ompl/control/planners/kpiece/src/KPIECE1.cpp
@@ -51,8 +51,8 @@ ompl::control::KPIECE1::KPIECE1(const SpaceInformationPtr &si) : base::Planner(s
selectBorderFraction_ = 0.8;
badScoreFactor_ = 0.45;
goodScoreFactor_ = 0.9;
- tree_.grid.onCellUpdate(computeImportance, NULL);
- lastGoalMotion_ = NULL;
+ tree_.grid.onCellUpdate(computeImportance, nullptr);
+ lastGoalMotion_ = nullptr;
Planner::declareParam<double>("goal_bias", this, &KPIECE1::setGoalBias, &KPIECE1::getGoalBias, "0.:.05:1.");
Planner::declareParam<double>("border_fraction", this, &KPIECE1::setBorderFraction, &KPIECE1::getBorderFraction, "0.:0.05:1.");
@@ -90,7 +90,7 @@ void ompl::control::KPIECE1::clear()
tree_.grid.clear();
tree_.size = 0;
tree_.iteration = 1;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::control::KPIECE1::freeMemory()
@@ -199,8 +199,8 @@ ompl::base::PlannerStatus ompl::control::KPIECE1::solve(const base::PlannerTermi
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
- Motion *solution = NULL;
- Motion *approxsol = NULL;
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
Control *rctrl = siC_->allocControl();
@@ -220,8 +220,8 @@ ompl::base::PlannerStatus ompl::control::KPIECE1::solve(const base::PlannerTermi
tree_.iteration++;
/* Decide on a state to expand from */
- Motion *existing = NULL;
- Grid::Cell *ecell = NULL;
+ Motion *existing = nullptr;
+ Grid::Cell *ecell = nullptr;
if (closeSamples.canSample() && rng_.uniform01() < goalBias_)
{
@@ -309,19 +309,19 @@ ompl::base::PlannerStatus ompl::control::KPIECE1::solve(const base::PlannerTermi
bool solved = false;
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
diff --git a/src/ompl/control/planners/ltl/Automaton.h b/src/ompl/control/planners/ltl/Automaton.h
index 1bd4b17..70b7e75 100644
--- a/src/ompl/control/planners/ltl/Automaton.h
+++ b/src/ompl/control/planners/ltl/Automaton.h
@@ -39,7 +39,7 @@
#include "ompl/control/planners/ltl/World.h"
#include "ompl/util/ClassForward.h"
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <limits>
#include <ostream>
#include <vector>
@@ -54,7 +54,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::AutomatonPtr
- \brief A boost shared pointer wrapper for ompl::control::Automaton */
+ \brief A shared pointer wrapper for ompl::control::Automaton */
/** \brief A class to represent a deterministic finite automaton,
each edge of which corresponds to a World.
@@ -86,7 +86,7 @@ namespace ompl
return *this;
}
- mutable boost::unordered_map<World, unsigned int> entries;
+ mutable std::unordered_map<World, unsigned int> entries;
};
/** \brief Creates an automaton with a given number of propositions and states. */
diff --git a/src/ompl/control/planners/ltl/LTLPlanner.h b/src/ompl/control/planners/ltl/LTLPlanner.h
index 31caa7a..054a7b7 100644
--- a/src/ompl/control/planners/ltl/LTLPlanner.h
+++ b/src/ompl/control/planners/ltl/LTLPlanner.h
@@ -41,7 +41,7 @@
#include "ompl/control/planners/ltl/ProductGraph.h"
#include "ompl/control/planners/ltl/LTLSpaceInformation.h"
#include "ompl/datastructures/PDF.h"
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <map>
#include <vector>
@@ -88,7 +88,7 @@ namespace ompl
/** \brief Helper debug method to return the sequence of high-level product
graph states corresponding to a sequence of low-level continous system states,
beginning from an optional initial high-level state. */
- std::vector<ProductGraph::State*> getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start = NULL) const;
+ std::vector<ProductGraph::State*> getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start = nullptr) const;
protected:
/** \brief Representation of a motion
@@ -140,7 +140,7 @@ namespace ompl
double weight;
PDF<Motion*> motions;
- boost::unordered_map< Motion*, PDF<Motion*>::Element* > motionElems;
+ std::unordered_map< Motion*, PDF<Motion*>::Element* > motionElems;
double volume;
double autWeight;
unsigned int numSel;
@@ -197,7 +197,7 @@ namespace ompl
double exploreTime_;
/** \brief Map of abstraction states to their details. */
- boost::unordered_map< ProductGraph::State*, ProductGraphStateInfo > abstractInfo_;
+ std::unordered_map< ProductGraph::State*, ProductGraphStateInfo > abstractInfo_;
private:
/** \brief Clears this planner's underlying tree of system states. */
diff --git a/src/ompl/control/planners/ltl/LTLProblemDefinition.h b/src/ompl/control/planners/ltl/LTLProblemDefinition.h
index 9d42ed8..ba95a02 100644
--- a/src/ompl/control/planners/ltl/LTLProblemDefinition.h
+++ b/src/ompl/control/planners/ltl/LTLProblemDefinition.h
@@ -51,7 +51,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::LTLProblemDefinitionPtr
- \brief A boost shared pointer wrapper for ompl::control::LTLProblemDefinition */
+ \brief A shared pointer wrapper for ompl::control::LTLProblemDefinition */
class LTLProblemDefinition : public base::ProblemDefinition
{
public:
diff --git a/src/ompl/control/planners/ltl/LTLSpaceInformation.h b/src/ompl/control/planners/ltl/LTLSpaceInformation.h
index 9b9bcd3..17dc383 100644
--- a/src/ompl/control/planners/ltl/LTLSpaceInformation.h
+++ b/src/ompl/control/planners/ltl/LTLSpaceInformation.h
@@ -51,7 +51,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::LTLSpaceInformationPtr
- \brief A boost shared pointer wrapper for ompl::control::LTLSpaceInformation */
+ \brief A shared pointer wrapper for ompl::control::LTLSpaceInformation */
class LTLSpaceInformation : public SpaceInformation
{
public:
diff --git a/src/ompl/control/planners/ltl/ProductGraph.h b/src/ompl/control/planners/ltl/ProductGraph.h
index fb5a11a..bb6a658 100644
--- a/src/ompl/control/planners/ltl/ProductGraph.h
+++ b/src/ompl/control/planners/ltl/ProductGraph.h
@@ -41,9 +41,9 @@
#include "ompl/control/planners/ltl/Automaton.h"
#include "ompl/control/planners/ltl/PropositionalDecomposition.h"
#include "ompl/util/ClassForward.h"
-#include <boost/function.hpp>
#include <boost/graph/adjacency_list.hpp>
-#include <boost/unordered_map.hpp>
+#include <functional>
+#include <unordered_map>
#include <map>
#include <ostream>
#include <vector>
@@ -58,7 +58,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::ProductGraphPtr
- \brief A boost shared pointer wrapper for ompl::control::ProductGraph */
+ \brief A shared pointer wrapper for ompl::control::ProductGraph */
/** \brief A ProductGraph represents the weighted, directed, graph-based
Cartesian product of a PropositionalDecomposition object,
@@ -67,6 +67,16 @@ namespace ompl
class ProductGraph
{
public:
+ class State;
+
+ /// @cond IGNORE
+ /** \brief Hash function for State to be used in std::unordered_map */
+ struct HashState
+ {
+ std::size_t operator()(const State &s) const;
+ };
+ /// @endcond
+
/** \brief A State of a ProductGraph represents a vertex in the graph-based
Cartesian product represented by the ProductGraph.
A State is simply a tuple consisting of a PropositionalDecomposition region,
@@ -91,17 +101,15 @@ namespace ompl
/** \brief Returns whether this State is equivalent to a given State,
by comparing their PropositionalDecomposition regions and
Automaton states. */
- bool operator==(const State& s) const;
+ bool operator==(const State& s) const;
/** \brief Returns whether this State is valid.
A State is valid if and only if none of its Automaton states
are dead states (a dead state has value -1). */
bool isValid(void) const;
- /// @cond IGNORE
- /** \brief Hash function for State to be used in boost::unordered_map */
- friend std::size_t hash_value(const ProductGraph::State& s);
- /// @endcond
+
+ friend struct HashState;
/** \brief Helper function to print this State to a given output stream. */
friend std::ostream& operator<<(std::ostream& out, const State& s);
@@ -159,7 +167,7 @@ namespace ompl
PropositionalDecomposition.
Dijkstra's shortest-path algorithm is used to compute the path with
the given edge-weight function. */
- std::vector<State*> computeLead(State* start, const boost::function<double(State*, State*)>& edgeWeight);
+ std::vector<State*> computeLead(State* start, const std::function<double(State*, State*)>& edgeWeight);
/** \brief Clears all memory belonging to this ProductGraph. */
void clear();
@@ -169,7 +177,7 @@ namespace ompl
which will be called exactly once on each State (including the given initial State)
that is added to the ProductGraph.
The default argument for the initialization method is a no-op method. */
- void buildGraph(State* start, const boost::function<void(State*)>& initialize = ProductGraph::noInit);
+ void buildGraph(State* start, const std::function<void(State*)>& initialize = ProductGraph::noInit);
/** \brief Returns whether the given State is an accepting State
in this ProductGraph.
@@ -197,7 +205,7 @@ namespace ompl
/** \brief Returns a ProductGraph State with initial co-safety and safety
Automaton states, and the PropositionalDecomposition region that contains
a given base::State. */
- State* getState(const base::State* cs) const;
+ State* getState(const base::State* cs) const;
/** \brief Returns a ProductGraph State with given co-safety and safety
Automaton states, and the PropositionalDecomposition region that contains
@@ -224,7 +232,7 @@ namespace ompl
s.cosafeState = cosafe;
s.safeState = safe;
State*& ret = stateToPtr_[s];
- if (ret == NULL) ret = new State(s);
+ if (ret == nullptr) ret = new State(s);
return ret;
}
@@ -248,18 +256,18 @@ namespace ompl
AutomatonPtr safety_;
GraphType graph_;
State* startState_;
- std::vector<State*> solutionStates_;
+ std::vector<State*> solutionStates_;
/* Only one State pointer will be allocated for each possible State
in the ProductGraph. There will exist situations in which
all we have are the component values (region, automaton states)
of a State and we want the actual State pointer.
We use this map to access it. */
- mutable boost::unordered_map<State, State*> stateToPtr_;
+ mutable std::unordered_map<State, State*, HashState> stateToPtr_;
/* Map from State pointer to the index of the corresponding vertex
in the graph. */
- boost::unordered_map<State*, int> stateToIndex_;
+ std::unordered_map<State*, int> stateToIndex_;
};
}
}
diff --git a/src/ompl/control/planners/ltl/PropositionalDecomposition.h b/src/ompl/control/planners/ltl/PropositionalDecomposition.h
index 24c0a9d..f0bed42 100644
--- a/src/ompl/control/planners/ltl/PropositionalDecomposition.h
+++ b/src/ompl/control/planners/ltl/PropositionalDecomposition.h
@@ -55,7 +55,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::PropositionalDecompositionPtr
- \brief A boost shared pointer wrapper for ompl::control::PropositionalDecomposition */
+ \brief A shared pointer wrapper for ompl::control::PropositionalDecomposition */
/** \brief A propositional decomposition wraps a given Decomposition
with a region-to-proposition assignment operator.
@@ -68,7 +68,7 @@ namespace ompl
PropositionalDecomposition(const DecompositionPtr& decomp);
/** \brief Clears all memory belonging to this propositional decomposition. */
- virtual ~PropositionalDecomposition(void);
+ virtual ~PropositionalDecomposition(void);
/** \brief Returns the World corresponding to a given region. */
virtual World worldAtRegion(int rid) = 0;
diff --git a/src/ompl/control/planners/ltl/World.h b/src/ompl/control/planners/ltl/World.h
index 5731a7a..1b1fe94 100644
--- a/src/ompl/control/planners/ltl/World.h
+++ b/src/ompl/control/planners/ltl/World.h
@@ -37,13 +37,33 @@
#ifndef OMPL_CONTROL_PLANNERS_LTL_WORLD_
#define OMPL_CONTROL_PLANNERS_LTL_WORLD_
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <string>
namespace ompl
{
namespace control
{
+ class World;
+ }
+}
+
+/// @cond IGNORE
+/** \brief Hash function for World to be used in std::unordered_map */
+namespace std
+{
+ template<>
+ struct hash<ompl::control::World>
+ {
+ size_t operator()(const ompl::control::World &w) const;
+ };
+}
+/// @endcond
+
+namespace ompl
+{
+ namespace control
+ {
/** \brief A class to represent an assignment of boolean values to propositions.
A World can be partially restrictive, i.e., some propositions do not have to
be assigned a value, in which case it can take on any value.
@@ -77,7 +97,7 @@ namespace ompl
/** \brief Returns this World's underlying proposition-to-boolean
assignment map. */
- const boost::unordered_map<unsigned int, bool>& props(void) const;
+ const std::unordered_map<unsigned int, bool>& props(void) const;
/** \brief Returns whether this World is equivalent to a given World,
by comparing their truth assignment maps. */
@@ -86,14 +106,11 @@ namespace ompl
/** \brief Clears this world's truth assignment. */
void clear(void);
- /// @cond IGNORE
- /** \brief Hash function for World to be used in boost::unordered_map */
- friend std::size_t hash_value(const World& w);
- /// @endcond
+ friend struct std::hash<World>;
protected:
unsigned int numProps_;
- boost::unordered_map<unsigned int, bool> props_;
+ std::unordered_map<unsigned int, bool> props_;
};
}
}
diff --git a/src/ompl/control/planners/ltl/src/Automaton.cpp b/src/ompl/control/planners/ltl/src/Automaton.cpp
index 7514a15..3d0c6de 100644
--- a/src/ompl/control/planners/ltl/src/Automaton.cpp
+++ b/src/ompl/control/planners/ltl/src/Automaton.cpp
@@ -1,9 +1,44 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Rice University
+* 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 Rice University 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: Matt Maly */
+
#include "ompl/control/planners/ltl/Automaton.h"
#include "ompl/control/planners/ltl/World.h"
#include <boost/range/irange.hpp>
-#include <boost/lexical_cast.hpp>
-#include <boost/unordered_map.hpp>
-#include <boost/unordered_set.hpp>
+#include <unordered_map>
+#include <unordered_set>
#include <boost/dynamic_bitset.hpp>
#include <ostream>
#include <limits>
@@ -12,7 +47,7 @@
int ompl::control::Automaton::TransitionMap::eval(const World& w) const
{
- typedef boost::unordered_map<World, unsigned int>::const_iterator DestIter;
+ typedef std::unordered_map<World, unsigned int>::const_iterator DestIter;
DestIter d = entries.find(w);
if (d != entries.end())
return d->second;
@@ -69,7 +104,7 @@ int ompl::control::Automaton::getStartState(void) const
}
void ompl::control::Automaton::addTransition(
- unsigned int src,
+ unsigned int src,
const World& w,
unsigned int dest)
{
@@ -130,7 +165,7 @@ void ompl::control::Automaton::print(std::ostream& out) const
out << (accepting_[i] ? "doublecircle" : "circle") << "]" << std::endl;
const TransitionMap& map = transitions_[i];
- boost::unordered_map<World, unsigned int>::const_iterator e;
+ std::unordered_map<World, unsigned int>::const_iterator e;
for (e = map.entries.begin(); e != map.entries.end(); ++e)
{
const World& w = e->first;
@@ -149,8 +184,8 @@ unsigned int ompl::control::Automaton::distFromAccepting(unsigned int s, unsigne
if (accepting_[s])
return 0;
std::queue<unsigned int> q;
- boost::unordered_set<unsigned int> processed;
- boost::unordered_map<unsigned int, unsigned int> distance;
+ std::unordered_set<unsigned int> processed;
+ std::unordered_map<unsigned int, unsigned int> distance;
q.push(s);
distance[s] = 0;
@@ -166,7 +201,7 @@ unsigned int ompl::control::Automaton::distFromAccepting(unsigned int s, unsigne
return distance[current];
}
const TransitionMap& map = transitions_[current];
- boost::unordered_map<World, unsigned int>::const_iterator e;
+ std::unordered_map<World, unsigned int>::const_iterator e;
for (e = map.entries.begin(); e != map.entries.end(); ++e)
{
unsigned int neighbor = e->second;
diff --git a/src/ompl/control/planners/ltl/src/LTLPlanner.cpp b/src/ompl/control/planners/ltl/src/LTLPlanner.cpp
index b8da807..6ba1ca4 100644
--- a/src/ompl/control/planners/ltl/src/LTLPlanner.cpp
+++ b/src/ompl/control/planners/ltl/src/LTLPlanner.cpp
@@ -1,3 +1,39 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Rice University
+* 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 Rice University 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: Matt Maly */
+
#include "ompl/control/planners/ltl/LTLPlanner.h"
#include "ompl/control/planners/PlannerIncludes.h"
#include "ompl/control/planners/ltl/ProductGraph.h"
@@ -5,7 +41,7 @@
#include "ompl/datastructures/PDF.h"
#include "ompl/util/Console.h"
#include <algorithm>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <limits>
#include <map>
#include <vector>
@@ -16,7 +52,7 @@ ompl::control::LTLPlanner::LTLPlanner(const LTLSpaceInformationPtr& ltlsi, const
ompl::base::Planner(ltlsi, "LTLPlanner"),
ltlsi_(ltlsi.get()),
abstraction_(a),
- prodStart_(NULL),
+ prodStart_(nullptr),
exploreTime_(exploreTime)
{
specs_.approximateSolutions = true;
@@ -60,7 +96,8 @@ ompl::base::PlannerStatus ompl::control::LTLPlanner::solve(const ompl::base::Pla
updateWeight(prodStart_);
availDist_.add(prodStart_, abstractInfo_[prodStart_].weight);
- abstraction_->buildGraph(prodStart_, boost::bind(<LPlanner::initAbstractInfo, this, _1));
+ abstraction_->buildGraph(prodStart_,
+ std::bind(<LPlanner::initAbstractInfo, this, std::placeholders::_1));
if (!sampler_)
sampler_ = si_->allocStateSampler();
@@ -72,7 +109,8 @@ ompl::base::PlannerStatus ompl::control::LTLPlanner::solve(const ompl::base::Pla
while (ptc()==false && !solved)
{
- const std::vector<ProductGraph::State*> lead = abstraction_->computeLead(prodStart_, boost::bind(<LPlanner::abstractEdgeWeight, this, _1, _2));
+ const std::vector<ProductGraph::State*> lead = abstraction_->computeLead(prodStart_,
+ std::bind(<LPlanner::abstractEdgeWeight, this, std::placeholders::_1, std::placeholders::_2));
buildAvail(lead);
solved = explore(lead, soln, exploreTime_);
}
@@ -81,7 +119,7 @@ ompl::base::PlannerStatus ompl::control::LTLPlanner::solve(const ompl::base::Pla
{
//build solution path
std::vector<Motion*> path;
- while (soln != NULL)
+ while (soln != nullptr)
{
path.push_back(soln);
soln = soln->parent;
@@ -89,7 +127,7 @@ ompl::base::PlannerStatus ompl::control::LTLPlanner::solve(const ompl::base::Pla
PathControl* pc = new PathControl(si_);
for (int i = path.size()-1; i >= 0; --i)
{
- if (path[i]->parent != NULL) {
+ if (path[i]->parent != nullptr) {
pc->append(path[i]->state, path[i]->control, path[i]->steps * ltlsi_->getPropagationStepSize());
}
else {
@@ -113,7 +151,7 @@ void ompl::control::LTLPlanner::getTree(std::vector<base::State*>& tree) const
std::vector<ompl::control::ProductGraph::State*> ompl::control::LTLPlanner::getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start) const
{
std::vector<ProductGraph::State*> hlPath(path.size());
- hlPath[0] = (start != NULL ? start : ltlsi_->getProdGraphState(path[0]));
+ hlPath[0] = (start != nullptr ? start : ltlsi_->getProdGraphState(path[0]));
for (unsigned int i = 1; i < path.size(); ++i)
{
hlPath[i] = ltlsi_->getProdGraphState(path[i]);
@@ -123,14 +161,14 @@ std::vector<ompl::control::ProductGraph::State*> ompl::control::LTLPlanner::getH
return hlPath;
}
-ompl::control::LTLPlanner::Motion::Motion(void) : state(NULL), control(NULL), parent(NULL), steps(0)
+ompl::control::LTLPlanner::Motion::Motion(void) : state(nullptr), control(nullptr), parent(nullptr), steps(0)
{
}
ompl::control::LTLPlanner::Motion::Motion(const SpaceInformation* si) :
state(si->allocState()),
control(si->allocControl()),
- parent(NULL),
+ parent(nullptr),
steps(0)
{
}
@@ -141,7 +179,7 @@ ompl::control::LTLPlanner::Motion::~Motion(void)
ompl::control::LTLPlanner::ProductGraphStateInfo::ProductGraphStateInfo(void) :
numSel(0),
- pdfElem(NULL)
+ pdfElem(nullptr)
{
}
@@ -163,10 +201,10 @@ void ompl::control::LTLPlanner::initAbstractInfo(ProductGraph::State* as)
{
ProductGraphStateInfo& info = abstractInfo_[as];
info.numSel = 0;
- info.pdfElem = NULL;
- info.volume = abstraction_->getRegionVolume(as);
- unsigned int autDist = std::max(abstraction_->getCosafeAutDistance(as),
- abstraction_->getSafeAutDistance(as));
+ info.pdfElem = nullptr;
+ info.volume = abstraction_->getRegionVolume(as);
+ unsigned int autDist = std::max(abstraction_->getCosafeAutDistance(as),
+ abstraction_->getSafeAutDistance(as));
//\todo try something larger than epsilon
if (autDist == 0)
info.autWeight = std::numeric_limits<double>::epsilon();
@@ -178,7 +216,7 @@ void ompl::control::LTLPlanner::initAbstractInfo(ProductGraph::State* as)
void ompl::control::LTLPlanner::buildAvail(const std::vector<ProductGraph::State*>& lead)
{
for (unsigned int i = 0; i < availDist_.size(); ++i)
- abstractInfo_[availDist_[i]].pdfElem = NULL;
+ abstractInfo_[availDist_[i]].pdfElem = nullptr;
availDist_.clear();
unsigned int numTreePts = 1;
for (int i = lead.size()-1; i >= 0; --i)
@@ -223,7 +261,7 @@ bool ompl::control::LTLPlanner::explore(const std::vector<ProductGraph::State*>&
{
si_->freeState(newState);
ltlsi_->freeControl(rctrl);
- continue;
+ continue;
}
Motion* m = new Motion();
m->state = newState;
@@ -237,12 +275,12 @@ bool ompl::control::LTLPlanner::explore(const std::vector<ProductGraph::State*>&
abstractInfo_[m->abstractState].addMotion(m);
updateWeight(m->abstractState);
// update weight if hl state already exists in avail
- if (abstractInfo_[m->abstractState].pdfElem != NULL)
+ if (abstractInfo_[m->abstractState].pdfElem != nullptr)
availDist_.update(abstractInfo_[m->abstractState].pdfElem, abstractInfo_[m->abstractState].weight);
else
{
// otherwise, only add hl state to avail if it already exists in lead
- if (std::find(lead.begin(), lead.end(), m->abstractState) != lead.end())
+ if (std::find(lead.begin(), lead.end(), m->abstractState) != lead.end())
{
PDF<ProductGraph::State*>::Element* elem = availDist_.add(m->abstractState, abstractInfo_[m->abstractState].weight);
abstractInfo_[m->abstractState].pdfElem = elem;
@@ -272,9 +310,9 @@ void ompl::control::LTLPlanner::clearMotions(void)
for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
{
Motion* m = *i;
- if (m->state != NULL)
+ if (m->state != nullptr)
si_->freeState(m->state);
- if (m->control != NULL)
+ if (m->control != nullptr)
ltlsi_->freeControl(m->control);
delete m;
}
diff --git a/src/ompl/control/planners/ltl/src/LTLProblemDefinition.cpp b/src/ompl/control/planners/ltl/src/LTLProblemDefinition.cpp
index 95ca0e8..af049e0 100644
--- a/src/ompl/control/planners/ltl/src/LTLProblemDefinition.cpp
+++ b/src/ompl/control/planners/ltl/src/LTLProblemDefinition.cpp
@@ -1,3 +1,39 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Rice University
+* 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 Rice University 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: Matt Maly */
+
#include "ompl/control/PathControl.h"
#include "ompl/control/planners/ltl/LTLProblemDefinition.h"
#include "ompl/control/planners/ltl/LTLSpaceInformation.h"
diff --git a/src/ompl/control/planners/ltl/src/ProductGraph.cpp b/src/ompl/control/planners/ltl/src/ProductGraph.cpp
index d455175..aa06577 100644
--- a/src/ompl/control/planners/ltl/src/ProductGraph.cpp
+++ b/src/ompl/control/planners/ltl/src/ProductGraph.cpp
@@ -1,3 +1,39 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Rice University
+* 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 Rice University 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: Matt Maly */
+
#include "ompl/control/planners/ltl/ProductGraph.h"
#include "ompl/base/State.h"
#include "ompl/control/planners/ltl/Automaton.h"
@@ -5,13 +41,13 @@
#include "ompl/control/planners/ltl/World.h"
#include "ompl/util/ClassForward.h"
#include "ompl/util/Console.h"
+#include "ompl/util/Hash.h"
#include <algorithm>
-#include <boost/function.hpp>
-#include <boost/functional/hash.hpp>
+#include <functional>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
-#include <boost/unordered_map.hpp>
-#include <boost/unordered_set.hpp>
+#include <unordered_map>
+#include <unordered_set>
#include <map>
#include <ostream>
#include <queue>
@@ -30,19 +66,18 @@ bool ompl::control::ProductGraph::State::isValid(void) const
return cosafeState != -1 && safeState != -1;
}
+std::size_t ompl::control::ProductGraph::HashState::operator()(const ompl::control::ProductGraph::State &s) const
+{
+ std::size_t hash = std::hash<int>()(s.decompRegion);
+ hash_combine(hash, s.cosafeState);
+ hash_combine(hash, s.safeState);
+ return hash;
+}
+
namespace ompl
{
namespace control
{
- std::size_t hash_value(const ProductGraph::State& s)
- {
- std::size_t hash = 0;
- boost::hash_combine(hash, s.decompRegion);
- boost::hash_combine(hash, s.cosafeState);
- boost::hash_combine(hash, s.safeState);
- return hash;
- }
-
std::ostream& operator<<(std::ostream& out, const ProductGraph::State& s)
{
out << "(" << s.decompRegion << "," << s.cosafeState << ",";
@@ -106,10 +141,10 @@ const ompl::control::AutomatonPtr& ompl::control::ProductGraph::getSafetyAutom()
std::vector<ompl::control::ProductGraph::State*>
ompl::control::ProductGraph::computeLead(
ProductGraph::State* start,
- const boost::function<double(ProductGraph::State*, ProductGraph::State*)>& edgeWeight)
+ const std::function<double(ProductGraph::State*, ProductGraph::State*)>& edgeWeight)
{
- std::vector<GraphType::vertex_descriptor> parents(boost::num_vertices(graph_));
- std::vector<double> distances(boost::num_vertices(graph_));
+ std::vector<GraphType::vertex_descriptor> parents(boost::num_vertices(graph_));
+ std::vector<double> distances(boost::num_vertices(graph_));
EdgeIter ei, eend;
//first build up the edge weights
for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
@@ -119,65 +154,65 @@ ompl::control::ProductGraph::computeLead(
graph_[*ei].cost = edgeWeight(graph_[src], graph_[target]);
}
int startIndex = stateToIndex_[start];
- boost::dijkstra_shortest_paths(graph_, boost::vertex(startIndex,graph_),
- boost::weight_map(get(&Edge::cost, graph_)).distance_map(
- boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
- )).predecessor_map(
- boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
- )
- );
- //pick state from solutionStates_ such that distance[state] is minimized
- State* bestSoln = *solutionStates_.begin();
- double cost = distances[boost::vertex(stateToIndex_[bestSoln], graph_)];
- for (std::vector<State*>::const_iterator s = solutionStates_.begin()+1; s != solutionStates_.end(); ++s)
- {
- if (distances[boost::vertex(stateToIndex_[*s], graph_)] < cost)
- {
- cost = distances[boost::vertex(stateToIndex_[*s], graph_)];
- bestSoln = *s;
- }
- }
- //build lead from bestSoln parents
- std::stack<State*> leadStack;
- while (!(bestSoln == start))
- {
- leadStack.push(bestSoln);
- bestSoln = graph_[parents[boost::vertex(stateToIndex_[bestSoln], graph_)]];
- }
- leadStack.push(bestSoln);
-
- std::vector<State*> lead;
- while (!leadStack.empty())
- {
- lead.push_back(leadStack.top());
- leadStack.pop();
+ boost::dijkstra_shortest_paths(graph_, boost::vertex(startIndex,graph_),
+ boost::weight_map(get(&Edge::cost, graph_)).distance_map(
+ boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
+ )).predecessor_map(
+ boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
+ )
+ );
+ //pick state from solutionStates_ such that distance[state] is minimized
+ State* bestSoln = *solutionStates_.begin();
+ double cost = distances[boost::vertex(stateToIndex_[bestSoln], graph_)];
+ for (std::vector<State*>::const_iterator s = solutionStates_.begin()+1; s != solutionStates_.end(); ++s)
+ {
+ if (distances[boost::vertex(stateToIndex_[*s], graph_)] < cost)
+ {
+ cost = distances[boost::vertex(stateToIndex_[*s], graph_)];
+ bestSoln = *s;
+ }
+ }
+ //build lead from bestSoln parents
+ std::stack<State*> leadStack;
+ while (!(bestSoln == start))
+ {
+ leadStack.push(bestSoln);
+ bestSoln = graph_[parents[boost::vertex(stateToIndex_[bestSoln], graph_)]];
+ }
+ leadStack.push(bestSoln);
+
+ std::vector<State*> lead;
+ while (!leadStack.empty())
+ {
+ lead.push_back(leadStack.top());
+ leadStack.pop();
// Truncate the lead as early when it hits the desired automaton states
// \todo: more elegant way to do this?
if (lead.back()->cosafeState == solutionStates_.front()->cosafeState
&& lead.back()->safeState == solutionStates_.front()->safeState)
break;
- }
- return lead;
+ }
+ return lead;
}
void ompl::control::ProductGraph::clear()
{
solutionStates_.clear();
stateToIndex_.clear();
- startState_ = NULL;
+ startState_ = nullptr;
graph_.clear();
- boost::unordered_map<State,State*>::iterator i;
+ std::unordered_map<State,State*,HashState>::iterator i;
for (i = stateToPtr_.begin(); i != stateToPtr_.end(); ++i)
delete i->second;
stateToPtr_.clear();
}
-void ompl::control::ProductGraph::buildGraph(State* start, const boost::function<void(State*)>& initialize)
+void ompl::control::ProductGraph::buildGraph(State* start, const std::function<void(State*)>& initialize)
{
graph_.clear();
solutionStates_.clear();
std::queue<State*> q;
- boost::unordered_set<State*> processed;
+ std::unordered_set<State*> processed;
std::vector<int> regNeighbors;
VertexIndexMap index = get(boost::vertex_index, graph_);
@@ -189,8 +224,8 @@ void ompl::control::ProductGraph::buildGraph(State* start, const boost::function
processed.insert(startState_);
OMPL_INFORM("Building graph from start state (%u,%u,%u) with index %d",
- startState_->decompRegion, startState_->cosafeState,
- startState_->safeState, stateToIndex_[startState_]);
+ startState_->decompRegion, startState_->cosafeState,
+ startState_->safeState, stateToIndex_[startState_]);
while (!q.empty())
{
@@ -213,18 +248,18 @@ void ompl::control::ProductGraph::buildGraph(State* start, const boost::function
State* nextState = getState(current, *r);
if (!nextState->isValid())
continue;
- //if this state is newly discovered,
- //then we can dynamically allocate a copy of it
+ //if this state is newly discovered,
+ //then we can dynamically allocate a copy of it
//and add the new pointer to the graph.
//either way, we need the pointer
- if (processed.find(nextState) == processed.end())
- {
- const GraphType::vertex_descriptor next = boost::add_vertex(graph_);
+ if (processed.find(nextState) == processed.end())
+ {
+ const GraphType::vertex_descriptor next = boost::add_vertex(graph_);
stateToIndex_[nextState] = index[next];
- graph_[boost::vertex(next,graph_)] = nextState;
- q.push(nextState);
+ graph_[boost::vertex(next,graph_)] = nextState;
+ q.push(nextState);
processed.insert(nextState);
- }
+ }
//whether or not the neighbor is newly discovered,
//we still need to add the edge to the graph
@@ -234,7 +269,7 @@ void ompl::control::ProductGraph::buildGraph(State* start, const boost::function
//graph_[edge].src = index[v];
//graph_[edge].dest = stateToIndex_[nextState];
}
- regNeighbors.clear();
+ regNeighbors.clear();
}
if (solutionStates_.empty())
{
@@ -284,9 +319,9 @@ ompl::control::ProductGraph::State* ompl::control::ProductGraph::getState(const
s.decompRegion = decomp_->locateRegion(cs);
s.cosafeState = cosafe;
s.safeState = safe;
- State*& ret = stateToPtr_[s];
- if (ret == NULL)
- ret = new State(s);
+ State*& ret = stateToPtr_[s];
+ if (ret == nullptr)
+ ret = new State(s);
return ret;
}
@@ -297,9 +332,9 @@ ompl::control::ProductGraph::State* ompl::control::ProductGraph::getState(const
const World nextWorld = decomp_->worldAtRegion(nextRegion);
s.cosafeState = cosafety_->step(parent->cosafeState, nextWorld);
s.safeState = safety_->step(parent->safeState, nextWorld);
- State*& ret = stateToPtr_[s];
- if (ret == NULL)
- ret = new State(s);
+ State*& ret = stateToPtr_[s];
+ if (ret == nullptr)
+ ret = new State(s);
return ret;
}
diff --git a/src/ompl/control/planners/ltl/src/PropositionalDecomposition.cpp b/src/ompl/control/planners/ltl/src/PropositionalDecomposition.cpp
index 397f079..38c23d9 100644
--- a/src/ompl/control/planners/ltl/src/PropositionalDecomposition.cpp
+++ b/src/ompl/control/planners/ltl/src/PropositionalDecomposition.cpp
@@ -1,3 +1,39 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Rice University
+* 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 Rice University 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: Matt Maly */
+
#include "ompl/control/planners/ltl/PropositionalDecomposition.h"
#include "ompl/base/State.h"
#include "ompl/control/planners/syclop/Decomposition.h"
diff --git a/src/ompl/control/planners/ltl/src/World.cpp b/src/ompl/control/planners/ltl/src/World.cpp
index 5e5d1cd..df8c782 100644
--- a/src/ompl/control/planners/ltl/src/World.cpp
+++ b/src/ompl/control/planners/ltl/src/World.cpp
@@ -1,8 +1,43 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Rice University
+* 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 Rice University 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: Matt Maly */
+
#include "ompl/control/planners/ltl/World.h"
#include "ompl/util/Console.h"
-#include <boost/functional/hash.hpp>
-#include <boost/lexical_cast.hpp>
-#include <boost/unordered_map.hpp>
+#include "ompl/util/Hash.h"
+#include <unordered_map>
#include <string>
ompl::control::World::World(unsigned int np) : numProps_(np)
@@ -11,7 +46,7 @@ ompl::control::World::World(unsigned int np) : numProps_(np)
bool ompl::control::World::operator[](unsigned int i) const
{
- boost::unordered_map<unsigned int, bool>::const_iterator p = props_.find(i);
+ std::unordered_map<unsigned int, bool>::const_iterator p = props_.find(i);
if (p == props_.end())
OMPL_ERROR("Proposition %u is not set in world", i);
return p->second;
@@ -29,7 +64,7 @@ unsigned int ompl::control::World::numProps() const
bool ompl::control::World::satisfies(const World& w) const
{
- boost::unordered_map<unsigned int, bool>::const_iterator p, q;
+ std::unordered_map<unsigned int, bool>::const_iterator p, q;
for (p = w.props_.begin(); p != w.props_.end(); ++p)
{
q = props_.find(p->first);
@@ -43,15 +78,15 @@ std::string ompl::control::World::formula(void) const
{
if (props_.empty())
return "true";
- boost::unordered_map<unsigned int, bool>::const_iterator p = props_.begin();
- std::string f = std::string(p->second ? "p" : "!p") + boost::lexical_cast<std::string>(p->first);
+ std::unordered_map<unsigned int, bool>::const_iterator p = props_.begin();
+ std::string f = std::string(p->second ? "p" : "!p") + std::to_string(p->first);
++p;
for (; p != props_.end(); ++p)
- f += std::string(p->second ? " & p" : " & !p") + boost::lexical_cast<std::string>(p->first);
+ f += std::string(p->second ? " & p" : " & !p") + std::to_string(p->first);
return f;
}
-const boost::unordered_map<unsigned int, bool>& ompl::control::World::props(void) const
+const std::unordered_map<unsigned int, bool>& ompl::control::World::props(void) const
{
return props_;
}
@@ -66,17 +101,11 @@ void ompl::control::World::clear(void)
props_.clear();
}
-namespace ompl
+size_t std::hash<ompl::control::World>::operator()(const ompl::control::World &w) const
{
- namespace control
- {
- std::size_t hash_value(const ompl::control::World& w)
- {
- std::size_t hash = 0;
- boost::unordered_map<unsigned int, bool>::const_iterator p;
- for (p = w.props_.begin(); p != w.props_.end(); ++p)
- boost::hash_combine(hash, *p);
- return hash;
- }
- }
+ std::size_t hash = 0;
+ std::unordered_map<unsigned int, bool>::const_iterator p;
+ for (p = w.props_.begin(); p != w.props_.end(); ++p)
+ ompl::hash_combine(hash, *p);
+ return hash;
}
diff --git a/src/ompl/control/planners/pdst/PDST.h b/src/ompl/control/planners/pdst/PDST.h
index bc06d22..067ccb1 100644
--- a/src/ompl/control/planners/pdst/PDST.h
+++ b/src/ompl/control/planners/pdst/PDST.h
@@ -152,13 +152,13 @@ namespace ompl
unsigned int controlDuration, double priority, Motion *parent)
: startState_(startState), endState_(endState), control_(control),
controlDuration_(controlDuration), priority_(priority), parent_(parent),
- cell_(NULL), heapElement_(NULL), isSplit_(false)
+ cell_(nullptr), heapElement_(nullptr), isSplit_(false)
{
}
/// constructor for start states
Motion(base::State *state)
- : startState_(state), endState_(state), control_(NULL), controlDuration_(0),
- priority_(0.), parent_(NULL), cell_(NULL), heapElement_(NULL), isSplit_(false)
+ : startState_(state), endState_(state), control_(nullptr), controlDuration_(0),
+ priority_(0.), parent_(nullptr), cell_(nullptr), heapElement_(nullptr), isSplit_(false)
{
}
/// The score is used to order motions in a priority queue.
@@ -198,7 +198,7 @@ namespace ompl
Cell(double volume, const base::RealVectorBounds &bounds,
unsigned int splitDimension = 0)
: volume_(volume), splitDimension_(splitDimension), splitValue_(0.0),
- left_(NULL), right_(NULL), bounds_(bounds)
+ left_(nullptr), right_(nullptr), bounds_(bounds)
{
}
@@ -218,7 +218,7 @@ namespace ompl
Cell* stab(const base::EuclideanProjection& projection) const
{
Cell *containingCell = const_cast<Cell*>(this);
- while (containingCell->left_ != NULL)
+ while (containingCell->left_ != nullptr)
{
if (projection[containingCell->splitDimension_] <= containingCell->splitValue_)
containingCell = containingCell->left_;
@@ -249,9 +249,9 @@ namespace ompl
unsigned int splitDimension_;
/// The midpoint between the bounds_ at the splitDimension_
double splitValue_;
- /// The left child cell (NULL for a leaf cell)
+ /// The left child cell (nullptr for a leaf cell)
Cell* left_;
- /// The right child cell (NULL for a leaf cell)
+ /// The right child cell (nullptr for a leaf cell)
Cell* right_;
/// A bounding box for this cell
base::RealVectorBounds bounds_;
@@ -273,7 +273,7 @@ namespace ompl
motion->heapElement_ = priorityQueue_.insert(motion);
}
/// \brief Select a state along motion and propagate a new motion from there.
- /// Return NULL if no valid motion could be generated starting at the
+ /// Return nullptr if no valid motion could be generated starting at the
/// selected state.
Motion* propagateFrom(Motion *motion, base::State*, base::State*);
/// \brief Find the max. duration that the control_ in motion can be applied s.t.
diff --git a/src/ompl/control/planners/pdst/src/PDST.cpp b/src/ompl/control/planners/pdst/src/PDST.cpp
index c00158c..f54f885 100644
--- a/src/ompl/control/planners/pdst/src/PDST.cpp
+++ b/src/ompl/control/planners/pdst/src/PDST.cpp
@@ -38,8 +38,8 @@
#include "ompl/control/planners/pdst/PDST.h"
ompl::control::PDST::PDST(const SpaceInformationPtr &si)
- : base::Planner(si, "PDST"), siC_(si.get()), bsp_(NULL), goalBias_(0.05),
- goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL)
+ : base::Planner(si, "PDST"), siC_(si.get()), bsp_(nullptr), goalBias_(0.05),
+ goalSampler_(nullptr), iteration_(1), lastGoalMotion_(nullptr)
{
Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
}
@@ -72,7 +72,7 @@ ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminat
// generated an approximate or exact solution. If solve is being called for the first
// time then initializes hasSolution to false and isApproximate to true.
double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
- bool hasSolution = lastGoalMotion_ != NULL;
+ bool hasSolution = lastGoalMotion_ != nullptr;
bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
unsigned int ndim = projectionEvaluator_->getDimension();
@@ -106,7 +106,7 @@ ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminat
priorityQueue_.update(motionSelected->heapElement_);
Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
- if (newMotion == NULL)
+ if (newMotion == nullptr)
continue;
addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
@@ -137,7 +137,7 @@ ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminat
addMotion(*m, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
}
- if (lastGoalMotion_ != NULL)
+ if (lastGoalMotion_ != nullptr)
hasSolution = true;
// If a solution path has been computed, save it in the problem definition object.
@@ -194,7 +194,7 @@ ompl::control::PDST::Motion* ompl::control::PDST::propagateFrom(
if (duration < siC_->getMinControlDuration())
{
siC_->freeControl(control);
- return NULL;
+ return nullptr;
}
return new Motion(si_->cloneState(start), si_->cloneState(rnd),
control, duration, ++iteration_, motion);
@@ -212,7 +212,7 @@ void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State *prev
return;
}
- Cell *cell = NULL, *prevCell = NULL;
+ Cell *cell = nullptr, *prevCell = nullptr;
si_->copyState(prevState, motion->startState_);
// propagate the motion, check for cell boundary crossings, and split as necessary
for (unsigned int i = 0, duration = 0 ; i < motion->controlDuration_ - 1 ; ++i, ++duration)
@@ -287,7 +287,7 @@ void ompl::control::PDST::clear()
sampler_.reset();
controlSampler_.reset();
iteration_ = 1;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
freeMemory();
bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
}
@@ -312,7 +312,7 @@ void ompl::control::PDST::freeMemory()
}
priorityQueue_.clear(); // clears the Element objects in the priority queue
delete bsp_;
- bsp_ = NULL;
+ bsp_ = nullptr;
}
void ompl::control::PDST::setup()
@@ -327,7 +327,7 @@ void ompl::control::PDST::setup()
if (bsp_)
delete bsp_;
bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const
@@ -341,7 +341,7 @@ void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const
priorityQueue_.getContent(motions);
// Add goal vertex
- if (lastGoalMotion_ != NULL)
+ if (lastGoalMotion_ != nullptr)
data.addGoalVertex(lastGoalMotion_->endState_);
for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
@@ -351,7 +351,7 @@ void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const
Motion *cur = *it, *ancestor;
unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
- if (cur->parent_ == NULL)
+ if (cur->parent_ == nullptr)
data.addStartVertex(base::PlannerDataVertex(cur->endState_));
else if (data.hasControls())
{
diff --git a/src/ompl/control/planners/rrt/RRT.h b/src/ompl/control/planners/rrt/RRT.h
index 450c54c..b9f07b7 100644
--- a/src/ompl/control/planners/rrt/RRT.h
+++ b/src/ompl/control/planners/rrt/RRT.h
@@ -132,12 +132,12 @@ namespace ompl
{
public:
- Motion() : state(NULL), control(NULL), steps(0), parent(NULL)
+ Motion() : state(nullptr), control(nullptr), steps(0), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state and the control */
- Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
+ Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(nullptr)
{
}
@@ -177,7 +177,7 @@ namespace ompl
const SpaceInformation *siC_;
/** \brief A nearest-neighbors datastructure containing the tree of motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
/** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
double goalBias_;
diff --git a/src/ompl/control/planners/rrt/src/RRT.cpp b/src/ompl/control/planners/rrt/src/RRT.cpp
index 9f60f40..3d6f349 100644
--- a/src/ompl/control/planners/rrt/src/RRT.cpp
+++ b/src/ompl/control/planners/rrt/src/RRT.cpp
@@ -44,7 +44,7 @@ ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT"
specs_.approximateSolutions = true;
siC_ = si.get();
addIntermediateStates_ = false;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
goalBias_ = 0.05;
@@ -62,7 +62,8 @@ void ompl::control::RRT::setup()
base::Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&RRT::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
}
void ompl::control::RRT::clear()
@@ -73,7 +74,7 @@ void ompl::control::RRT::clear()
freeMemory();
if (nn_)
nn_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::control::RRT::freeMemory()
@@ -120,8 +121,8 @@ ompl::base::PlannerStatus ompl::control::RRT::solve(const base::PlannerTerminati
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
- Motion *solution = NULL;
- Motion *approxsol = NULL;
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
Motion *rmotion = new Motion(siC_);
@@ -222,19 +223,19 @@ ompl::base::PlannerStatus ompl::control::RRT::solve(const base::PlannerTerminati
bool solved = false;
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
diff --git a/src/ompl/control/planners/sst/SST.h b/src/ompl/control/planners/sst/SST.h
new file mode 100644
index 0000000..309b9a7
--- /dev/null
+++ b/src/ompl/control/planners/sst/SST.h
@@ -0,0 +1,290 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Rutgers the State University of New Jersey, New Brunswick
+* 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 Rutgers University 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.
+*********************************************************************/
+
+/* Authors: Zakary Littlefield */
+
+#ifndef OMPL_CONTROL_PLANNERS_SST_SST_
+#define OMPL_CONTROL_PLANNERS_SST_SST_
+
+#include "ompl/control/planners/PlannerIncludes.h"
+#include "ompl/datastructures/NearestNeighbors.h"
+
+namespace ompl
+{
+ namespace control
+ {
+ /**
+ @anchor cSST
+ @par Short description
+ \ref cSST "SST" (Stable Sparse RRT) is a asymptotically near-optimal incremental
+ sampling-based motion planning algorithm for systems with dynamics. It makes use
+ of random control inputs to perform a search for the best control inputs to explore
+ the state space.
+ @par External documentation
+ Yanbo Li, Zakary Littlefield, Kostas E. Bekris, Sampling-based
+ Asymptotically Optimal Sampling-based Kinodynamic Planning.
+ [[PDF]](http://arxiv.org/abs/1407.2896)
+ */
+ class SST : public base::Planner
+ {
+ public:
+
+ /** \brief Constructor */
+ SST(const SpaceInformationPtr &si);
+
+ virtual ~SST();
+
+ virtual void setup();
+
+ /** \brief Continue solving for some amount of time. Return true if solution was found. */
+ virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
+
+ virtual void getPlannerData(base::PlannerData &data) const;
+
+ /** \brief Clear datastructures. Call this function if the
+ input data to the planner has changed and you do not
+ want to continue planning */
+ virtual void clear();
+
+ /** In the process of randomly selecting states in the state
+ space to attempt to go towards, the algorithm may in fact
+ choose the actual goal state, if it knows it, with some
+ probability. This probability is a real number between 0.0
+ and 1.0; its value should usually be around 0.05 and
+ should not be too large. It is probably a good idea to use
+ the default value. */
+ void setGoalBias(double goalBias)
+ {
+ goalBias_ = goalBias;
+ }
+
+ /** \brief Get the goal bias the planner is using */
+ double getGoalBias() const
+ {
+ return goalBias_;
+ }
+
+ /**
+ \brief Set the radius for selecting nodes relative to random sample.
+
+ This radius is used to mimic behavior of RRT* in that it promotes
+ extending from nodes with good path cost from the root of the tree.
+ Making this radius larger will provide higher quality paths, but has two
+ major drawbacks; exploration will occur much more slowly and exploration
+ around the boundary of the state space may become impossible. */
+ void setSelectionRadius(double selectionRadius)
+ {
+ selectionRadius_ = selectionRadius;
+ }
+
+ /** \brief Get the selection radius the planner is using */
+ double getSelectionRadius() const
+ {
+ return selectionRadius_;
+ }
+
+
+ /**
+ \brief Set the radius for pruning nodes.
+
+ This is the radius used to surround nodes in the witness set.
+ Within this radius around a state in the witness set, only one
+ active tree node can exist. This limits the size of the tree and
+ forces computation to focus on low path costs nodes. If this value
+ is too large, narrow passages will be impossible to traverse. In addition,
+ children nodes may be removed if they are not at least this distance away
+ from their parent nodes.*/
+ void setPruningRadius(double pruningRadius)
+ {
+ pruningRadius_ = pruningRadius;
+ }
+
+ /** \brief Get the pruning radius the planner is using */
+ double getPruningRadius() const
+ {
+ return pruningRadius_;
+ }
+
+ /** \brief Set a different nearest neighbors datastructure */
+ template<template<typename T> class NN>
+ void setNearestNeighbors()
+ {
+ nn_.reset(new NN<Motion*>());
+ witnesses_.reset(new NN<Motion*>());
+ }
+
+ protected:
+
+
+ /** \brief Representation of a motion
+
+ This only contains pointers to parent motions as we
+ only need to go backwards in the tree. */
+ class Motion
+ {
+ public:
+
+ Motion() : accCost_(0), state_(nullptr), control_(nullptr), steps_(0), parent_(nullptr), numChildren_(0), inactive_(false)
+ {
+ }
+
+ /** \brief Constructor that allocates memory for the state and the control */
+ Motion(const SpaceInformation *si) : accCost_(0), state_(si->allocState()), control_(si->allocControl()), steps_(0), parent_(nullptr), numChildren_(0), inactive_(false)
+ {
+ }
+
+ virtual ~Motion()
+ {
+ }
+
+ virtual base::State* getState() const
+ {
+ return state_;
+ }
+ virtual Motion* getParent() const
+ {
+ return parent_;
+ }
+
+ base::Cost accCost_;
+
+ /** \brief The state contained by the motion */
+ base::State *state_;
+
+ /** \brief The control contained by the motion */
+ Control *control_;
+
+ /** \brief The number of steps_ the control is applied for */
+ unsigned int steps_;
+
+ /** \brief The parent motion in the exploration tree */
+ Motion *parent_;
+
+ /** \brief Number of children */
+ unsigned numChildren_;
+
+ /** \brief If inactive, this node is not considered for selection.*/
+ bool inactive_;
+
+
+ };
+
+ class Witness : public Motion
+ {
+ public:
+
+ Witness() : Motion(), rep_(nullptr)
+ {
+ }
+
+ Witness(const SpaceInformation *si) : Motion(si), rep_(nullptr)
+ {
+ }
+ virtual base::State* getState() const
+ {
+ return rep_->state_;
+ }
+ virtual Motion* getParent() const
+ {
+ return rep_->parent_;
+ }
+
+ void linkRep(Motion *lRep)
+ {
+ rep_ = lRep;
+ }
+
+ /** \brief The node in the tree that is within the pruning radius.*/
+ Motion* rep_;
+ };
+
+ /** \brief Finds the best node in the tree withing the selection radius around a random sample.*/
+ Motion* selectNode(Motion *sample);
+
+ /** \brief Find the closest witness node to a newly generated potential node.*/
+ Witness* findClosestWitness(Motion *node);
+
+ /** \brief Free the memory allocated by this planner */
+ void freeMemory();
+
+ /** \brief Compute distance between motions (actually distance between contained states) */
+ double distanceFunction(const Motion *a, const Motion *b) const
+ {
+ return si_->distance(a->state_, b->state_);
+ }
+
+ /** \brief State sampler */
+ base::StateSamplerPtr sampler_;
+
+ /** \brief Control sampler */
+ ControlSamplerPtr controlSampler_;
+
+ /** \brief The base::SpaceInformation cast as control::SpaceInformation, for convenience */
+ const SpaceInformation *siC_;
+
+ /** \brief A nearest-neighbors datastructure containing the tree of motions */
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
+
+
+ /** \brief A nearest-neighbors datastructure containing the tree of witness motions */
+ std::shared_ptr< NearestNeighbors<Motion*> > witnesses_;
+
+ /** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
+ double goalBias_;
+
+ /** \brief The radius for determining the node selected for extension. */
+ double selectionRadius_;
+
+ /** \brief The radius for determining the size of the pruning region. */
+ double pruningRadius_;
+
+ /** \brief The random number generator */
+ RNG rng_;
+
+ /** \brief The best solution we found so far. */
+ std::vector<base::State*> prevSolution_;
+ std::vector<Control*> prevSolutionControls_;
+ std::vector<unsigned> prevSolutionSteps_;
+
+ /** \brief The best solution cost we found so far. */
+ base::Cost prevSolutionCost_;
+
+ /** \brief The optimization objective. */
+ base::OptimizationObjectivePtr opt_;
+
+ };
+ }
+}
+
+#endif
\ No newline at end of file
diff --git a/src/ompl/control/planners/sst/src/SST.cpp b/src/ompl/control/planners/sst/src/SST.cpp
new file mode 100644
index 0000000..8cdc605
--- /dev/null
+++ b/src/ompl/control/planners/sst/src/SST.cpp
@@ -0,0 +1,453 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Rutgers the State University of New Jersey, New Brunswick
+* 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 Rutgers University 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.
+*********************************************************************/
+
+/* Authors: Zakary Littlefield */
+
+#include "ompl/control/planners/sst/SST.h"
+#include "ompl/base/goals/GoalSampleableRegion.h"
+#include "ompl/base/objectives/MinimaxObjective.h"
+#include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
+#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
+#include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
+#include "ompl/tools/config/SelfConfig.h"
+#include <limits>
+
+ompl::control::SST::SST(const SpaceInformationPtr &si) : base::Planner(si, "SST")
+{
+ specs_.approximateSolutions = true;
+ siC_ = si.get();
+ prevSolution_.clear();
+ prevSolutionControls_.clear();
+ prevSolutionSteps_.clear();
+
+ goalBias_ = 0.05;
+ selectionRadius_ = 0.2;
+ pruningRadius_ = 0.1;
+
+ Planner::declareParam<double>("goal_bias", this, &SST::setGoalBias, &SST::getGoalBias, "0.:.05:1.");
+ Planner::declareParam<double>("selection_radius", this, &SST::setSelectionRadius, &SST::getSelectionRadius, "0.:.1:100");
+ Planner::declareParam<double>("pruning_radius", this, &SST::setPruningRadius, &SST::getPruningRadius, "0.:.1:100");
+}
+
+ompl::control::SST::~SST()
+{
+ freeMemory();
+}
+
+void ompl::control::SST::setup()
+{
+ base::Planner::setup();
+ if (!nn_)
+ nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
+ nn_->setDistanceFunction(std::bind(&SST::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
+ if (!witnesses_)
+ witnesses_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
+ witnesses_->setDistanceFunction(std::bind(&SST::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
+
+ if (pdef_)
+ {
+ if (pdef_->hasOptimizationObjective())
+ {
+ opt_ = pdef_->getOptimizationObjective();
+ if (dynamic_cast<base::MaximizeMinClearanceObjective*>(opt_.get()) || dynamic_cast<base::MinimaxObjective*>(opt_.get()))
+ OMPL_WARN("%s: Asymptotic near-optimality has only been proven with Lipschitz continuous cost functions w.r.t. state and control. This optimization objective will result in undefined behavior", getName().c_str());
+ }
+ else
+ {
+ OMPL_WARN("%s: No optimization object set. Using path length", getName().c_str());
+ opt_.reset(new base::PathLengthOptimizationObjective(si_));
+ pdef_->setOptimizationObjective(opt_);
+ }
+ }
+
+ prevSolutionCost_ = opt_->infiniteCost();
+
+}
+
+void ompl::control::SST::clear()
+{
+ Planner::clear();
+ sampler_.reset();
+ controlSampler_.reset();
+ freeMemory();
+ if (nn_)
+ nn_->clear();
+ if (witnesses_)
+ witnesses_->clear();
+ prevSolutionCost_ = opt_->infiniteCost();
+}
+
+void ompl::control::SST::freeMemory()
+{
+ if (nn_)
+ {
+ std::vector<Motion*> motions;
+ nn_->list(motions);
+ for (unsigned int i = 0 ; i < motions.size() ; ++i)
+ {
+ if (motions[i]->state_)
+ si_->freeState(motions[i]->state_);
+ if (motions[i]->control_)
+ siC_->freeControl(motions[i]->control_);
+ delete motions[i];
+ }
+ }
+ if (witnesses_)
+ {
+ std::vector<Motion*> witnesses;
+ witnesses_->list(witnesses);
+ for (unsigned int i = 0 ; i < witnesses.size() ; ++i)
+ {
+ delete witnesses[i];
+ }
+ }
+ for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
+ {
+ if (prevSolution_[i])
+ si_->freeState(prevSolution_[i]);
+ }
+ prevSolution_.clear();
+ for (unsigned int i = 0 ; i < prevSolutionControls_.size() ; ++i)
+ {
+ if (prevSolutionControls_[i])
+ siC_->freeControl(prevSolutionControls_[i]);
+ }
+ prevSolutionControls_.clear();
+ prevSolutionSteps_.clear();
+}
+
+ompl::control::SST::Motion* ompl::control::SST::selectNode(ompl::control::SST::Motion *sample)
+{
+ std::vector<Motion*> ret;
+ Motion *selected = nullptr;
+ base::Cost bestCost = opt_->infiniteCost();
+ nn_->nearestR(sample, selectionRadius_, ret);
+ for (unsigned int i = 0; i < ret.size(); i++)
+ {
+ if (!ret[i]->inactive_ && opt_->isCostBetterThan(ret[i]->accCost_, bestCost))
+ {
+ bestCost = ret[i]->accCost_;
+ selected = ret[i];
+ }
+ }
+ if (selected == nullptr)
+ {
+ int k = 1;
+ while (selected == nullptr)
+ {
+ nn_->nearestK(sample, k, ret);
+ for (unsigned int i=0; i < ret.size() && selected == nullptr; i++)
+ if (!ret[i]->inactive_)
+ selected = ret[i];
+ k += 5;
+ }
+ }
+ return selected;
+}
+
+ompl::control::SST::Witness* ompl::control::SST::findClosestWitness(ompl::control::SST::Motion *node)
+{
+ if(witnesses_->size() > 0)
+ {
+ Witness* closest = static_cast<Witness*>(witnesses_->nearest(node));
+ if (distanceFunction(closest,node) > pruningRadius_)
+ {
+ closest = new Witness(siC_);
+ closest->linkRep(node);
+ si_->copyState(closest->state_, node->state_);
+ witnesses_->add(closest);
+ }
+ return closest;
+ }
+ else
+ {
+ Witness* closest = new Witness(siC_);
+ closest->linkRep(node);
+ si_->copyState(closest->state_, node->state_);
+ witnesses_->add(closest);
+ return closest;
+ }
+}
+
+ompl::base::PlannerStatus ompl::control::SST::solve(const base::PlannerTerminationCondition &ptc)
+{
+ checkValidity();
+ base::Goal *goal = pdef_->getGoal().get();
+ base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
+
+ while (const base::State *st = pis_.nextStart())
+ {
+ Motion *motion = new Motion(siC_);
+ si_->copyState(motion->state_, st);
+ siC_->nullControl(motion->control_);
+ nn_->add(motion);
+ motion->accCost_ = opt_->identityCost();
+ findClosestWitness(motion);
+ }
+
+ if (nn_->size() == 0)
+ {
+ OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
+ return base::PlannerStatus::INVALID_START;
+ }
+
+ if (!sampler_)
+ sampler_ = si_->allocStateSampler();
+ if (!controlSampler_)
+ controlSampler_ = siC_->allocControlSampler();
+
+ OMPL_INFORM("%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size());
+
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
+ double approxdif = std::numeric_limits<double>::infinity();
+ bool sufficientlyShort = false;
+
+ Motion *rmotion = new Motion(siC_);
+ base::State *rstate = rmotion->state_;
+ Control *rctrl = rmotion->control_;
+ base::State *xstate = si_->allocState();
+
+ unsigned iterations = 0;
+
+ while (ptc == false)
+ {
+
+ /* sample random state (with goal biasing) */
+ if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
+ goal_s->sampleGoal(rstate);
+ else
+ sampler_->sampleUniform(rstate);
+
+ /* find closest state in the tree */
+ Motion *nmotion = selectNode(rmotion);
+
+
+
+ /* sample a random control that attempts to go towards the random state, and also sample a control duration */
+ controlSampler_->sample(rctrl);
+ unsigned int cd = rng_.uniformInt(siC_->getMinControlDuration(),siC_->getMaxControlDuration());
+ unsigned int propCd = siC_->propagateWhileValid(nmotion->state_,rctrl,cd,rstate);
+
+
+ if (propCd == cd)
+ {
+ base::Cost incCost = opt_->motionCost(nmotion->state_, rstate);
+ base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
+ Witness* closestWitness = findClosestWitness(rmotion);
+
+
+ if (closestWitness->rep_ == rmotion || opt_->isCostBetterThan(cost,closestWitness->rep_->accCost_))
+ {
+ Motion* oldRep = closestWitness->rep_;
+ /* create a motion */
+ Motion *motion = new Motion(siC_);
+ motion->accCost_ = cost;
+ si_->copyState(motion->state_, rmotion->state_);
+ siC_->copyControl(motion->control_, rctrl);
+ motion->steps_ = cd;
+ motion->parent_ = nmotion;
+ nmotion->numChildren_++;
+ closestWitness->linkRep(motion);
+
+ nn_->add(motion);
+ double dist = 0.0;
+ bool solv = goal->isSatisfied(motion->state_, &dist);
+ if (solv && opt_->isCostBetterThan(motion->accCost_,prevSolutionCost_))
+ {
+ approxdif = dist;
+ solution = motion;
+
+ for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
+ if (prevSolution_[i])
+ si_->freeState(prevSolution_[i]);
+ prevSolution_.clear();
+ for (unsigned int i = 0 ; i < prevSolutionControls_.size() ; ++i)
+ if (prevSolutionControls_[i])
+ siC_->freeControl(prevSolutionControls_[i]);
+ prevSolutionControls_.clear();
+ prevSolutionSteps_.clear();
+
+
+ Motion* solTrav = solution;
+ while(solTrav->parent_!=nullptr)
+ {
+ prevSolution_.push_back(si_->cloneState(solTrav->state_) );
+ prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_) );
+ prevSolutionSteps_.push_back(solTrav->steps_ );
+ solTrav = solTrav->parent_;
+ }
+ prevSolution_.push_back(si_->cloneState(solTrav->state_) );
+ prevSolutionCost_ = solution->accCost_;
+
+
+
+
+ OMPL_INFORM("Found solution with cost %.2f",solution->accCost_.value());
+ sufficientlyShort = opt_->isSatisfied(solution->accCost_);
+ if (sufficientlyShort)
+ break;
+ }
+ if (solution==nullptr && dist < approxdif)
+ {
+ approxdif = dist;
+ approxsol = motion;
+
+
+
+ for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
+ if (prevSolution_[i])
+ si_->freeState(prevSolution_[i]);
+ prevSolution_.clear();
+ for (unsigned int i = 0 ; i < prevSolutionControls_.size() ; ++i)
+ if (prevSolutionControls_[i])
+ siC_->freeControl(prevSolutionControls_[i]);
+ prevSolutionControls_.clear();
+ prevSolutionSteps_.clear();
+
+ Motion *solTrav = approxsol;
+ while (solTrav->parent_!=nullptr)
+ {
+ prevSolution_.push_back(si_->cloneState(solTrav->state_) );
+ prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_) );
+ prevSolutionSteps_.push_back(solTrav->steps_ );
+ solTrav = solTrav->parent_;
+ }
+ prevSolution_.push_back(si_->cloneState(solTrav->state_) );
+ }
+
+ if (oldRep != rmotion)
+ {
+ oldRep->inactive_ = true;
+ nn_->remove(oldRep);
+ while (oldRep->inactive_ && oldRep->numChildren_==0)
+ {
+ if (oldRep->state_)
+ si_->freeState(oldRep->state_);
+ if (oldRep->control_)
+ siC_->freeControl(oldRep->control_);
+
+ oldRep->state_=nullptr;
+ oldRep->control_=nullptr;
+ oldRep->parent_->numChildren_--;
+ Motion* oldRepParent = oldRep->parent_;
+ delete oldRep;
+ oldRep = oldRepParent;
+ }
+ }
+
+ }
+ }
+ iterations++;
+ }
+
+ bool solved = false;
+ bool approximate = false;
+ if (solution == nullptr)
+ {
+ solution = approxsol;
+ approximate = true;
+ }
+
+ if (solution != nullptr)
+ {
+ /* set the solution path */
+ PathControl *path = new PathControl(si_);
+ for (int i = prevSolution_.size() - 1 ; i >= 1 ; --i)
+ path->append(prevSolution_[i], prevSolutionControls_[i-1], prevSolutionSteps_[i-1] * siC_->getPropagationStepSize());
+ path->append(prevSolution_[0]);
+ solved = true;
+ pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
+ }
+
+ si_->freeState(xstate);
+ if (rmotion->state_)
+ si_->freeState(rmotion->state_);
+ if (rmotion->control_)
+ siC_->freeControl(rmotion->control_);
+ delete rmotion;
+
+ OMPL_INFORM("%s: Created %u states in %u iterations", getName().c_str(), nn_->size(),iterations);
+
+ return base::PlannerStatus(solved, approximate);
+}
+
+void ompl::control::SST::getPlannerData(base::PlannerData &data) const
+{
+ Planner::getPlannerData(data);
+
+ std::vector<Motion*> motions;
+ std::vector<Motion*> allMotions;
+ if (nn_)
+ nn_->list(motions);
+
+ for(unsigned i=0;i<motions.size();i++)
+ {
+ if(motions[i]->numChildren_==0)
+ {
+ allMotions.push_back(motions[i]);
+ }
+ }
+ for(unsigned i=0;i<allMotions.size();i++)
+ {
+ if(allMotions[i]->parent_!=nullptr)
+ {
+ allMotions.push_back(allMotions[i]->parent_);
+ }
+ }
+
+ double delta = siC_->getPropagationStepSize();
+
+ if (prevSolution_.size()!=0)
+ data.addGoalVertex(base::PlannerDataVertex(prevSolution_[0]));
+
+ for (unsigned int i = 0 ; i < allMotions.size() ; ++i)
+ {
+ const Motion *m = allMotions[i];
+ if (m->parent_)
+ {
+ if (data.hasControls())
+ data.addEdge(base::PlannerDataVertex(m->parent_->state_),
+ base::PlannerDataVertex(m->state_),
+ control::PlannerDataEdgeControl(m->control_, m->steps_ * delta));
+ else
+ data.addEdge(base::PlannerDataVertex(m->parent_->state_),
+ base::PlannerDataVertex(m->state_));
+ }
+ else
+ data.addStartVertex(base::PlannerDataVertex(m->state_));
+ }
+}
diff --git a/src/ompl/control/planners/syclop/Decomposition.h b/src/ompl/control/planners/syclop/Decomposition.h
index 01cf351..0be442b 100644
--- a/src/ompl/control/planners/syclop/Decomposition.h
+++ b/src/ompl/control/planners/syclop/Decomposition.h
@@ -56,7 +56,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::DecompositionPtr
- \brief A boost shared pointer wrapper for ompl::control::Decomposition */
+ \brief A shared pointer wrapper for ompl::control::Decomposition */
/** \brief A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are denoted by integers. */
class Decomposition
diff --git a/src/ompl/control/planners/syclop/GridDecomposition.h b/src/ompl/control/planners/syclop/GridDecomposition.h
index 1028730..e7f7e62 100644
--- a/src/ompl/control/planners/syclop/GridDecomposition.h
+++ b/src/ompl/control/planners/syclop/GridDecomposition.h
@@ -38,8 +38,8 @@
#define OMPL_CONTROL_PLANNERS_SYCLOP_GRIDDECOMPOSITION_
#include <cstdlib>
-#include <boost/shared_ptr.hpp>
-#include <boost/unordered_map.hpp>
+#include <memory>
+#include <unordered_map>
#include "ompl/base/spaces/RealVectorBounds.h"
#include "ompl/base/State.h"
#include "ompl/control/planners/syclop/Decomposition.h"
@@ -100,7 +100,7 @@ namespace ompl
int length_;
double cellVolume_;
- mutable boost::unordered_map<int, boost::shared_ptr<base::RealVectorBounds> > regToBounds_;
+ mutable std::unordered_map<int, std::shared_ptr<base::RealVectorBounds> > regToBounds_;
private:
const int numGridCells_;
diff --git a/src/ompl/control/planners/syclop/Syclop.h b/src/ompl/control/planners/syclop/Syclop.h
index 00f378b..ffbf0da 100644
--- a/src/ompl/control/planners/syclop/Syclop.h
+++ b/src/ompl/control/planners/syclop/Syclop.h
@@ -40,11 +40,12 @@
#include <boost/graph/astar_search.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include "ompl/control/planners/PlannerIncludes.h"
#include "ompl/control/planners/syclop/Decomposition.h"
#include "ompl/control/planners/syclop/GridDecomposition.h"
#include "ompl/datastructures/PDF.h"
+#include "ompl/util/Hash.h"
#include <map>
#include <vector>
@@ -85,10 +86,10 @@ namespace ompl
of \f$t\f$.
Additional edge cost factors can be added
with the addEdgeCostFactor() function, and Syclop's list of edge cost factors can be cleared using clearEdgeCostFactors() . */
- typedef boost::function<double(int, int)> EdgeCostFactorFn;
+ typedef std::function<double(int, int)> EdgeCostFactorFn;
/** \brief Leads should consist of a path of adjacent regions in the decomposition that start with the start region and end at the end region. Default is \f$A^\ast\f$ search. */
- typedef boost::function<void(int, int, std::vector<int>&)> LeadComputeFn;
+ typedef std::function<void(int, int, std::vector<int>&)> LeadComputeFn;
/** \brief Constructor. Requires a Decomposition, which Syclop uses to create high-level leads. */
Syclop(const SpaceInformationPtr& si, const DecompositionPtr &d, const std::string& plannerName) : ompl::base::Planner(si, plannerName),
@@ -251,11 +252,11 @@ namespace ompl
class Motion
{
public:
- Motion() : state(NULL), control(NULL), parent(NULL), steps(0)
+ Motion() : state(nullptr), control(nullptr), parent(nullptr), steps(0)
{
}
/** \brief Constructor that allocates memory for the state and the control */
- Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), parent(NULL), steps(0)
+ Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), parent(nullptr), steps(0)
{
}
virtual ~Motion()
@@ -283,12 +284,20 @@ namespace ompl
virtual ~Region()
{
}
+
+#if __cplusplus >= 201103L
+ Region(const Region&) = default;
+ Region& operator=(const Region&) = default;
+ Region(Region&&) = default;
+ Region& operator=(Region&&) = default;
+#endif
+
/** \brief Clears motions and coverage information from this region. */
void clear()
{
motions.clear();
covGridCells.clear();
- pdfElem = NULL;
+ pdfElem = nullptr;
}
/** \brief The cells of the underlying coverage grid that contain tree motions from this region */
@@ -391,6 +400,20 @@ namespace ompl
RNG rng_;
private:
+ /// @cond IGNORE
+ /** \brief Hash function for std::pair<int,int> to be used in std::unordered_map */
+ struct HashRegionPair
+ {
+ size_t operator()(const std::pair<int,int> &p) const
+ {
+ std::size_t hash = std::hash<int>()(p.first);
+ hash_combine(hash, p.second);
+ return hash;
+ }
+ };
+ /// @endcond
+
+
/** \brief Syclop uses a CoverageGrid to estimate coverage in its assigned Decomposition.
The CoverageGrid should have finer resolution than the Decomposition. */
class CoverageGrid : public GridDecomposition
@@ -500,7 +523,7 @@ namespace ompl
private:
RNG rng;
PDF<int> regions;
- boost::unordered_map<const int, PDF<int>::Element*> regToElem;
+ std::unordered_map<int, PDF<int>::Element*> regToElem;
};
/// @endcond
@@ -549,7 +572,7 @@ namespace ompl
/** \brief Default edge cost factor, which is used by Syclop for edge weights between adjacent Regions. */
double defaultEdgeCost(int r, int s);
- /** \brief Lead computaton boost::function object */
+ /** \brief Lead computaton std::function object */
LeadComputeFn leadComputeFn;
/** \brief The current computed lead */
std::vector<int> lead_;
@@ -564,7 +587,7 @@ namespace ompl
/** \brief This value stores whether the graph structure has been built */
bool graphReady_;
/** \brief Maps pairs of regions to adjacency objects */
- boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
+ std::unordered_map<std::pair<int,int>, Adjacency*, HashRegionPair> regionsToEdge_;
/** \brief The total number of motions in the low-level tree */
unsigned int numMotions_;
/** \brief The set of all regions that contain start states */
diff --git a/src/ompl/control/planners/syclop/SyclopRRT.h b/src/ompl/control/planners/syclop/SyclopRRT.h
index de2c397..a0aa0af 100644
--- a/src/ompl/control/planners/syclop/SyclopRRT.h
+++ b/src/ompl/control/planners/syclop/SyclopRRT.h
@@ -100,7 +100,7 @@ namespace ompl
base::StateSamplerPtr sampler_;
DirectedControlSamplerPtr controlSampler_;
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
bool regionalNN_;
/** \brief The most recent goal motion. Used for PlannerData computation */
diff --git a/src/ompl/control/planners/syclop/src/GridDecomposition.cpp b/src/ompl/control/planners/syclop/src/GridDecomposition.cpp
index 106a5cc..40e9f88 100644
--- a/src/ompl/control/planners/syclop/src/GridDecomposition.cpp
+++ b/src/ompl/control/planners/syclop/src/GridDecomposition.cpp
@@ -271,7 +271,7 @@ const ompl::base::RealVectorBounds& ompl::control::GridDecomposition::getRegionB
regionBounds->low[i] = bounds_.low[i] + length*rc[i];
regionBounds->high[i] = regionBounds->low[i] + length;
}
- regToBounds_[rid] = boost::shared_ptr<ompl::base::RealVectorBounds>(regionBounds);
+ regToBounds_[rid] = std::shared_ptr<ompl::base::RealVectorBounds>(regionBounds);
return *regToBounds_[rid].get();
}
diff --git a/src/ompl/control/planners/syclop/src/Syclop.cpp b/src/ompl/control/planners/syclop/src/Syclop.cpp
index 813d040..5948ebb 100644
--- a/src/ompl/control/planners/syclop/src/Syclop.cpp
+++ b/src/ompl/control/planners/syclop/src/Syclop.cpp
@@ -49,9 +49,11 @@ void ompl::control::Syclop::setup()
{
base::Planner::setup();
if (!leadComputeFn)
- setLeadComputeFn(boost::bind(&ompl::control::Syclop::defaultComputeLead, this, _1, _2, _3));
+ setLeadComputeFn(std::bind(&ompl::control::Syclop::defaultComputeLead, this,
+ std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
buildGraph();
- addEdgeCostFactor(boost::bind(&ompl::control::Syclop::defaultEdgeCost, this, _1, _2));
+ addEdgeCostFactor(std::bind(&ompl::control::Syclop::defaultEdgeCost, this,
+ std::placeholders::_1, std::placeholders::_2));
}
void ompl::control::Syclop::clear()
@@ -105,7 +107,7 @@ ompl::base::PlannerStatus ompl::control::Syclop::solve(const base::PlannerTermin
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
std::vector<Motion*> newMotions;
- const Motion *solution = NULL;
+ const Motion *solution = nullptr;
base::Goal *goal = pdef_->getGoal().get();
double goalDist = std::numeric_limits<double>::infinity();
bool solved = false;
@@ -172,7 +174,7 @@ ompl::base::PlannerStatus ompl::control::Syclop::solve(const base::PlannerTermin
}
/* If this region already exists in availDist, update its weight. */
- if (newRegionObj.pdfElem != NULL)
+ if (newRegionObj.pdfElem != nullptr)
availDist_.update(newRegionObj.pdfElem, newRegionObj.weight);
/* Otherwise, only add this region to availDist
if it already exists in the lead. */
@@ -188,10 +190,10 @@ ompl::base::PlannerStatus ompl::control::Syclop::solve(const base::PlannerTermin
}
}
bool addedSolution = false;
- if (solution != NULL)
+ if (solution != nullptr)
{
std::vector<const Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -229,7 +231,7 @@ void ompl::control::Syclop::initRegion(Region &r)
r.volume = 1.0;
r.percentValidCells = 1.0;
r.freeVolume = 1.0;
- r.pdfElem = NULL;
+ r.pdfElem = nullptr;
}
void ompl::control::Syclop::setupRegionEstimates()
@@ -378,7 +380,7 @@ int ompl::control::Syclop::selectRegion()
void ompl::control::Syclop::computeAvailableRegions()
{
for (unsigned int i = 0; i < availDist_.size(); ++i)
- graph_[boost::vertex(availDist_[i],graph_)].pdfElem = NULL;
+ graph_[boost::vertex(availDist_[i],graph_)].pdfElem = nullptr;
availDist_.clear();
for (int i = lead_.size()-1; i >= 0; --i)
{
diff --git a/src/ompl/control/planners/syclop/src/SyclopEST.cpp b/src/ompl/control/planners/syclop/src/SyclopEST.cpp
index 0f45631..c4db77a 100644
--- a/src/ompl/control/planners/syclop/src/SyclopEST.cpp
+++ b/src/ompl/control/planners/syclop/src/SyclopEST.cpp
@@ -42,7 +42,7 @@ void ompl::control::SyclopEST::setup()
Syclop::setup();
sampler_ = si_->allocStateSampler();
controlSampler_ = siC_->allocControlSampler();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::control::SyclopEST::clear()
@@ -50,7 +50,7 @@ void ompl::control::SyclopEST::clear()
Syclop::clear();
freeMemory();
motions_.clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::control::SyclopEST::getPlannerData(base::PlannerData &data) const
diff --git a/src/ompl/control/planners/syclop/src/SyclopRRT.cpp b/src/ompl/control/planners/syclop/src/SyclopRRT.cpp
index 46166c1..0ade194 100644
--- a/src/ompl/control/planners/syclop/src/SyclopRRT.cpp
+++ b/src/ompl/control/planners/syclop/src/SyclopRRT.cpp
@@ -43,14 +43,15 @@ void ompl::control::SyclopRRT::setup()
Syclop::setup();
sampler_ = si_->allocStateSampler();
controlSampler_ = siC_->allocDirectedControlSampler();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
// Create a default GNAT nearest neighbors structure if the user doesn't want
// the default regionalNN check from the discretization
if (!nn_ && !regionalNN_)
{
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&SyclopRRT::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&SyclopRRT::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
}
}
@@ -60,7 +61,7 @@ void ompl::control::SyclopRRT::clear()
freeMemory();
if (nn_)
nn_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::control::SyclopRRT::getPlannerData(base::PlannerData &data) const
diff --git a/src/ompl/control/spaces/src/DiscreteControlSpace.cpp b/src/ompl/control/spaces/src/DiscreteControlSpace.cpp
index 62be8d4..1aa5384 100644
--- a/src/ompl/control/spaces/src/DiscreteControlSpace.cpp
+++ b/src/ompl/control/spaces/src/DiscreteControlSpace.cpp
@@ -85,7 +85,7 @@ void ompl::control::DiscreteControlSpace::printControl(const Control *control, s
if (control)
out << control->as<ControlType>()->value;
else
- out << "NULL";
+ out << "nullptr";
out << ']' << std::endl;
}
diff --git a/src/ompl/control/spaces/src/RealVectorControlSpace.cpp b/src/ompl/control/spaces/src/RealVectorControlSpace.cpp
index 8af83bc..670756f 100644
--- a/src/ompl/control/spaces/src/RealVectorControlSpace.cpp
+++ b/src/ompl/control/spaces/src/RealVectorControlSpace.cpp
@@ -36,7 +36,6 @@
#include "ompl/control/spaces/RealVectorControlSpace.h"
#include "ompl/util/Exception.h"
-#include <boost/lexical_cast.hpp>
#include <cstring>
#include <limits>
@@ -61,8 +60,8 @@ void ompl::control::RealVectorControlSpace::setBounds(const base::RealVectorBoun
bounds.check();
if (bounds.low.size() != dimension_)
throw Exception("Bounds do not match dimension of control space: expected dimension " +
- boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
- boost::lexical_cast<std::string>(bounds.low.size()));
+ std::to_string(dimension_) + " but got dimension " +
+ std::to_string(bounds.low.size()));
bounds_ = bounds;
}
@@ -123,7 +122,7 @@ void ompl::control::RealVectorControlSpace::nullControl(Control *control) const
double* ompl::control::RealVectorControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
{
- return index < dimension_ ? static_cast<ControlType*>(control)->values + index : NULL;
+ return index < dimension_ ? static_cast<ControlType*>(control)->values + index : nullptr;
}
void ompl::control::RealVectorControlSpace::printControl(const Control *control, std::ostream &out) const
@@ -140,7 +139,7 @@ void ompl::control::RealVectorControlSpace::printControl(const Control *control,
}
}
else
- out << "NULL";
+ out << "nullptr";
out << ']' << std::endl;
}
diff --git a/src/ompl/control/src/ControlSpace.cpp b/src/ompl/control/src/ControlSpace.cpp
index 5387358..20a298f 100644
--- a/src/ompl/control/src/ControlSpace.cpp
+++ b/src/ompl/control/src/ControlSpace.cpp
@@ -102,7 +102,7 @@ void ompl::control::ControlSpace::clearControlSamplerAllocator()
double* ompl::control::ControlSpace::getValueAddressAtIndex(Control* /*control*/, const unsigned int /*index*/) const
{
- return NULL;
+ return nullptr;
}
void ompl::control::ControlSpace::printControl(const Control *control, std::ostream &out) const
@@ -253,7 +253,7 @@ double* ompl::control::CompoundControlSpace::getValueAddressAtIndex(Control *con
else
break;
}
- return NULL;
+ return nullptr;
}
void ompl::control::CompoundControlSpace::printControl(const Control *control, std::ostream &out) const
diff --git a/src/ompl/control/src/PlannerData.cpp b/src/ompl/control/src/PlannerData.cpp
index 56186de..ebf8f37 100644
--- a/src/ompl/control/src/PlannerData.cpp
+++ b/src/ompl/control/src/PlannerData.cpp
@@ -36,7 +36,7 @@
#include "ompl/control/PlannerData.h"
-ompl::control::PlannerData::PlannerData(const SpaceInformationPtr &siC) : base::PlannerData(boost::static_pointer_cast<base::SpaceInformation>(siC)), siC_(siC)
+ompl::control::PlannerData::PlannerData(const SpaceInformationPtr &siC) : base::PlannerData(std::static_pointer_cast<base::SpaceInformation>(siC)), siC_(siC)
{
}
diff --git a/src/ompl/control/src/SimpleDirectedControlSampler.cpp b/src/ompl/control/src/SimpleDirectedControlSampler.cpp
index f1a3f6c..fd2974c 100644
--- a/src/ompl/control/src/SimpleDirectedControlSampler.cpp
+++ b/src/ompl/control/src/SimpleDirectedControlSampler.cpp
@@ -48,7 +48,7 @@ ompl::control::SimpleDirectedControlSampler::~SimpleDirectedControlSampler()
unsigned int ompl::control::SimpleDirectedControlSampler::sampleTo(Control *control, const base::State *source, base::State *dest)
{
- return getBestControl(control, source, dest, NULL);
+ return getBestControl(control, source, dest, nullptr);
}
unsigned int ompl::control::SimpleDirectedControlSampler::sampleTo(Control *control, const Control *previous, const base::State *source, base::State *dest)
diff --git a/src/ompl/datastructures/BinaryHeap.h b/src/ompl/datastructures/BinaryHeap.h
index cd773b5..697686d 100644
--- a/src/ompl/datastructures/BinaryHeap.h
+++ b/src/ompl/datastructures/BinaryHeap.h
@@ -79,8 +79,8 @@ namespace ompl
BinaryHeap()
{
- eventAfterInsert_ = NULL;
- eventBeforeRemove_ = NULL;
+ eventAfterInsert_ = nullptr;
+ eventBeforeRemove_ = nullptr;
}
~BinaryHeap()
@@ -111,10 +111,10 @@ namespace ompl
vector_.clear();
}
- /** \brief Return the top element. NULL for an empty heap. */
+ /** \brief Return the top element. nullptr for an empty heap. */
Element* top() const
{
- return vector_.empty() ? NULL : vector_.at(0);
+ return vector_.empty() ? nullptr : vector_.at(0);
}
/** \brief Remove the top element */
diff --git a/src/ompl/datastructures/DynamicSSSP.h b/src/ompl/datastructures/DynamicSSSP.h
index 7c4e21f..17d1e81 100644
--- a/src/ompl/datastructures/DynamicSSSP.h
+++ b/src/ompl/datastructures/DynamicSSSP.h
@@ -49,7 +49,7 @@ dynamic graph problems, Theor. Comput. Sci., vol. 158, no. 1&2, pp.
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
-#include <boost/unordered_set.hpp>
+#include <unordered_set>
namespace ompl
{
@@ -264,7 +264,7 @@ namespace ompl
{
}
- bool operator()(std::size_t id1, std::size_t id2)
+ bool operator()(std::size_t id1, std::size_t id2) const
{
return (cost_[id1] < cost_[id2]);
}
@@ -274,7 +274,7 @@ namespace ompl
typedef std::set<std::size_t, IsLessThan> Queue;
typedef Queue::iterator QueueIter;
- typedef boost::unordered_set<std::size_t> IntSet;
+ typedef std::unordered_set<std::size_t> IntSet;
typedef IntSet::iterator IntSetIter;
Graph* graph_;
diff --git a/src/ompl/datastructures/GreedyKCenters.h b/src/ompl/datastructures/GreedyKCenters.h
index 9c74dc7..d1f9d80 100644
--- a/src/ompl/datastructures/GreedyKCenters.h
+++ b/src/ompl/datastructures/GreedyKCenters.h
@@ -38,6 +38,7 @@
#define OMPL_DATASTRUCTURES_GREEDY_K_CENTERS_
#include "ompl/util/RandomNumbers.h"
+#include <functional>
#include <boost/numeric/ublas/matrix.hpp>
namespace ompl
@@ -50,7 +51,7 @@ namespace ompl
{
public:
/** \brief The definition of a distance function */
- typedef boost::function<double(const _T&, const _T&)> DistanceFunction;
+ typedef std::function<double(const _T&, const _T&)> DistanceFunction;
/** \brief A matrix type for storing distances between points and centers */
typedef boost::numeric::ublas::matrix<double> Matrix;
diff --git a/src/ompl/datastructures/Grid.h b/src/ompl/datastructures/Grid.h
index 57d7c8b..cb8b593 100644
--- a/src/ompl/datastructures/Grid.h
+++ b/src/ompl/datastructures/Grid.h
@@ -40,7 +40,7 @@
#include <vector>
#include <iostream>
#include <cstdlib>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <algorithm>
namespace ompl
@@ -115,14 +115,14 @@ namespace ompl
/// Check if a cell exists at the specified coordinate
bool has(const Coord &coord) const
{
- return getCell(coord) != NULL;
+ return getCell(coord) != nullptr;
}
/// Get the cell at a specified coordinate
Cell* getCell(const Coord &coord) const
{
iterator pos = hash_.find(const_cast<Coord*>(&coord));
- Cell *c = (pos != hash_.end()) ? pos->second : NULL;
+ Cell *c = (pos != hash_.end()) ? pos->second : nullptr;
return c;
}
@@ -150,14 +150,14 @@ namespace ompl
coord[i]--;
iterator pos = hash_.find(&coord);
- Cell *cell = (pos != hash_.end()) ? pos->second : NULL;
+ Cell *cell = (pos != hash_.end()) ? pos->second : nullptr;
if (cell)
list.push_back(cell);
coord[i] += 2;
pos = hash_.find(&coord);
- cell = (pos != hash_.end()) ? pos->second : NULL;
+ cell = (pos != hash_.end()) ? pos->second : nullptr;
if (cell)
list.push_back(cell);
@@ -168,7 +168,7 @@ namespace ompl
/// Get the connected components formed by the cells in this grid (based on neighboring relation)
std::vector< std::vector<Cell*> > components() const
{
- typedef boost::unordered_map<Coord*, int, HashFunCoordPtr, EqualCoordPtr> ComponentHash;
+ typedef std::unordered_map<Coord*, int, HashFunCoordPtr, EqualCoordPtr> ComponentHash;
typedef typename ComponentHash::iterator CHit;
int components = 0;
@@ -223,7 +223,7 @@ namespace ompl
/// Return the list of future neighbors.
/// Note: this call only creates the cell, but does not add it to the grid.
/// It however updates the neighbor count for neighboring cells
- virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL)
+ virtual Cell* createCell(const Coord& coord, CellArray *nbh = nullptr)
{
Cell *cell = new Cell();
cell->coord = coord;
@@ -356,7 +356,7 @@ namespace ompl
};
/// Define the datatype for the used hash structure
- typedef boost::unordered_map<Coord*, Cell*, HashFunCoordPtr, EqualCoordPtr> CoordHash;
+ typedef std::unordered_map<Coord*, Cell*, HashFunCoordPtr, EqualCoordPtr> CoordHash;
/// Helper to sort components by size
struct SortComponents
diff --git a/src/ompl/datastructures/GridB.h b/src/ompl/datastructures/GridB.h
index f86f167..a632847 100644
--- a/src/ompl/datastructures/GridB.h
+++ b/src/ompl/datastructures/GridB.h
@@ -168,7 +168,7 @@ namespace ompl
}
/// Create a cell but do not add it to the grid; update neighboring cells however
- virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL)
+ virtual Cell* createCell(const Coord& coord, CellArray *nbh = nullptr)
{
CellX* cell = new CellX();
cell->coord = coord;
@@ -303,9 +303,9 @@ namespace ompl
void setupHeaps()
{
eventCellUpdate_ = &noCellUpdate;
- eventCellUpdateData_ = NULL;
- internal_.onAfterInsert(&setHeapElementI, NULL);
- external_.onAfterInsert(&setHeapElementE, NULL);
+ eventCellUpdateData_ = nullptr;
+ internal_.onAfterInsert(&setHeapElementI, nullptr);
+ external_.onAfterInsert(&setHeapElementE, nullptr);
}
/// Clear the data from both heaps
diff --git a/src/ompl/datastructures/GridN.h b/src/ompl/datastructures/GridN.h
index 01e775a..5a01d83 100644
--- a/src/ompl/datastructures/GridN.h
+++ b/src/ompl/datastructures/GridN.h
@@ -163,7 +163,7 @@ namespace ompl
/// this call only creates the cell, but does not add it to
/// the grid. It however updates the neighbor count for
/// neighboring cells
- virtual BaseCell* createCell(const Coord& coord, BaseCellArray *nbh = NULL)
+ virtual BaseCell* createCell(const Coord& coord, BaseCellArray *nbh = nullptr)
{
Cell *cell = new Cell();
cell->coord = coord;
diff --git a/src/ompl/datastructures/LPAstarOnGraph.h b/src/ompl/datastructures/LPAstarOnGraph.h
index 4075460..218d2e2 100644
--- a/src/ompl/datastructures/LPAstarOnGraph.h
+++ b/src/ompl/datastructures/LPAstarOnGraph.h
@@ -46,13 +46,18 @@ Lifelong Planning A. Artif. Intell. 155(1-2): 93-146 (2004)
#include <set>
#include <map>
#include <list>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <iterator>
-#include <boost/foreach.hpp>
#include <iostream>
#include <cassert>
+// workaround for bug in Boost 1.60; see https://svn.boost.org/trac/boost/ticket/11880
+#include <boost/version.hpp>
+#if BOOST_VERSION > 105900
+#include <boost/type_traits/ice.hpp>
+#endif
+
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/adjacency_list.hpp>
@@ -169,8 +174,8 @@ namespace ompl
}
// now get path
- Node* res = (target_->costToCome() == std::numeric_limits<double>::infinity() ? NULL : target_);
- while (res != NULL)
+ Node* res = (target_->costToCome() == std::numeric_limits<double>::infinity() ? nullptr : target_);
+ while (res != nullptr)
{
path.push_front(res->getId());
res = res->getParent();
@@ -205,7 +210,7 @@ namespace ompl
{
public:
Node (double costToCome, double costToGo, double rhs,
- std::size_t& dataId, Node* parentNode = NULL)
+ std::size_t& dataId, Node* parentNode = nullptr)
: g(costToCome), h(costToGo), r(rhs), isInQ(false), parent(parentNode), id(dataId)
{
calculateKey();
@@ -293,11 +298,11 @@ namespace ompl
{
return h(id);
}
- boost::hash<std::size_t> h;
+ std::hash<std::size_t> h;
}; // Hash
typedef std::multiset<Node*, LessThanNodeK> Queue;
- typedef boost::unordered_map<std::size_t, Node*, Hash> IdNodeMap;
+ typedef std::unordered_map<std::size_t, Node*, Hash> IdNodeMap;
typedef typename IdNodeMap::iterator IdNodeMapIter;
typedef typename boost::property_map<Graph, boost::edge_weight_t>::type WeightMap;
@@ -359,7 +364,7 @@ namespace ompl
{
// iterate over all incoming neighbors of the node n_v and get the best parent
double min = std::numeric_limits<double>::infinity();
- Node* best = NULL;
+ Node* best = nullptr;
typename boost::graph_traits<Graph>::in_edge_iterator ei, ei_end;
for (boost::tie(ei, ei_end) = boost::in_edges(n_v->getId(), graph_); ei != ei_end; ++ei)
diff --git a/src/ompl/datastructures/NearestNeighbors.h b/src/ompl/datastructures/NearestNeighbors.h
index 90821bb..9aea443 100644
--- a/src/ompl/datastructures/NearestNeighbors.h
+++ b/src/ompl/datastructures/NearestNeighbors.h
@@ -38,8 +38,7 @@
#define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_
#include <vector>
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
+#include <functional>
#include <ompl/base/StateSpace.h>
namespace ompl
@@ -52,7 +51,7 @@ namespace ompl
public:
/** \brief The definition of a distance function */
- typedef boost::function<double(const _T&, const _T&)> DistanceFunction;
+ typedef std::function<double(const _T&, const _T&)> DistanceFunction;
NearestNeighbors()
{
diff --git a/src/ompl/datastructures/NearestNeighborsFLANN.h b/src/ompl/datastructures/NearestNeighborsFLANN.h
index 48c6921..8333084 100644
--- a/src/ompl/datastructures/NearestNeighborsFLANN.h
+++ b/src/ompl/datastructures/NearestNeighborsFLANN.h
@@ -88,7 +88,7 @@ namespace ompl
{
public:
- NearestNeighborsFLANN(const boost::shared_ptr<flann::IndexParams> ¶ms)
+ NearestNeighborsFLANN(const std::shared_ptr<flann::IndexParams> ¶ms)
: index_(0), params_(params), searchParams_(32, 0., true), dimension_(1)
{
}
@@ -104,7 +104,7 @@ namespace ompl
if (index_)
{
delete index_;
- index_ = NULL;
+ index_ = nullptr;
}
data_.clear();
}
@@ -237,14 +237,14 @@ namespace ompl
///
/// The parameters determine the type of nearest neighbor
/// data structure to be constructed.
- virtual void setIndexParams(const boost::shared_ptr<flann::IndexParams> ¶ms)
+ virtual void setIndexParams(const std::shared_ptr<flann::IndexParams> ¶ms)
{
params_ = params;
rebuildIndex();
}
/// \brief Get the FLANN parameters used to build the current index.
- virtual const boost::shared_ptr<flann::IndexParams>& getIndexParams() const
+ virtual const std::shared_ptr<flann::IndexParams>& getIndexParams() const
{
return params_;
}
@@ -309,7 +309,7 @@ namespace ompl
/// \brief The FLANN index parameters. This contains both the type of
/// index and the parameters for that type.
- boost::shared_ptr<flann::IndexParams> params_;
+ std::shared_ptr<flann::IndexParams> params_;
/// \brief The parameters used to seach for nearest neighbors.
mutable flann::SearchParams searchParams_;
@@ -334,7 +334,7 @@ namespace ompl
public:
NearestNeighborsFLANNLinear()
: NearestNeighborsFLANN<_T, _Dist>(
- boost::shared_ptr<flann::LinearIndexParams>(
+ std::shared_ptr<flann::LinearIndexParams>(
new flann::LinearIndexParams()))
{
}
@@ -346,7 +346,7 @@ namespace ompl
public:
NearestNeighborsFLANNHierarchicalClustering()
: NearestNeighborsFLANN<_T, _Dist>(
- boost::shared_ptr<flann::HierarchicalClusteringIndexParams>(
+ std::shared_ptr<flann::HierarchicalClusteringIndexParams>(
new flann::HierarchicalClusteringIndexParams()))
{
}
diff --git a/src/ompl/datastructures/NearestNeighborsGNAT.h b/src/ompl/datastructures/NearestNeighborsGNAT.h
index 0cbfb65..dcb81f8 100644
--- a/src/ompl/datastructures/NearestNeighborsGNAT.h
+++ b/src/ompl/datastructures/NearestNeighborsGNAT.h
@@ -43,7 +43,7 @@
#include "ompl/datastructures/PDF.h"
#endif
#include "ompl/util/Exception.h"
-#include <boost/unordered_set.hpp>
+#include <unordered_set>
#include <queue>
#include <algorithm>
@@ -105,7 +105,7 @@ namespace ompl
, double estimatedDimension = 6.0
#endif
)
- : NearestNeighbors<_T>(), tree_(NULL), degree_(degree),
+ : NearestNeighbors<_T>(), tree_(nullptr), degree_(degree),
minDegree_(std::min(degree,minDegree)), maxDegree_(std::max(maxDegree,degree)),
maxNumPtsPerLeaf_(maxNumPtsPerLeaf), size_(0),
rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
@@ -135,7 +135,7 @@ namespace ompl
if (tree_)
{
delete tree_;
- tree_ = NULL;
+ tree_ = nullptr;
}
size_ = 0;
removed_.clear();
@@ -280,7 +280,7 @@ namespace ompl
if (!gnat.removed_.empty())
{
out << "Elements marked for removal:\n";
- for (typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
+ for (typename std::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
it != gnat.removed_.end(); it++)
out << **it << '\t';
out << std::endl;
@@ -293,12 +293,12 @@ namespace ompl
void integrityCheck()
{
std::vector<_T> lst;
- boost::unordered_set<const _T*> tmp;
+ std::unordered_set<const _T*> tmp;
// get all elements, including those marked for removal
removed_.swap(tmp);
list(lst);
// check if every element marked for removal is also in the tree
- for (typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
+ for (typename std::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
{
unsigned int i;
for (i=0; i<lst.size(); ++i)
@@ -792,7 +792,7 @@ namespace ompl
/// \brief The data structure used to split data into subtrees.
GreedyKCenters<_T> pivotSelector_;
/// \brief Cache of removed elements.
- boost::unordered_set<const _T*> removed_;
+ std::unordered_set<const _T*> removed_;
#ifdef GNAT_SAMPLER
/// \brief Estimated dimension of the local free space.
double estimatedDimension_;
diff --git a/src/ompl/datastructures/NearestNeighborsGNATNoThreadSafety.h b/src/ompl/datastructures/NearestNeighborsGNATNoThreadSafety.h
index 55aecfe..00b18e2 100644
--- a/src/ompl/datastructures/NearestNeighborsGNATNoThreadSafety.h
+++ b/src/ompl/datastructures/NearestNeighborsGNATNoThreadSafety.h
@@ -44,7 +44,7 @@
#include "ompl/datastructures/PDF.h"
#endif
#include "ompl/util/Exception.h"
-#include <boost/unordered_set.hpp>
+#include <unordered_set>
#include <queue>
#include <algorithm>
@@ -93,7 +93,7 @@ namespace ompl
, double estimatedDimension = 6.0
#endif
)
- : NearestNeighbors<_T>(), tree_(NULL), degree_(degree),
+ : NearestNeighbors<_T>(), tree_(nullptr), degree_(degree),
minDegree_(std::min(degree,minDegree)), maxDegree_(std::max(maxDegree,degree)),
maxNumPtsPerLeaf_(maxNumPtsPerLeaf), size_(0),
rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
@@ -124,7 +124,7 @@ namespace ompl
if (tree_)
{
delete tree_;
- tree_ = NULL;
+ tree_ = nullptr;
}
size_ = 0;
removed_.clear();
@@ -272,7 +272,7 @@ namespace ompl
if (!gnat.removed_.empty())
{
out << "Elements marked for removal:\n";
- for (typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
+ for (typename std::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
it != gnat.removed_.end(); it++)
out << **it << '\t';
out << std::endl;
@@ -285,12 +285,12 @@ namespace ompl
void integrityCheck()
{
std::vector<_T> lst;
- boost::unordered_set<const _T*> tmp;
+ std::unordered_set<const _T*> tmp;
// get all elements, including those marked for removal
removed_.swap(tmp);
list(lst);
// check if every element marked for removal is also in the tree
- for (typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
+ for (typename std::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
{
unsigned int i;
for (i=0; i<lst.size(); ++i)
@@ -788,7 +788,7 @@ namespace ompl
/// \brief The data structure used to split data into subtrees.
GreedyKCenters<_T> pivotSelector_;
/// \brief Cache of removed elements.
- boost::unordered_set<const _T*> removed_;
+ std::unordered_set<const _T*> removed_;
/// \name Internal scratch space
/// \{
diff --git a/src/ompl/datastructures/Permutation.h b/src/ompl/datastructures/Permutation.h
index b33eea0..805dfc6 100644
--- a/src/ompl/datastructures/Permutation.h
+++ b/src/ompl/datastructures/Permutation.h
@@ -37,42 +37,36 @@
#ifndef OMPL_DATASTRUCTURES_PERMUTATION_
#define OMPL_DATASTRUCTURES_PERMUTATION_
-#include <boost/random/mersenne_twister.hpp>
-#include <boost/random/uniform_int.hpp>
-#include <boost/random/variate_generator.hpp>
+#include <random>
namespace ompl
{
/// \brief A permutation of indices into an array
///
- /// This class tends to be faster than std::shuffle when permute is called
- /// several times, since the random number generator doesn't need to be
- /// allocated each time.
+ /// This class tends to be faster than the two-argument version of
+ /// std::random_shuffle when permute is called several times, since
+ /// the random number generator doesn't need to be allocated each time.
class Permutation : public std::vector<int>
{
public:
- /// \brief Create a permutation of the number 1, ... , n
- Permutation(std::size_t n) : std::vector<int>(n), rng_(generator_, dist_)
+ /// \brief Create a permutation of the numbers 0, ... , n - 1
+ Permutation(std::size_t n) : std::vector<int>(n)
{
permute(n);
}
- /// \brief Create a permutation of the number 1,...,n
+ /// \brief Create a permutation of the numbers 0, ..., n - 1
void permute(unsigned int n)
{
if (size() < n)
resize(n);
for (unsigned int i = 0; i < n; ++i)
operator[](i) = i;
- std::random_shuffle(begin(), begin() + n, rng_);
+ std::shuffle(begin(), begin() + n, generator_);
}
private:
/// Mersenne twister random number generator
- boost::mt19937 generator_;
- /// Uniform distribution over integers
- boost::uniform_int<> dist_;
- /// Variate generator
- boost::variate_generator<boost::mt19937&, boost::uniform_int<> > rng_;
+ std::mt19937 generator_;
};
}
diff --git a/src/ompl/extensions/morse/MorseControlSpace.h b/src/ompl/extensions/morse/MorseControlSpace.h
index b178e13..9c42820 100644
--- a/src/ompl/extensions/morse/MorseControlSpace.h
+++ b/src/ompl/extensions/morse/MorseControlSpace.h
@@ -45,7 +45,7 @@ namespace ompl
namespace control
{
-
+
/** \brief Representation of controls applied in MORSE
environments. This is an array of double values. */
class MorseControlSpace : public RealVectorControlSpace
diff --git a/src/ompl/extensions/morse/MorseEnvironment.h b/src/ompl/extensions/morse/MorseEnvironment.h
index 1d27b65..31a5ccc 100644
--- a/src/ompl/extensions/morse/MorseEnvironment.h
+++ b/src/ompl/extensions/morse/MorseEnvironment.h
@@ -45,7 +45,7 @@
#include "ompl/base/State.h"
#include "ompl/util/ClassForward.h"
-#include "boost/thread/mutex.hpp"
+#include <mutex>
#include <limits>
#include <vector>
@@ -61,31 +61,31 @@ namespace ompl
/// @endcond
/** \class ompl::base::MorseEnvironmentPtr
- \brief A boost shared pointer wrapper for ompl::base::MorseEnvironment */
-
+ \brief A shared pointer wrapper for ompl::base::MorseEnvironment */
+
/** \brief This class contains the MORSE constructs OMPL needs to know about when planning. */
class MorseEnvironment
{
public:
-
+
/** \brief The dimension of the control space for this simulation */
const unsigned int controlDim_;
-
+
/** \brief Upper and lower bounds for each control dimension */
const std::vector<double> controlBounds_;
-
+
/** \brief The number of rigid bodies in the simulation */
const unsigned int rigidBodies_;
-
+
/** \brief Upper and lower bounds on position in each spatial dimension */
std::vector<double> positionBounds_;
-
+
/** \brief Upper and lower bounds on linear velocity in each spatial dimension */
std::vector<double> linvelBounds_;
-
+
/** \brief Upper and lower bounds on angular velocity in each spatial dimension */
std::vector<double> angvelBounds_;
-
+
/** \brief The simulation step size */
double stepSize_;
@@ -94,12 +94,12 @@ namespace ompl
/** \brief The maximum number of times a control is applied in sequence */
unsigned int maxControlSteps_;
-
+
/** \brief Indicates whether the simulation has been shut down externally */
bool simRunning_;
-
+
/** \brief Lock to use when performing simulations in the world */
- mutable boost::mutex mutex_;
+ mutable std::mutex mutex_;
MorseEnvironment(const unsigned int controlDim, const std::vector<double> &controlBounds,
const unsigned int rigidBodies, const std::vector<double> &positionBounds,
@@ -140,19 +140,19 @@ namespace ompl
/** \brief Get the control bounds -- the bounding box in which to sample controls */
void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const;
-
-
+
+
// These functions require interprocess communication and are left to be implemented in Python
-
+
/** \brief Query the internal state of the simulation */
virtual void readState(State *state) = 0;
-
+
/** \brief Overwrite the internal state of the simulation */
virtual void writeState(const State *state) = 0;
-
+
/** \brief Configure simulation to proceed under a new control */
virtual void applyControl(const std::vector<double> &control) = 0;
-
+
/** \brief Proceed with the simulation for the given number of seconds */
virtual void worldStep(const double dur) = 0;
};
diff --git a/src/ompl/extensions/morse/MorseGoal.h b/src/ompl/extensions/morse/MorseGoal.h
index 949c6f7..fa1fedf 100644
--- a/src/ompl/extensions/morse/MorseGoal.h
+++ b/src/ompl/extensions/morse/MorseGoal.h
@@ -47,7 +47,7 @@ namespace ompl
namespace base
{
-
+
/** \brief This is a goal class that is more amenable to Python */
class MorseGoal : public Goal
{
@@ -56,17 +56,17 @@ namespace ompl
: Goal(si), distance_(std::numeric_limits<double>::max())
{
}
-
+
/** \brief Where MORSE will store the distance to goal during an isSatisfied() call */
mutable double distance_;
-
+
/** \brief Return true if \e state satisfies the goal */
bool isSatisfied(const State *state) const;
-
+
/** \brief Return true if \e state satisfies the goal, and store the distance
to the goal in \e distance */
bool isSatisfied(const State *state, double *distance) const;
-
+
/** \brief To be implemented in Python; behaves like isSatisfied(state, &distance_) */
virtual bool isSatisfied_Py(const State *state) const = 0;
};
diff --git a/src/ompl/extensions/morse/MorseProjection.h b/src/ompl/extensions/morse/MorseProjection.h
index c508309..3127342 100644
--- a/src/ompl/extensions/morse/MorseProjection.h
+++ b/src/ompl/extensions/morse/MorseProjection.h
@@ -43,7 +43,7 @@ namespace ompl
{
namespace base
{
-
+
/** \brief This class implements a generic projection for the MorseStateSpace,
namely, the subspace representing the x and y positions of every rigid body */
class MorseProjection : public ProjectionEvaluator
@@ -51,23 +51,23 @@ namespace ompl
public:
/** \brief Construct a projection evaluator for a specific state space */
MorseProjection(const StateSpacePtr &space);
-
+
/** \brief Perform configuration steps, if needed */
void setup();
-
+
/** \brief Return the dimension of the projection defined by this evaluator */
virtual unsigned int getDimension() const;
-
+
/** \brief Set the default cell dimensions for this
projection. The default implementation of this
function sets the size to 1.0 for all dimensions.
setup() calls this function if no cell
dimensions have been previously set. */
virtual void defaultCellSizes();
-
+
/** \brief Compute the projection as an array of double values */
virtual void project(const State *state, EuclideanProjection &projection) const;
-
+
protected:
/** \brief The state space this projection operates on */
MorseStateSpace *space_;
diff --git a/src/ompl/extensions/morse/MorseSimpleSetup.h b/src/ompl/extensions/morse/MorseSimpleSetup.h
index b57bf0e..59b2e93 100644
--- a/src/ompl/extensions/morse/MorseSimpleSetup.h
+++ b/src/ompl/extensions/morse/MorseSimpleSetup.h
@@ -55,7 +55,7 @@ namespace ompl
/** \brief Pointer to the environment representing the MORSE simulation */
const base::MorseEnvironmentPtr env_;
-
+
/** \brief The control space is assumed to be
MorseControlSpace. The state space is assumed to
be MorseStateSpace. Constructor only needs the MORSE
@@ -85,10 +85,10 @@ namespace ompl
for planning. The solve() method will call this
function automatically. */
void setup();
-
+
/** \brief Run the planner until solution is found or user shuts down MORSE */
base::PlannerStatus solve();
-
+
/** \brief Set the MORSE world to the states that are
contained in a given path, sequentially. */
void playPath(const base::PathPtr &path) const;
diff --git a/src/ompl/extensions/morse/MorseStateSpace.h b/src/ompl/extensions/morse/MorseStateSpace.h
index 0fc453e..267dc70 100644
--- a/src/ompl/extensions/morse/MorseStateSpace.h
+++ b/src/ompl/extensions/morse/MorseStateSpace.h
@@ -44,7 +44,7 @@ namespace ompl
{
namespace base
{
-
+
/** \brief State space representing MORSE states */
class MorseStateSpace : public CompoundStateSpace
{
diff --git a/src/ompl/extensions/morse/MorseTerminationCondition.h b/src/ompl/extensions/morse/MorseTerminationCondition.h
index 3f48b6e..4be15e5 100644
--- a/src/ompl/extensions/morse/MorseTerminationCondition.h
+++ b/src/ompl/extensions/morse/MorseTerminationCondition.h
@@ -44,25 +44,25 @@ namespace ompl
{
namespace base
{
-
+
/** \brief This class represents a termination condition for the planner that
only terminates if the user shuts down the MORSE simulation */
class MorseTerminationCondition : public PlannerTerminationCondition
{
public:
-
+
/** \brief The representation of the MORSE simulation environment */
const MorseEnvironmentPtr env_;
-
+
MorseTerminationCondition(const MorseEnvironmentPtr env) :
- PlannerTerminationCondition(NULL), env_(env)
+ PlannerTerminationCondition(nullptr), env_(env)
{
}
-
+
~MorseTerminationCondition()
{
}
-
+
/** \brief Return true if the simulation is still running */
bool eval() const;
};
diff --git a/src/ompl/extensions/morse/src/MorseGoal.cpp b/src/ompl/extensions/morse/src/MorseGoal.cpp
index 4e55b9e..a924915 100644
--- a/src/ompl/extensions/morse/src/MorseGoal.cpp
+++ b/src/ompl/extensions/morse/src/MorseGoal.cpp
@@ -40,11 +40,11 @@ bool ompl::base::MorseGoal::isSatisfied(const State *state) const
{
return isSatisfied_Py(state);
}
-
+
bool ompl::base::MorseGoal::isSatisfied(const State *state, double *distance) const
{
bool sat = isSatisfied_Py(state);
- if (distance != NULL)
+ if (distance != nullptr)
{
// the Python object will set distance_ during isSatisfied_py() call
*distance = distance_;
diff --git a/src/ompl/extensions/morse/src/MorseStatePropagator.cpp b/src/ompl/extensions/morse/src/MorseStatePropagator.cpp
index df2ab00..1f2e597 100644
--- a/src/ompl/extensions/morse/src/MorseStatePropagator.cpp
+++ b/src/ompl/extensions/morse/src/MorseStatePropagator.cpp
@@ -49,7 +49,7 @@ ompl::control::MorseStatePropagator::MorseStatePropagator(const SpaceInformation
void ompl::control::MorseStatePropagator::propagate(const base::State *state, const Control *control, const double duration, base::State *result) const
{
- boost::mutex::scoped_lock lock(env_->mutex_);
+ std::lock_guard<std::mutex> lock(env_->mutex_);
// place the MORSE world at the start state
si_->getStateSpace()->as<base::MorseStateSpace>()->writeState(state);
diff --git a/src/ompl/extensions/morse/src/MorseStateSpace.cpp b/src/ompl/extensions/morse/src/MorseStateSpace.cpp
index 9d8b638..4a1bcd3 100644
--- a/src/ompl/extensions/morse/src/MorseStateSpace.cpp
+++ b/src/ompl/extensions/morse/src/MorseStateSpace.cpp
@@ -39,8 +39,6 @@
#include "ompl/base/spaces/SO3StateSpace.h"
#include "ompl/base/spaces/DiscreteStateSpace.h"
-#include <boost/lexical_cast.hpp>
-
ompl::base::MorseStateSpace::MorseStateSpace(const MorseEnvironmentPtr &env, double positionWeight, double linVelWeight,
double angVelWeight, double orientationWeight) :
CompoundStateSpace(), env_(env)
@@ -49,7 +47,7 @@ ompl::base::MorseStateSpace::MorseStateSpace(const MorseEnvironmentPtr &env, dou
type_ = STATE_SPACE_TYPE_COUNT + 1;
for (unsigned int i = 0 ; i < env_->rigidBodies_; ++i)
{
- std::string body = ":B" + boost::lexical_cast<std::string>(i);
+ std::string body = ":B" + std::to_string(i);
addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), positionWeight); // position
components_.back()->setName(components_.back()->getName() + body + ":position");
@@ -66,7 +64,7 @@ ompl::base::MorseStateSpace::MorseStateSpace(const MorseEnvironmentPtr &env, dou
// Add the goal region satisfaction flag as a subspace.
addSubspace(StateSpacePtr(new DiscreteStateSpace(0, 1)), 0.01);
components_.back()->setName(components_.back()->getName() + ":goalRegionSat");
-
+
lock();
setBounds();
}
diff --git a/src/ompl/extensions/opende/OpenDEEnvironment.h b/src/ompl/extensions/opende/OpenDEEnvironment.h
index 6941805..ef03c5e 100644
--- a/src/ompl/extensions/opende/OpenDEEnvironment.h
+++ b/src/ompl/extensions/opende/OpenDEEnvironment.h
@@ -48,7 +48,7 @@
#include <vector>
#include <string>
#include <map>
-#include <boost/thread/mutex.hpp>
+#include <mutex>
namespace ompl
{
@@ -61,7 +61,7 @@ namespace ompl
/// @endcond
/** \class ompl::control::OpenDEEnvironmentPtr
- \brief A boost shared pointer wrapper for ompl::control::OpenDEEnvironment */
+ \brief A shared pointer wrapper for ompl::control::OpenDEEnvironment */
/** \brief This class contains the OpenDE constructs OMPL needs to know about when planning. */
class OpenDEEnvironment
@@ -102,9 +102,9 @@ namespace ompl
unsigned int minControlSteps_;
/** \brief Lock to use when performing simulations in the world. (OpenDE simulations are NOT thread safe) */
- mutable boost::mutex mutex_;
+ mutable std::mutex mutex_;
- OpenDEEnvironment() : world_(NULL), verboseContacts_(false), maxContacts_(3), stepSize_(0.05), maxControlSteps_(100), minControlSteps_(5)
+ OpenDEEnvironment() : world_(nullptr), verboseContacts_(false), maxContacts_(3), stepSize_(0.05), maxControlSteps_(100), minControlSteps_(5)
{
contactGroup_ = dJointGroupCreate(0);
}
diff --git a/src/ompl/extensions/opende/src/OpenDEEnvironment.cpp b/src/ompl/extensions/opende/src/OpenDEEnvironment.cpp
index 6ccc8a2..8c26e74 100644
--- a/src/ompl/extensions/opende/src/OpenDEEnvironment.cpp
+++ b/src/ompl/extensions/opende/src/OpenDEEnvironment.cpp
@@ -35,7 +35,6 @@
/* Author: Ioan Sucan */
#include "ompl/extensions/opende/OpenDEEnvironment.h"
-#include <boost/lexical_cast.hpp>
unsigned int ompl::control::OpenDEEnvironment::getMaxContacts(dGeomID /*geom1*/, dGeomID /*geom2*/) const
{
@@ -61,7 +60,7 @@ std::string ompl::control::OpenDEEnvironment::getGeomName(dGeomID geom) const
{
std::map<dGeomID, std::string>::const_iterator it = geomNames_.find(geom);
if (it == geomNames_.end())
- return boost::lexical_cast<std::string>(reinterpret_cast<unsigned long>(geom));
+ return std::to_string(reinterpret_cast<unsigned long>(geom));
else
return it->second;
}
diff --git a/src/ompl/extensions/opende/src/OpenDESimpleSetup.cpp b/src/ompl/extensions/opende/src/OpenDESimpleSetup.cpp
index b28c638..042f1d7 100644
--- a/src/ompl/extensions/opende/src/OpenDESimpleSetup.cpp
+++ b/src/ompl/extensions/opende/src/OpenDESimpleSetup.cpp
@@ -36,7 +36,7 @@
#include "ompl/extensions/opende/OpenDESimpleSetup.h"
#include "ompl/util/Exception.h"
-#include <boost/thread.hpp>
+#include <thread>
ompl::control::OpenDESimpleSetup::OpenDESimpleSetup(const ControlSpacePtr &space) : SimpleSetup(space)
{
@@ -123,7 +123,7 @@ void ompl::control::OpenDESimpleSetup::playPath(const base::PathPtr &path, doubl
getStateSpace()->as<OpenDEStateSpace>()->writeState(pg.getState(0));
for (unsigned int i = 1 ; i < pg.getStateCount() ; ++i)
{
- boost::this_thread::sleep(d);
+ std::this_thread::sleep_for(d);
getStateSpace()->as<OpenDEStateSpace>()->writeState(pg.getState(i));
}
}
diff --git a/src/ompl/extensions/opende/src/OpenDEStateSpace.cpp b/src/ompl/extensions/opende/src/OpenDEStateSpace.cpp
index 2f77678..35f2973 100644
--- a/src/ompl/extensions/opende/src/OpenDEStateSpace.cpp
+++ b/src/ompl/extensions/opende/src/OpenDEStateSpace.cpp
@@ -36,7 +36,6 @@
#include "ompl/extensions/opende/OpenDEStateSpace.h"
#include "ompl/util/Console.h"
-#include <boost/lexical_cast.hpp>
#include <limits>
#include <queue>
@@ -48,7 +47,7 @@ ompl::control::OpenDEStateSpace::OpenDEStateSpace(const OpenDEEnvironmentPtr &en
type_ = base::STATE_SPACE_TYPE_COUNT + 1;
for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
{
- std::string body = ":B" + boost::lexical_cast<std::string>(i);
+ std::string body = ":B" + std::to_string(i);
addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), positionWeight); // position
components_.back()->setName(components_.back()->getName() + body + ":position");
diff --git a/src/ompl/extensions/triangle/TriangularDecomposition.h b/src/ompl/extensions/triangle/TriangularDecomposition.h
index 258652d..f5dde97 100644
--- a/src/ompl/extensions/triangle/TriangularDecomposition.h
+++ b/src/ompl/extensions/triangle/TriangularDecomposition.h
@@ -62,7 +62,6 @@ namespace ompl
Vertex(void) {}
Vertex(double vx, double vy);
bool operator==(const Vertex& v) const;
- friend std::size_t hash_value(const Vertex& v);
double x, y;
};
diff --git a/src/ompl/extensions/triangle/src/TriangularDecomposition.cpp b/src/ompl/extensions/triangle/src/TriangularDecomposition.cpp
index 60e7738..e2c9965 100644
--- a/src/ompl/extensions/triangle/src/TriangularDecomposition.cpp
+++ b/src/ompl/extensions/triangle/src/TriangularDecomposition.cpp
@@ -41,13 +41,12 @@
#include "ompl/control/planners/syclop/Decomposition.h"
#include "ompl/control/planners/syclop/GridDecomposition.h"
#include "ompl/util/RandomNumbers.h"
+#include "ompl/util/Hash.h"
#include <ostream>
#include <vector>
#include <set>
#include <string>
-#include <boost/functional/hash.hpp>
-#include <boost/lexical_cast.hpp>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <cstdlib>
extern "C"
@@ -58,6 +57,20 @@ extern "C"
#include <triangle.h>
}
+namespace std
+{
+ template<>
+ struct hash<ompl::control::TriangularDecomposition::Vertex>
+ {
+ size_t operator()(const ompl::control::TriangularDecomposition::Vertex &v) const
+ {
+ std::size_t hash = std::hash<double>()(v.x);
+ ompl::hash_combine(hash, v.y);
+ return hash;
+ }
+ };
+}
+
ompl::control::TriangularDecomposition::TriangularDecomposition(const base::RealVectorBounds &bounds,
const std::vector<Polygon> &holes, const std::vector<Polygon> &intRegs) :
Decomposition(2, bounds),
@@ -198,20 +211,6 @@ bool ompl::control::TriangularDecomposition::Vertex::operator==(const Vertex &v)
return x == v.x && y == v.y;
}
-namespace ompl
-{
- namespace control
- {
- std::size_t hash_value(const TriangularDecomposition::Vertex &v)
- {
- std::size_t hash = 0;
- boost::hash_combine(hash, v.x);
- boost::hash_combine(hash, v.y);
- return hash;
- }
- }
-}
-
int ompl::control::TriangularDecomposition::createTriangles()
{
/* create a conforming Delaunay triangulation
@@ -219,7 +218,7 @@ int ompl::control::TriangularDecomposition::createTriangles()
the total area of the decomposition space */
const base::RealVectorBounds& bounds = getBounds();
const double maxTriangleArea = bounds.getVolume() * triAreaPct_;
- std::string triswitches = "pDznQA -a" + boost::lexical_cast<std::string>(maxTriangleArea);
+ std::string triswitches = "pDznQA -a" + std::to_string(maxTriangleArea);
struct triangulateio in;
/* Some vertices may be duplicates, such as when an obstacle has a vertex equivalent
@@ -228,7 +227,7 @@ int ompl::control::TriangularDecomposition::createTriangles()
so, to prevent duplicate vertices, we use a hashmap from Vertex to the index for
that Vertex in the pointlist. We'll fill the map with Vertex objects,
and then we'll actually add them to the pointlist. */
- boost::unordered_map<Vertex, int> pointIndex;
+ std::unordered_map<Vertex, int> pointIndex;
// First, add the points from the bounding box
pointIndex[Vertex(bounds.low[0], bounds.low[1])] = 0;
@@ -274,7 +273,7 @@ int ompl::control::TriangularDecomposition::createTriangles()
in.pointlist = (REAL*) malloc(2*in.numberofpoints*sizeof(REAL));
//add unique vertices from our map, using their assigned indices
- typedef boost::unordered_map<Vertex, int>::const_iterator IndexIter;
+ typedef std::unordered_map<Vertex, int>::const_iterator IndexIter;
for (IndexIter i = pointIndex.begin(); i != pointIndex.end(); ++i)
{
const Vertex& v = i->first;
@@ -326,7 +325,7 @@ int ompl::control::TriangularDecomposition::createTriangles()
For now, we'll assume that each obstacle is convex, and we'll
generate the interior points ourselves using getPointInPoly. */
in.numberofholes = holes_.size();
- in.holelist = NULL;
+ in.holelist = nullptr;
if (in.numberofholes > 0)
{
/* holelist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of interior points.
@@ -344,7 +343,7 @@ int ompl::control::TriangularDecomposition::createTriangles()
region-of-interest in intRegs_. We follow the same assumption as before
that each region-of-interest is convex. */
in.numberofregions = intRegs_.size();
- in.regionlist = NULL;
+ in.regionlist = nullptr;
if (in.numberofregions > 0)
{
/* regionlist is a sequence (x1 y1 L1 -1 x2 y2 L2 -1 ...) of ordered triples,
@@ -365,30 +364,30 @@ int ompl::control::TriangularDecomposition::createTriangles()
}
//mark remaining input fields as unused
- in.segmentmarkerlist = (int*) NULL;
+ in.segmentmarkerlist = (int*) nullptr;
in.numberofpointattributes = 0;
- in.pointattributelist = NULL;
- in.pointmarkerlist = NULL;
+ in.pointattributelist = nullptr;
+ in.pointmarkerlist = nullptr;
//initialize output libtriangle structure, which will hold the results of the triangulation
struct triangulateio out;
- out.pointlist = (REAL*) NULL;
- out.pointattributelist = (REAL*) NULL;
- out.pointmarkerlist = (int*) NULL;
- out.trianglelist = (int*) NULL;
- out.triangleattributelist = (REAL*) NULL;
- out.neighborlist = (int*) NULL;
- out.segmentlist = (int*) NULL;
- out.segmentmarkerlist = (int*) NULL;
- out.edgelist = (int*) NULL;
- out.edgemarkerlist = (int*) NULL;
- out.pointlist = (REAL*) NULL;
- out.pointattributelist = (REAL*) NULL;
- out.trianglelist = (int*) NULL;
- out.triangleattributelist = (REAL*) NULL;
+ out.pointlist = (REAL*) nullptr;
+ out.pointattributelist = (REAL*) nullptr;
+ out.pointmarkerlist = (int*) nullptr;
+ out.trianglelist = (int*) nullptr;
+ out.triangleattributelist = (REAL*) nullptr;
+ out.neighborlist = (int*) nullptr;
+ out.segmentlist = (int*) nullptr;
+ out.segmentmarkerlist = (int*) nullptr;
+ out.edgelist = (int*) nullptr;
+ out.edgemarkerlist = (int*) nullptr;
+ out.pointlist = (REAL*) nullptr;
+ out.pointattributelist = (REAL*) nullptr;
+ out.trianglelist = (int*) nullptr;
+ out.triangleattributelist = (REAL*) nullptr;
//call the triangulation routine
- triangulate(const_cast<char*>(triswitches.c_str()), &in, &out, NULL);
+ triangulate(const_cast<char*>(triswitches.c_str()), &in, &out, nullptr);
triangles_.resize(out.numberoftriangles);
intRegInfo_.resize(out.numberoftriangles);
diff --git a/src/ompl/geometric/HillClimbing.h b/src/ompl/geometric/HillClimbing.h
index 7567305..4fc3077 100644
--- a/src/ompl/geometric/HillClimbing.h
+++ b/src/ompl/geometric/HillClimbing.h
@@ -73,7 +73,7 @@ namespace ompl
/** \brief Try to improve a state (reduce distance to goal). The updates are performed by sampling near the
state, within the specified distance. If improvements were found, the function returns true and the better
goal distance is optionally returned */
- bool tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance, double *betterGoalDistance = NULL) const;
+ bool tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance, double *betterGoalDistance = nullptr) const;
/** \brief Set the number of steps to perform */
void setMaxImproveSteps(unsigned int steps)
diff --git a/src/ompl/geometric/PathGeometric.h b/src/ompl/geometric/PathGeometric.h
index d8303b5..fdb5633 100644
--- a/src/ompl/geometric/PathGeometric.h
+++ b/src/ompl/geometric/PathGeometric.h
@@ -82,9 +82,9 @@ namespace ompl
/** \brief Assignment operator */
PathGeometric& operator=(const PathGeometric &other);
-
+
/** \brief The sum of the costs for the sequence of segments that make up the path, computed using
- OptimizationObjective::motionCost(). OptimizationObjective::initialCost() and OptimizationObjective::terminalCost()
+ OptimizationObjective::motionCost(). OptimizationObjective::initialCost() and OptimizationObjective::terminalCost()
are also used in the computation for the first and last states, respectively. Empty paths have identity cost. */
virtual base::Cost cost(const base::OptimizationObjectivePtr &obj) const;
@@ -155,7 +155,7 @@ namespace ompl
/** \brief Check if the path is valid. If it is not,
attempts are made to fix the path by sampling around
invalid states. Not more than \e attempts samples are
- drawn.
+ drawn.
\return A pair of boolean values is returned. The first
value represents the validity of the path before any
change was made. The second value represents the
diff --git a/src/ompl/geometric/PathHybridization.h b/src/ompl/geometric/PathHybridization.h
index 5c52ac7..350c0c2 100644
--- a/src/ompl/geometric/PathHybridization.h
+++ b/src/ompl/geometric/PathHybridization.h
@@ -55,7 +55,7 @@ namespace ompl
/// @endcond
/** \class ompl::geometric::PathHybridizationPtr
- \brief A boost shared pointer wrapper for ompl::geometric::PathHybridization */
+ \brief A shared pointer wrapper for ompl::geometric::PathHybridization */
/** \brief Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution.
diff --git a/src/ompl/geometric/PathSimplifier.h b/src/ompl/geometric/PathSimplifier.h
index 9c55733..74dc22e 100644
--- a/src/ompl/geometric/PathSimplifier.h
+++ b/src/ompl/geometric/PathSimplifier.h
@@ -58,7 +58,7 @@ namespace ompl
/// @endcond
/** \class ompl::geometric::PathSimplifierPtr
- \brief A boost shared pointer wrapper for ompl::geometric::PathSimplifier */
+ \brief A shared pointer wrapper for ompl::geometric::PathSimplifier */
/** \brief This class contains routines that attempt to simplify geometric paths.
@@ -71,7 +71,7 @@ namespace ompl
/** \brief Create an instance for a specified space information.
Optionally, a GoalSampleableRegion may be passed in to attempt
improvements at the end of the path as well. */
- PathSimplifier(const base::SpaceInformationPtr &si, const base::GoalPtr& goal = base::GoalPtr());
+ PathSimplifier(const base::SpaceInformationPtr &si, const base::GoalPtr& goal = ompl::base::GoalPtr());
virtual ~PathSimplifier()
{
@@ -258,7 +258,7 @@ namespace ompl
base::SpaceInformationPtr si_;
/** \brief The goal object for the path simplifier. Used for end-of-path improvements */
- boost::shared_ptr<base::GoalSampleableRegion> gsr_;
+ std::shared_ptr<base::GoalSampleableRegion> gsr_;
/** \brief Flag indicating whether the states removed from a motion should be freed */
bool freeStates_;
diff --git a/src/ompl/geometric/SimpleSetup.h b/src/ompl/geometric/SimpleSetup.h
index 05092a7..2a68a5b 100644
--- a/src/ompl/geometric/SimpleSetup.h
+++ b/src/ompl/geometric/SimpleSetup.h
@@ -58,7 +58,7 @@ namespace ompl
/// @endcond
/** \class ompl::geometric::SimpleSetupPtr
- \brief A boost shared pointer wrapper for ompl::geometric::SimpleSetup */
+ \brief A shared pointer wrapper for ompl::geometric::SimpleSetup */
/** \brief Create the set of classes typically needed to solve a
geometric problem */
diff --git a/src/ompl/geometric/planners/AnytimePathShortening.cpp b/src/ompl/geometric/planners/AnytimePathShortening.cpp
index a4bc5ba..5d02d57 100644
--- a/src/ompl/geometric/planners/AnytimePathShortening.cpp
+++ b/src/ompl/geometric/planners/AnytimePathShortening.cpp
@@ -40,14 +40,14 @@
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
-#include <boost/thread.hpp>
+#include <thread>
ompl::geometric::AnytimePathShortening::AnytimePathShortening (const ompl::base::SpaceInformationPtr &si) :
ompl::base::Planner(si, "APS"),
shortcut_(true),
hybridize_(true),
maxHybridPaths_(24),
- defaultNumPlanners_(std::max(1u, boost::thread::hardware_concurrency()))
+ defaultNumPlanners_(std::max(1u, std::thread::hardware_concurrency()))
{
specs_.approximateSolutions = true;
specs_.multithreaded = true;
@@ -59,7 +59,7 @@ ompl::geometric::AnytimePathShortening::AnytimePathShortening (const ompl::base:
Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners, &AnytimePathShortening::getDefaultNumPlanners, "0:64");
addPlannerProgressProperty("best cost REAL",
- boost::bind(&AnytimePathShortening::getBestCost, this));
+ std::bind(&AnytimePathShortening::getBestCost, this));
}
ompl::geometric::AnytimePathShortening::~AnytimePathShortening()
@@ -97,9 +97,9 @@ void ompl::geometric::AnytimePathShortening::setProblemDefinition(const ompl::ba
ompl::base::PlannerStatus ompl::geometric::AnytimePathShortening::solve(const ompl::base::PlannerTerminationCondition &ptc)
{
base::Goal *goal = pdef_->getGoal().get();
- std::vector<boost::thread*> threads(planners_.size());
+ std::vector<std::thread*> threads(planners_.size());
geometric::PathHybridization phybrid(si_);
- base::Path *bestSln = NULL;
+ base::Path *bestSln = nullptr;
base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
if (!opt)
@@ -128,7 +128,7 @@ ompl::base::PlannerStatus ompl::geometric::AnytimePathShortening::solve(const om
// Spawn a thread for each planner. This will shortcut the best path after solving.
for (size_t i = 0; i < threads.size(); ++i)
- threads[i] = new boost::thread(boost::bind(&AnytimePathShortening::threadSolve, this, planners_[i].get(), ptc));
+ threads[i] = new std::thread(std::bind(&AnytimePathShortening::threadSolve, this, planners_[i].get(), ptc));
// Join each thread, and then delete it
for (std::size_t i = 0 ; i < threads.size() ; ++i)
@@ -296,5 +296,5 @@ std::string ompl::geometric::AnytimePathShortening::getBestCost() const
base::Cost bestCost(std::numeric_limits<double>::quiet_NaN());
if (pdef_ && pdef_->getSolutionCount() > 0)
bestCost = base::Cost(pdef_->getSolutionPath()->length());
- return boost::lexical_cast<std::string>(bestCost);
+ return std::to_string(bestCost.value());
}
diff --git a/src/ompl/geometric/planners/bitstar/BITstar.h b/src/ompl/geometric/planners/bitstar/BITstar.h
index dd3c5d8..929d06e 100644
--- a/src/ompl/geometric/planners/bitstar/BITstar.h
+++ b/src/ompl/geometric/planners/bitstar/BITstar.h
@@ -69,11 +69,19 @@ namespace ompl
/**
@anchor gBITstar
- \ref gBITstar "BIT*" (Batch Informed Trees) is an \e anytime asymptotically optimal sampling-based
+ \ref gBITstar "BIT*" (Batch Informed Trees) is an \e anytime almost surely asymptotically optimal sampling-based
planning algorithm. It approaches problems by assuming that a \e simple solution exists and only
goes onto consider \e complex solutions when that proves incorrect. It accomplishes this by using
heuristics to search in order of decreasing potential solution quality.
+ Both a k-nearest and r-disc version are available, with the k-nearest selected by default.
+ It is recommended that you try both variants, with the r-disc version being recommended *if* it finds an
+ initial solution in a suitable amount of time (which it probably will). In general, both variants work,
+ but for a small number of problems the calculation of the radius in the r-disc version appears to be too
+ small and does not create an implicit graph that is sufficiently dense (hence the default choice).
+ This is a question of the random geometric graph theory underpinning this type of almost surely asymptotically
+ optimal planner and certainly merits further review.
+
This implementation of BIT* can handle multiple starts, multiple goals, a variety of optimization objectives
(e.g., path length), and with \ref gBITstarSetJustInTimeSampling "just-in-time sampling", infinite problem domains.
Note that for some of optimization objectives, the user must specify a suitable heuristic and that when
@@ -112,15 +120,22 @@ namespace ompl
class IntegratedQueue;
//Helpful typedefs:
/** \brief A vertex shared pointer. */
- typedef boost::shared_ptr<Vertex> VertexPtr;
+ typedef std::shared_ptr<Vertex> VertexPtr;
/** \brief A \e constant vertex shared pointer. */
- typedef boost::shared_ptr<const Vertex> VertexConstPtr;
+ typedef std::shared_ptr<const Vertex> VertexConstPtr;
/** \brief A vertex weak pointer. */
- typedef boost::weak_ptr<Vertex> VertexWeakPtr;
+ typedef std::weak_ptr<Vertex> VertexWeakPtr;
/** \brief An integrated queue shared pointer. */
- typedef boost::shared_ptr<IntegratedQueue> IntegratedQueuePtr;
+ typedef std::shared_ptr<IntegratedQueue> IntegratedQueuePtr;
/** \brief The vertex id type */
typedef unsigned int VertexId;
+ /** \brief A pair of vertices, i.e., an edge. */
+ typedef std::pair<VertexPtr, VertexPtr> VertexPtrPair;
+ /** \brief A pair of const vertices, i.e., an edge. */
+ typedef std::pair<VertexConstPtr, VertexConstPtr> VertexConstPtrPair;
+ /** \brief The OMPL::NearestNeighbors structure. */
+ typedef std::shared_ptr< NearestNeighbors<VertexPtr> > VertexPtrNNPtr;
+
/** \brief Construct! */
BITstar(const base::SpaceInformationPtr& si, const std::string& name = "BITstar");
@@ -242,12 +257,6 @@ namespace ompl
/** \brief Get whether unconnected samples are dropped on pruning. */
bool getDropSamplesOnPrune() const;
- /** \brief Enable tracking of failed edges. This currently is too expensive to be useful.*/
- void setUseFailureTracking(bool trackFailures);
-
- /** \brief Get whether a failed edge list is in use.*/
- bool getUseFailureTracking() const;
-
/** \brief Stop the planner each time a solution improvement is found. Useful
for examining the intermediate solutions found by BIT*. */
void setStopOnSolnImprovement(bool stopOnChange);
@@ -259,13 +268,6 @@ namespace ompl
protected:
//Everything is only protected so we can create modifications without duplicating code by deriving from the class:
- //Typedefs:
- /** \brief A pair of vertices, i.e., an edge. */
- typedef std::pair<VertexPtr, VertexPtr> VertexPtrPair;
-
- /** \brief The OMPL::NearestNeighbors structure. */
- typedef boost::shared_ptr< NearestNeighbors<VertexPtr> > VertexPtrNNPtr;
-
//Functions:
/** \brief A debug function: Estimate the measure of the free/obstace space via sampling. */
void estimateMeasures();
@@ -279,7 +281,7 @@ namespace ompl
void newBatch();
/** \brief Update the list of free samples */
- void updateSamples(const VertexPtr& vertex);
+ void updateSamples(const VertexConstPtr& vertex);
/** \brief Prune the problem. Returns true if pruning was done. */
virtual bool prune();
@@ -306,7 +308,7 @@ namespace ompl
void pruneSamples();
/** \brief Checks an edge for collision. A wrapper to SpaceInformation->checkMotion that tracks number of collision checks. */
- bool checkEdge(const VertexPtrPair& edge);
+ bool checkEdge(const VertexConstPtrPair& edge);
/** \brief Actually remove a sample from its NN struct.*/
void dropSample(VertexPtr oldSample);
@@ -326,50 +328,50 @@ namespace ompl
/** \brief Add a vertex to the graph */
void addVertex(const VertexPtr& newVertex, const bool& removeFromFree);
- /** \brief Get the nearest samples from the freeStateNN_ using the appropriate "near" definition (i.e., k or r). */
- void nearestSamples(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourSamples);
+ /** \brief Get the nearest samples from the freeStateNN_ using the appropriate "near" definition (i.e., k or r). If using k-nearest, returns the target k, otherwise returns 0u. */
+ unsigned int nearestSamples(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourSamples);
- /** \brief Get the nearest samples from the vertexNN_ using the appropriate "near" definition (i.e., k or r). */
- void nearestVertices(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourVertices);
+ /** \brief Get the nearest samples from the vertexNN_ using the appropriate "near" definition (i.e., k or r). If using k-nearest, returns the target k, otherwise returns 0u. */
+ unsigned int nearestVertices(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourVertices);
///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////
//Helper functions for sorting queues/nearest-neighbour structures and the related calculations.
/** \brief The distance function used for nearest neighbours. Calculates the distance directionally from the given state to all the other states (can be used on states either in our out of the graph).*/
- double nnDistance(const VertexPtr& a, const VertexPtr& b) const;
+ double nnDistance(const VertexConstPtr& a, const VertexConstPtr& b) const;
///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////
//Helper functions for various heuristics.
/** \brief Calculates a heuristic estimate of the cost of a solution constrained to pass through a vertex, independent of the current cost-to-come. I.e., combines the heuristic estimates of the cost-to-come and cost-to-go. */
- ompl::base::Cost lowerBoundHeuristicVertex(const VertexPtr& edgePair) const;
+ ompl::base::Cost lowerBoundHeuristicVertex(const VertexConstPtr& vertex) const;
/** \brief Calculates a heuristic estimate of the cost of a solution constrained to pass through a vertex, dependent on the current cost-to-come. I.e., combines the current cost-to-come with a heuristic estimate of the cost-to-go. */
- ompl::base::Cost currentHeuristicVertex(const VertexPtr& edgePair) const;
+ ompl::base::Cost currentHeuristicVertex(const VertexConstPtr& vertex) const;
/** \brief Calculates a heuristic estimate of the cost of a solution constrained to go through an edge, independent of the cost-to-come of the parent state. I.e., combines the heuristic estimates of the cost-to-come, edge cost, and cost-to-go. */
- ompl::base::Cost lowerBoundHeuristicEdge(const VertexPtrPair& edgePair) const;
+ ompl::base::Cost lowerBoundHeuristicEdge(const VertexConstPtrPair& edgePair) const;
/** \brief Calculates a heuristic estimate of the cost of a solution constrained to go through an edge, dependent on the cost-to-come of the parent state. I.e., combines the current cost-to-come with heuristic estimates of the edge cost, and cost-to-go. */
- ompl::base::Cost currentHeuristicEdge(const VertexPtrPair& edgePair) const;
+ ompl::base::Cost currentHeuristicEdge(const VertexConstPtrPair& edgePair) const;
/** \brief Calculates a heuristic estimate of the cost of a path to the \e target of an edge, dependent on the cost-to-come of the parent state. I.e., combines the current cost-to-come with heuristic estimates of the edge cost. */
- ompl::base::Cost currentHeuristicEdgeTarget(const VertexPtrPair& edgePair) const;
+ ompl::base::Cost currentHeuristicEdgeTarget(const VertexConstPtrPair& edgePair) const;
/** \brief Calculate a heuristic estimate of the cost-to-come for a Vertex */
- ompl::base::Cost costToComeHeuristic(const VertexPtr& vertex) const;
+ ompl::base::Cost costToComeHeuristic(const VertexConstPtr& vertex) const;
/** \brief Calculate a heuristic estimate of the cost an edge between two Vertices */
- ompl::base::Cost edgeCostHeuristic(const VertexPtrPair& edgePair) const;
+ ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair& edgePair) const;
/** \brief Calculate a heuristic estimate of the cost-to-go for a Vertex */
- ompl::base::Cost costToGoHeuristic(const VertexPtr& vertex) const;
+ ompl::base::Cost costToGoHeuristic(const VertexConstPtr& vertex) const;
/** \brief The true cost of an edge, including collisions.*/
- ompl::base::Cost trueEdgeCost(const VertexPtrPair& edgePair) const;
+ ompl::base::Cost trueEdgeCost(const VertexConstPtrPair& edgePair) const;
/** \brief Calculate the max req'd cost to define a neighbourhood around a state. Currently only implemented for path-length problems, for which the neighbourhood cost is the f-value of the vertex plus 2r. */
- ompl::base::Cost neighbourhoodCost(const VertexPtr& vertex) const;
+ ompl::base::Cost neighbourhoodCost(const VertexConstPtr& vertex) const;
/** \brief Compare whether cost a is worse than cost b by checking whether b is better than a. */
bool isCostWorseThan(const ompl::base::Cost& a, const ompl::base::Cost& b) const;
@@ -546,6 +548,12 @@ namespace ompl
/** \brief The integrated queue of vertices to expand and edges to process ordered on "f-value", i.e., estimated solution cost. Remaining vertex queue "size" and edge queue size are accessible via vertexQueueSizeProgressProperty and edgeQueueSizeProgressProperty, respectively. */
IntegratedQueuePtr intQueue_;
+ /** \brief A copy of the new samples from this batch */
+ std::vector<VertexPtr> newSamples_;
+
+ /** \brief A copy of the vertices recycled into samples during this batch */
+ std::vector<VertexPtr> recycledSamples_;
+
/** \brief The number of states (vertices or samples) that were generated from a uniform distribution. Only valid when refreshSamplesOnPrune_ is true, in which case it's used to calculate the RGG term of the uniform subgraph.*/
unsigned int numUniformStates_;
@@ -658,9 +666,6 @@ namespace ompl
/** \brief Whether to refresh (i.e., forget) unconnected samples on pruning (param) */
bool dropSamplesOnPrune_;
- /** \brief Track edges that have been checked and failed so they never reenter the queue. (param) */
- bool useFailureTracking_;
-
/** \brief Whether to stop the planner as soon as the path changes (param) */
bool stopOnSolnChange_;
///////////////////////////////////////
diff --git a/src/ompl/geometric/planners/bitstar/datastructures/IdGenerator.h b/src/ompl/geometric/planners/bitstar/datastructures/IdGenerator.h
index 7128ade..07f76e2 100644
--- a/src/ompl/geometric/planners/bitstar/datastructures/IdGenerator.h
+++ b/src/ompl/geometric/planners/bitstar/datastructures/IdGenerator.h
@@ -40,14 +40,10 @@
//I am member class of the BITstar class, so I need to include it's definition to be aware of the class BITstar. It has a forward declaration to me.
#include "ompl/geometric/planners/bitstar/BITstar.h"
-//For locking the ID generator:
-#include <boost/thread/mutex.hpp>
+#include <thread>
+#include <mutex>
//For boost::scoped_ptr
#include <boost/scoped_ptr.hpp>
-//For the boost::once_flag and other tools for making sure an init function is only called once.
-#include <boost/thread/once.hpp>
-//For boost::lock_guard
-#include <boost/thread/locks.hpp>
namespace ompl
{
@@ -71,7 +67,7 @@ namespace ompl
BITstar::VertexId getNewId()
{
//Create a scoped mutex copy of idMutex that unlocks when it goes out of scope:
- boost::lock_guard<boost::mutex> lockGuard(idMutex_);
+ std::lock_guard<std::mutex> lockGuard(idMutex_);
//Return the next id, purposefully post-decrementing:
return nextId_++;
@@ -82,7 +78,7 @@ namespace ompl
//The next ID to be returned:
BITstar::VertexId nextId_;
//The mutex
- boost::mutex idMutex_;
+ std::mutex idMutex_;
};
} //geometric
} //ompl
@@ -93,7 +89,7 @@ namespace
{
//Global variables:
//The initialization flag stating that the ID generator has been created:
- static boost::once_flag g_IdInited = BOOST_ONCE_INIT;
+ static std::once_flag g_IdInited;
//A pointer to the actual ID generator
static boost::scoped_ptr<ompl::geometric::BITstar::IdGenerator> g_IdGenerator;
@@ -106,7 +102,7 @@ namespace
//A function to get the current ID generator:
ompl::geometric::BITstar::IdGenerator& getIdGenerator()
{
- boost::call_once(&initIdGenerator, g_IdInited);
+ std::call_once(g_IdInited, &initIdGenerator);
return *g_IdGenerator;
}
}
diff --git a/src/ompl/geometric/planners/bitstar/datastructures/IntegratedQueue.h b/src/ompl/geometric/planners/bitstar/datastructures/IntegratedQueue.h
index a8a665a..93039d1 100644
--- a/src/ompl/geometric/planners/bitstar/datastructures/IntegratedQueue.h
+++ b/src/ompl/geometric/planners/bitstar/datastructures/IntegratedQueue.h
@@ -46,10 +46,10 @@
#include <list>
//std::multimap
#include <map>
-//boost::unordered_map (pre-C++11 std::unordered_map)
-#include <boost/unordered_map.hpp>
-//For boost::function
-#include <boost/function.hpp>
+//std::unordered_map
+#include <unordered_map>
+//For std::function
+#include <functional>
//OMPL:
//The cost class:
@@ -92,26 +92,23 @@ namespace ompl
public:
////////////////////////////////
//Data typedefs:
- /** \brief A typedef for a pair of vertices, i.e., an edge */
- typedef std::pair<VertexPtr, VertexPtr> VertexPtrPair;
-
/** \brief A typedef for a pair of costs, i.e., the edge sorting key */
typedef std::pair<ompl::base::Cost, ompl::base::Cost> CostPair;
-
- /** \brief A typedef for the nearest-neighbour struct */
- typedef boost::shared_ptr< NearestNeighbors<VertexPtr> > VertexPtrNNPtr;
////////////////////////////////
////////////////////////////////
//Function typedefs:
- /** \brief A boost::function definition of a heuristic function for a vertex. */
- typedef boost::function<ompl::base::Cost (const VertexPtr&)> VertexHeuristicFunc;
+ /** \brief A std::function definition of a heuristic function for a vertex. */
+ typedef std::function<ompl::base::Cost (const VertexConstPtr&)> VertexHeuristicFunc;
+
+ /** \brief A std::function definition of a heuristic function for an edge. */
+ typedef std::function<ompl::base::Cost (const VertexConstPtrPair&)> EdgeHeuristicFunc;
- /** \brief A boost::function definition of a heuristic function for an edge. */
- typedef boost::function<ompl::base::Cost (const VertexPtrPair&)> EdgeHeuristicFunc;
+ /** \brief A std::function definition for the distance between two vertices. */
+ typedef std::function<double (const VertexConstPtr&, const VertexConstPtr&)> DistanceFunc;
- /** \brief A boost::function definition for the neighbourhood of a vertex . */
- typedef boost::function<void (const VertexPtr&, std::vector<VertexPtr>*)> NeighbourhoodFunc;
+ /** \brief A std::function definition for the neighbourhood of a vertex . */
+ typedef std::function<unsigned int (const VertexPtr&, std::vector<VertexPtr>*)> NeighbourhoodFunc;
////////////////////////////////
@@ -119,17 +116,11 @@ namespace ompl
////////////////////////////////
//Public functions:
/** \brief Construct an integrated queue. */
- //boost::make_shared can only take 9 arguments, so be careful:
- IntegratedQueue(const ompl::base::OptimizationObjectivePtr& opt, const NeighbourhoodFunc& nearSamplesFunc, const NeighbourhoodFunc& nearVerticesFunc, const VertexHeuristicFunc& lowerBoundHeuristicVertex, const VertexHeuristicFunc& currentHeuristicVertex, const EdgeHeuristicFunc& lowerBoundHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdgeTarget);
+ //std::make_shared can only take 9 arguments, so be careful:
+ IntegratedQueue(const ompl::base::OptimizationObjectivePtr& opt, const DistanceFunc& distanceFunc, const NeighbourhoodFunc& nearSamplesFunc, const NeighbourhoodFunc& nearVerticesFunc, const VertexHeuristicFunc& lowerBoundHeuristicVertex, const VertexHeuristicFunc& currentHeuristicVertex, const EdgeHeuristicFunc& lowerBoundHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdgeTarget);
virtual ~IntegratedQueue();
- /** \brief Enable tracking of failed edges. This currently is too expensive to be useful.*/
- void setUseFailureTracking(bool trackFailures);
-
- /** \brief Get whether a failed edge list is in use.*/
- bool getUseFailureTracking() const;
-
/** \brief Delay considering rewiring edges until an initial solution is found. This improves
the time required to find an initial solution when doing so requires multiple batches and has
no effects on theoretical asymptotic optimality (as the rewiring edges are eventually considered). */
@@ -147,7 +138,7 @@ namespace ompl
void insertEdge(const VertexPtrPair& newEdge);
/** \brief Erase a vertex from the vertex expansion queue. Will disconnect the vertex from its parent and remove the associated incoming and outgoing edges from the edge queue as requested.*/
- void eraseVertex(const VertexPtr& oldVertex, bool disconnectParent, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN);
+ void eraseVertex(const VertexPtr& oldVertex, bool disconnectParent, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);
//////////////////
//////////////////
@@ -198,10 +189,10 @@ namespace ompl
void markVertexUnsorted(const VertexPtr& vertex);
/** \brief Prune the vertex queue of vertices whose their lower-bound heuristic is greater then the threshold. Descendents of pruned vertices that are not pruned themselves are returned to the set of free states. Returns the number of vertices pruned (either removed completely or moved to the set of free states). */
- std::pair<unsigned int, unsigned int> prune(const VertexPtr& pruneStartPtr, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN);
+ std::pair<unsigned int, unsigned int> prune(const VertexPtr& pruneStartPtr, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);
/** \brief Resort the queue, only reinserting edges/vertices if their lower-bound heuristic is less then the threshold. Descendents of pruned vertices that are not pruned themselves are returned to the set of free states. Requires first marking the queue as unsorted. Returns the number of vertices pruned (either removed completely or moved to the set of free states). */
- std::pair<unsigned int, unsigned int> resort(const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN);
+ std::pair<unsigned int, unsigned int> resort(const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);
/** \brief Finish the queue, clearing all the edge containers and moving the vertex expansion token to the end. After a call to finish, isEmpty() will return true. Keeps threshold, list of unsorted vertices, etc.*/
void finish();
@@ -263,16 +254,16 @@ namespace ompl
////////////////////////////////
//Helpful typedefs:
/** \brief A typedef to the underlying vertex queue as a multiset. The advantage to a multimap over a multiset is that a copy of the key is stored with the value, which guarantees that the ordering remains sane. Even if the inherent key for a value has changed, it will still be sorted under the old key until manually updated and the map will be sorted */
- typedef std::multimap<ompl::base::Cost, VertexPtr, boost::function<bool (const ompl::base::Cost&, const ompl::base::Cost&)> > CostToVertexMMap;
+ typedef std::multimap<ompl::base::Cost, VertexPtr, std::function<bool (const ompl::base::Cost&, const ompl::base::Cost&)> > CostToVertexMMap;
/** \brief A typedef to the underlying edge queue as a multimap. Multimapped for the same reason as CostToVertexMMap */
- typedef std::multimap<CostPair, VertexPtrPair, boost::function<bool (const CostPair&, const CostPair&)> > CostToVertexPtrPairMMap;
+ typedef std::multimap<CostPair, VertexPtrPair, std::function<bool (const CostPair&, const CostPair&)> > CostToVertexPtrPairMMap;
/** \brief A typedef for an iterator into the vertex queue multimap */
typedef CostToVertexMMap::iterator VertexQueueIter;
/** \brief A typedef for an unordered_map of vertex queue iterators indexed on vertex*/
- typedef boost::unordered_map<BITstar::VertexId, VertexQueueIter> VertexIdToVertexQueueIterUMap;
+ typedef std::unordered_map<BITstar::VertexId, VertexQueueIter> VertexIdToVertexQueueIterUMap;
/** \brief A typedef for an iterator into the edge queue multimap */
typedef CostToVertexPtrPairMMap::iterator EdgeQueueIter;
@@ -281,74 +272,7 @@ namespace ompl
typedef std::list<EdgeQueueIter> EdgeQueueIterList;
/** \brief A typedef for an unordered_map of edge queue iterators indexed by vertex*/
- typedef boost::unordered_map<BITstar::VertexId, EdgeQueueIterList> VertexIdToEdgeQueueIterListUMap;
- ////////////////////////////////
-
- ////////////////////////////////
- //Member variables:
-
- /** \brief My optimization objective. */
- ompl::base::OptimizationObjectivePtr opt_;
-
- /** \brief The function to find nearby samples. */
- NeighbourhoodFunc nearSamplesFunc_;
-
- /** \brief The function to find nearby samples. */
- NeighbourhoodFunc nearVerticesFunc_;
-
- /** \brief The lower-bounding heuristic for a vertex. */
- VertexHeuristicFunc lowerBoundHeuristicVertexFunc_;
-
- /** \brief The current heuristic for a vertex. */
- VertexHeuristicFunc currentHeuristicVertexFunc_;
-
- /** \brief The lower-bounding heuristic for an edge. */
- EdgeHeuristicFunc lowerBoundHeuristicEdgeFunc_;
-
- /** \brief The current heuristic for an edge. */
- EdgeHeuristicFunc currentHeuristicEdgeFunc_;
-
- /** \brief The current heuristic to the end of an edge. */
- EdgeHeuristicFunc currentHeuristicEdgeTargetFunc_;
-
- /** \brief Whether to use failure tracking or not */
- bool useFailureTracking_;
-
- /** \brief Whether to delay rewiring until an initial solution is found or not */
- bool delayRewiring_;
-
- /** \brief Whether to use parent lookup tables or not */
- bool outgoingLookupTables_;
-
- /** \brief Whether to use child lookup tables or not */
- bool incomingLookupTables_;
-
- /** \brief The underlying queue of vertices. Sorted by vertexQueueComparison. */
- CostToVertexMMap vertexQueue_;
-
- /** \brief The next vertex in the expansion queue to expand*/
- VertexQueueIter vertexToExpand_;
-
- /** \brief The underlying queue of edges. Sorted by edgeQueueComparison. */
- CostToVertexPtrPairMMap edgeQueue_;
-
- /** \brief A lookup from vertex to iterator in the vertex queue */
- VertexIdToVertexQueueIterUMap vertexIterLookup_;
-
- /** \brief A unordered map from a vertex to all the edges in the queue emanating from the vertex: */
- VertexIdToEdgeQueueIterListUMap outgoingEdges_;
-
- /** \brief A unordered map from a vertex to all the edges in the queue leading into the vertex: */
- VertexIdToEdgeQueueIterListUMap incomingEdges_;
-
- /** \brief A list of vertices that we will need to process when resorting the queue: */
- std::list<VertexPtr> resortVertices_;
-
- /** \brief The maximum heuristic value allowed for vertices/edges in the queue.*/
- ompl::base::Cost costThreshold_;
-
- /** \brief Whether the problem has a solution */
- bool hasSolution_;
+ typedef std::unordered_map<BITstar::VertexId, EdgeQueueIterList> VertexIdToEdgeQueueIterListUMap;
////////////////////////////////
@@ -365,6 +289,9 @@ namespace ompl
/** \brief Attempt to add an edge to the queue. Checks that the edge meets the queueing condition and that it is not in the failed set (if appropriate). */
void queueupEdge(const VertexPtr& parent, const VertexPtr& child);
+
+ /** \brief Given two subsets containing (up to) the k-nearest members of each, finds the k-nearest of the union */
+ void processKNearest(unsigned int k, const VertexConstPtr& vertex, std::vector<VertexPtr>* kNearSamples, std::vector<VertexPtr>* kNearVertices);
////////////////////////////////
@@ -374,7 +301,7 @@ namespace ompl
void reinsertVertex(const VertexPtr& unorderedVertex);
/** \brief Prune a branch of the graph. Returns the number of vertices removed, and the number of said vertices that are completely thrown away (i.e., are not even useful as a sample) */
- std::pair<unsigned int, unsigned int> pruneBranch(const VertexPtr& branchBase, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN);
+ std::pair<unsigned int, unsigned int> pruneBranch(const VertexPtr& branchBase, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);
/** \brief Disconnect a vertex from its parent by removing the edges stored in itself, and its parents. Cascades cost updates if requested.*/
void disconnectParent(const VertexPtr& oldVertex, bool cascadeCostUpdates);
@@ -384,7 +311,7 @@ namespace ompl
/** \brief Remove a vertex from the queue and optionally its entries in the various lookups. Returns the number of vertices that are completely deleted. */
//This is *NOT* by const-reference so that the oldVertex pointer doesn't go out of scope on me... which was happening if it was being called with an iter->second where the iter gets deleted in this function...
- unsigned int vertexRemoveHelper(VertexPtr oldVertex, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, bool removeLookups);
+ unsigned int vertexRemoveHelper(VertexPtr oldVertex, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices, bool removeLookups);
////////////////////////////////
////////////////////////////////
@@ -437,6 +364,74 @@ namespace ompl
/** \brief Compare whether cost a is worse or equivalent to cost b by checking that a is not better than b. */
bool isCostWorseThanOrEquivalentTo(const ompl::base::Cost& a, const ompl::base::Cost& b) const;
////////////////////////////////
+
+
+
+ ////////////////////////////////
+ //Member variables:
+ /** \brief My optimization objective. */
+ ompl::base::OptimizationObjectivePtr opt_;
+
+ /** \brief The distance function */
+ DistanceFunc distanceFunc_;
+
+ /** \brief The function to find nearby samples. */
+ NeighbourhoodFunc nearSamplesFunc_;
+
+ /** \brief The function to find nearby samples. */
+ NeighbourhoodFunc nearVerticesFunc_;
+
+ /** \brief The lower-bounding heuristic for a vertex. */
+ VertexHeuristicFunc lowerBoundHeuristicVertexFunc_;
+
+ /** \brief The current heuristic for a vertex. */
+ VertexHeuristicFunc currentHeuristicVertexFunc_;
+
+ /** \brief The lower-bounding heuristic for an edge. */
+ EdgeHeuristicFunc lowerBoundHeuristicEdgeFunc_;
+
+ /** \brief The current heuristic for an edge. */
+ EdgeHeuristicFunc currentHeuristicEdgeFunc_;
+
+ /** \brief The current heuristic to the end of an edge. */
+ EdgeHeuristicFunc currentHeuristicEdgeTargetFunc_;
+
+ /** \brief Whether to delay rewiring until an initial solution is found or not */
+ bool delayRewiring_;
+
+ /** \brief Whether to use parent lookup tables or not */
+ bool outgoingLookupTables_;
+
+ /** \brief Whether to use child lookup tables or not */
+ bool incomingLookupTables_;
+
+ /** \brief The underlying queue of vertices. Sorted by vertexQueueComparison. */
+ CostToVertexMMap vertexQueue_;
+
+ /** \brief The next vertex in the expansion queue to expand*/
+ VertexQueueIter vertexToExpand_;
+
+ /** \brief The underlying queue of edges. Sorted by edgeQueueComparison. */
+ CostToVertexPtrPairMMap edgeQueue_;
+
+ /** \brief A lookup from vertex to iterator in the vertex queue */
+ VertexIdToVertexQueueIterUMap vertexIterLookup_;
+
+ /** \brief A unordered map from a vertex to all the edges in the queue emanating from the vertex: */
+ VertexIdToEdgeQueueIterListUMap outgoingEdges_;
+
+ /** \brief A unordered map from a vertex to all the edges in the queue leading into the vertex: */
+ VertexIdToEdgeQueueIterListUMap incomingEdges_;
+
+ /** \brief A list of vertices that we will need to process when resorting the queue: */
+ std::list<VertexPtr> resortVertices_;
+
+ /** \brief The maximum heuristic value allowed for vertices/edges in the queue.*/
+ ompl::base::Cost costThreshold_;
+
+ /** \brief Whether the problem has a solution */
+ bool hasSolution_;
+ ////////////////////////////////
}; //class: IntegratedQueue
} //geometric
} //ompl
diff --git a/src/ompl/geometric/planners/bitstar/datastructures/Vertex.h b/src/ompl/geometric/planners/bitstar/datastructures/Vertex.h
index 0a15f76..c15895e 100644
--- a/src/ompl/geometric/planners/bitstar/datastructures/Vertex.h
+++ b/src/ompl/geometric/planners/bitstar/datastructures/Vertex.h
@@ -40,12 +40,10 @@
//vector
#include <vector>
-//Boost
//shared and weak pointers
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
+#include <memory>
//For unordered sets of failed children:
-#include <boost/unordered_set.hpp>
+#include <unordered_set>
//OMPL:
//The space information
@@ -154,6 +152,24 @@ namespace ompl
/** \brief Mark the vertex as old. */
void markOld();
+ /** \brief Returns true if the vertex has been expanded towards samples. */
+ bool hasBeenExpandedToSamples() const;
+
+ /** \brief Mark the vertex as expanded towards samples. */
+ void markExpandedToSamples();
+
+ /** \brief Mark the vertex as not expanded towards samples. */
+ void markUnexpandedToSamples();
+
+ /** \brief Returns true if the vertex has been expanded towards vertices. */
+ bool hasBeenExpandedToVertices() const;
+
+ /** \brief Mark the vertex as expanded towards vertices. */
+ void markExpandedToVertices();
+
+ /** \brief Mark the vertex as not expanded towards vertices. */
+ void markUnexpandedToVertices();
+
/** \brief Whether the vertex has been pruned */
bool isPruned() const;
@@ -174,9 +190,6 @@ namespace ompl
void updateCostAndDepth(bool cascadeUpdates = true);
private:
- /** \brief The type of container used to store the failed children */
- typedef boost::unordered_set<BITstar::VertexId> FailedIdUSet;
-
/** \brief The vertex ID */
BITstar::VertexId vId_;
@@ -192,9 +205,15 @@ namespace ompl
/** \brief Whether the vertex is a root */
bool isRoot_;
- /** \brief Whether the vertex is a new. Vertices are new until marked old. */
+ /** \brief Whether the vertex is new. */
bool isNew_;
+ /** \brief Whether the vertex had been expanded to samples. */
+ bool hasBeenExpandedToSamples_;
+
+ /** \brief Whether the vertex has been expanded to vertices. */
+ bool hasBeenExpandedToVertices_;
+
/** \brief Whether the vertex is pruned. Vertices throw if any member function other than isPruned() is access after they are pruned. */
bool isPruned_;
@@ -211,10 +230,7 @@ namespace ompl
ompl::base::Cost cost_;
/** \brief The child states as weak pointers, such that the ownership loop is broken and a state can be deleted once it's children are.*/
- std::vector<VertexWeakPtr> childWPtrs_;
-
- /** \brief The unordered set of failed child vertices*/
- FailedIdUSet failedVIds_;
+ std::vector<VertexWeakPtr> childWPtrs_;
/** \brief A helper function to check that the vertex is not pruned and throw if so */
diff --git a/src/ompl/geometric/planners/bitstar/datastructures/src/IntegratedQueue.cpp b/src/ompl/geometric/planners/bitstar/datastructures/src/IntegratedQueue.cpp
index 137c8ab..57273a0 100644
--- a/src/ompl/geometric/planners/bitstar/datastructures/src/IntegratedQueue.cpp
+++ b/src/ompl/geometric/planners/bitstar/datastructures/src/IntegratedQueue.cpp
@@ -47,8 +47,9 @@ namespace ompl
{
/////////////////////////////////////////////////////////////////////////////////////////////
//Public functions:
- BITstar::IntegratedQueue::IntegratedQueue(const ompl::base::OptimizationObjectivePtr& opt, const NeighbourhoodFunc& nearSamplesFunc, const NeighbourhoodFunc& nearVerticesFunc, const VertexHeuristicFunc& lowerBoundHeuristicVertex, const VertexHeuristicFunc& currentHeuristicVertex, const EdgeHeuristicFunc& lowerBoundHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdgeTarget)
+ BITstar::IntegratedQueue::IntegratedQueue(const ompl::base::OptimizationObjectivePtr& opt, const DistanceFunc& distanceFunc, const NeighbourhoodFunc& nearSamplesFunc, const NeighbourhoodFunc& nearVerticesFunc, const VertexHeuristicFunc& lowerBoundHeuristicVertex, const VertexHeuristicFunc& currentHeuristicVertex, const EdgeHeuristicFunc& lowerBoundHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdgeTarget)
: opt_(opt),
+ distanceFunc_(distanceFunc),
nearSamplesFunc_(nearSamplesFunc),
nearVerticesFunc_(nearVerticesFunc),
lowerBoundHeuristicVertexFunc_(lowerBoundHeuristicVertex),
@@ -56,13 +57,14 @@ namespace ompl
lowerBoundHeuristicEdgeFunc_(lowerBoundHeuristicEdge),
currentHeuristicEdgeFunc_(currentHeuristicEdge),
currentHeuristicEdgeTargetFunc_(currentHeuristicEdgeTarget),
- useFailureTracking_(false),
delayRewiring_(true),
outgoingLookupTables_(true),
incomingLookupTables_(true),
- vertexQueue_( boost::bind(&BITstar::IntegratedQueue::vertexQueueComparison, this, _1, _2) ), //This tells the vertexQueue_ to use the vertexQueueComparison for sorting
+ vertexQueue_( std::bind(&BITstar::IntegratedQueue::vertexQueueComparison, this,
+ std::placeholders::_1, std::placeholders::_2) ), //This tells the vertexQueue_ to use the vertexQueueComparison for sorting
vertexToExpand_( vertexQueue_.begin() ),
- edgeQueue_( boost::bind(&BITstar::IntegratedQueue::edgeQueueComparison, this, _1, _2) ), //This tells the edgeQueue_ to use the edgeQueueComparison for sorting
+ edgeQueue_( std::bind(&BITstar::IntegratedQueue::edgeQueueComparison, this,
+ std::placeholders::_1, std::placeholders::_2) ), //This tells the edgeQueue_ to use the edgeQueueComparison for sorting
vertexIterLookup_(),
outgoingEdges_(),
incomingEdges_(),
@@ -98,7 +100,7 @@ namespace ompl
- void BITstar::IntegratedQueue::eraseVertex(const VertexPtr& oldVertex, bool disconnectParent, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN)
+ void BITstar::IntegratedQueue::eraseVertex(const VertexPtr& oldVertex, bool disconnectParent, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices)
{
//If requested, disconnect from parent, cascading cost updates:
if (disconnectParent == true)
@@ -107,7 +109,7 @@ namespace ompl
}
//Remove it from vertex queue and lookup, and edge queues (as requested):
- this->vertexRemoveHelper(oldVertex, vertexNN, freeStateNN, true);
+ this->vertexRemoveHelper(oldVertex, vertexNN, freeStateNN, recycledVertices, true);
}
@@ -128,7 +130,7 @@ namespace ompl
- BITstar::IntegratedQueue::VertexPtrPair BITstar::IntegratedQueue::frontEdge()
+ BITstar::VertexPtrPair BITstar::IntegratedQueue::frontEdge()
{
if (this->isEmpty() == true)
{
@@ -195,7 +197,7 @@ namespace ompl
- BITstar::IntegratedQueue::VertexPtrPair BITstar::IntegratedQueue::popFrontEdge()
+ BITstar::VertexPtrPair BITstar::IntegratedQueue::popFrontEdge()
{
VertexPtrPair rval;
@@ -425,7 +427,7 @@ namespace ompl
- std::pair<unsigned int, unsigned int> BITstar::IntegratedQueue::prune(const VertexPtr& pruneStartPtr, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN)
+ std::pair<unsigned int, unsigned int> BITstar::IntegratedQueue::prune(const VertexPtr& pruneStartPtr, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices)
{
if (this->isSorted() == false)
{
@@ -477,7 +479,7 @@ namespace ompl
--queueIter;
//Prune the branch:
- numPruned = this->pruneBranch(pruneIter->second, vertexNN, freeStateNN);
+ numPruned = this->pruneBranch(pruneIter->second, vertexNN, freeStateNN, recycledVertices);
}
//No else, skip this vertex.
@@ -491,10 +493,10 @@ namespace ompl
- std::pair<unsigned int, unsigned int> BITstar::IntegratedQueue::resort(const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN)
+ std::pair<unsigned int, unsigned int> BITstar::IntegratedQueue::resort(const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices)
{
//Variable:
- typedef boost::unordered_map<BITstar::VertexId, VertexPtr> VertexIdToVertexPtrUMap;
+ typedef std::unordered_map<BITstar::VertexId, VertexPtr> VertexIdToVertexPtrUMap;
typedef std::map<unsigned int, VertexIdToVertexPtrUMap> DepthToUMapMap;
//The number of vertices and samples pruned, respectively:
std::pair<unsigned int, unsigned int> numPruned;
@@ -531,12 +533,12 @@ namespace ompl
//Make sure it has not already been returned to the set of samples:
if (vIter->second->isInTree() == true)
{
- //Are we pruning the vertex from the queue?
- if (this->vertexPruneCondition(vIter->second) == true)
+ //Are we pruning the vertex from the queue (and do we have "permission" to do so)?
+ if (this->vertexPruneCondition(vIter->second) == true && static_cast<bool>(vertexNN) == true && static_cast<bool>(freeStateNN) == true)
{
//The vertex should just be pruned and forgotten about.
//Prune the branch:
- numPruned = this->pruneBranch(vIter->second, vertexNN, freeStateNN);
+ numPruned = this->pruneBranch(vIter->second, vertexNN, freeStateNN, recycledVertices);
}
else
{
@@ -949,24 +951,47 @@ namespace ompl
//Variables:
//The vector of nearby samples (either within r or the k-nearest)
std::vector<VertexPtr> neighbourSamples;
+ //The vector of nearby vertices
+ std::vector<VertexPtr> neighbourVertices;
+ //Are we using k-nearest?
+ bool usingKNearest;
+ //If we're using k-nearest, what number that is
+ unsigned int k;
- //Get the set of nearby free states:
- nearSamplesFunc_(vertex, &neighbourSamples);
+ //Get the set of nearby free states, returns the number k if it's k nearest, 0u otherwise
+ k = nearSamplesFunc_(vertex, &neighbourSamples);
+
+ //Decode if we're using k-nearest for readability
+ usingKNearest = (k > 0u);
+
+ //If we're usjng k-nearest, we always have to also get the neighbourVertices and the do some post-processing
+ if (usingKNearest == true)
+ {
+ //Get the set of nearby vertices
+ nearVerticesFunc_(vertex, &neighbourVertices);
+
+ //Post process them:
+ this->processKNearest(k, vertex, &neighbourSamples, &neighbourVertices);
+ }
+ //No else
//Add edges to unconnected targets who could ever provide a better solution:
- //Is the vertex new?
- if (vertex->isNew() == true)
+ //Has the vertex been expanded into edges towards unconnected samples before?
+ if (vertex->hasBeenExpandedToSamples() == false)
{
- //The vertex is new, that means none of its outgoing edges to unconnected vertices have been considered before. Add them all
+ //It has not, that means none of its outgoing edges have been considered. Add them all
for (unsigned int i = 0u; i < neighbourSamples.size(); ++i)
{
//Attempt to queue the edge.
this->queueupEdge(vertex, neighbourSamples.at(i));
}
+
+ //Mark it as expanded
+ vertex->markExpandedToSamples();
}
else
{
- //The vertex is old, which means that outgoing edges to old unconnected vertices have been considered before. Only add those that lead to new vertices
+ //It has, which means that outgoing edges to old unconnected vertices have already been considered. Only add those that lead to new vertices
for (unsigned int i = 0u; i < neighbourSamples.size(); ++i)
{
//Is the target new?
@@ -979,15 +1004,16 @@ namespace ompl
}
}
- //If it is a new and either we're not delaying rewiring or we have a solution, we also add rewiring candidates:
- if (vertex->isNew() == true && (delayRewiring_ == false || hasSolution_ == true))
+ //If the vertex has never been expanded into possible rewiring edges *and* either we're not delaying rewiring or we have a solution, we add those rewiring candidates:
+ if (vertex->hasBeenExpandedToVertices() == false && (delayRewiring_ == false || hasSolution_ == true))
{
- //Variables:
- //The vector of vertices within r of the vertexf
- std::vector<VertexPtr> neighbourVertices;
-
- //Get the set of nearby free states:
- nearVerticesFunc_(vertex, &neighbourVertices);
+ //If we're not using k-nearest, we will not have gotten the neighbour vertices yet, get them now
+ if (usingKNearest == false)
+ {
+ //Get the set of nearby vertices
+ nearVerticesFunc_(vertex, &neighbourVertices);
+ }
+ //No else
//Iterate over the vector of connected targets and add only those who could ever provide a better solution:
for (unsigned int i = 0u; i < neighbourVertices.size(); ++i)
@@ -1016,8 +1042,8 @@ namespace ompl
//No else
}
- //Mark the vertex as old
- vertex->markOld();
+ //Mark the vertex as expanded into rewirings
+ vertex->markExpandedToVertices();
}
//No else
}
@@ -1028,38 +1054,66 @@ namespace ompl
void BITstar::IntegratedQueue::queueupEdge(const VertexPtr& parent, const VertexPtr& child)
{
- //Variables:
- //A bool to store the conditional failed edge check
- bool previouslyFailed;
+ //Variable:
+ //The edge:
+ VertexPtrPair newEdge;
- //See if we're checking for previous failure:
- if (useFailureTracking_ == true)
- {
- previouslyFailed = parent->hasAlreadyFailed(child);
- }
- else
+ //Make the edge
+ newEdge = std::make_pair(parent, child);
+
+ //Should this edge be in the queue? I.e., is it *not* due to be pruned:
+ if (this->edgePruneCondition(newEdge) == false)
{
- previouslyFailed = false;
+ this->edgeInsertHelper(newEdge, edgeQueue_.end());
}
+ //No else, it can never provide a better solution
+ }
- //Make sure the edge has not already failed
- if (previouslyFailed == false)
- {
- //Variable:
- //The edge:
- VertexPtrPair newEdge;
- //Make the edge
- newEdge = std::make_pair(parent, child);
- //Should this edge be in the queue? I.e., is it *not* due to be pruned:
- if (this->edgePruneCondition(newEdge) == false)
+ void BITstar::IntegratedQueue::processKNearest(unsigned int k, const VertexConstPtr& vertex, std::vector<VertexPtr>* kNearSamples, std::vector<VertexPtr>* kNearVertices)
+ {
+ //Variables
+ //The position in the sample vector
+ unsigned int samplePos;
+ //The position in the vertex vector
+ unsigned int vertexPos;
+
+ //Iterate through the first k in the combined vectors
+ samplePos = 0u;
+ vertexPos = 0u;
+ while (samplePos + vertexPos < k && (samplePos < kNearSamples->size() || vertexPos < kNearVertices->size()))
+ {
+ //Where along are we in the relative vectors?
+ if (samplePos < kNearSamples->size() && vertexPos >= kNearVertices->size())
{
- this->edgeInsertHelper(newEdge, edgeQueue_.end());
+ //There are just samples left. Easy, move the sample token:
+ ++samplePos;
+ }
+ else if (samplePos >= kNearSamples->size() && vertexPos < kNearVertices->size())
+ {
+ //There are just vertices left. Easy, move the vertex token:
+ ++vertexPos;
+ }
+ else
+ {
+ //Both are left, which is closest?
+ if ( distanceFunc_(kNearVertices->at(vertexPos), vertex) < distanceFunc_(kNearSamples->at(samplePos), vertex) )
+ {
+ //The vertex is closer than the sample, move that token:
+ ++vertexPos;
+ }
+ else
+ {
+ //The vertex is not closer than the sample, move the sample token:
+ ++samplePos;
+ }
}
- //No else, we assume that it's better to calculate this condition multiple times than have the list of failed sets become too large...?
}
- //No else
+
+ //Now erase the extra. Resize will truncate the extras
+ kNearSamples->resize(samplePos);
+ kNearVertices->resize(vertexPos);
}
@@ -1101,7 +1155,7 @@ namespace ompl
}
//Remove myself, not touching my lookup entries
- this->vertexRemoveHelper(unorderedVertex, VertexPtrNNPtr(), VertexPtrNNPtr(), false);
+ this->vertexRemoveHelper(unorderedVertex, VertexPtrNNPtr(), VertexPtrNNPtr(), nullptr, false);
//Reinsert myself, expanding if I cross the token if I am not already expanded
this->vertexInsertHelper(unorderedVertex, alreadyExpanded == false);
@@ -1143,7 +1197,7 @@ namespace ompl
- std::pair<unsigned int, unsigned int> BITstar::IntegratedQueue::pruneBranch(const VertexPtr& branchBase, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN)
+ std::pair<unsigned int, unsigned int> BITstar::IntegratedQueue::pruneBranch(const VertexPtr& branchBase, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices)
{
//We must iterate over the children of this vertex and prune each one.
//Then we must decide if this vertex (a) gets deleted or (b) placed back on the sample set.
@@ -1172,7 +1226,7 @@ namespace ompl
branchBase->getChildren(&children);
//Remove myself from everything:
- numPruned.second = this->vertexRemoveHelper(branchBase, vertexNN, freeStateNN, true);
+ numPruned.second = this->vertexRemoveHelper(branchBase, vertexNN, freeStateNN, recycledVertices, true);
//Prune my children:
for (unsigned int i = 0u; i < children.size(); ++i)
@@ -1182,7 +1236,7 @@ namespace ompl
std::pair<unsigned int, unsigned int> childNumPruned;
//Prune my children:
- childNumPruned = this->pruneBranch(children.at(i), vertexNN, freeStateNN);
+ childNumPruned = this->pruneBranch(children.at(i), vertexNN, freeStateNN, recycledVertices);
//Update my counter:
numPruned.first = numPruned.first + childNumPruned.first;
@@ -1297,7 +1351,7 @@ namespace ompl
- unsigned int BITstar::IntegratedQueue::vertexRemoveHelper(VertexPtr oldVertex, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, bool removeLookups)
+ unsigned int BITstar::IntegratedQueue::vertexRemoveHelper(VertexPtr oldVertex, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices, bool removeLookups)
{
//Variable
//The number of samples deleted (i.e., if this vertex is NOT moved to a sample, this is a 1)
@@ -1348,7 +1402,7 @@ namespace ompl
}
//Check if I have been given permission to change sets:
- if (static_cast<bool>(vertexNN) == true && static_cast<bool>(freeStateNN) == true)
+ if (static_cast<bool>(vertexNN) == true && static_cast<bool>(freeStateNN) == true && static_cast<bool>(recycledVertices) == true)
{
//Check if I should be discarded completely:
if (this->samplePruneCondition(oldVertex) == true)
@@ -1375,11 +1429,17 @@ namespace ompl
//Remove myself from the nearest neighbour structure:
vertexNN->remove(oldVertex);
+ //Mark myself as a "new" sample. This assures that all possible incoming edges will be considered
+ oldVertex->markNew();
+
+ //Add myself to the list of recycled vertices:
+ recycledVertices->push_back(oldVertex);
+
//And add the vertex to the set of samples, keeping the incoming edges:
freeStateNN->add(oldVertex);
}
}
- //Else, if I was given null pointers to the NN structs, that's because this sample is not allowed to change sets.
+ //Else, if I was given null pointers, that's because this sample is not allowed to change sets.
}
else
{
@@ -1611,20 +1671,6 @@ namespace ompl
/////////////////////////////////////////////////////////////////////////////////////////////
//Boring sets/gets (Public):
- void BITstar::IntegratedQueue::setUseFailureTracking(bool trackFailures)
- {
- useFailureTracking_ = trackFailures;
- }
-
-
-
- bool BITstar::IntegratedQueue::getUseFailureTracking() const
- {
- return useFailureTracking_;
- }
-
-
-
void BITstar::IntegratedQueue::setDelayedRewiring(bool delayRewiring)
{
delayRewiring_ = delayRewiring;
diff --git a/src/ompl/geometric/planners/bitstar/datastructures/src/Vertex.cpp b/src/ompl/geometric/planners/bitstar/datastructures/src/Vertex.cpp
index 31b84e3..7e49196 100644
--- a/src/ompl/geometric/planners/bitstar/datastructures/src/Vertex.cpp
+++ b/src/ompl/geometric/planners/bitstar/datastructures/src/Vertex.cpp
@@ -52,13 +52,14 @@ namespace ompl
state_( si_->allocState() ),
isRoot_(root),
isNew_(true),
+ hasBeenExpandedToSamples_(false),
+ hasBeenExpandedToVertices_(false),
isPruned_(false),
depth_(0u),
parentSPtr_( VertexPtr() ),
edgeCost_( opt_->infiniteCost() ),
cost_( opt_->infiniteCost() ),
- childWPtrs_(),
- failedVIds_()
+ childWPtrs_()
{
if (this->isRoot() == true)
{
@@ -410,44 +411,79 @@ namespace ompl
- bool BITstar::Vertex::isPruned() const
+ bool BITstar::Vertex::hasBeenExpandedToSamples() const
{
- return isPruned_;
+ this->assertNotPruned();
+
+ return hasBeenExpandedToSamples_;
}
- void BITstar::Vertex::markPruned()
+ void BITstar::Vertex::markExpandedToSamples()
{
this->assertNotPruned();
- isPruned_ = true;
+ hasBeenExpandedToSamples_ = true;
}
- void BITstar::Vertex::markUnpruned()
+ void BITstar::Vertex::markUnexpandedToSamples()
{
- isPruned_ = false;
+ this->assertNotPruned();
+
+ hasBeenExpandedToSamples_ = false;
+ }
+
+
+
+ bool BITstar::Vertex::hasBeenExpandedToVertices() const
+ {
+ this->assertNotPruned();
+
+ return hasBeenExpandedToVertices_;
+ }
+
+
+
+ void BITstar::Vertex::markExpandedToVertices()
+ {
+ this->assertNotPruned();
+
+ hasBeenExpandedToVertices_ = true;
}
- void BITstar::Vertex::markAsFailedChild(const VertexConstPtr& failedChild)
+ void BITstar::Vertex::markUnexpandedToVertices()
{
this->assertNotPruned();
- failedVIds_.insert( failedChild->getId() );
+ hasBeenExpandedToVertices_ = false;
+ }
+
+
+
+ bool BITstar::Vertex::isPruned() const
+ {
+ return isPruned_;
}
- bool BITstar::Vertex::hasAlreadyFailed(const VertexConstPtr& potentialChild) const
+ void BITstar::Vertex::markPruned()
{
this->assertNotPruned();
- //Return true if there is more than 0 of this pointer.
- return failedVIds_.count( potentialChild->getId() ) > 0u;
+ isPruned_ = true;
+ }
+
+
+
+ void BITstar::Vertex::markUnpruned()
+ {
+ isPruned_ = false;
}
/////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/src/ompl/geometric/planners/bitstar/src/BITstar.cpp b/src/ompl/geometric/planners/bitstar/src/BITstar.cpp
index 775e849..193cdf3 100644
--- a/src/ompl/geometric/planners/bitstar/src/BITstar.cpp
+++ b/src/ompl/geometric/planners/bitstar/src/BITstar.cpp
@@ -43,14 +43,10 @@
#include <sstream>
//For stream manipulations
#include <iomanip>
-//For boost make_shared
-#include <boost/make_shared.hpp>
-//For boost::bind
-#include <boost/bind.hpp>
+//For std::bind
+#include <functional>
//For boost math constants
#include <boost/math/constants/constants.hpp>
-//For boost::math::isfinite
-#include <boost/math/special_functions/fpclassify.hpp>
//For OMPL_INFORM et al.
#include "ompl/util/Console.h"
@@ -87,6 +83,8 @@ namespace ompl
freeStateNN_(),
vertexNN_(),
intQueue_(),
+ newSamples_(),
+ recycledSamples_(),
numUniformStates_(0u),
r_(0.0), //Purposeful Gibberish
k_rgg_(0.0), //Purposeful Gibberish
@@ -113,18 +111,30 @@ namespace ompl
numEdgeCollisionChecks_(0u),
numNearestNeighbours_(0u),
numEdgesProcessed_(0u),
- useStrictQueueOrdering_(true),
+ useStrictQueueOrdering_(false),
rewireFactor_(2.0),
samplesPerBatch_(100u),
useKNearest_(true),
usePruning_(true),
- pruneFraction_(0.02),
- delayRewiring_(false),
+ pruneFraction_(0.01),
+ delayRewiring_(true),
useJustInTimeSampling_(false),
dropSamplesOnPrune_(false),
- useFailureTracking_(false),
stopOnSolnChange_(false)
{
+ //Make sure the default name reflects the default k-nearest setting, if not overridden to something else
+ if (useKNearest_ == true && Planner::getName() == "BITstar")
+ {
+ //It's the current default r-disc BIT* name, but we're using k-nearest, so change
+ Planner::setName("kBITstar");
+ }
+ else if (useKNearest_ == false && Planner::getName() == "kBITstar")
+ {
+ //It's the current default k-nearest BIT* name, but we're using r-disc, so change
+ Planner::setName("BITstar");
+ }
+ //It's not default named, don't change it
+
//Specify my planner specs:
Planner::specs_.recognizedGoal = ompl::base::GOAL_SAMPLEABLE_REGION;
Planner::specs_.multithreaded = false;
@@ -149,25 +159,25 @@ namespace ompl
//Planner::declareParam<bool>("use_edge_failure_tracking", this, &BITstar::setUseFailureTracking, &BITstar::getUseFailureTracking, "0,1");
//Register my progress info:
- addPlannerProgressProperty("best cost DOUBLE", boost::bind(&BITstar::bestCostProgressProperty, this));
- addPlannerProgressProperty("number of segments in solution path INTEGER", boost::bind(&BITstar::bestLengthProgressProperty, this));
- addPlannerProgressProperty("current free states INTEGER", boost::bind(&BITstar::currentFreeProgressProperty, this));
- addPlannerProgressProperty("current graph vertices INTEGER", boost::bind(&BITstar::currentVertexProgressProperty, this));
- addPlannerProgressProperty("state collision checks INTEGER", boost::bind(&BITstar::stateCollisionCheckProgressProperty, this));
- addPlannerProgressProperty("edge collision checks INTEGER", boost::bind(&BITstar::edgeCollisionCheckProgressProperty, this));
- addPlannerProgressProperty("nearest neighbour calls INTEGER", boost::bind(&BITstar::nearestNeighbourProgressProperty, this));
+ addPlannerProgressProperty("best cost DOUBLE", std::bind(&BITstar::bestCostProgressProperty, this));
+ addPlannerProgressProperty("number of segments in solution path INTEGER", std::bind(&BITstar::bestLengthProgressProperty, this));
+ addPlannerProgressProperty("current free states INTEGER", std::bind(&BITstar::currentFreeProgressProperty, this));
+ addPlannerProgressProperty("current graph vertices INTEGER", std::bind(&BITstar::currentVertexProgressProperty, this));
+ addPlannerProgressProperty("state collision checks INTEGER", std::bind(&BITstar::stateCollisionCheckProgressProperty, this));
+ addPlannerProgressProperty("edge collision checks INTEGER", std::bind(&BITstar::edgeCollisionCheckProgressProperty, this));
+ addPlannerProgressProperty("nearest neighbour calls INTEGER", std::bind(&BITstar::nearestNeighbourProgressProperty, this));
//Extra progress info that aren't necessary for every day use. Uncomment if desired.
- //addPlannerProgressProperty("vertex queue size INTEGER", boost::bind(&BITstar::vertexQueueSizeProgressProperty, this));
- //addPlannerProgressProperty("edge queue size INTEGER", boost::bind(&BITstar::edgeQueueSizeProgressProperty, this));
- //addPlannerProgressProperty("iterations INTEGER", boost::bind(&BITstar::iterationProgressProperty, this));
- //addPlannerProgressProperty("batches INTEGER", boost::bind(&BITstar::batchesProgressProperty, this));
- //addPlannerProgressProperty("graph prunings INTEGER", boost::bind(&BITstar::pruningProgressProperty, this));
- //addPlannerProgressProperty("total states generated INTEGER", boost::bind(&BITstar::totalStatesCreatedProgressProperty, this));
- //addPlannerProgressProperty("vertices constructed INTEGER", boost::bind(&BITstar::verticesConstructedProgressProperty, this));
- //addPlannerProgressProperty("states pruned INTEGER", boost::bind(&BITstar::statesPrunedProgressProperty, this));
- //addPlannerProgressProperty("graph vertices disconnected INTEGER", boost::bind(&BITstar::verticesDisconnectedProgressProperty, this));
- //addPlannerProgressProperty("rewiring edges INTEGER", boost::bind(&BITstar::rewiringProgressProperty, this));
+ //addPlannerProgressProperty("vertex queue size INTEGER", std::bind(&BITstar::vertexQueueSizeProgressProperty, this));
+ //addPlannerProgressProperty("edge queue size INTEGER", std::bind(&BITstar::edgeQueueSizeProgressProperty, this));
+ //addPlannerProgressProperty("iterations INTEGER", std::bind(&BITstar::iterationProgressProperty, this));
+ //addPlannerProgressProperty("batches INTEGER", std::bind(&BITstar::batchesProgressProperty, this));
+ //addPlannerProgressProperty("graph prunings INTEGER", std::bind(&BITstar::pruningProgressProperty, this));
+ //addPlannerProgressProperty("total states generated INTEGER", std::bind(&BITstar::totalStatesCreatedProgressProperty, this));
+ //addPlannerProgressProperty("vertices constructed INTEGER", std::bind(&BITstar::verticesConstructedProgressProperty, this));
+ //addPlannerProgressProperty("states pruned INTEGER", std::bind(&BITstar::statesPrunedProgressProperty, this));
+ //addPlannerProgressProperty("graph vertices disconnected INTEGER", std::bind(&BITstar::verticesDisconnectedProgressProperty, this));
+ //addPlannerProgressProperty("rewiring edges INTEGER", std::bind(&BITstar::rewiringProgressProperty, this));
}
@@ -196,7 +206,7 @@ namespace ompl
if (Planner::pdef_->hasOptimizationObjective() == false)
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", Planner::getName().c_str());
- Planner::pdef_->setOptimizationObjective( boost::make_shared<base::PathLengthOptimizationObjective> (Planner::si_) );
+ Planner::pdef_->setOptimizationObjective( std::make_shared<base::PathLengthOptimizationObjective> (Planner::si_) );
}
//If the problem definition *has* a goal, make sure it is of appropriate type
@@ -230,13 +240,22 @@ namespace ompl
//No else, already allocated (by a call to setNearestNeighbors())
//Configure:
- freeStateNN_->setDistanceFunction(boost::bind(&BITstar::nnDistance, this, _1, _2));
- vertexNN_->setDistanceFunction(boost::bind(&BITstar::nnDistance, this, _1, _2));
+ freeStateNN_->setDistanceFunction(std::bind(&BITstar::nnDistance, this,
+ std::placeholders::_1, std::placeholders::_2));
+ vertexNN_->setDistanceFunction(std::bind(&BITstar::nnDistance, this,
+ std::placeholders::_1, std::placeholders::_2));
//Configure the queue
- //boost::make_shared can only take 9 arguments, so be careful:
- intQueue_ = boost::make_shared<IntegratedQueue> (opt_, boost::bind(&BITstar::nearestSamples, this, _1, _2), boost::bind(&BITstar::nearestVertices, this, _1, _2), boost::bind(&BITstar::lowerBoundHeuristicVertex, this, _1), boost::bind(&BITstar::currentHeuristicVertex, this, _1), boost::bind(&BITstar::lowerBoundHeuristicEdge, this, _1), boost::bind(&BITstar::currentHeuristicEdge, this, _1), boost::bind(&BITstar::currentHeuristicEdgeTarget, this, _1));
- intQueue_->setUseFailureTracking(useFailureTracking_);
+ //std::make_shared can only take 9 arguments, so be careful:
+ intQueue_ = std::make_shared<IntegratedQueue> (opt_,
+ std::bind(&BITstar::nnDistance, this, std::placeholders::_1, std::placeholders::_2),
+ std::bind(&BITstar::nearestSamples, this, std::placeholders::_1, std::placeholders::_2),
+ std::bind(&BITstar::nearestVertices, this, std::placeholders::_1, std::placeholders::_2),
+ std::bind(&BITstar::lowerBoundHeuristicVertex, this, std::placeholders::_1),
+ std::bind(&BITstar::currentHeuristicVertex, this, std::placeholders::_1),
+ std::bind(&BITstar::lowerBoundHeuristicEdge, this, std::placeholders::_1),
+ std::bind(&BITstar::currentHeuristicEdge, this, std::placeholders::_1),
+ std::bind(&BITstar::currentHeuristicEdgeTarget, this, std::placeholders::_1));
intQueue_->setDelayedRewiring(delayRewiring_);
//Set the best-cost, pruned-cost, sampled-cost and min-cost to the proper opt_-based values:
@@ -252,7 +271,7 @@ namespace ompl
prunedMeasure_ = Planner::si_->getSpaceMeasure();
//Does the problem have finite boundaries?
- if (boost::math::isfinite(prunedMeasure_) == false)
+ if (std::isfinite(prunedMeasure_) == false)
{
//It does not, so let's estimate a measure of the planning problem.
//A not horrible place to start would be hypercube proportional to the distance between the start and goal. It's not *great*, but at least it sort of captures the order-of-magnitude of the problem.
@@ -331,6 +350,10 @@ namespace ompl
vertexNN_.reset();
}
+ //The list of new and recycled samples
+ newSamples_.clear();
+ recycledSamples_.clear();
+
//The queue:
if (static_cast<bool>(intQueue_) == true)
{
@@ -348,7 +371,6 @@ namespace ompl
//delayRewiring_
//useJustInTimeSampling_
//dropSamplesOnPrune_
- //useFailureTracking_
//stopOnSolnChange_
//Reset the various calculations? TODO: Should I recalculate them?
@@ -510,7 +532,7 @@ namespace ompl
else
{
//An empty edge:
- nextEdge = std::make_pair<ompl::base::State*, ompl::base::State*>(NULL, NULL);
+ nextEdge = std::make_pair<ompl::base::State*, ompl::base::State*>(nullptr, nullptr);
}
return nextEdge;
@@ -572,8 +594,8 @@ namespace ompl
else
{
//The problem isn't setup yet, create NN structs of the specified type:
- freeStateNN_ = boost::make_shared< NN<VertexPtr> >();
- vertexNN_ = boost::make_shared< NN<VertexPtr> >();
+ freeStateNN_ = std::make_shared< NN<VertexPtr> >();
+ vertexNN_ = std::make_shared< NN<VertexPtr> >();
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
@@ -719,21 +741,9 @@ namespace ompl
}
//No else, this edge may be useful at some later date.
}
- else if (useFailureTracking_ == true)
- {
- //If the edge failed, and we're tracking failures, record.
- //This edge has a collision and can never be helpful. Poor edge. Add the target to the list of failed children for the source:
- bestEdge.first->markAsFailedChild(bestEdge.second);
- }
- //No else, we failed and we're not tracking those
- }
- else if (useFailureTracking_ == true)
- {
- //If the edge failed, and we're tracking failures, record.
- //This edge either has a very high edge cost and can never be helpful. Poor edge. Add the target to the list of failed children for the source
- bestEdge.first->markAsFailedChild(bestEdge.second);
+ //No else, we failed
}
- //No else, we failed and we're not tracking those
+ //No else, we failed
}
else if (intQueue_->isSorted() == false)
{
@@ -772,13 +782,33 @@ namespace ompl
//Set the cost sampled to the minimum
costSampled_ = minCost_;
- //Finally, update the nearest-neighbour terms for the number of samples we *will* have.
+ //Update the nearest-neighbour terms for the number of samples we *will* have.
this->updateNearestTerms();
+
+ //Relabel all the previous samples as old
+ for (unsigned int i = 0u; i < newSamples_.size(); ++i)
+ {
+ //If the sample still exists, mark as old. It can get pruned during a resort.
+ if (newSamples_.at(i)->isPruned() == false)
+ {
+ newSamples_.at(i)->markOld();
+ }
+ //No else, this sample has been pruned and will shortly disappear
+ }
+
+ //Clear the list of new samples
+ newSamples_.clear();
+
+ //Make the recycled vertices to new:
+ newSamples_ = recycledSamples_;
+
+ //Clear the list of recycled
+ recycledSamples_.clear();
}
- void BITstar::updateSamples(const VertexPtr& vertex)
+ void BITstar::updateSamples(const VertexConstPtr& vertex)
{
//Variable
//The required cost to contain the neighbourhood of this vertex:
@@ -828,7 +858,7 @@ namespace ompl
{
//Variable
//The new state:
- VertexPtr newState = boost::make_shared<Vertex>(Planner::si_, opt_);
+ VertexPtr newState = std::make_shared<Vertex>(Planner::si_, opt_);
//Sample in the interval [costSampled_, costReqd):
sampler_->sampleUniform(newState->state(), costSampled_, costReqd);
@@ -869,9 +899,7 @@ namespace ompl
{
//Variables:
//The current measure of the problem space:
- double informedMeasure;
-
- informedMeasure = sampler_->getInformedMeasure(bestCost_);
+ double informedMeasure = sampler_->getInformedMeasure(bestCost_);
//Is there good reason to prune? I.e., is the informed subset measurably less than the total problem domain? If an informed measure is not available, we'll assume yes:
if ( (sampler_->hasInformedMeasure() == true && informedMeasure < si_->getSpaceMeasure()) || (sampler_->hasInformedMeasure() == false) )
@@ -893,7 +921,7 @@ namespace ompl
//Prune the graph. This can be done extra efficiently by using some info in the integrated queue.
//This requires access to the nearest neighbour structures so vertices can be moved to free states.s
- numPruned = intQueue_->prune(curGoalVertex_, vertexNN_, freeStateNN_);
+ numPruned = intQueue_->prune(curGoalVertex_, vertexNN_, freeStateNN_, &recycledSamples_);
//The number of vertices and samples pruned are incrementally updated.
numVerticesDisconnected_ = numVerticesDisconnected_ + numPruned.first;
@@ -923,9 +951,18 @@ namespace ompl
//The number of vertices and samples pruned
std::pair<unsigned int, unsigned int> numPruned;
- //Resorting requires access to the nearest neighbour structures so vertices can be pruned instead of resorted.
- //The number of vertices pruned is also incrementally updated.
- numPruned = intQueue_->resort(vertexNN_, freeStateNN_);
+ //During resorting we can be lazy and skip resorting vertices that will just be pruned later. So, are we using pruning?
+ if (usePruning_ == true)
+ {
+ //We are, give the queue access to the nearest neighbour structures so vertices can be pruned instead of resorted.
+ //The number of vertices pruned is also incrementally updated.
+ numPruned = intQueue_->resort(vertexNN_, freeStateNN_, &recycledSamples_);
+ }
+ else
+ {
+ //We are not, give it empty NN structs
+ numPruned = intQueue_->resort(VertexPtrNNPtr(), VertexPtrNNPtr(), nullptr);
+ }
//The number of vertices and samples pruned are incrementally updated.
numVerticesDisconnected_ = numVerticesDisconnected_ + numPruned.first;
@@ -940,12 +977,12 @@ namespace ompl
{
//Variable
//The path geometric
- boost::shared_ptr<ompl::geometric::PathGeometric> pathGeoPtr;
+ std::shared_ptr<ompl::geometric::PathGeometric> pathGeoPtr;
//The reverse path of state pointers
std::vector<const ompl::base::State*> reversePath;
//Allocate the pathGeoPtr
- pathGeoPtr = boost::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
+ pathGeoPtr = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
//Get the reversed path
reversePath = this->bestPathFromGoalToStart();
@@ -1020,7 +1057,7 @@ namespace ompl
do
{
//Variable
- //A new goal pointer, if there are none, it will be NULL.
+ //A new goal pointer, if there are none, it will be nullptr.
//We will wait for the duration of PTC for a new goal to appear.
const ompl::base::State* newGoal = Planner::pis_.nextGoal(ptc);
@@ -1031,7 +1068,7 @@ namespace ompl
rebuildQueue = (startVertices_.size() > 0u);
//Allocate the vertex pointer
- goalVertices_.push_back(boost::make_shared<Vertex>(Planner::si_, opt_));
+ goalVertices_.push_back(std::make_shared<Vertex>(Planner::si_, opt_));
//Copy the value into the state
Planner::si_->copyState(goalVertices_.back()->state(), newGoal);
@@ -1056,7 +1093,7 @@ namespace ompl
const ompl::base::State* newStart = Planner::pis_.nextStart();
//Allocate the vertex pointer:
- startVertices_.push_back(boost::make_shared<Vertex>(Planner::si_, opt_, true));
+ startVertices_.push_back(std::make_shared<Vertex>(Planner::si_, opt_, true));
//Copy the value into the state:
Planner::si_->copyState(startVertices_.back()->state(), newStart);
@@ -1220,7 +1257,7 @@ namespace ompl
++numFreeStatesPruned_;
//Remove the start vertex completely from the queue, they don't have parents
- intQueue_->eraseVertex(*startIter, false, vertexNN_, freeStateNN_);
+ intQueue_->eraseVertex(*startIter, false, vertexNN_, freeStateNN_, &recycledSamples_);
//Store the start vertex in the pruned list, in case it later needs to be readded:
prunedStartVertices_.push_back(*startIter);
@@ -1261,7 +1298,7 @@ namespace ompl
++numFreeStatesPruned_;
//And erase it from the queue:
- intQueue_->eraseVertex(*goalIter, (*goalIter)->hasParent(), vertexNN_, freeStateNN_);
+ intQueue_->eraseVertex(*goalIter, (*goalIter)->hasParent(), vertexNN_, freeStateNN_, &recycledSamples_);
//Store the start vertex in the pruned list, in case it later needs to be readded:
prunedGoalVertices_.push_back(*goalIter);
@@ -1329,10 +1366,10 @@ namespace ompl
- bool BITstar::checkEdge(const VertexPtrPair& edge)
+ bool BITstar::checkEdge(const VertexConstPtrPair& edge)
{
++numEdgeCollisionChecks_;
- return Planner::si_->checkMotion(edge.first->state(), edge.second->state());
+ return Planner::si_->checkMotion(edge.first->stateConst(), edge.second->stateConst());
}
@@ -1374,9 +1411,11 @@ namespace ompl
}
else
{
- //If not, we just add the vertex, first connect:
+ //If not, we just add the vertex, first mark the target vertex as no longer new and unexpanded:
+ newEdge.second->markUnexpandedToSamples();
+ newEdge.second->markUnexpandedToVertices();
- //Add a child to the parent, not updating costs:
+ //Then add a child to the parent, not updating costs:
newEdge.first->addChild(newEdge.second, false);
//Add a parent to the child, updating descendant costs if requested:
@@ -1533,6 +1572,9 @@ namespace ompl
//Mark as new
newSample->markNew();
+ //Add to the list of new samples
+ newSamples_.push_back(newSample);
+
//Add to the NN structure:
freeStateNN_->add(newSample);
}
@@ -1566,7 +1608,7 @@ namespace ompl
- void BITstar::nearestSamples(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourSamples)
+ unsigned int BITstar::nearestSamples(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourSamples)
{
//Make sure sampling has happened first:
this->updateSamples(vertex);
@@ -1577,16 +1619,18 @@ namespace ompl
if (useKNearest_ == true)
{
freeStateNN_->nearestK(vertex, k_, *neighbourSamples);
+ return k_;
}
else
{
freeStateNN_->nearestR(vertex, r_, *neighbourSamples);
+ return 0u;
}
}
- void BITstar::nearestVertices(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourVertices)
+ unsigned int BITstar::nearestVertices(const VertexPtr& vertex, std::vector<VertexPtr>* neighbourVertices)
{
//Increment our counter:
++numNearestNeighbours_;
@@ -1594,67 +1638,69 @@ namespace ompl
if (useKNearest_ == true)
{
vertexNN_->nearestK(vertex, k_, *neighbourVertices);
+ return k_;
}
else
{
vertexNN_->nearestR(vertex, r_, *neighbourVertices);
+ return 0u;
}
}
- double BITstar::nnDistance(const VertexPtr& a, const VertexPtr& b) const
+ double BITstar::nnDistance(const VertexConstPtr& a, const VertexConstPtr& b) const
{
//Using RRTstar as an example, this order gives us the distance FROM the queried state TO the other neighbours in the structure.
//The distance function between two states
- if (!a->state())
+ if (!a->stateConst())
{
throw ompl::Exception("a->state is unallocated");
}
- if (!b->state())
+ if (!b->stateConst())
{
throw ompl::Exception("b->state is unallocated");
}
- return Planner::si_->distance(b->state(), a->state());
+ return Planner::si_->distance(b->stateConst(), a->stateConst());
}
- ompl::base::Cost BITstar::lowerBoundHeuristicVertex(const VertexPtr& vertex) const
+ ompl::base::Cost BITstar::lowerBoundHeuristicVertex(const VertexConstPtr& vertex) const
{
return opt_->combineCosts( this->costToComeHeuristic(vertex), this->costToGoHeuristic(vertex) );
}
- ompl::base::Cost BITstar::currentHeuristicVertex(const VertexPtr& vertex) const
+ ompl::base::Cost BITstar::currentHeuristicVertex(const VertexConstPtr& vertex) const
{
return opt_->combineCosts( vertex->getCost(), this->costToGoHeuristic(vertex) );
}
- ompl::base::Cost BITstar::lowerBoundHeuristicEdge(const VertexPtrPair& edgePair) const
+ ompl::base::Cost BITstar::lowerBoundHeuristicEdge(const VertexConstPtrPair& edgePair) const
{
return this->combineCosts(this->costToComeHeuristic(edgePair.first), this->edgeCostHeuristic(edgePair), this->costToGoHeuristic(edgePair.second));
}
- ompl::base::Cost BITstar::currentHeuristicEdge(const VertexPtrPair& edgePair) const
+ ompl::base::Cost BITstar::currentHeuristicEdge(const VertexConstPtrPair& edgePair) const
{
return opt_->combineCosts(this->currentHeuristicEdgeTarget(edgePair), this->costToGoHeuristic(edgePair.second));
}
- ompl::base::Cost BITstar::currentHeuristicEdgeTarget(const VertexPtrPair& edgePair) const
+ ompl::base::Cost BITstar::currentHeuristicEdgeTarget(const VertexConstPtrPair& edgePair) const
{
return opt_->combineCosts(edgePair.first->getCost(), this->edgeCostHeuristic(edgePair));
}
- ompl::base::Cost BITstar::costToComeHeuristic(const VertexPtr& vertex) const
+ ompl::base::Cost BITstar::costToComeHeuristic(const VertexConstPtr& vertex) const
{
//Variable
//The current best cost to the state, initialize to infinity
@@ -1664,7 +1710,7 @@ namespace ompl
for (std::list<VertexPtr>::const_iterator startIter = startVertices_.begin(); startIter != startVertices_.end(); ++startIter)
{
//Update the cost-to-come as the better of the best so far and the new one
- curBest = opt_->betterCost(curBest, opt_->motionCostHeuristic((*startIter)->state(), vertex->state()));
+ curBest = opt_->betterCost(curBest, opt_->motionCostHeuristic((*startIter)->stateConst(), vertex->stateConst()));
}
//Return
@@ -1673,14 +1719,14 @@ namespace ompl
- ompl::base::Cost BITstar::edgeCostHeuristic(const VertexPtrPair& edgePair) const
+ ompl::base::Cost BITstar::edgeCostHeuristic(const VertexConstPtrPair& edgePair) const
{
- return opt_->motionCostHeuristic(edgePair.first->state(), edgePair.second->state());
+ return opt_->motionCostHeuristic(edgePair.first->stateConst(), edgePair.second->stateConst());
}
- ompl::base::Cost BITstar::costToGoHeuristic(const VertexPtr& vertex) const
+ ompl::base::Cost BITstar::costToGoHeuristic(const VertexConstPtr& vertex) const
{
//Variable
//The current best cost to a goal from the state, initialize to infinity
@@ -1690,7 +1736,7 @@ namespace ompl
for (std::list<VertexPtr>::const_iterator goalIter = goalVertices_.begin(); goalIter != goalVertices_.end(); ++goalIter)
{
//Update the cost-to-go as the better of the best so far and the new one
- curBest = opt_->betterCost(curBest, opt_->motionCostHeuristic(vertex->state(), (*goalIter)->state()));
+ curBest = opt_->betterCost(curBest, opt_->motionCostHeuristic(vertex->stateConst(), (*goalIter)->stateConst()));
}
//Return
@@ -1698,22 +1744,23 @@ namespace ompl
}
- ompl::base::Cost BITstar::trueEdgeCost(const VertexPtrPair& edgePair) const
+ ompl::base::Cost BITstar::trueEdgeCost(const VertexConstPtrPair& edgePair) const
{
- return opt_->motionCost(edgePair.first->state(), edgePair.second->state());
+ return opt_->motionCost(edgePair.first->stateConst(), edgePair.second->stateConst());
}
- ompl::base::Cost BITstar::neighbourhoodCost(const VertexPtr& vertex) const
+ ompl::base::Cost BITstar::neighbourhoodCost(const VertexConstPtr& vertex) const
{
+ //Even though the problem domain is defined by prunedCost_ (the cost the last time we pruned), there is no point generating samples outside bestCost_ (which may be less).
if (useJustInTimeSampling_ == true)
{
- return opt_->betterCost(prunedCost_, opt_->combineCosts(this->lowerBoundHeuristicVertex(vertex), ompl::base::Cost(2.0 * r_)));
+ return opt_->betterCost(bestCost_, opt_->combineCosts(this->lowerBoundHeuristicVertex(vertex), ompl::base::Cost(2.0 * r_)));
}
else
{
- return prunedCost_;
+ return bestCost_;
}
}
@@ -2031,14 +2078,24 @@ namespace ompl
//Check if the flag has changed
if (useKNearest != useKNearest_)
{
+ //If the planner is default named, we change it:
+ if (useKNearest_ == true && Planner::getName() == "kBITstar")
+ {
+ //It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
+ Planner::setName("BITstar");
+ }
+ else if (useKNearest_ == false && Planner::getName() == "BITstar")
+ {
+ //It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
+ Planner::setName("kBITstar");
+ }
+ //It's not default named, don't change it
+
//Set the k-nearest flag
useKNearest_ = useKNearest;
if (useKNearest_ == true)
{
- //Warn that this isn't exactly implemented
- OMPL_WARN("%s: The implementation of k-nearest is overly conservative, as it considers the k-nearest samples and the k-nearest vertices, instead of just the combined k-nearest.", Planner::getName().c_str()); //This is because we have a separate nearestNeighbours structure for samples and vertices and you don't know what fraction of K to ask for from each...
-
//Check that we're not doing JIT
if (useJustInTimeSampling_ == true)
{
@@ -2082,7 +2139,7 @@ namespace ompl
{
if (prune == false)
{
- OMPL_WARN("%s: Turning pruning off does not turn a fake pruning on, as it should.", Planner::getName().c_str());
+ OMPL_WARN("%s: Turning pruning off has never really been tested.", Planner::getName().c_str());
}
usePruning_ = prune;
@@ -2199,27 +2256,6 @@ namespace ompl
- void BITstar::setUseFailureTracking(bool trackFailures)
- {
- //Store
- useFailureTracking_ = trackFailures;
-
- //Configure queue if constructed:
- if (intQueue_)
- {
- intQueue_->setUseFailureTracking(useFailureTracking_);
- }
- }
-
-
-
- bool BITstar::getUseFailureTracking() const
- {
- return useFailureTracking_;
- }
-
-
-
void BITstar::setStopOnSolnImprovement(bool stopOnChange)
{
stopOnSolnChange_ = stopOnChange;
@@ -2243,42 +2279,42 @@ namespace ompl
std::string BITstar::bestCostProgressProperty() const
{
- return boost::lexical_cast<std::string>(this->bestCost().value());
+ return std::to_string(this->bestCost().value());
}
std::string BITstar::bestLengthProgressProperty() const
{
- return boost::lexical_cast<std::string>(bestLength_);
+ return std::to_string(bestLength_);
}
std::string BITstar::currentFreeProgressProperty() const
{
- return boost::lexical_cast<std::string>(freeStateNN_->size());
+ return std::to_string(freeStateNN_->size());
}
std::string BITstar::currentVertexProgressProperty() const
{
- return boost::lexical_cast<std::string>(vertexNN_->size());
+ return std::to_string(vertexNN_->size());
}
std::string BITstar::vertexQueueSizeProgressProperty() const
{
- return boost::lexical_cast<std::string>(intQueue_->numVertices());
+ return std::to_string(intQueue_->numVertices());
}
std::string BITstar::edgeQueueSizeProgressProperty() const
{
- return boost::lexical_cast<std::string>(intQueue_->numEdges());
+ return std::to_string(intQueue_->numEdges());
}
@@ -2292,7 +2328,7 @@ namespace ompl
std::string BITstar::iterationProgressProperty() const
{
- return boost::lexical_cast<std::string>(this->numIterations());
+ return std::to_string(this->numIterations());
}
@@ -2306,77 +2342,77 @@ namespace ompl
std::string BITstar::batchesProgressProperty() const
{
- return boost::lexical_cast<std::string>(this->numBatches());
+ return std::to_string(this->numBatches());
}
std::string BITstar::pruningProgressProperty() const
{
- return boost::lexical_cast<std::string>(numPrunings_);
+ return std::to_string(numPrunings_);
}
std::string BITstar::totalStatesCreatedProgressProperty() const
{
- return boost::lexical_cast<std::string>(numSamples_);
+ return std::to_string(numSamples_);
}
std::string BITstar::verticesConstructedProgressProperty() const
{
- return boost::lexical_cast<std::string>(numVertices_);
+ return std::to_string(numVertices_);
}
std::string BITstar::statesPrunedProgressProperty() const
{
- return boost::lexical_cast<std::string>(numFreeStatesPruned_);
+ return std::to_string(numFreeStatesPruned_);
}
std::string BITstar::verticesDisconnectedProgressProperty() const
{
- return boost::lexical_cast<std::string>(numVerticesDisconnected_);
+ return std::to_string(numVerticesDisconnected_);
}
std::string BITstar::rewiringProgressProperty() const
{
- return boost::lexical_cast<std::string>(numRewirings_);
+ return std::to_string(numRewirings_);
}
std::string BITstar::stateCollisionCheckProgressProperty() const
{
- return boost::lexical_cast<std::string>(numStateCollisionChecks_);
+ return std::to_string(numStateCollisionChecks_);
}
std::string BITstar::edgeCollisionCheckProgressProperty() const
{
- return boost::lexical_cast<std::string>(numEdgeCollisionChecks_);
+ return std::to_string(numEdgeCollisionChecks_);
}
std::string BITstar::nearestNeighbourProgressProperty() const
{
- return boost::lexical_cast<std::string>(numNearestNeighbours_);
+ return std::to_string(numNearestNeighbours_);
}
std::string BITstar::edgesProcessedProgressProperty() const
{
- return boost::lexical_cast<std::string>(numEdgesProcessed_);
+ return std::to_string(numEdgesProcessed_);
}
/////////////////////////////////////////////////////////////////////////////////////////////
}//geometric
diff --git a/src/ompl/geometric/planners/cforest/CForest.h b/src/ompl/geometric/planners/cforest/CForest.h
index 8bb6f5a..676977d 100644
--- a/src/ompl/geometric/planners/cforest/CForest.h
+++ b/src/ompl/geometric/planners/cforest/CForest.h
@@ -42,7 +42,7 @@
#include "ompl/geometric/planners/PlannerIncludes.h"
#include "ompl/tools/config/SelfConfig.h"
-#include <boost/thread.hpp>
+#include <mutex>
#include <vector>
@@ -189,7 +189,7 @@ namespace ompl
std::vector<base::StateSamplerPtr> samplers_;
/** \brief Stores the states already shared to check if a specific state has been shared. */
- boost::unordered_set<const base::State *> statesShared_;
+ std::unordered_set<const base::State *> statesShared_;
/** \brief Cost of the best path found so far among planners. */
base::Cost bestCost_;
@@ -201,10 +201,10 @@ namespace ompl
unsigned int numStatesShared_;
/** \brief Mutex to control the access to the newSolutionFound() method. */
- boost::mutex newSolutionFoundMutex_;
+ std::mutex newSolutionFoundMutex_;
/** \brief Mutex to control the access to samplers_ */
- boost::mutex addSamplerMutex_;
+ std::mutex addSamplerMutex_;
/** \brief Flag to control whether the search is focused. */
bool focusSearch_;
diff --git a/src/ompl/geometric/planners/cforest/CForestStateSampler.h b/src/ompl/geometric/planners/cforest/CForestStateSampler.h
index 2aaead7..f0cee00 100644
--- a/src/ompl/geometric/planners/cforest/CForestStateSampler.h
+++ b/src/ompl/geometric/planners/cforest/CForestStateSampler.h
@@ -39,7 +39,7 @@
#include "ompl/base/StateSpace.h"
-#include <boost/thread/mutex.hpp>
+#include <mutex>
namespace ompl
{
@@ -98,7 +98,7 @@ namespace ompl
StateSamplerPtr sampler_;
/** \brief Lock to control the access to the statesToSample_ vector. */
- boost::mutex statesLock_;
+ std::mutex statesLock_;
};
}
diff --git a/src/ompl/geometric/planners/cforest/CForestStateSpaceWrapper.h b/src/ompl/geometric/planners/cforest/CForestStateSpaceWrapper.h
index 82bf152..d5eabe4 100644
--- a/src/ompl/geometric/planners/cforest/CForestStateSpaceWrapper.h
+++ b/src/ompl/geometric/planners/cforest/CForestStateSpaceWrapper.h
@@ -56,7 +56,7 @@ namespace ompl
{
public:
CForestStateSpaceWrapper(geometric::CForest *cforest, base::StateSpace *space)
- : cforest_(cforest), space_(space), planner_(NULL)
+ : cforest_(cforest), space_(space), planner_(nullptr)
{
setName(space->getName() + "CForestWrapper");
}
diff --git a/src/ompl/geometric/planners/cforest/src/CForest.cpp b/src/ompl/geometric/planners/cforest/src/CForest.cpp
index d63acc3..28603d3 100644
--- a/src/ompl/geometric/planners/cforest/src/CForest.cpp
+++ b/src/ompl/geometric/planners/cforest/src/CForest.cpp
@@ -37,6 +37,7 @@
#include "ompl/geometric/planners/cforest/CForest.h"
#include "ompl/geometric/planners/rrt/RRTstar.h"
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
+#include <thread>
ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::Planner(si, "CForest")
{
@@ -47,16 +48,16 @@ ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::P
numStatesShared_ = 0;
focusSearch_ = true;
- numThreads_ = std::max(boost::thread::hardware_concurrency(), 2u);
+ numThreads_ = std::max(std::thread::hardware_concurrency(), 2u);
Planner::declareParam<bool>("focus_search", this, &CForest::setFocusSearch, &CForest::getFocusSearch, "0,1");
Planner::declareParam<unsigned int>("num_threads", this, &CForest::setNumThreads, &CForest::getNumThreads, "0:64");
addPlannerProgressProperty("best cost REAL",
- boost::bind(&CForest::getBestCost, this));
+ std::bind(&CForest::getBestCost, this));
addPlannerProgressProperty("shared paths INTEGER",
- boost::bind(&CForest::getNumPathsShared, this));
+ std::bind(&CForest::getNumPathsShared, this));
addPlannerProgressProperty("shared states INTEGER",
- boost::bind(&CForest::getNumStatesShared, this));
+ std::bind(&CForest::getNumStatesShared, this));
}
ompl::geometric::CForest::~CForest()
@@ -65,7 +66,7 @@ ompl::geometric::CForest::~CForest()
void ompl::geometric::CForest::setNumThreads(unsigned int numThreads)
{
- numThreads_ = numThreads ? numThreads : std::max(boost::thread::hardware_concurrency(), 2u);
+ numThreads_ = numThreads ? numThreads : std::max(std::thread::hardware_concurrency(), 2u);
}
void ompl::geometric::CForest::addPlannerInstanceInternal(const base::PlannerPtr &planner)
@@ -166,21 +167,24 @@ void ompl::geometric::CForest::setup()
ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc)
{
+ typedef void(CForest::*solveFunctionType)(base::Planner*, const base::PlannerTerminationCondition&);
+
checkValidity();
time::point start = time::now();
- std::vector<boost::thread*> threads(planners_.size());
+ std::vector<std::thread*> threads(planners_.size());
const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback();
if (prevSolutionCallback)
OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
- pdef_->setIntermediateSolutionCallback(boost::bind(&CForest::newSolutionFound, this, _1, _2, _3));
+ pdef_->setIntermediateSolutionCallback(std::bind(&CForest::newSolutionFound, this,
+ std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
bestCost_ = opt_->infiniteCost();
// run each planner in its own thread, with the same ptc.
for (std::size_t i = 0 ; i < threads.size() ; ++i)
- threads[i] = new boost::thread(boost::bind(&CForest::solve, this, planners_[i].get(), ptc));
+ threads[i] = new std::thread(std::bind((solveFunctionType)&CForest::solve, this, planners_[i].get(), ptc));
for (std::size_t i = 0 ; i < threads.size() ; ++i)
{
@@ -196,17 +200,17 @@ ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTer
std::string ompl::geometric::CForest::getBestCost() const
{
- return boost::lexical_cast<std::string>(bestCost_);
+ return std::to_string(bestCost_.value());
}
std::string ompl::geometric::CForest::getNumPathsShared() const
{
- return boost::lexical_cast<std::string>(numPathsShared_);
+ return std::to_string(numPathsShared_);
}
std::string ompl::geometric::CForest::getNumStatesShared() const
{
- return boost::lexical_cast<std::string>(numStatesShared_);
+ return std::to_string(numStatesShared_);
}
void ompl::geometric::CForest::newSolutionFound(const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost)
diff --git a/src/ompl/geometric/planners/cforest/src/CForestStateSampler.cpp b/src/ompl/geometric/planners/cforest/src/CForestStateSampler.cpp
index 4555da4..6b42b35 100644
--- a/src/ompl/geometric/planners/cforest/src/CForestStateSampler.cpp
+++ b/src/ompl/geometric/planners/cforest/src/CForestStateSampler.cpp
@@ -62,7 +62,7 @@ void ompl::base::CForestStateSampler::sampleGaussian(State *state, const State *
void ompl::base::CForestStateSampler::setStatesToSample(const std::vector<const State *> &states)
{
- boost::mutex::scoped_lock slock(statesLock_);
+ std::lock_guard<std::mutex> slock(statesLock_);
for (size_t i = 0; i < statesToSample_.size(); ++i)
space_->freeState(statesToSample_[i]);
statesToSample_.clear();
@@ -79,7 +79,7 @@ void ompl::base::CForestStateSampler::setStatesToSample(const std::vector<const
void ompl::base::CForestStateSampler::getNextSample(State *state)
{
- boost::mutex::scoped_lock slock(statesLock_);
+ std::lock_guard<std::mutex> slock(statesLock_);
space_->copyState(state, statesToSample_.back());
space_->freeState(statesToSample_.back());
statesToSample_.pop_back();
@@ -87,7 +87,7 @@ void ompl::base::CForestStateSampler::getNextSample(State *state)
void ompl::base::CForestStateSampler::clear()
{
- boost::mutex::scoped_lock slock(statesLock_);
+ std::lock_guard<std::mutex> slock(statesLock_);
for (size_t i = 0; i < statesToSample_.size(); ++i)
space_->freeState(statesToSample_[i]);
statesToSample_.clear();
diff --git a/src/ompl/geometric/planners/est/BiEST.h b/src/ompl/geometric/planners/est/BiEST.h
new file mode 100644
index 0000000..a4a2f31
--- /dev/null
+++ b/src/ompl/geometric/planners/est/BiEST.h
@@ -0,0 +1,181 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Rice University
+* 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 Rice University 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: Ryan Luna */
+
+#ifndef OMPL_GEOMETRIC_PLANNERS_EST_BIEST_
+#define OMPL_GEOMETRIC_PLANNERS_EST_BIEST_
+
+#include "ompl/geometric/planners/PlannerIncludes.h"
+#include "ompl/datastructures/NearestNeighbors.h"
+#include "ompl/datastructures/PDF.h"
+#include <vector>
+
+namespace ompl
+{
+
+ namespace geometric
+ {
+
+ /**
+ @anchor gBiEST
+ @par Short description
+ EST is a tree-based motion planner that attempts to detect
+ the less explored area of the space by measuring the density
+ of the explored space, biasing exploration toward parts of
+ the space with lowest density.
+ @par External documentation
+ D. Hsu, J.-C. Latombe, and R. Motwani, Path planning in expansive configuration spaces,
+ <em>Intl. J. Computational Geometry and Applications</em>,
+ vol. 9, no. 4-5, pp. 495–512, 1999. DOI: [10.1142/S0218195999000285](http://dx.doi.org/10.1142/S0218195999000285)<br>
+ [[PDF]](http://bigbird.comp.nus.edu.sg/pmwiki/farm/motion/uploads/Site/ijcga96.pdf)
+ */
+
+ /** \brief Bi-directional Expansive Space Trees */
+ class BiEST : public base::Planner
+ {
+ public:
+
+ /** \brief Constructor */
+ BiEST(const base::SpaceInformationPtr &si);
+
+ virtual ~BiEST();
+
+ virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
+
+ virtual void clear();
+
+ /** \brief Set the range the planner is supposed to use.
+
+ This parameter greatly influences the runtime of the
+ algorithm. It represents the maximum length of a
+ motion to be added in the tree of motions. */
+ void setRange(double distance)
+ {
+ maxDistance_ = distance;
+
+ // Make the neighborhood radius smaller than sampling range to
+ // keep probabilities relatively high for rejection sampling
+ nbrhoodRadius_ = maxDistance_ / 3.0;
+ }
+
+ /** \brief Get the range the planner is using */
+ double getRange() const
+ {
+ return maxDistance_;
+ }
+
+ virtual void setup();
+
+ virtual void getPlannerData(base::PlannerData &data) const;
+
+ protected:
+
+ /// \brief The definition of a motion
+ class Motion
+ {
+ public:
+
+ Motion() : state(NULL), parent(NULL), element(NULL), root(NULL)
+ {
+ }
+
+ /// \brief Constructor that allocates memory for the state
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL), element(NULL), root(NULL)
+ {
+ }
+
+ ~Motion()
+ {
+ }
+
+ /// \brief The state contained by the motion
+ base::State *state;
+
+ /// \brief The parent motion in the exploration tree
+ Motion *parent;
+
+ /// \brief A pointer to the corresponding element in the probability distribution function
+ PDF<Motion*>::Element *element;
+
+ /// \brief The root node of the tree this motion is in
+ const base::State *root;
+ };
+
+ /// \brief Compute distance between motions (actually distance between contained states)
+ double distanceFunction(const Motion *a, const Motion *b) const
+ {
+ return si_->distance(a->state, b->state);
+ }
+
+ /// \brief A nearest-neighbors datastructure containing the tree of motions
+ std::shared_ptr< NearestNeighbors<Motion*> > nnStart_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nnGoal_;
+
+ /// \brief The set of all states in the start tree
+ std::vector<Motion*> startMotions_;
+ std::vector<Motion*> goalMotions_;
+
+ /// \brief The probability distribution function over states in each tree
+ PDF<Motion*> startPdf_;
+ PDF<Motion*> goalPdf_;
+
+ ///\brief Free the memory allocated by this planner
+ void freeMemory();
+
+ /// \brief Add a motion to the exploration tree
+ void addMotion(Motion* motion, std::vector<Motion*>& motions,
+ PDF<Motion*>& pdf, std::shared_ptr< NearestNeighbors<Motion*> > nn,
+ const std::vector<Motion*>& neighbors);
+
+ /// \brief Valid state sampler
+ base::ValidStateSamplerPtr sampler_;
+
+ /// \brief The maximum length of a motion to be added to a tree
+ double maxDistance_;
+
+ /// \brief The radius considered for neighborhood
+ double nbrhoodRadius_;
+
+ /// \brief The random number generator
+ RNG rng_;
+
+ /// \brief The pair of states in each tree connected during planning. Used for PlannerData computation
+ std::pair<base::State*, base::State*> connectionPoint_;
+ };
+
+ }
+}
+
+#endif
diff --git a/src/ompl/geometric/planners/est/EST.h b/src/ompl/geometric/planners/est/EST.h
index 0426f3d..6461e13 100644
--- a/src/ompl/geometric/planners/est/EST.h
+++ b/src/ompl/geometric/planners/est/EST.h
@@ -1,7 +1,7 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
-* Copyright (c) 2008, Willow Garage, Inc.
+* Copyright (c) 2015, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
* 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
+* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -32,14 +32,13 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/* Author: Ioan Sucan */
+/* Author: Ryan Luna */
#ifndef OMPL_GEOMETRIC_PLANNERS_EST_EST_
#define OMPL_GEOMETRIC_PLANNERS_EST_EST_
-#include "ompl/datastructures/Grid.h"
#include "ompl/geometric/planners/PlannerIncludes.h"
-#include "ompl/base/ProjectionEvaluator.h"
+#include "ompl/datastructures/NearestNeighbors.h"
#include "ompl/datastructures/PDF.h"
#include <vector>
@@ -53,15 +52,9 @@ namespace ompl
@anchor gEST
@par Short description
EST is a tree-based motion planner that attempts to detect
- the less explored area of the space through the use of a
- grid imposed on a projection of the state space. Using this
- information, EST continues tree expansion primarily from
- less explored areas. It is important to set the projection
- the algorithm uses (setProjectionEvaluator() function). If
- no projection is set, the planner will attempt to use the
- default projection associated to the state space. An
- exception is thrown if no default projection is available
- either.
+ the less explored area of the space by measuring the density
+ of the explored space, biasing exploration toward parts of
+ the space with lowest density.
@par External documentation
D. Hsu, J.-C. Latombe, and R. Motwani, Path planning in expansive configuration spaces,
<em>Intl. J. Computational Geometry and Applications</em>,
@@ -117,43 +110,23 @@ namespace ompl
return maxDistance_;
}
- /** \brief Set the projection evaluator. This class is
- able to compute the projection of a given state. */
- void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
- {
- projectionEvaluator_ = projectionEvaluator;
- }
-
- /** \brief Set the projection evaluator (select one from
- the ones registered with the state space). */
- void setProjectionEvaluator(const std::string &name)
- {
- projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
- }
-
- /** \brief Get the projection evaluator */
- const base::ProjectionEvaluatorPtr& getProjectionEvaluator() const
- {
- return projectionEvaluator_;
- }
-
virtual void setup();
virtual void getPlannerData(base::PlannerData &data) const;
protected:
- /** \brief The definition of a motion */
+ /// \brief The definition of a motion
class Motion
{
public:
- Motion() : state(NULL), parent(NULL)
+ Motion() : state(NULL), parent(NULL), element(NULL)
{
}
- /** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
+ /// \brief Constructor that allocates memory for the state
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL), element(NULL)
{
}
@@ -161,94 +134,53 @@ namespace ompl
{
}
- /** \brief The state contained by the motion */
- base::State *state;
+ /// \brief The state contained by the motion
+ base::State *state;
- /** \brief The parent motion in the exploration tree */
- Motion *parent;
- };
-
- struct MotionInfo;
-
- /** \brief A grid cell */
- typedef Grid<MotionInfo>::Cell GridCell;
+ /// \brief The parent motion in the exploration tree
+ Motion *parent;
- /** \brief A PDF of grid cells */
- typedef PDF<GridCell*> CellPDF;
-
- /** \brief A struct containing an array of motions and a corresponding PDF element */
- struct MotionInfo
- {
- Motion* operator[](unsigned int i)
- {
- return motions_[i];
- }
- const Motion* operator[](unsigned int i) const
- {
- return motions_[i];
- }
- void push_back(Motion *m)
- {
- motions_.push_back(m);
- }
- unsigned int size() const
- {
- return motions_.size();
- }
- bool empty() const
- {
- return motions_.empty();
- }
- std::vector<Motion*> motions_;
- CellPDF::Element *elem_;
+ /// \brief A pointer to the corresponding element in the probability distribution function
+ PDF<Motion*>::Element *element;
};
-
- /** \brief The data contained by a tree of exploration */
- struct TreeData
+ /// \brief Compute distance between motions (actually distance between contained states)
+ double distanceFunction(const Motion *a, const Motion *b) const
{
- TreeData() : grid(0), size(0)
- {
- }
+ return si_->distance(a->state, b->state);
+ }
- /** \brief A grid where each cell contains an array of motions */
- Grid<MotionInfo> grid;
+ /// \brief A nearest-neighbors datastructure containing the tree of motions
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
- /** \brief The total number of motions in the grid */
- unsigned int size;
- };
+ /// \brief The set of all states in the tree
+ std::vector<Motion*> motions_;
- /** \brief Free the memory allocated by this planner */
- void freeMemory();
+ /// \brief The probability distribution function over states in the tree
+ PDF<Motion*> pdf_;
- /** \brief Add a motion to the exploration tree */
- void addMotion(Motion *motion);
+ ///\brief Free the memory allocated by this planner
+ void freeMemory();
- /** \brief Select a motion to continue the expansion of the tree from */
- Motion* selectMotion();
+ /// \brief Add a motion to the exploration tree
+ void addMotion(Motion *motion, const std::vector<Motion*>& neighbors);
- /** \brief Valid state sampler */
+ /// \brief Valid state sampler
base::ValidStateSamplerPtr sampler_;
- /** \brief The exploration tree constructed by this algorithm */
- TreeData tree_;
-
- /** \brief This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on a projection of the state space. */
- base::ProjectionEvaluatorPtr projectionEvaluator_;
-
- /** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
+ /// \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available)
double goalBias_;
- /** \brief The maximum length of a motion to be added to a tree */
+ /// \brief The maximum length of a motion to be added to a tree
double maxDistance_;
+ /// \brief The radius considered for neighborhood
+ double nbrhoodRadius_;
+
/** \brief The random number generator */
RNG rng_;
- /** \brief The PDF used for selecting a cell from which to sample a motion */
- CellPDF pdf_;
-
- /** \brief The most recent goal motion. Used for PlannerData computation */
+ /// \brief The most recent goal motion. Used for PlannerData computation
Motion *lastGoalMotion_;
};
diff --git a/src/ompl/geometric/planners/est/EST.h b/src/ompl/geometric/planners/est/ProjEST.h
similarity index 94%
copy from src/ompl/geometric/planners/est/EST.h
copy to src/ompl/geometric/planners/est/ProjEST.h
index 0426f3d..1c87ce8 100644
--- a/src/ompl/geometric/planners/est/EST.h
+++ b/src/ompl/geometric/planners/est/ProjEST.h
@@ -34,8 +34,8 @@
/* Author: Ioan Sucan */
-#ifndef OMPL_GEOMETRIC_PLANNERS_EST_EST_
-#define OMPL_GEOMETRIC_PLANNERS_EST_EST_
+#ifndef OMPL_GEOMETRIC_PLANNERS_EST_PROJEST_
+#define OMPL_GEOMETRIC_PLANNERS_EST_PROJEST_
#include "ompl/datastructures/Grid.h"
#include "ompl/geometric/planners/PlannerIncludes.h"
@@ -50,12 +50,12 @@ namespace ompl
{
/**
- @anchor gEST
+ @anchor gProjEST
@par Short description
- EST is a tree-based motion planner that attempts to detect
+ ProjEST is a tree-based motion planner that attempts to detect
the less explored area of the space through the use of a
grid imposed on a projection of the state space. Using this
- information, EST continues tree expansion primarily from
+ information, ProjEST continues tree expansion primarily from
less explored areas. It is important to set the projection
the algorithm uses (setProjectionEvaluator() function). If
no projection is set, the planner will attempt to use the
@@ -70,14 +70,14 @@ namespace ompl
*/
/** \brief Expansive Space Trees */
- class EST : public base::Planner
+ class ProjEST : public base::Planner
{
public:
/** \brief Constructor */
- EST(const base::SpaceInformationPtr &si);
+ ProjEST(const base::SpaceInformationPtr &si);
- virtual ~EST();
+ virtual ~ProjEST();
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
@@ -148,12 +148,12 @@ namespace ompl
{
public:
- Motion() : state(NULL), parent(NULL)
+ Motion() : state(nullptr), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
{
}
diff --git a/src/ompl/geometric/planners/est/src/BiEST.cpp b/src/ompl/geometric/planners/est/src/BiEST.cpp
new file mode 100644
index 0000000..3305314
--- /dev/null
+++ b/src/ompl/geometric/planners/est/src/BiEST.cpp
@@ -0,0 +1,317 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Rice University
+* 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 Rice University 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: Ryan Luna */
+
+#include "ompl/geometric/planners/est/BiEST.h"
+#include "ompl/base/goals/GoalSampleableRegion.h"
+#include "ompl/tools/config/SelfConfig.h"
+#include <limits>
+#include <cassert>
+
+ompl::geometric::BiEST::BiEST(const base::SpaceInformationPtr &si) : base::Planner(si, "BiEST")
+{
+ specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
+ specs_.directed = true;
+ maxDistance_ = 0.0;
+ connectionPoint_ = std::make_pair<ompl::base::State*, ompl::base::State*>(NULL, NULL);
+
+ Planner::declareParam<double>("range", this, &BiEST::setRange, &BiEST::getRange, "0.:1.:10000.");
+}
+
+ompl::geometric::BiEST::~BiEST()
+{
+ freeMemory();
+}
+
+void ompl::geometric::BiEST::setup()
+{
+ Planner::setup();
+
+ if (maxDistance_ < 1e-3)
+ {
+ tools::SelfConfig sc(si_, getName());
+ sc.configurePlannerRange(maxDistance_);
+
+ // Make the neighborhood radius smaller than sampling range to
+ // keep probabilities relatively high for rejection sampling
+ nbrhoodRadius_ = maxDistance_ / 3.0;
+ }
+
+ if (!nnStart_)
+ nnStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
+ if (!nnGoal_)
+ nnGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
+ nnStart_->setDistanceFunction(std::bind(&BiEST::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
+ nnGoal_->setDistanceFunction(std::bind(&BiEST::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
+}
+
+void ompl::geometric::BiEST::clear()
+{
+ Planner::clear();
+ sampler_.reset();
+ freeMemory();
+ if (nnStart_)
+ nnStart_->clear();
+ if (nnGoal_)
+ nnGoal_->clear();
+
+ startMotions_.clear();
+ startPdf_.clear();
+
+ goalMotions_.clear();
+ goalPdf_.clear();
+
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+}
+
+void ompl::geometric::BiEST::freeMemory()
+{
+ for(size_t i = 0; i < startMotions_.size(); ++i)
+ {
+ if (startMotions_[i]->state)
+ si_->freeState(startMotions_[i]->state);
+ delete startMotions_[i];
+ }
+
+ for(size_t i = 0; i < goalMotions_.size(); ++i)
+ {
+ if (goalMotions_[i]->state)
+ si_->freeState(goalMotions_[i]->state);
+ delete goalMotions_[i];
+ }
+}
+
+ompl::base::PlannerStatus ompl::geometric::BiEST::solve(const base::PlannerTerminationCondition &ptc)
+{
+ checkValidity();
+ base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
+
+ if (!goal)
+ {
+ OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
+ return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
+ }
+
+ std::vector<Motion*> neighbors;
+
+ while (const base::State *st = pis_.nextStart())
+ {
+ Motion *motion = new Motion(si_);
+ si_->copyState(motion->state, st);
+ motion->root = motion->state;
+
+ nnStart_->nearestR(motion, nbrhoodRadius_, neighbors);
+ addMotion(motion, startMotions_, startPdf_, nnStart_, neighbors);
+ }
+
+ if (startMotions_.size() == 0)
+ {
+ OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
+ return base::PlannerStatus::INVALID_START;
+ }
+
+ if (!goal->couldSample())
+ {
+ OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
+ return base::PlannerStatus::INVALID_GOAL;
+ }
+
+ if (!sampler_)
+ sampler_ = si_->allocValidStateSampler();
+
+ OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), startMotions_.size() + goalMotions_.size());
+
+ base::State *xstate = si_->allocState();
+ Motion* xmotion = new Motion();
+
+ bool startTree = true;
+ bool solved = false;
+
+ while (ptc == false && !solved)
+ {
+ // Make sure goal tree has at least one state.
+ if (goalMotions_.size() == 0 || pis_.getSampledGoalsCount() < goalMotions_.size() / 2)
+ {
+ const base::State *st = goalMotions_.size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
+ if (st)
+ {
+ Motion *motion = new Motion(si_);
+ si_->copyState(motion->state, st);
+ motion->root = motion->state;
+
+ nnGoal_->nearestR(motion, nbrhoodRadius_, neighbors);
+ addMotion(motion, goalMotions_, goalPdf_, nnGoal_, neighbors);
+ }
+
+ if (goalMotions_.size() == 0)
+ {
+ OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
+ break;
+ }
+ }
+
+ // Pointers to the tree structure we are expanding
+ std::vector<Motion*>& motions = startTree ? startMotions_ : goalMotions_;
+ PDF<Motion*>& pdf = startTree ? startPdf_ : goalPdf_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn = startTree ? nnStart_ : nnGoal_;
+
+ // Select a state to expand from
+ Motion *existing = pdf.sample(rng_.uniform01());
+ assert(existing);
+
+ // Sample a state in the neighborhood
+ if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
+ continue;
+
+ // Compute neighborhood of candidate state
+ xmotion->state = xstate;
+ nn->nearestR(xmotion, nbrhoodRadius_, neighbors);
+
+ // reject state with probability proportional to neighborhood density
+ if (neighbors.size())
+ {
+ double p = 1.0 - (1.0 / neighbors.size());
+ if (rng_.uniform01() < p)
+ continue;
+ }
+
+ // Is motion good?
+ if (si_->checkMotion(existing->state, xstate))
+ {
+ // create a motion
+ Motion *motion = new Motion(si_);
+ si_->copyState(motion->state, xstate);
+ motion->parent = existing;
+ motion->root = existing->root;
+
+ // add it to everything
+ addMotion(motion, motions, pdf, nn, neighbors);
+
+ // try to connect this state to the other tree
+ // Get all states in the other tree within a maxDistance_ ball (bigger than "neighborhood" ball)
+ startTree ? nnGoal_->nearestR(motion, maxDistance_, neighbors) : nnStart_->nearestR(motion, maxDistance_, neighbors);
+ for(size_t i = 0; i < neighbors.size() && !solved; ++i)
+ {
+ if (goal->isStartGoalPairValid(motion->root, neighbors[i]->root) &&
+ si_->checkMotion(motion->state, neighbors[i]->state)) // win! solution found.
+ {
+ connectionPoint_ = std::make_pair(motion->state, neighbors[i]->state);
+
+ Motion* startMotion = startTree ? motion : neighbors[i];
+ Motion* goalMotion = startTree ? neighbors[i] : motion;
+
+ Motion *solution = startMotion;
+ std::vector<Motion*> mpath1;
+ while (solution != NULL)
+ {
+ mpath1.push_back(solution);
+ solution = solution->parent;
+ }
+
+ solution = goalMotion;
+ std::vector<Motion*> mpath2;
+ while (solution != NULL)
+ {
+ mpath2.push_back(solution);
+ solution = solution->parent;
+ }
+
+ PathGeometric *path = new PathGeometric(si_);
+ path->getStates().reserve(mpath1.size() + mpath2.size());
+ for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
+ path->append(mpath1[i]->state);
+ for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
+ path->append(mpath2[i]->state);
+
+ pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
+ solved = true;
+ }
+ }
+ }
+
+ // swap trees for next iteration
+ startTree = !startTree;
+ }
+
+ si_->freeState(xstate);
+ delete xmotion;
+
+ OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), startMotions_.size() + goalMotions_.size(), startMotions_.size(), goalMotions_.size());
+ return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
+}
+
+void ompl::geometric::BiEST::addMotion(Motion* motion, std::vector<Motion*>& motions,
+ PDF<Motion*>& pdf, std::shared_ptr< NearestNeighbors<Motion*> > nn,
+ const std::vector<Motion*>& neighbors)
+{
+ // Updating neighborhood size counts
+ for(size_t i = 0; i < neighbors.size(); ++i)
+ {
+ PDF<Motion*>::Element *elem = neighbors[i]->element;
+ double w = pdf.getWeight(elem);
+ pdf.update(elem, w / (w + 1.));
+ }
+
+ motion->element = pdf.add(motion, 1. / (neighbors.size() + 1.)); // +1 for self
+ motions.push_back(motion);
+ nn->add(motion);
+}
+
+void ompl::geometric::BiEST::getPlannerData(base::PlannerData &data) const
+{
+ Planner::getPlannerData(data);
+
+ for (unsigned int i = 0 ; i < startMotions_.size() ; ++i)
+ {
+ if (startMotions_[i]->parent == NULL)
+ data.addStartVertex(base::PlannerDataVertex(startMotions_[i]->state, 1));
+ else
+ data.addEdge(base::PlannerDataVertex(startMotions_[i]->parent->state, 1),
+ base::PlannerDataVertex(startMotions_[i]->state, 1));
+ }
+
+ for (unsigned int i = 0 ; i < goalMotions_.size() ; ++i)
+ {
+ if (goalMotions_[i]->parent == NULL)
+ data.addGoalVertex(base::PlannerDataVertex(goalMotions_[i]->state, 2));
+ else
+ // The edges in the goal tree are reversed to be consistent with start tree
+ data.addEdge(base::PlannerDataVertex(goalMotions_[i]->state, 2),
+ base::PlannerDataVertex(goalMotions_[i]->parent->state, 2));
+ }
+
+ // Add the edge connecting the two trees
+ data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
+}
diff --git a/src/ompl/geometric/planners/est/src/EST.cpp b/src/ompl/geometric/planners/est/src/EST.cpp
index 6341a47..ee06fd5 100644
--- a/src/ompl/geometric/planners/est/src/EST.cpp
+++ b/src/ompl/geometric/planners/est/src/EST.cpp
@@ -1,7 +1,7 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
-* Copyright (c) 2008, Willow Garage, Inc.
+* Copyright (c) 2015, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
* 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
+* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/* Author: Ioan Sucan */
+/* Author: Ryan Luna */
#include "ompl/geometric/planners/est/EST.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
@@ -61,10 +61,14 @@ void ompl::geometric::EST::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
- sc.configureProjectionEvaluator(projectionEvaluator_);
sc.configurePlannerRange(maxDistance_);
- tree_.grid.setDimension(projectionEvaluator_->getDimension());
+ // Make the neighborhood radius smaller than sampling range to keep probabilities relatively high for rejection sampling
+ nbrhoodRadius_ = maxDistance_ / 3.0;
+
+ if (!nn_)
+ nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
+ nn_->setDistanceFunction(std::bind(&EST::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::EST::clear()
@@ -72,22 +76,21 @@ void ompl::geometric::EST::clear()
Planner::clear();
sampler_.reset();
freeMemory();
- tree_.grid.clear();
- tree_.size = 0;
+ if (nn_)
+ nn_->clear();
+
+ motions_.clear();
pdf_.clear();
lastGoalMotion_ = NULL;
}
void ompl::geometric::EST::freeMemory()
{
- for (Grid<MotionInfo>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
+ for(size_t i = 0; i < motions_.size(); ++i)
{
- for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
- {
- if (it->second->data[i]->state)
- si_->freeState(it->second->data[i]->state);
- delete it->second->data[i];
- }
+ if (motions_[i]->state)
+ si_->freeState(motions_[i]->state);
+ delete motions_[i];
}
}
@@ -97,14 +100,18 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
+ std::vector<Motion*> neighbors;
+
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
- addMotion(motion);
+
+ nn_->nearestR(motion, nbrhoodRadius_, neighbors);
+ addMotion(motion, neighbors);
}
- if (tree_.grid.size() == 0)
+ if (motions_.size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
@@ -113,34 +120,60 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
- OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
+ OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), motions_.size());
Motion *solution = NULL;
Motion *approxsol = NULL;
double approxdif = std::numeric_limits<double>::infinity();
base::State *xstate = si_->allocState();
+ Motion* xmotion = new Motion();
while (ptc == false)
{
- /* Decide on a state to expand from */
- Motion *existing = selectMotion();
+ // Select a state to expand from
+ Motion *existing = pdf_.sample(rng_.uniform01());
assert(existing);
- /* sample random state (with goal biasing) */
+ // Sample random state in the neighborhood (with goal biasing)
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
+ {
goal_s->sampleGoal(xstate);
+
+ // Compute neighborhood of candidate motion
+ xmotion->state = xstate;
+ nn_->nearestR(xmotion, nbrhoodRadius_, neighbors);
+ }
else
+ {
+ // Sample a state in the neighborhood
if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
continue;
+ // Compute neighborhood of candidate state
+ xmotion->state = xstate;
+ nn_->nearestR(xmotion, nbrhoodRadius_, neighbors);
+
+ // reject state with probability proportional to neighborhood density
+ if (neighbors.size())
+ {
+ double p = 1.0 - (1.0 / neighbors.size());
+ if (rng_.uniform01() < p)
+ continue;
+ }
+ }
+
+ // Is motion good?
if (si_->checkMotion(existing->state, xstate))
{
- /* create a motion */
+ // create a motion
Motion *motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
- addMotion(motion);
+ // add it to everything
+ addMotion(motion, neighbors);
+
+ // done?
double dist = 0.0;
bool solved = goal->isSatisfied(motion->state, &dist);
if (solved)
@@ -169,7 +202,7 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
{
lastGoalMotion_ = solution;
- /* construct the solution path */
+ // construct the solution path
std::vector<Motion*> mpath;
while (solution != NULL)
{
@@ -177,7 +210,7 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
solution = solution->parent;
}
- /* set the solution path */
+ // set the solution path
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
@@ -186,55 +219,42 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
}
si_->freeState(xstate);
+ delete xmotion;
- OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size());
+ OMPL_INFORM("%s: Created %u states", getName().c_str(), motions_.size());
return base::PlannerStatus(solved, approximate);
}
-ompl::geometric::EST::Motion* ompl::geometric::EST::selectMotion()
+void ompl::geometric::EST::addMotion(Motion *motion, const std::vector<Motion*>& neighbors)
{
- GridCell* cell = pdf_.sample(rng_.uniform01());
- return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
-}
-
-void ompl::geometric::EST::addMotion(Motion *motion)
-{
- Grid<MotionInfo>::Coord coord;
- projectionEvaluator_->computeCoordinates(motion->state, coord);
- GridCell* cell = tree_.grid.getCell(coord);
- if (cell)
- {
- cell->data.push_back(motion);
- pdf_.update(cell->data.elem_, 1.0/cell->data.size());
- }
- else
+ // Updating neighborhood size counts
+ for(size_t i = 0; i < neighbors.size(); ++i)
{
- cell = tree_.grid.createCell(coord);
- cell->data.push_back(motion);
- tree_.grid.add(cell);
- cell->data.elem_ = pdf_.add(cell, 1.0);
+ PDF<Motion*>::Element *elem = neighbors[i]->element;
+ double w = pdf_.getWeight(elem);
+ pdf_.update(elem, w / (w + 1.));
}
- tree_.size++;
+
+ // now add new motion to the data structures
+ motion->element = pdf_.add(motion, 1. / (neighbors.size() + 1.)); // +1 for self
+ motions_.push_back(motion);
+ nn_->add(motion);
}
void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
- std::vector<MotionInfo> motions;
- tree_.grid.getContent(motions);
-
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
- for (unsigned int i = 0 ; i < motions.size() ; ++i)
- for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
- {
- if (motions[i][j]->parent == NULL)
- data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state));
- else
- data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state),
- base::PlannerDataVertex(motions[i][j]->state));
- }
+ for (unsigned int i = 0 ; i < motions_.size() ; ++i)
+ {
+ if (motions_[i]->parent == NULL)
+ data.addStartVertex(base::PlannerDataVertex(motions_[i]->state));
+ else
+ data.addEdge(base::PlannerDataVertex(motions_[i]->parent->state),
+ base::PlannerDataVertex(motions_[i]->state));
+ }
}
diff --git a/src/ompl/geometric/planners/est/src/EST.cpp b/src/ompl/geometric/planners/est/src/ProjEST.cpp
similarity index 85%
copy from src/ompl/geometric/planners/est/src/EST.cpp
copy to src/ompl/geometric/planners/est/src/ProjEST.cpp
index 6341a47..9d4139f 100644
--- a/src/ompl/geometric/planners/est/src/EST.cpp
+++ b/src/ompl/geometric/planners/est/src/ProjEST.cpp
@@ -34,30 +34,30 @@
/* Author: Ioan Sucan */
-#include "ompl/geometric/planners/est/EST.h"
+#include "ompl/geometric/planners/est/ProjEST.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
#include <limits>
#include <cassert>
-ompl::geometric::EST::EST(const base::SpaceInformationPtr &si) : base::Planner(si, "EST")
+ompl::geometric::ProjEST::ProjEST(const base::SpaceInformationPtr &si) : base::Planner(si, "ProjEST")
{
specs_.approximateSolutions = true;
specs_.directed = true;
goalBias_ = 0.05;
maxDistance_ = 0.0;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
- Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange, "0.:1.:10000.");
- Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias, "0.:.05:1.");
+ Planner::declareParam<double>("range", this, &ProjEST::setRange, &ProjEST::getRange, "0.:1.:10000.");
+ Planner::declareParam<double>("goal_bias", this, &ProjEST::setGoalBias, &ProjEST::getGoalBias, "0.:.05:1.");
}
-ompl::geometric::EST::~EST()
+ompl::geometric::ProjEST::~ProjEST()
{
freeMemory();
}
-void ompl::geometric::EST::setup()
+void ompl::geometric::ProjEST::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
@@ -67,7 +67,7 @@ void ompl::geometric::EST::setup()
tree_.grid.setDimension(projectionEvaluator_->getDimension());
}
-void ompl::geometric::EST::clear()
+void ompl::geometric::ProjEST::clear()
{
Planner::clear();
sampler_.reset();
@@ -75,10 +75,10 @@ void ompl::geometric::EST::clear()
tree_.grid.clear();
tree_.size = 0;
pdf_.clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
-void ompl::geometric::EST::freeMemory()
+void ompl::geometric::ProjEST::freeMemory()
{
for (Grid<MotionInfo>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
{
@@ -91,7 +91,7 @@ void ompl::geometric::EST::freeMemory()
}
}
-ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTerminationCondition &ptc)
+ompl::base::PlannerStatus ompl::geometric::ProjEST::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
@@ -115,8 +115,8 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
- Motion *solution = NULL;
- Motion *approxsol = NULL;
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
base::State *xstate = si_->allocState();
@@ -159,19 +159,19 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
bool solved = false;
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -192,13 +192,13 @@ ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTermina
return base::PlannerStatus(solved, approximate);
}
-ompl::geometric::EST::Motion* ompl::geometric::EST::selectMotion()
+ompl::geometric::ProjEST::Motion* ompl::geometric::ProjEST::selectMotion()
{
GridCell* cell = pdf_.sample(rng_.uniform01());
- return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
+ return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : nullptr;
}
-void ompl::geometric::EST::addMotion(Motion *motion)
+void ompl::geometric::ProjEST::addMotion(Motion *motion)
{
Grid<MotionInfo>::Coord coord;
projectionEvaluator_->computeCoordinates(motion->state, coord);
@@ -218,7 +218,7 @@ void ompl::geometric::EST::addMotion(Motion *motion)
tree_.size++;
}
-void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
+void ompl::geometric::ProjEST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
@@ -231,7 +231,7 @@ void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
{
- if (motions[i][j]->parent == NULL)
+ if (motions[i][j]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state),
diff --git a/src/ompl/geometric/planners/experience/LightningRetrieveRepair.h b/src/ompl/geometric/planners/experience/LightningRetrieveRepair.h
index 0b72e77..e44b613 100644
--- a/src/ompl/geometric/planners/experience/LightningRetrieveRepair.h
+++ b/src/ompl/geometric/planners/experience/LightningRetrieveRepair.h
@@ -58,7 +58,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::LightningRetrieveRepairPtr
- \brief A boost shared pointer wrapper for ompl::base::LightningRetrieveRepair */
+ \brief A shared pointer wrapper for ompl::base::LightningRetrieveRepair */
/**
@anchor LightningRetrieveRepair - Lightning Retrieve and Repair
diff --git a/src/ompl/geometric/planners/experience/ThunderRetrieveRepair.h b/src/ompl/geometric/planners/experience/ThunderRetrieveRepair.h
index ad0323b..e6d738e 100644
--- a/src/ompl/geometric/planners/experience/ThunderRetrieveRepair.h
+++ b/src/ompl/geometric/planners/experience/ThunderRetrieveRepair.h
@@ -58,7 +58,7 @@ namespace ompl
/// @endcond
/** \class ompl::base::ThunderRetrieveRepairPtr
- \brief A boost shared pointer wrapper for ompl::base::ThunderRetrieveRepair */
+ \brief A shared pointer wrapper for ompl::base::ThunderRetrieveRepair */
/**
@anchor ThunderRetrieveRepair
diff --git a/src/ompl/geometric/planners/experience/src/LightningRetrieveRepair.cpp b/src/ompl/geometric/planners/experience/src/LightningRetrieveRepair.cpp
index 2aeca3e..efda29b 100644
--- a/src/ompl/geometric/planners/experience/src/LightningRetrieveRepair.cpp
+++ b/src/ompl/geometric/planners/experience/src/LightningRetrieveRepair.cpp
@@ -42,7 +42,7 @@
#include "ompl/tools/config/MagicConstants.h"
#include "ompl/tools/lightning/LightningDB.h"
-#include <boost/thread.hpp>
+#include <thread>
#include <limits>
@@ -339,7 +339,7 @@ bool ompl::geometric::LightningRetrieveRepair::findBestPath(const base::State *s
return true;
}
-bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
+bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
ompl::geometric::PathGeometric &primaryPath)
{
// \todo: we should reuse our collision checking from the previous step to make this faster
diff --git a/src/ompl/geometric/planners/experience/src/ThunderRetrieveRepair.cpp b/src/ompl/geometric/planners/experience/src/ThunderRetrieveRepair.cpp
index df660ab..22f5357 100644
--- a/src/ompl/geometric/planners/experience/src/ThunderRetrieveRepair.cpp
+++ b/src/ompl/geometric/planners/experience/src/ThunderRetrieveRepair.cpp
@@ -43,7 +43,7 @@
#include <ompl/tools/thunder/ThunderDB.h>
#include "ompl/tools/config/MagicConstants.h"
-#include <boost/thread.hpp>
+#include <thread>
#include <limits>
@@ -105,7 +105,7 @@ void ThunderRetrieveRepair::setup(void)
if (!repairPlanner_)
{
// Set the repair planner
- boost::shared_ptr<RRTConnect> repair_planner( new RRTConnect( si_ ) );
+ std::shared_ptr<RRTConnect> repair_planner( new RRTConnect( si_ ) );
OMPL_DEBUG("No repairing planner specified. Using default: %s", repair_planner->getName().c_str() );
repairPlanner_ = repair_planner; //Planner( repair_planer );
@@ -171,7 +171,7 @@ base::PlannerStatus ThunderRetrieveRepair::solve(const base::PlannerTerminationC
//ompl::geometric::PathGeometric pg = candidateSolution.getGeometricPath(); // TODO do not copy to new type
path_simplifier_->simplify(candidateSolution.getGeometricPath(), ptc);
double simplifyTime = time::seconds(time::now() - simplifyStart);
- OMPL_INFORM("ThunderRetrieveRepair: Path simplification took %f seconds and removed %d states",
+ OMPL_INFORM("ThunderRetrieveRepair: Path simplification took %f seconds and removed %d states",
simplifyTime, numStates - candidateSolution.getGeometricPath().getStateCount());
}
diff --git a/src/ompl/geometric/planners/fmt/BFMT.h b/src/ompl/geometric/planners/fmt/BFMT.h
new file mode 100644
index 0000000..b2d61b8
--- /dev/null
+++ b/src/ompl/geometric/planners/fmt/BFMT.h
@@ -0,0 +1,624 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2013, Autonomous Systems Laboratory, Stanford University
+* 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 Stanford University 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.
+*********************************************************************/
+
+/* Authors: Joseph Starek (Stanford) */
+/* Co-developers: Javier V Gomez (UC3M)*/
+/* Algorithm design: Joseph Starek (Stanford), Ed Schmerling (Stanford), Lucas Janson (Stanford) and Marco Pavone (Stanford) */
+/* Acknowledgements for insightful comments: Ashley Clark (Stanford) */
+
+#ifndef OMPL_GEOMETRIC_PLANNERS_BIDIRECTIONALFMT_H
+#define OMPL_GEOMETRIC_PLANNERS_BIDIRECTIONALFMT_H
+
+#include <ompl/geometric/planners/PlannerIncludes.h>
+#include <ompl/base/goals/GoalSampleableRegion.h>
+#include <ompl/datastructures/NearestNeighbors.h>
+#include <ompl/datastructures/BinaryHeap.h>
+#include <ompl/base/OptimizationObjective.h>
+#include <map>
+
+namespace ompl
+{
+
+ namespace geometric
+ {
+
+ /**
+ @anchor gBFMT
+ @par Short description
+ \ref gBFMT "BFMT*" is an asymptotically-optimal, bidirectional sampling-based
+ motion planning algorithm, which is guaranteed to converge to a shortest
+ path solution. The algorithm is specifically aimed at solving complex
+ motion planning problems in high-dimensional configuration spaces.
+ The \ref gBFMT "BFMT*" algorithm essentially performs a lazy dynamic
+ programming recursion on a set of probabilistically-drawn samples to
+ grow two trees of paths, which moves steadily outward in cost-to-come space,
+ one from the start state and the other one from the goal state.
+
+ @par Deviation from the paper
+ The implementation includes a cache in the collision checking since the original
+ algorithm could check the same collision more than once. It increases the
+ memory requirements to O(n logn), but as samples tend to infinity this
+ bound tend to O(n).
+
+ @par External documentation
+ J. A. Starek, J. V. Gomez, E. Schmerling, L. Janson, L. Moreno, and M. Pavone,
+ An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional Motion Planning,
+ inIEEE/RSJ International Conference on Intelligent Robots Systems, 2015.
+ [[PDF]](http://arxiv.org/pdf/1507.07602.pdf)
+ */
+ /** @brief Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed
+ by J. Starek, J.V. Gomez, et al. */
+ class BFMT : public ompl::base::Planner {
+ public:
+
+ /** \brief Tree identifier */
+ enum TreeType { FWD = 0, REV = 1 };
+
+ /** \brief Exploration strategy identifier */
+ enum ExploreType { SWAP_EVERY_TIME = 0, CHOOSE_SMALLEST_Z = 1 };
+
+ /** \brief Termination strategy identifier */
+ enum TerminateType { FEASIBILITY = 0, OPTIMALITY = 1 };
+
+ BFMT(const base::SpaceInformationPtr &si);
+
+ virtual ~BFMT();
+
+ virtual void setup(void);
+
+ virtual base::PlannerStatus solve(const base::PlannerTerminationCondition& ptc);
+
+ virtual void clear();
+
+ virtual void getPlannerData(base::PlannerData &data) const;
+
+ /** \brief Set the number of states that the planner should sample.
+ The planner will sample this number of states in addition to the
+ initial states. If any of the goal states are not reachable from
+ the randomly sampled states, those goal states will also be
+ added. The default value is 1000 */
+ void setNumSamples(const unsigned int numSamples)
+ {
+ numSamples_ = numSamples;
+ }
+
+ /** \brief Get the number of states that the planner will sample */
+ unsigned int getNumSamples() const
+ {
+ return numSamples_;
+ }
+
+ /** \brief If nearestK is true, FMT will be run using the Knearest strategy */
+ void setNearestK(bool nearestK)
+ {
+ nearestK_ = nearestK;
+ }
+
+ /** \brief Get the state of the nearestK strategy */
+ bool getNearestK() const
+ {
+ return nearestK_;
+ }
+
+ /** \brief The planner searches for neighbors of a node within a
+ cost r, where r is the value described for BFMT* in Section 4
+ of [L. Janson, E. Schmerling, A. Clark, M. Pavone. Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. The International Journal of Robotics Research, 34(7):883-921, 2015](http://arxiv.org/pdf/1306.3532.pdf). For guaranteed asymptotic
+ convergence, the user should choose a constant multiplier for
+ the search radius that is greater than one. The default value is 1.1.
+ In general, a radius multiplier between 0.9 and 5 appears to
+ perform the best */
+ void setRadiusMultiplier(const double radiusMultiplier)
+ {
+ if (radiusMultiplier <= 0.0)
+ throw Exception("Radius multiplier must be greater than zero");
+ radiusMultiplier_ = radiusMultiplier;
+ }
+
+ /** \brief Get the multiplier used for the nearest neighbors search
+ radius */
+ double getRadiusMultiplier() const
+ {
+ return radiusMultiplier_;
+ }
+
+ /** \brief Store the volume of the obstacle-free configuration space.
+ If no value is specified, the default assumes an obstacle-free
+ unit hypercube, freeSpaceVolume = (maximumExtent/sqrt(dimension))^(dimension) */
+ void setFreeSpaceVolume(const double freeSpaceVolume)
+ {
+ if (freeSpaceVolume < 0.0)
+ throw Exception("Free space volume should be greater than zero");
+ freeSpaceVolume_ = freeSpaceVolume;
+ }
+
+ /** \brief Get the volume of the free configuration space that is
+ being used by the planner */
+ double getFreeSpaceVolume() const
+ {
+ return freeSpaceVolume_;
+ }
+
+ /** \brief Sets the collision check caching to save calls to the collision
+ checker with slightly memory usage as a counterpart */
+ void setCacheCC(bool ccc)
+ {
+ cacheCC_ = ccc;
+ }
+
+ /** \brief Get the state of the collision check caching */
+ bool getCacheCC() const
+ {
+ return cacheCC_;
+ }
+
+ /** \brief Activates the cost to go heuristics when ordering the heap */
+ void setHeuristics(bool h)
+ {
+ heuristics_ = h;
+ }
+
+ /** \brief Returns true if the heap is ordered taking into account
+ cost to go heuristics */
+ bool getHeuristics() const
+ {
+ return heuristics_;
+ }
+
+ /** \brief Activates the extended FMT*: adding new samples if planner does not finish successfully. */
+ void setExtendedFMT(bool e)
+ {
+ extendedFMT_ = e;
+ }
+
+ /** \brief Returns true if the extended FMT* is activated. */
+ bool getExtendedFMT() const
+ {
+ return extendedFMT_;
+ }
+
+ /** \brief Sets exploration strategy: balanced true expands one tree every iteration.
+ False will select the tree with lowest maximum cost to go. */
+ void setExploration(bool balanced)
+ {
+ exploration_ = SWAP_EVERY_TIME;
+ if (balanced) {
+ exploration_ = CHOOSE_SMALLEST_Z;
+ }
+ }
+
+ /** \brief Returns the exploration strategy. */
+ bool getExploration() const
+ {
+ return (exploration_ == CHOOSE_SMALLEST_Z);
+ }
+
+ /** \brief Sets the termination strategy: optimality true finishes when the best
+ possible path is found. Otherwise, the algorithm will finish when the first
+ feasible path is found. */
+ void setTermination(bool optimality)
+ {
+ termination_ = FEASIBILITY;
+ if (optimality) {
+ termination_ = OPTIMALITY;
+ }
+ }
+
+ /** \brief Returns the termination strategy. */
+ bool getTermination() const
+ {
+ return (termination_ == OPTIMALITY);
+ }
+
+ /** \brief Sets Nearest Neighbors precomputation. Currently, it precomputes
+ once solve() has been called. */
+ void setPrecomputeNN(bool p)
+ {
+ precomputeNN_ = p;
+ }
+
+ /** \brief Returns true if Nearest Neighbor precomputation is done. */
+ bool setPrecomputeNN() const
+ {
+ return precomputeNN_;
+ }
+
+ /** \brief Representation of a bidirectional motion. */
+ class BiDirMotion {
+ public:
+
+ /** \brief The FMT* planner begins with all nodes included in
+ set Unvisited "Waiting for optimal connection". As nodes are
+ connected to the tree, they are transferred into set Open
+ "Horizon of explored tree." Once a node in Open is no longer
+ close enough to the frontier to connect to any more nodes in
+ Unvisited, it is removed from Open. These three SetTypes are flags
+ indicating which set the node belongs to; Open, Unvisited, or Closed (neither) */
+ enum SetType { SET_CLOSED, SET_OPEN, SET_UNVISITED };
+
+ BiDirMotion(TreeType* tree)
+ : state_(NULL), tree_(tree)
+ {
+ parent_[FWD] = NULL;
+ parent_[REV] = NULL;
+ cost_[FWD] = base::Cost(0.0);
+ cost_[REV] = base::Cost(0.0);
+ hcost_[FWD] = base::Cost(0.0);
+ hcost_[REV] = base::Cost(0.0);
+ currentSet_[FWD] = SET_UNVISITED;
+ currentSet_[REV] = SET_UNVISITED;
+ }
+
+ /** \brief Constructor that allocates memory for the state */
+ BiDirMotion(const base::SpaceInformationPtr &si, TreeType* tree)
+ : state_(si->allocState()), tree_(tree)
+ {
+ parent_[FWD] = NULL;
+ parent_[REV] = NULL;
+ cost_[FWD] = base::Cost(0.0);
+ cost_[REV] = base::Cost(0.0);
+ hcost_[FWD] = base::Cost(0.0);
+ hcost_[REV] = base::Cost(0.0);
+ currentSet_[FWD] = SET_UNVISITED;
+ currentSet_[REV] = SET_UNVISITED;
+ }
+
+ typedef std::vector<BiDirMotion*> BiDirMotionPtrs;
+
+ /** \brief The state contained by the motion */
+ base::State *state_;
+
+ /** \brief The parent motion in the exploration tree */
+ BiDirMotion* parent_[2];
+
+ /** \brief The set of motions descending from the current motion */
+ BiDirMotionPtrs children_[2];
+
+ /** \brief Current set in which the motion is included. */
+ SetType currentSet_[2];
+
+ /** \brief Tree identifier */
+ TreeType* tree_;
+
+ /** \brief The cost of this motion */
+ base::Cost cost_[2];
+
+ /** \brief The minimum cost to go of this motion (heuristically computed) */
+ base::Cost hcost_[2];
+
+ /** \brief Contains the connections attempted FROM this node */
+ std::set<BiDirMotion *> collChecksDone_;
+
+ /** \brief Set the state associated with the motion */
+ inline base::Cost getCost(void) const
+ {
+ return this->cost_[*tree_];
+ }
+
+ /** \brief Get cost of this motion in the inactive tree */
+ inline base::Cost getOtherCost(void) const
+ {
+ return this->cost_[(*tree_+1) % 2];
+
+ }
+
+ /** \brief Set the cost of the motion */
+ inline void setCost(base::Cost cost)
+ {
+ this->cost_[*tree_] = cost;
+ }
+
+ /** \brief Set the parent of the motion */
+ inline void setParent(BiDirMotion* parent)
+ {
+ this->parent_[*tree_] = parent;
+ }
+
+ /** \brief Get the parent of the motion */
+ inline BiDirMotion* getParent(void) const
+ {
+ return this->parent_[*tree_];
+ }
+
+ /** \brief Set the children of the motion */
+ inline void setChildren(BiDirMotionPtrs children)
+ {
+ this->children_[*tree_] = children;
+ }
+
+ /** \brief Get the children of the motion */
+ inline BiDirMotionPtrs getChildren(void) const
+ {
+ return this->children_[*tree_];
+ }
+
+ /** \brief Set the current set of the motion */
+ inline void setCurrentSet(SetType set)
+ {
+ this->currentSet_[*tree_] = set;
+ }
+
+ /** \brief Fet the current set of the motion */
+ inline SetType getCurrentSet(void) const
+ {
+ return this->currentSet_[*tree_];
+ }
+
+ /** \brief Get set of this motion in the inactive tree */
+ inline SetType getOtherSet(void) const
+ {
+ return this->currentSet_[(*tree_+1) % 2];
+ }
+
+ /** \brief Set tree identifier for this motion */
+ inline void setTreeType(TreeType* treePtr)
+ {
+ this->tree_ = treePtr;
+ }
+
+ /** \brief Get tree identifier for this motion */
+ inline TreeType getTreeType(void) const
+ {
+ return *tree_;
+ }
+
+ /** \brief Set the state associated with the motion */
+ void setState(base::State *state) {
+ state_ = state;
+ }
+
+ /** \brief Get the state associated with the motion */
+ base::State* getState() const {
+ return state_;
+ }
+
+ /** \brief Returns true if the connection to m has been already
+ tested and failed because of a collision */
+ bool alreadyCC(BiDirMotion *m)
+ {
+ if (collChecksDone_.find(m) == collChecksDone_.end())
+ return false;
+ return true;
+ }
+
+ /** \brief Caches a failed collision check to m */
+ void addCC(BiDirMotion *m)
+ {
+ collChecksDone_.insert(m);
+ }
+
+ /** \brief Set the cost to go heuristic cost */
+ void setHeuristicCost(const base::Cost h)
+ {
+ hcost_[*tree_] = h;
+ }
+
+ /** \brief Get the cost to go heuristic cost */
+ base::Cost getHeuristicCost() const
+ {
+ return hcost_[*tree_];
+ }
+ };
+
+ typedef std::vector<BiDirMotion*> BiDirMotionPtrs;
+
+ protected:
+
+ /** \brief Comparator used to order motions in a binary heap */
+ struct BiDirMotionCompare {
+ bool operator()(const BiDirMotion* p1, const BiDirMotion* p2) const {
+ if (heuristics_)
+ return ( opt_->combineCosts(p1->getCost(), p1->getHeuristicCost()).value() < opt_->combineCosts(p2->getCost(), p2->getHeuristicCost()).value() );
+ else
+ return (p1->getCost().value() < p2->getCost().value());
+ }
+
+ base::OptimizationObjective* opt_;
+ bool heuristics_;
+ };
+
+ typedef ompl::BinaryHeap<BiDirMotion*, BiDirMotionCompare> BiDirMotionBinHeap;
+
+ /** \brief Change the active tree */
+ void swapTrees();
+
+ /** \brief Sets forward tree active */
+ void useFwdTree()
+ {
+ tree_ = FWD;
+ }
+
+ /** \brief Sets reverse tree active */
+ void useRevTree()
+ {
+ tree_ = REV;
+ }
+
+ /** \brief Compute the distance between two motions as the cost
+ between their contained states. Note that for computationally
+ intensive cost functions, the cost between motions should be
+ stored to avoid duplicate calculations */
+ double distanceFunction(const BiDirMotion* a, const BiDirMotion* b) const
+ {
+ return opt_->motionCost(a->getState(), b->getState()).value();
+ }
+
+ /** \brief Compute the volume of the unit ball in a given dimension */
+ double calculateUnitBallVolume(const unsigned int dimension) const;
+
+ /** \brief Calculate the radius to use for nearest neighbor searches,
+ using the bound given in [L. Janson, E. Schmerling, A. Clark, M. Pavone. Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. The International Journal of Robotics Research, 34(7):883-921, 2015](http://arxiv.org/pdf/1306.3532.pdf). The radius depends on
+ the radiusMultiplier parameter, the volume of the free
+ configuration space, the volume of the unit ball in the current
+ dimension, and the number of nodes in the graph */
+ double calculateRadius(unsigned int dimension, unsigned int n) const;
+
+ /** \brief Free the memory allocated by this planner */
+ void freeMemory();
+
+ /** \brief Save the neighbors within a neighborhood of a given state. The strategy
+ used (nearestK or nearestR depends on the planner configuration */
+ void saveNeighborhood(std::shared_ptr< NearestNeighbors<BiDirMotion*> > nn, BiDirMotion* m);
+
+ /** \brief Sample a state from the free configuration space and save
+ it into the nearest neighbors data structure */
+ void sampleFree(std::shared_ptr<NearestNeighbors<BiDirMotion*> > nn,
+ const base::PlannerTerminationCondition &ptc );
+
+ /** \brief Carries out some planner checks */
+ void initializeProblem(base::GoalSampleableRegion*& goal_s);
+
+ /** \brief Complete one iteration of the main loop of the BFMT* algorithm:
+ Find K nearest nodes in set Unvisited (or within a radius r) of the node z.
+ Attempt to connect them to their optimal cost-to-come parent
+ in set Open. Remove all newly connected nodes fromUnvisited and insert
+ them into Open. Remove motion z from Open, and update z to be the
+ current lowest cost-to-come node in Open */
+ void expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point);
+
+ /** \brief Executes the actual planning algorithm, swapping and expanding the trees */
+ bool plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&z, const base::PlannerTerminationCondition& ptc);
+
+ /** \brief Checks if the termination condition is met */
+ bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition& ptc);
+
+ /** \brief Chooses and expand a tree according to the exploration strategy */
+ void chooseTreeAndExpansionNode(BiDirMotion *&z);
+
+ /** \brief Trace the path along a tree towards the root (forward or reverse) */
+ void tracePath(BiDirMotion *z, BiDirMotionPtrs& path);
+
+ /** \brief For a motion m, updates the stored neighborhoods of all its neighbors by
+ by inserting m (maintaining the cost-based sorting) */
+ void updateNeighborhood(BiDirMotion *m, const std::vector<BiDirMotion *> nbh);
+
+ /** \brief Extended FMT strategy: inserts a new motion in open if the heap is empty */
+ void insertNewSampleInOpen(const base::PlannerTerminationCondition& ptc);
+
+ /** \brief The number of samples to use when planning */
+ unsigned int numSamples_;
+
+ /** \brief This planner uses a nearest neighbor search radius
+ proportional to the lower bound for optimality derived for FMT*
+ in Section 4 of [L. Janson, E. Schmerling, A. Clark, M. Pavone. Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. The International Journal of Robotics Research, 34(7):883-921, 2015](http://arxiv.org/pdf/1306.3532.pdf). The radius multiplier
+ is the multiplier for the lower bound. For guaranteed asymptotic
+ convergence, the user should choose a multiplier for the search
+ radius that is greater than one. The default value is 1.1.
+ In general, a radius between 0.9 and 5 appears to perform the best */
+ double radiusMultiplier_;
+
+ /** \brief The volume of numSathe free configuration space, computed
+ as an upper bound with 95% confidence */
+ double freeSpaceVolume_;
+
+ /** \brief Number of collision checks performed by the algorithm */
+ unsigned int collisionChecks_;
+
+ /** \brief Flag to activate the K nearest neighbors strategy */
+ bool nearestK_;
+
+ /** \brief Radius employed in the nearestR strategy. */
+ double NNr_;
+
+ /** \brief K used in the nearestK strategy */
+ unsigned int NNk_;
+
+ /** \brief Active tree */
+ TreeType tree_;
+
+ /** \brief Exploration strategy used */
+ ExploreType exploration_;
+
+ /** \brief Termination strategy used */
+ TerminateType termination_;
+
+ /** \brief If true all the nearest neighbors maps are precomputed before solving. */
+ bool precomputeNN_;
+
+ /** \brief A nearest-neighbor datastructure containing the set of all motions */
+ std::shared_ptr< NearestNeighbors<BiDirMotion*> > nn_;
+
+ /** \brief A map linking a motion to all of the motions within a
+ distance r of that motion */
+ std::map<BiDirMotion*, BiDirMotionPtrs > neighborhoods_;
+
+ /** \brief A binary heap for storing explored motions in
+ cost-to-come sorted order. The motions in Open have been explored,
+ yet are still close enough to the frontier of the explored set Open
+ to be connected to nodes in the unexplored set Unvisited */
+ BiDirMotionBinHeap Open_[2];
+
+ /** \brief Map to know the corresponding heap element from the given motion */
+ std::map<BiDirMotion*, BiDirMotionBinHeap::Element*> Open_elements[2];
+
+ /** \brief State sampler */
+ base::StateSamplerPtr sampler_;
+
+ /** \brief The cost objective function */
+ base::OptimizationObjectivePtr opt_;
+
+ /** \brief Flag to activate the cost to go heuristics */
+ bool heuristics_;
+
+ /** \brief Goal state caching to accelerate cost to go heuristic computation */
+ base::State* heurGoalState_[2];
+
+ /** \brief Flag to activate the collision check caching */
+ bool cacheCC_;
+
+ /** \brief Add new samples if the tree was not able to find a solution. */
+ bool extendedFMT_;
+
+ // For sorting a list of costs and getting only their sorted indices
+ struct CostIndexCompare
+ {
+ CostIndexCompare(const std::vector<base::Cost>& costs,
+ const base::OptimizationObjective &opt) :
+ costs_(costs), opt_(opt)
+ {}
+ bool operator()(unsigned i, unsigned j)
+ {
+ return (costs_[i].value() < costs_[j].value());
+ }
+ const std::vector<base::Cost>& costs_;
+ const base::OptimizationObjective &opt_;
+ };
+
+ };
+
+ } // End "geometric" namespace
+} // End "ompl" namespace
+
+
+#endif /* OMPL_GEOMETRIC_PLANNERS_BIDIRECTIONALFMT_H */
+
diff --git a/src/ompl/geometric/planners/fmt/FMT.h b/src/ompl/geometric/planners/fmt/FMT.h
index fdab166..70387f4 100644
--- a/src/ompl/geometric/planners/fmt/FMT.h
+++ b/src/ompl/geometric/planners/fmt/FMT.h
@@ -73,10 +73,18 @@ namespace ompl
memory requirements to O(n logn), but as samples tend to infinity this
bound tend to O(n).
+ It also implements the resampling strategy (extended FMT) included in the
+ BiDirectional FMT* paper.
+
@par External documentation
L. Janson, E. Schmerling, A. Clark, M. Pavone. Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. The International Journal of Robotics Research, 34(7):883-921, 2015.
DOI: [10.1177/0278364915577958](http://dx.doi.org/10.1177/0278364915577958)<br>
[[PDF]](http://arxiv.org/pdf/1306.3532.pdf)
+
+ J. A. Starek, J. V. Gomez, E. Schmerling, L. Janson, L. Moreno, and M. Pavone,
+ An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional Motion Planning,
+ in IEEE/RSJ International Conference on Intelligent Robots Systems, 2015.
+ [[PDF]](http://arxiv.org/pdf/1507.07602.pdf)
*/
/** @brief Asymptotically Optimal Fast Marching Tree algorithm developed
by L. Janson and M. Pavone. */
@@ -176,7 +184,7 @@ namespace ompl
}
/** \brief Activates the cost to go heuristics when ordering the heap */
- void setHeuristics (bool h)
+ void setHeuristics(bool h)
{
heuristics_ = h;
}
@@ -188,6 +196,18 @@ namespace ompl
return heuristics_;
}
+ /** \brief Activates the extended FMT*: adding new samples if planner does not finish successfully. */
+ void setExtendedFMT(bool e)
+ {
+ extendedFMT_ = e;
+ }
+
+ /** \brief Returns true if the extended FMT* is activated. */
+ bool getExtendedFMT() const
+ {
+ return extendedFMT_;
+ }
+
protected:
/** \brief Representation of a motion
*/
@@ -205,13 +225,13 @@ namespace ompl
enum SetType { SET_CLOSED, SET_OPEN, SET_UNVISITED };
Motion()
- : state_(NULL), parent_(NULL), cost_(0.0), currentSet_(SET_UNVISITED)
+ : state_(nullptr), parent_(nullptr), cost_(0.0), currentSet_(SET_UNVISITED)
{
}
/** \brief Constructor that allocates memory for the state */
Motion(const base::SpaceInformationPtr &si)
- : state_(si->allocState()), parent_(NULL), cost_(0.0), currentSet_(SET_UNVISITED)
+ : state_(si->allocState()), parent_(nullptr), cost_(0.0), currentSet_(SET_UNVISITED)
{
}
@@ -294,6 +314,12 @@ namespace ompl
return hcost_;
}
+ /** \brief Get the children of the motion */
+ std::vector<Motion*>& getChildren()
+ {
+ return children_;
+ }
+
protected:
/** \brief The state contained by the motion */
@@ -313,12 +339,15 @@ namespace ompl
/** \brief Contains the connections attempted FROM this node */
std::set<Motion*> collChecksDone_;
+
+ /** \brief The set of motions descending from the current motion */
+ std::vector<Motion*> children_;
};
/** \brief Comparator used to order motions in a binary heap */
struct MotionCompare
{
- MotionCompare() : opt_(NULL), heuristics_(false)
+ MotionCompare() : opt_(nullptr), heuristics_(false)
{
}
@@ -376,8 +405,7 @@ namespace ompl
void saveNeighborhood(Motion *m);
/** \brief Trace the path from a goal state back to the start state
- and save the result as a solution in the Problem Definiton.
- */
+ and save the result as a solution in the Problem Definiton. */
void traceSolutionPathThroughTree(Motion *goalMotion);
/** \brief Complete one iteration of the main loop of the FMT* algorithm:
@@ -388,6 +416,14 @@ namespace ompl
current lowest cost-to-come node in Open */
bool expandTreeFromNode(Motion **z);
+ /** \brief For a motion m, updates the stored neighborhoods of all its neighbors by
+ by inserting m (maintaining the cost-based sorting). Computes the nearest neighbors
+ if there is no stored neighborhood. */
+ void updateNeighborhood(Motion *m, const std::vector<Motion *> nbh);
+
+ /** \brief Returns the best parent and the connection cost in the neighborhood of a motion m. */
+ Motion* getBestParent(Motion *m, std::vector<Motion*> &neighbors, base::Cost &cMin);
+
/** \brief A binary heap for storing explored motions in
cost-to-come sorted order */
typedef ompl::BinaryHeap<Motion*, MotionCompare> MotionBinHeap;
@@ -438,7 +474,7 @@ namespace ompl
double radiusMultiplier_;
/** \brief A nearest-neighbor datastructure containing the set of all motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
/** \brief State sampler */
base::StateSamplerPtr sampler_;
@@ -451,6 +487,25 @@ namespace ompl
/** \brief Goal state caching to accelerate cost to go heuristic computation */
base::State* goalState_;
+
+ /** \brief Add new samples if the tree was not able to find a solution. */
+ bool extendedFMT_;
+
+ // For sorting a list of costs and getting only their sorted indices
+ struct CostIndexCompare
+ {
+ CostIndexCompare(const std::vector<base::Cost>& costs,
+ const base::OptimizationObjective &opt) :
+ costs_(costs), opt_(opt)
+ {}
+ bool operator()(unsigned i, unsigned j)
+ {
+ return opt_.isCostBetterThan(costs_[i],costs_[j]);
+ }
+ const std::vector<base::Cost>& costs_;
+ const base::OptimizationObjective &opt_;
+ };
+
};
}
}
diff --git a/src/ompl/geometric/planners/fmt/src/BFMT.cpp b/src/ompl/geometric/planners/fmt/src/BFMT.cpp
new file mode 100644
index 0000000..7b2e3fc
--- /dev/null
+++ b/src/ompl/geometric/planners/fmt/src/BFMT.cpp
@@ -0,0 +1,884 @@
+
+#include <boost/math/constants/constants.hpp>
+#include <boost/math/distributions/binomial.hpp>
+#include <ompl/datastructures/BinaryHeap.h>
+#include <ompl/tools/config/SelfConfig.h>
+
+#include <ompl/datastructures/NearestNeighborsGNAT.h>
+#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
+#include <ompl/geometric/planners/fmt/BFMT.h>
+
+#include <fstream>
+#include <ompl/base/spaces/RealVectorStateSpace.h>
+
+namespace ompl {
+namespace geometric {
+
+BFMT::BFMT(const base::SpaceInformationPtr &si)
+ : base::Planner(si, "BFMT")
+ , numSamples_(1000)
+ , radiusMultiplier_(1.0)
+ , freeSpaceVolume_(si_->getStateSpace()->getMeasure()) // An upper bound on the free space volume is the total space volume; the free fraction is estimated in sampleFree
+ , collisionChecks_(0)
+ , nearestK_(true)
+ , NNr_(0)
+ , NNk_(0)
+ , tree_(FWD)
+ , exploration_(SWAP_EVERY_TIME)
+ , termination_(OPTIMALITY)
+ , precomputeNN_(false)
+ , heuristics_(true)
+ , cacheCC_(true)
+ , extendedFMT_(true)
+{
+ specs_.approximateSolutions = false;
+ specs_.directed = false;
+
+ ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &BFMT::setNumSamples, &BFMT::getNumSamples, "10:10:1000000");
+ ompl::base::Planner::declareParam<double>("radius_multiplier", this, &BFMT::setRadiusMultiplier, &BFMT::getRadiusMultiplier, "0.1:0.05:50.");
+ ompl::base::Planner::declareParam<bool>("nearest_k", this, &BFMT::setNearestK, &BFMT::getNearestK, "0,1");
+ ompl::base::Planner::declareParam<bool>("balanced", this, &BFMT::setExploration, &BFMT::getExploration, "0,1");
+ ompl::base::Planner::declareParam<bool>("optimality", this, &BFMT::setTermination, &BFMT::getTermination, "0,1");
+ ompl::base::Planner::declareParam<bool>("heuristics", this, &BFMT::setHeuristics, &BFMT::getHeuristics, "0,1");
+ ompl::base::Planner::declareParam<bool>("cache_cc", this, &BFMT::setCacheCC, &BFMT::getCacheCC, "0,1");
+ ompl::base::Planner::declareParam<bool>("extended_fmt", this, &BFMT::setExtendedFMT, &BFMT::getExtendedFMT, "0,1");
+}
+
+ompl::geometric::BFMT::~BFMT()
+{
+ freeMemory();
+}
+
+void BFMT::setup(void)
+{
+ if (pdef_)
+ {
+ /* Setup the optimization objective. If no optimization objective was
+ specified, then default to optimizing path length as computed by the
+ distance() function in the state space */
+ if (pdef_->hasOptimizationObjective())
+ opt_ = pdef_->getOptimizationObjective();
+ else
+ {
+ OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", getName().c_str());
+ opt_.reset(new base::PathLengthOptimizationObjective(si_));
+ // Store the new objective in the problem def'n
+ pdef_->setOptimizationObjective(opt_);
+ }
+ Open_[0].getComparisonOperator().opt_ = opt_.get();
+ Open_[0].getComparisonOperator().heuristics_ = heuristics_;
+ Open_[1].getComparisonOperator().opt_ = opt_.get();
+ Open_[1].getComparisonOperator().heuristics_ = heuristics_;
+
+ if (!nn_)
+ nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<BiDirMotion*>(this));
+ nn_->setDistanceFunction(std::bind(&BFMT::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
+
+ if (nearestK_ && !nn_->reportsSortedResults())
+ {
+ OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy disabled.", getName().c_str());
+ nearestK_ = false;
+ }
+ }
+ else
+ {
+ OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
+ setup_ = false;
+ }
+}
+
+void BFMT::freeMemory()
+{
+ if (nn_)
+ {
+ BiDirMotionPtrs motions;
+ nn_->list(motions);
+ for (unsigned int i = 0 ; i < motions.size() ; ++i)
+ {
+ si_->freeState(motions[i]->getState());
+ delete motions[i];
+ }
+ }
+}
+
+void BFMT::clear()
+{
+ Planner::clear();
+ sampler_.reset();
+ freeMemory();
+ if (nn_)
+ nn_->clear();
+ Open_[FWD].clear();
+ Open_[REV].clear();
+ Open_elements[FWD].clear();
+ Open_elements[REV].clear();
+ neighborhoods_.clear();
+ collisionChecks_ = 0;
+}
+
+void BFMT::getPlannerData(base::PlannerData &data) const
+{
+ base::Planner::getPlannerData(data);
+ BiDirMotionPtrs motions;
+ nn_->list(motions);
+
+ int numStartNodes = 0;
+ int numGoalNodes = 0;
+ int numEdges = 0;
+ int numFwdEdges = 0;
+ int numRevEdges = 0;
+
+ int fwd_tree_tag = 1;
+ int rev_tree_tag = 2;
+
+ for (unsigned int k = 0; k < motions.size(); ++k)
+ {
+ BiDirMotion* motion = motions[k];
+ bool inFwdTree = (motion->currentSet_[FWD] != BiDirMotion::SET_UNVISITED);
+
+ // For samples added to the fwd tree, add incoming edges (from fwd tree parent)
+ if (inFwdTree)
+ {
+ if (motion->parent_[FWD] == NULL)
+ {
+ // Motion is a forward tree root node
+ ++numStartNodes;
+ } else
+ {
+ bool success = data.addEdge(
+ base::PlannerDataVertex(motion->parent_[FWD]->getState(), fwd_tree_tag),
+ base::PlannerDataVertex(motion->getState(), fwd_tree_tag));
+ if (success)
+ {
+ ++numFwdEdges;
+ ++numEdges;
+ }
+ }
+ }
+ }
+
+ // The edges in the goal tree are reversed so that they are in the same direction as start tree
+ for (unsigned int k = 0; k < motions.size(); ++k)
+ {
+ BiDirMotion* motion = motions[k];
+ bool inRevTree = (motion->currentSet_[REV] != BiDirMotion::SET_UNVISITED);
+
+ // For samples added to a tree, add incoming edges (from fwd tree parent)
+ if (inRevTree)
+ {
+ if (motion->parent_[REV] == NULL)
+ {
+ // Motion is a reverse tree root node
+ ++numGoalNodes;
+ } else {
+ bool success = data.addEdge(
+ base::PlannerDataVertex(motion->getState(), rev_tree_tag),
+ base::PlannerDataVertex(motion->parent_[REV]->getState(), rev_tree_tag));
+ if (success)
+ {
+ ++numRevEdges;
+ ++numEdges;
+ }
+ }
+ }
+ }
+}
+
+void BFMT::saveNeighborhood(std::shared_ptr< NearestNeighbors<BiDirMotion*> > nn,
+ BiDirMotion* m)
+{
+ // Check if neighborhood has already been saved
+ if (neighborhoods_.find(m) == neighborhoods_.end())
+ {
+ BiDirMotionPtrs neighborhood;
+ if (nearestK_)
+ nn_->nearestK(m, NNk_, neighborhood);
+ else
+ nn_->nearestR(m, NNr_, neighborhood);
+
+ if (!neighborhood.empty())
+ {
+ // Save the neighborhood but skip the first element (m)
+ neighborhoods_[m] = std::vector<BiDirMotion*>(neighborhood.size()-1, 0);
+ std::copy(neighborhood.begin()+1, neighborhood.end(), neighborhoods_[m].begin());
+ }
+ else
+ {
+ // Save an empty neighborhood
+ neighborhoods_[m] = std::vector<BiDirMotion*>(0);
+ }
+ }
+}
+
+void BFMT::sampleFree(std::shared_ptr<NearestNeighbors<BiDirMotion*> > nn,
+ const base::PlannerTerminationCondition &ptc)
+{
+ unsigned int nodeCount = 0;
+ unsigned int sampleAttempts = 0;
+ BiDirMotion *motion = new BiDirMotion(si_, &tree_);
+
+ // Sample numSamples_ number of nodes from the free configuration space
+ while (nodeCount < numSamples_ && !ptc)
+ {
+ sampler_->sampleUniform(motion->getState());
+ sampleAttempts++;
+ if (si_->isValid(motion->getState()))
+ { // collision checking
+ ++nodeCount;
+ nn->add(motion);
+ motion = new BiDirMotion(si_, &tree_);
+ }
+ }
+ si_->freeState(motion->getState());
+ delete motion;
+
+ // 95% confidence limit for an upper bound for the true free space volume
+ freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) * si_->getStateSpace()->getMeasure();
+}
+
+double BFMT::calculateUnitBallVolume(const unsigned int dimension) const {
+ if (dimension == 0)
+ return 1.0;
+ else if (dimension == 1)
+ return 2.0;
+ return 2.0 * boost::math::constants::pi<double>() / dimension
+ * calculateUnitBallVolume(dimension-2);
+}
+
+double BFMT::calculateRadius(const unsigned int dimension, const unsigned int n) const {
+
+ double a = 1.0 / (double)dimension;
+ double unitBallVolume = calculateUnitBallVolume(dimension);
+
+ return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) * std::pow(log((double)n) / (double)n, a);
+}
+
+void BFMT::initializeProblem(base::GoalSampleableRegion*& goal_s)
+{
+ checkValidity();
+ if (!sampler_)
+ {
+ sampler_ = si_->allocStateSampler();
+ }
+ goal_s = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
+}
+
+base::PlannerStatus BFMT::solve(const base::PlannerTerminationCondition& ptc)
+{
+ base::GoalSampleableRegion *goal_s;
+ initializeProblem(goal_s);
+ if (!goal_s)
+ {
+ OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
+ return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
+ }
+
+ useFwdTree();
+
+ // Add start states to Unvisitedfwd and Openfwd
+ bool valid_initMotion = false;
+ BiDirMotion *initMotion;
+ while (const base::State *st = pis_.nextStart())
+ {
+ initMotion = new BiDirMotion(si_, &tree_);
+ si_->copyState(initMotion->getState(), st);
+
+ initMotion->currentSet_[REV] = BiDirMotion::SET_UNVISITED;
+ nn_->add(initMotion); // S <-- {x_init}
+ if (si_->isValid(initMotion->getState()))
+ {
+ // Take the first valid initial state as the forward tree root
+ Open_elements[FWD][initMotion] = Open_[FWD].insert(initMotion);
+ initMotion->currentSet_[FWD] = BiDirMotion::SET_OPEN;
+ initMotion->cost_[FWD] = opt_->initialCost(initMotion->getState());
+ valid_initMotion = true;
+ heurGoalState_[1] = initMotion->getState();
+ }
+ }
+
+ if (!initMotion || !valid_initMotion)
+ {
+ OMPL_ERROR("Start state undefined or invalid.");
+ return base::PlannerStatus::INVALID_START;
+ }
+
+ // Sample N free states in configuration state_
+ sampleFree(nn_, ptc); // S <-- SAMPLEFREE(N)
+ OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
+
+ // Calculate the nearest neighbor search radius
+ if (nearestK_)
+ {
+ NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
+ (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
+ log((double)nn_->size()));
+ OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
+ }
+ else
+ {
+ NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
+ OMPL_DEBUG("Using radius of %f", NNr_);
+ }
+
+ // Add goal states to Unvisitedrev and Openrev
+ bool valid_goalMotion = false;
+ BiDirMotion *goalMotion;
+ while (const base::State *st = pis_.nextGoal())
+ {
+ goalMotion = new BiDirMotion(si_, &tree_);
+ si_->copyState(goalMotion->getState(), st);
+
+ goalMotion->currentSet_[FWD] = BiDirMotion::SET_UNVISITED;
+ nn_->add(goalMotion); // S <-- {x_goal}
+ if (si_->isValid(goalMotion->getState()))
+ {
+ // Take the first valid goal state as the reverse tree root
+ Open_elements[REV][goalMotion] = Open_[REV].insert(goalMotion);
+ goalMotion->currentSet_[REV] = BiDirMotion::SET_OPEN;
+ goalMotion->cost_[REV] = opt_->terminalCost(goalMotion->getState());
+ valid_goalMotion = true;
+ heurGoalState_[0] = goalMotion->getState();
+ }
+ }
+
+ if (!goalMotion || !valid_goalMotion)
+ {
+ OMPL_ERROR("Goal state undefined or invalid.");
+ return base::PlannerStatus::INVALID_GOAL;
+ }
+
+ useRevTree();
+
+ // Plan a path
+ BiDirMotion *connection_point = NULL;
+ bool earlyFailure = true;
+
+ if (initMotion != NULL && goalMotion != NULL)
+ {
+ earlyFailure = plan(initMotion, goalMotion, connection_point, ptc);
+ }
+ else
+ {
+ OMPL_ERROR("Initial/goal state(s) are undefined!");
+ }
+
+ if (earlyFailure)
+ {
+ return base::PlannerStatus(false,false);
+ }
+
+ // Save the best path (through z)
+ if (!ptc)
+ {
+ base::Cost fwd_cost, rev_cost, connection_cost;
+
+ // Construct the solution path
+ useFwdTree();
+ BiDirMotionPtrs path_fwd;
+ tracePath(connection_point, path_fwd);
+ fwd_cost = connection_point->getCost();
+
+ useRevTree();
+ BiDirMotionPtrs path_rev;
+ tracePath(connection_point, path_rev);
+ rev_cost = connection_point->getCost();
+
+ // ASSUMES FROM THIS POINT THAT z = path_fwd[0] = path_rev[0]
+ // Remove the first element, z, in the traced reverse path
+ // (the same as the first element in the traced forward path)
+ if (path_rev.size() > 1)
+ {
+ connection_cost = base::Cost(rev_cost.value() - path_rev[1]->getCost().value());
+ path_rev.erase(path_rev.begin());
+ }
+ else if (path_fwd.size() > 1)
+ {
+ connection_cost = base::Cost(fwd_cost.value() - path_fwd[1]->getCost().value());
+ path_fwd.erase(path_fwd.begin());
+ }
+ else
+ {
+ OMPL_ERROR("Solution path traced incorrectly or otherwise constructed improperly \
+ through forward/reverse trees (both paths are one node in length, each).");
+ }
+
+ // Adjust costs/parents in reverse tree nodes as cost/direction from forward tree root
+ useFwdTree();
+ path_rev[0]->setCost(base::Cost(path_fwd[0]->getCost().value() + connection_cost.value()));
+ path_rev[0]->setParent(path_fwd[0]);
+ for (unsigned int i = 1; i < path_rev.size(); ++i)
+ {
+ path_rev[i]->setCost(base::Cost(fwd_cost.value() + (rev_cost.value() - path_rev[i]->getCost().value())));
+ path_rev[i]->setParent(path_rev[i-1]);
+ }
+
+ BiDirMotionPtrs mpath;
+ std::reverse(path_rev.begin(), path_rev.end());
+ mpath.reserve(path_fwd.size() + path_rev.size()); // preallocate memory
+ mpath.insert(mpath.end(), path_rev.begin(), path_rev.end());
+ mpath.insert(mpath.end(), path_fwd.begin(), path_fwd.end());
+
+ // Set the solution path
+ PathGeometric *path = new PathGeometric(si_);
+ for (int i = mpath.size() - 1 ; i >= 0 ; --i)
+ {
+ path->append(mpath[i]->getState());
+ }
+
+ static const bool approximate = false;
+ static const double cost_difference_from_goal = 0.0;
+ pdef_->addSolutionPath(base::PathPtr(path), approximate, cost_difference_from_goal, getName());
+
+ OMPL_DEBUG("Total path cost: %f\n", fwd_cost.value() + rev_cost.value());
+ return base::PlannerStatus(true, false);
+
+ }
+ else
+ {
+ // Planner terminated without accomplishing goal
+ return base::PlannerStatus(false, false);
+ }
+}
+
+
+void BFMT::expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point)
+{
+ // Define Opennew and set it to NULL
+ BiDirMotionPtrs Open_new;
+
+ // Define Znear as all unexplored nodes in the neighborhood around z
+ BiDirMotionPtrs zNear;
+ const BiDirMotionPtrs &zNeighborhood = neighborhoods_[z];
+
+ for (unsigned int i = 0; i < zNeighborhood.size(); ++i)
+ {
+ if (zNeighborhood[i]->getCurrentSet() == BiDirMotion::SET_UNVISITED)
+ {
+ zNear.push_back(zNeighborhood[i]);
+ }
+ }
+
+ // For each node x in Znear
+ for (unsigned int i = 0; i < zNear.size(); ++i)
+ {
+ BiDirMotion *x = zNear.at(i);
+ if (!precomputeNN_)
+ saveNeighborhood(nn_, x); // nearest neighbors
+
+ // Define Xnear as all frontier nodes in the neighborhood around the unexplored node x
+ BiDirMotionPtrs xNear;
+ const BiDirMotionPtrs &xNeighborhood = neighborhoods_[x];
+ for (unsigned int j = 0; j < xNeighborhood.size(); ++j)
+ {
+ if (xNeighborhood[j]->getCurrentSet() == BiDirMotion::SET_OPEN)
+ {
+ xNear.push_back(xNeighborhood[j]);
+ }
+ }
+ // Find the node in Xnear with minimum cost-to-come in the current tree
+ BiDirMotion* xMin = NULL;
+ double cMin = std::numeric_limits<double>::infinity();
+ for (unsigned int j = 0; j < xNear.size(); ++j)
+ {
+ // check if node costs are smaller than minimum
+ double cNew = xNear.at(j)->getCost().value() + distanceFunction(xNear.at(j), x);
+
+ if (cNew < cMin)
+ {
+ xMin = xNear.at(j);
+ cMin = cNew;
+ }
+ }
+
+ // xMin was found
+ if (xMin != NULL)
+ {
+ bool collision_free = false;
+ if (cacheCC_)
+ {
+ if (!xMin->alreadyCC(x))
+ {
+ collision_free = si_->checkMotion(xMin->getState(), x->getState());
+ ++collisionChecks_;
+ // Due to FMT3* design, it is only necessary to save unsuccesful
+ // connection attemps because of collision
+ if (!collision_free)
+ xMin->addCC(x);
+ }
+ }
+ else
+ {
+ ++collisionChecks_;
+ collision_free = si_->checkMotion(xMin->getState(), x->getState());
+ }
+
+ if (collision_free)
+ { // motion between yMin and x is obstacle free
+ // add edge from xMin to x
+ x->setParent(xMin);
+ x->setCost(base::Cost(cMin));
+ xMin->getChildren().push_back(x);
+
+ if (heuristics_)
+ x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), heurGoalState_[tree_]));
+
+ // check if new node x is in the other tree; if so, save result
+ if (x->getOtherSet() != BiDirMotion::SET_UNVISITED)
+ {
+ if (connection_point == NULL)
+ {
+ connection_point = x;
+ if (termination_ == FEASIBILITY)
+ {
+ break;
+ }
+ }
+ else
+ {
+ if ((connection_point->cost_[FWD].value() + connection_point->cost_[REV].value())
+ > (x->cost_[FWD].value() + x->cost_[REV].value()))
+ {
+ connection_point = x;
+ }
+ }
+ }
+
+ Open_new.push_back(x); // add x to Open_new
+ x->setCurrentSet(BiDirMotion::SET_CLOSED); // remove x from Unvisited
+ }
+ }
+ } // End "for x in Znear"
+
+ // Remove motion z from binary heap and map
+ BiDirMotionBinHeap::Element* zElement = Open_elements[tree_][z];
+ Open_[tree_].remove(zElement);
+ Open_elements[tree_].erase(z);
+ z->setCurrentSet(BiDirMotion::SET_CLOSED);
+
+ // add nodes in Open_new to Open
+ for (unsigned int i = 0; i < Open_new.size(); i++)
+ {
+ Open_elements[tree_][Open_new.at(i)] = Open_[tree_].insert(Open_new.at(i));
+ Open_new.at(i)->setCurrentSet(BiDirMotion::SET_OPEN);
+ }
+}
+
+
+bool BFMT::plan(BiDirMotion *x_init, BiDirMotion *x_goal,
+ BiDirMotion *&connection_point, const base::PlannerTerminationCondition& ptc)
+{
+ // If pre-computation, find neighborhoods for all N sample nodes plus initial
+ // and goal state(s). Otherwise compute the neighborhoods of the initial and
+ // goal states separately and compute the others as needed.
+ BiDirMotionPtrs sampleNodes;
+ nn_->list(sampleNodes);
+ /// \todo This precomputation is useful only if the same planner is used many times.
+ /// otherwise is probably a waste of time. Do a real precomputation before calling solve().
+ if (precomputeNN_)
+ {
+ for (unsigned int i = 0; i < sampleNodes.size(); i++)
+ {
+ saveNeighborhood(nn_, sampleNodes[i]); // nearest neighbors
+ }
+ }
+ else
+ {
+ saveNeighborhood(nn_, x_init); // nearest neighbors
+ saveNeighborhood(nn_, x_goal); // nearest neighbors
+ }
+
+ // Copy nodes in the sample set to Unvisitedfwd. Overwrite the label of the initial
+ // node with set Open for the forward tree, since it starts in set Openfwd.
+ useFwdTree();
+ for (unsigned int i = 0; i < sampleNodes.size(); i++)
+ {
+ sampleNodes[i]->setCurrentSet(BiDirMotion::SET_UNVISITED);
+ }
+ x_init->setCurrentSet(BiDirMotion::SET_OPEN);
+
+ // Copy nodes in the sample set to Unvisitedrev. Overwrite the label of the goal
+ // node with set Open for the reverse tree, since it starts in set Openrev.
+ useRevTree();
+ for (unsigned int i = 0; i < sampleNodes.size(); i++)
+ {
+ sampleNodes[i]->setCurrentSet(BiDirMotion::SET_UNVISITED);
+ }
+ x_goal->setCurrentSet(BiDirMotion::SET_OPEN);
+
+ // Expand the trees until reaching the termination condition
+ bool earlyFailure = false;
+ bool success = false;
+
+ useFwdTree();
+ BiDirMotion *z = x_init;
+
+ while (!success)
+ {
+ expandTreeFromNode(z, connection_point);
+
+ // Check if the algorithm should terminate. Possibly redefines connection_point.
+ if (termination(z, connection_point, ptc))
+ success = true;
+ else
+ {
+ if (Open_[tree_].empty()) // If this heap is empty...
+ {
+ if (!extendedFMT_) // ... eFMT not enabled...
+ {
+ if (Open_[(tree_+1) % 2].empty()) // ... and this one, failure.
+ {
+ OMPL_INFORM("Both Open are empty before path was found --> no feasible path exists");
+ earlyFailure = true;
+ return earlyFailure;
+ }
+ }
+ else // However, if eFMT is enabled, run it.
+ insertNewSampleInOpen(ptc);
+ }
+
+ // This function will be always reached with at least one state in one heap.
+ // However, if ptc terminates, we should skip this.
+ if (!ptc)
+ chooseTreeAndExpansionNode(z);
+ else
+ return true;
+ }
+ }
+ earlyFailure = false;
+ return earlyFailure;
+}
+
+void BFMT::insertNewSampleInOpen(const base::PlannerTerminationCondition& ptc)
+{
+ // Sample and connect samples to tree only if there is
+ // a possibility to connect to unvisited nodes.
+ std::vector<BiDirMotion*> nbh;
+ std::vector<base::Cost> costs;
+ std::vector<base::Cost> incCosts;
+ std::vector<std::size_t> sortedCostIndices;
+
+ // our functor for sorting nearest neighbors
+ CostIndexCompare compareFn(costs, *opt_);
+
+ BiDirMotion *m = new BiDirMotion(si_, &tree_);
+ while (!ptc && Open_[tree_].empty()) //&& oneSample)
+ {
+ // Get new sample and check whether it is valid.
+ sampler_->sampleUniform(m->getState());
+ if (!si_->isValid(m->getState()))
+ continue;
+
+ // Get neighbours of the new sample.
+ std::vector<BiDirMotion*> yNear;
+ if (nearestK_)
+ nn_->nearestK(m, NNk_, nbh);
+ else
+ nn_->nearestR(m, NNr_, nbh);
+
+ yNear.reserve(nbh.size());
+ for (std::size_t j = 0; j < nbh.size(); ++j)
+ {
+ if (nbh[j]->getCurrentSet() == BiDirMotion::SET_CLOSED)
+ {
+ if (nearestK_)
+ {
+ // Only include neighbors that are mutually k-nearest
+ // Relies on NN datastructure returning k-nearest in sorted order
+ const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
+ const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());
+
+ if (opt_->isCostBetterThan(worstCost, connCost))
+ continue;
+ else
+ yNear.push_back(nbh[j]);
+ }
+ else
+ yNear.push_back(nbh[j]);
+ }
+ }
+
+ // Sample again if the new sample does not connect to the tree.
+ if (yNear.empty())
+ continue;
+
+ // cache for distance computations
+ //
+ // Our cost caches only increase in size, so they're only
+ // resized if they can't fit the current neighborhood
+ if (costs.size() < yNear.size())
+ {
+ costs.resize(yNear.size());
+ incCosts.resize(yNear.size());
+ sortedCostIndices.resize(yNear.size());
+ }
+
+ // Finding the nearest neighbor to connect to
+ // By default, neighborhood states are sorted by cost, and collision checking
+ // is performed in increasing order of cost
+ //
+ // calculate all costs and distances
+ for (std::size_t i = 0 ; i < yNear.size(); ++i)
+ {
+ incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
+ costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
+ }
+
+ // sort the nodes
+ //
+ // we're using index-value pairs so that we can get at
+ // original, unsorted indices
+ for (std::size_t i = 0; i < yNear.size(); ++i)
+ sortedCostIndices[i] = i;
+ std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
+ compareFn);
+
+ // collision check until a valid motion is found
+ for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
+ i != sortedCostIndices.begin() + yNear.size();
+ ++i)
+ {
+ ++collisionChecks_;
+ if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
+ {
+ const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
+ m->setParent(yNear[*i]);
+ yNear[*i]->getChildren().push_back(m);
+ m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
+ m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), heurGoalState_[tree_]));
+ m->setCurrentSet(BiDirMotion::SET_OPEN);
+ Open_elements[tree_][m] = Open_[tree_].insert(m);
+
+ nn_->add(m);
+ saveNeighborhood(nn_, m);
+ updateNeighborhood(m, nbh);
+
+ break;
+ }
+ }
+ } // While Open_[tree_] empty
+}
+
+bool BFMT::termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition& ptc)
+{
+ bool terminate = false;
+ switch (termination_)
+ {
+ case FEASIBILITY:
+ // Test if a connection point was found during tree expansion
+ return (connection_point != NULL || ptc);
+ break;
+
+ case OPTIMALITY:
+ // Test if z is in SET_CLOSED (interior) of other tree
+ if (ptc)
+ terminate = true;
+ else if (z->getOtherSet() == BiDirMotion::SET_CLOSED)
+ terminate = true;
+
+ break;
+ };
+ return terminate;
+}
+
+// Choose exploration tree and node z to expand
+void BFMT::chooseTreeAndExpansionNode(BiDirMotion *&z)
+{
+ switch (exploration_)
+ {
+ case SWAP_EVERY_TIME:
+ if (Open_[(tree_+1) % 2].empty())
+ z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit condition in plan())
+ else
+ {
+ z = Open_[(tree_+1) % 2].top()->data; // Take top of opposite tree heap as new z
+ swapTrees(); // Swap to the opposite tree
+ }
+ break;
+
+ case CHOOSE_SMALLEST_Z:
+ BiDirMotion *z1, *z2;
+ if (Open_[(tree_+1) % 2].empty())
+ z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit condition in plan())
+ else if (Open_[tree_].empty())
+ {
+ z = Open_[(tree_+1) % 2].top()->data; // Take top of opposite tree heap as new z
+ swapTrees(); // Swap to the opposite tree
+ } else {
+ z1 = Open_[tree_].top()->data;
+ z2 = Open_[(tree_+1) % 2].top()->data;
+
+ if (z1->getCost().value() < z2->getOtherCost().value())
+ z = z1;
+ else
+ {
+ z = z2;
+ swapTrees();
+ }
+ }
+ break;
+ };
+}
+
+// Trace a path of nodes along a tree towards the root (forward or reverse)
+void BFMT::tracePath(BiDirMotion *z, BiDirMotionPtrs& path)
+{
+ BiDirMotion* solution = z;
+
+ while (solution != NULL)
+ {
+ path.push_back(solution);
+ solution = solution->getParent();
+ }
+}
+
+void BFMT::swapTrees()
+{
+ tree_ = (TreeType) ((((int) tree_) + 1) % 2);
+}
+
+void BFMT::updateNeighborhood(BiDirMotion *m, const std::vector<BiDirMotion *> nbh)
+{
+ // Neighborhoods are only updated if the new motion is within bounds (k nearest or within r).
+ for (std::size_t i = 0; i < nbh.size(); ++i)
+ {
+ // If CLOSED, that neighborhood won't be used again.
+ // Else, if neighhboorhod already exists, we have to insert the node in
+ // the corresponding place of the neighborhood of the neighbor of m.
+ if (nbh[i]->getCurrentSet() == BiDirMotion::SET_CLOSED)
+ continue;
+ else
+ {
+ auto it = neighborhoods_.find(nbh[i]);
+ if (it != neighborhoods_.end())
+ {
+ if (!it->second.size())
+ continue;
+
+ const base::Cost connCost = opt_->motionCost(nbh[i]->getState(), m->getState());
+ const base::Cost worstCost = opt_->motionCost(it->second.back()->getState(), nbh[i]->getState());
+
+ if (opt_->isCostBetterThan(worstCost, connCost))
+ continue;
+ else
+ {
+ // insert the neighbor in the vector in the correct order
+ std::vector<BiDirMotion*> &nbhToUpdate = it->second;
+ for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
+ {
+ // If connection to the new state is better than the current neighbor tested, insert.
+ const base::Cost cost = opt_->motionCost(nbh[i]->getState(), nbhToUpdate[j]->getState());
+ if (opt_->isCostBetterThan(connCost, cost))
+ {
+ nbhToUpdate.insert(nbhToUpdate.begin()+j, m);
+ break;
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+} // End "geometric" namespace
+} // End "ompl" namespace
diff --git a/src/ompl/geometric/planners/fmt/src/FMT.cpp b/src/ompl/geometric/planners/fmt/src/FMT.cpp
index 781d096..73e2719 100644
--- a/src/ompl/geometric/planners/fmt/src/FMT.cpp
+++ b/src/ompl/geometric/planners/fmt/src/FMT.cpp
@@ -59,10 +59,11 @@ ompl::geometric::FMT::FMT(const base::SpaceInformationPtr &si)
, cacheCC_(true)
, heuristics_(false)
, radiusMultiplier_(1.1)
+ , extendedFMT_(true)
{
// An upper bound on the free space volume is the total space volume; the free fraction is estimated in sampleFree
freeSpaceVolume_ = si_->getStateSpace()->getMeasure();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
specs_.approximateSolutions = false;
specs_.directed = false;
@@ -70,7 +71,9 @@ ompl::geometric::FMT::FMT(const base::SpaceInformationPtr &si)
ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &FMT::setNumSamples, &FMT::getNumSamples, "10:10:1000000");
ompl::base::Planner::declareParam<double>("radius_multiplier", this, &FMT::setRadiusMultiplier, &FMT::getRadiusMultiplier, "0.1:0.05:50.");
ompl::base::Planner::declareParam<bool>("nearest_k", this, &FMT::setNearestK, &FMT::getNearestK, "0,1");
+ ompl::base::Planner::declareParam<bool>("cache_cc", this, &FMT::setCacheCC, &FMT::getCacheCC, "0,1");
ompl::base::Planner::declareParam<bool>("heuristics", this, &FMT::setHeuristics, &FMT::getHeuristics, "0,1");
+ ompl::base::Planner::declareParam<bool>("extended_fmt", this, &FMT::setExtendedFMT, &FMT::getExtendedFMT, "0,1");
}
ompl::geometric::FMT::~FMT()
@@ -91,13 +94,16 @@ void ompl::geometric::FMT::setup()
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", getName().c_str());
opt_.reset(new base::PathLengthOptimizationObjective(si_));
+ // Store the new objective in the problem def'n
+ pdef_->setOptimizationObjective(opt_);
}
Open_.getComparisonOperator().opt_ = opt_.get();
Open_.getComparisonOperator().heuristics_ = heuristics_;
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&FMT::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&FMT::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
if (nearestK_ && !nn_->reportsSortedResults())
{
@@ -130,7 +136,7 @@ void ompl::geometric::FMT::freeMemory()
void ompl::geometric::FMT::clear()
{
Planner::clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
sampler_.reset();
freeMemory();
if (nn_)
@@ -153,7 +159,7 @@ void ompl::geometric::FMT::getPlannerData(base::PlannerData &data) const
unsigned int size = motions.size();
for (unsigned int i = 0; i < size; ++i)
{
- if (motions[i]->getParent() == NULL)
+ if (motions[i]->getParent() == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->getState()));
else
data.addEdge(base::PlannerDataVertex(motions[i]->getParent()->getState()),
@@ -283,7 +289,7 @@ ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTermina
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
- Motion *initMotion = NULL;
+ Motion *initMotion = nullptr;
if (!goal)
{
@@ -336,11 +342,123 @@ ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTermina
Motion *z = initMotion; // z <-- xinit
saveNeighborhood(z);
- while (!ptc && !(plannerSuccess = goal->isSatisfied(z->getState())))
+ while (!ptc)
{
+ if ((plannerSuccess = goal->isSatisfied(z->getState())))
+ break;
+
successfulExpansion = expandTreeFromNode(&z);
- if (!successfulExpansion)
- return base::PlannerStatus(false, false);
+
+ if (!extendedFMT_ && !successfulExpansion)
+ break;
+ else if (extendedFMT_ && !successfulExpansion)
+ {
+ //Apply RRT*-like connections: sample and connect samples to tree
+ std::vector<Motion*> nbh;
+ std::vector<base::Cost> costs;
+ std::vector<base::Cost> incCosts;
+ std::vector<std::size_t> sortedCostIndices;
+
+ // our functor for sorting nearest neighbors
+ CostIndexCompare compareFn(costs, *opt_);
+
+ Motion *m = new Motion(si_);
+ while (!ptc && Open_.empty())
+ {
+ sampler_->sampleUniform(m->getState());
+
+ if (!si_->isValid(m->getState()))
+ continue;
+
+ if (nearestK_)
+ nn_->nearestK(m, NNk_, nbh);
+ else
+ nn_->nearestR(m, NNr_, nbh);
+
+ // Get neighbours in the tree.
+ std::vector<Motion*> yNear;
+ yNear.reserve(nbh.size());
+ for (std::size_t j = 0; j < nbh.size(); ++j)
+ {
+ if (nbh[j]->getSetType() == Motion::SET_CLOSED)
+ {
+ if (nearestK_)
+ {
+ // Only include neighbors that are mutually k-nearest
+ // Relies on NN datastructure returning k-nearest in sorted order
+ const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
+ const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());
+
+ if (opt_->isCostBetterThan(worstCost, connCost))
+ continue;
+ else
+ yNear.push_back(nbh[j]);
+ }
+ else
+ yNear.push_back(nbh[j]);
+ }
+ }
+
+ // Sample again if the new sample does not connect to the tree.
+ if (yNear.empty())
+ continue;
+
+ // cache for distance computations
+ //
+ // Our cost caches only increase in size, so they're only
+ // resized if they can't fit the current neighborhood
+ if (costs.size() < yNear.size())
+ {
+ costs.resize(yNear.size());
+ incCosts.resize(yNear.size());
+ sortedCostIndices.resize(yNear.size());
+ }
+
+ // Finding the nearest neighbor to connect to
+ // By default, neighborhood states are sorted by cost, and collision checking
+ // is performed in increasing order of cost
+ //
+ // calculate all costs and distances
+ for (std::size_t i = 0 ; i < yNear.size(); ++i)
+ {
+ incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
+ costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
+ }
+
+ // sort the nodes
+ //
+ // we're using index-value pairs so that we can get at
+ // original, unsorted indices
+ for (std::size_t i = 0; i < yNear.size(); ++i)
+ sortedCostIndices[i] = i;
+ std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
+ compareFn);
+
+ // collision check until a valid motion is found
+ for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
+ i != sortedCostIndices.begin() + yNear.size();
+ ++i)
+ {
+ if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
+ {
+ m->setParent(yNear[*i]);
+ yNear[*i]->getChildren().push_back(m);
+ const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
+ m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
+ m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
+ m->setSetType(Motion::SET_OPEN);
+
+ nn_->add(m);
+ saveNeighborhood(m);
+ updateNeighborhood(m,nbh);
+
+ Open_.insert(m);
+ z = m;
+ break;
+ }
+ }
+ } // while (!ptc && Open_.empty())
+ } // else if (extendedFMT_ && !successfulExpansion)
} // While not at goal
if (plannerSuccess)
@@ -366,7 +484,7 @@ void ompl::geometric::FMT::traceSolutionPathThroughTree(Motion *goalMotion)
Motion *solution = goalMotion;
// Construct the solution path
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->getParent();
@@ -405,7 +523,7 @@ bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
- xNear.push_back(zNeighborhood[i]);
+ xNear.push_back(x);
}
else
xNear.push_back(x);
@@ -432,25 +550,12 @@ bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
}
// Find the lowest cost-to-come connection from Open to x
- Motion *yMin = NULL;
base::Cost cMin(std::numeric_limits<double>::infinity());
- const unsigned int yNearSize = yNear.size();
- for (unsigned int j = 0; j < yNearSize; ++j)
- {
- const base::State *yState = yNear[j]->getState();
- const base::Cost dist = opt_->motionCost(yState, x->getState());
- const base::Cost cNew = opt_->combineCosts(yNear[j]->getCost(), dist);
-
- if (opt_->isCostBetterThan(cNew, cMin))
- {
- yMin = yNear[j];
- cMin = cNew;
- }
- }
+ Motion *yMin = getBestParent(x, yNear, cMin);
yNear.clear();
// If an optimal connection from Open to x was found
- if (yMin != NULL)
+ if (yMin != nullptr)
{
bool collision_free = false;
if (cacheCC_)
@@ -477,6 +582,8 @@ bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
x->setParent(yMin);
x->setCost(cMin);
x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
+ yMin->getChildren().push_back(x);
+
// Add x to Open
Open_new.push_back(x);
// Remove x from Unvisited
@@ -489,9 +596,9 @@ bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
Open_.pop();
(*z)->setSetType(Motion::SET_CLOSED);
- // Add the nodes in H_new to H
+ // Add the nodes in Open_new to Open
unsigned int openNewSize = Open_new.size();
- for (unsigned int i = 0; i < openNewSize; i++)
+ for (unsigned int i = 0; i < openNewSize; ++i)
{
Open_.insert(Open_new[i]);
Open_new[i]->setSetType(Motion::SET_OPEN);
@@ -500,7 +607,8 @@ bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
if (Open_.empty())
{
- OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
+ if(!extendedFMT_)
+ OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
return false;
}
@@ -509,3 +617,74 @@ bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
return true;
}
+
+ompl::geometric::FMT::Motion* ompl::geometric::FMT::getBestParent(Motion *m, std::vector<Motion*> &neighbors, base::Cost &cMin)
+{
+ Motion *min = nullptr;
+ const unsigned int neighborsSize = neighbors.size();
+ for (unsigned int j = 0; j < neighborsSize; ++j)
+ {
+ const base::State *s = neighbors[j]->getState();
+ const base::Cost dist = opt_->motionCost(s, m->getState());
+ const base::Cost cNew = opt_->combineCosts(neighbors[j]->getCost(), dist);
+
+ if (opt_->isCostBetterThan(cNew, cMin))
+ {
+ min = neighbors[j];
+ cMin = cNew;
+ }
+ }
+ return min;
+}
+
+void ompl::geometric::FMT::updateNeighborhood(Motion *m, const std::vector<Motion*> nbh)
+{
+ for (std::size_t i = 0; i < nbh.size(); ++i)
+ {
+ // If CLOSED, the neighborhood already exists. If neighborhood already exists, we have
+ // to insert the node in the corresponding place of the neighborhood of the neighbor of m.
+ if (nbh[i]->getSetType() == Motion::SET_CLOSED || neighborhoods_.find(nbh[i]) != neighborhoods_.end())
+ {
+ const base::Cost connCost = opt_->motionCost(nbh[i]->getState(), m->getState());
+ const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[i]].back()->getState(), nbh[i]->getState());
+
+ if (opt_->isCostBetterThan(worstCost, connCost))
+ continue;
+ else
+ {
+ // Insert the neighbor in the vector in the correct order
+ std::vector<Motion*> &nbhToUpdate = neighborhoods_[nbh[i]];
+ for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
+ {
+ // If connection to the new state is better than the current neighbor tested, insert.
+ const base::Cost cost = opt_->motionCost(nbh[i]->getState(), nbhToUpdate[j]->getState());
+ if (opt_->isCostBetterThan(connCost, cost))
+ {
+ nbhToUpdate.insert(nbhToUpdate.begin()+j, m);
+ break;
+ }
+ }
+ }
+ }
+ else
+ {
+ std::vector<Motion*> nbh2;
+ if (nearestK_)
+ nn_->nearestK(m, NNk_, nbh2);
+ else
+ nn_->nearestR(m, NNr_, nbh2);
+
+ if (!nbh2.empty())
+ {
+ // Save the neighborhood but skip the first element, since it will be motion m
+ neighborhoods_[nbh[i]] = std::vector<Motion*>(nbh2.size() - 1, 0);
+ std::copy(nbh2.begin() + 1, nbh2.end(), neighborhoods_[nbh[i]].begin());
+ }
+ else
+ {
+ // Save an empty neighborhood
+ neighborhoods_[nbh[i]] = std::vector<Motion*>(0);
+ }
+ }
+ }
+}
diff --git a/src/ompl/geometric/planners/kpiece/BKPIECE1.h b/src/ompl/geometric/planners/kpiece/BKPIECE1.h
index 457ba9b..8a10985 100644
--- a/src/ompl/geometric/planners/kpiece/BKPIECE1.h
+++ b/src/ompl/geometric/planners/kpiece/BKPIECE1.h
@@ -183,12 +183,12 @@ namespace ompl
{
public:
- Motion() : root(NULL), state(NULL), parent(NULL)
+ Motion() : root(nullptr), state(nullptr), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : root(nullptr), state(si->allocState()), parent(nullptr)
{
}
diff --git a/src/ompl/geometric/planners/kpiece/Discretization.h b/src/ompl/geometric/planners/kpiece/Discretization.h
index eae6b04..de74348 100644
--- a/src/ompl/geometric/planners/kpiece/Discretization.h
+++ b/src/ompl/geometric/planners/kpiece/Discretization.h
@@ -40,7 +40,7 @@
#include "ompl/base/Planner.h"
#include "ompl/datastructures/GridB.h"
#include "ompl/util/Exception.h"
-#include <boost/function.hpp>
+#include <functional>
#include <vector>
#include <limits>
#include <cassert>
@@ -117,12 +117,12 @@ namespace ompl
typedef typename Grid::Coord Coord;
/** \brief The signature of a function that frees the memory for a motion */
- typedef typename boost::function<void(Motion*)> FreeMotionFn;
+ typedef typename std::function<void(Motion*)> FreeMotionFn;
- Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(NULL),
+ Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(nullptr),
freeMotion_(freeMotion)
{
- grid_.onCellUpdate(computeImportance, NULL);
+ grid_.onCellUpdate(computeImportance, nullptr);
selectBorderFraction_ = 0.9;
}
@@ -163,7 +163,7 @@ namespace ompl
freeMemory();
size_ = 0;
iteration_ = 1;
- recentCell_ = NULL;
+ recentCell_ = nullptr;
}
void countIteration()
@@ -297,7 +297,7 @@ namespace ompl
for (unsigned int i = 0 ; i < cdata.size() ; ++i)
for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
{
- if (cdata[i]->motions[j]->parent == NULL)
+ if (cdata[i]->motions[j]->parent == nullptr)
{
if (start)
data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
diff --git a/src/ompl/geometric/planners/kpiece/KPIECE1.h b/src/ompl/geometric/planners/kpiece/KPIECE1.h
index 71b935f..33797d4 100644
--- a/src/ompl/geometric/planners/kpiece/KPIECE1.h
+++ b/src/ompl/geometric/planners/kpiece/KPIECE1.h
@@ -202,12 +202,12 @@ namespace ompl
{
public:
- Motion() : state(NULL), parent(NULL)
+ Motion() : state(nullptr), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
{
}
diff --git a/src/ompl/geometric/planners/kpiece/LBKPIECE1.h b/src/ompl/geometric/planners/kpiece/LBKPIECE1.h
index 811ef1f..cfec376 100644
--- a/src/ompl/geometric/planners/kpiece/LBKPIECE1.h
+++ b/src/ompl/geometric/planners/kpiece/LBKPIECE1.h
@@ -169,12 +169,12 @@ namespace ompl
{
public:
- Motion() : root(NULL), state(NULL), parent(NULL), valid(false)
+ Motion() : root(nullptr), state(nullptr), parent(nullptr), valid(false)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
+ Motion(const base::SpaceInformationPtr &si) : root(nullptr), state(si->allocState()), parent(nullptr), valid(false)
{
}
diff --git a/src/ompl/geometric/planners/kpiece/src/BKPIECE1.cpp b/src/ompl/geometric/planners/kpiece/src/BKPIECE1.cpp
index dc0ea1c..fe139cc 100644
--- a/src/ompl/geometric/planners/kpiece/src/BKPIECE1.cpp
+++ b/src/ompl/geometric/planners/kpiece/src/BKPIECE1.cpp
@@ -40,15 +40,15 @@
#include <cassert>
ompl::geometric::BKPIECE1::BKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "BKPIECE1"),
- dStart_(boost::bind(&BKPIECE1::freeMotion, this, _1)),
- dGoal_(boost::bind(&BKPIECE1::freeMotion, this, _1))
+ dStart_(std::bind(&BKPIECE1::freeMotion, this, std::placeholders::_1)),
+ dGoal_(std::bind(&BKPIECE1::freeMotion, this, std::placeholders::_1))
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
minValidPathFraction_ = 0.5;
failedExpansionScoreFactor_ = 0.5;
maxDistance_ = 0.0;
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange, "0.:1.:10000.");
Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction, "0.:.05:1.");
@@ -146,8 +146,8 @@ ompl::base::PlannerStatus ompl::geometric::BKPIECE1::solve(const base::PlannerTe
}
}
- Discretization<Motion>::Cell *ecell = NULL;
- Motion *existing = NULL;
+ Discretization<Motion>::Cell *ecell = nullptr;
+ Motion *existing = nullptr;
disc.selectMotion(existing, ecell);
assert(existing);
if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
@@ -185,14 +185,14 @@ ompl::base::PlannerStatus ompl::geometric::BKPIECE1::solve(const base::PlannerTe
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
- while (motion != NULL)
+ while (motion != nullptr)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
- while (connectOther != NULL)
+ while (connectOther != nullptr)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
@@ -247,14 +247,14 @@ void ompl::geometric::BKPIECE1::clear()
sampler_.reset();
dStart_.clear();
dGoal_.clear();
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
}
void ompl::geometric::BKPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
- dStart_.getPlannerData(data, 1, true, NULL);
- dGoal_.getPlannerData(data, 2, false, NULL);
+ dStart_.getPlannerData(data, 1, true, nullptr);
+ dGoal_.getPlannerData(data, 2, false, nullptr);
// Insert the edge connecting the two trees
data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
diff --git a/src/ompl/geometric/planners/kpiece/src/KPIECE1.cpp b/src/ompl/geometric/planners/kpiece/src/KPIECE1.cpp
index ae69125..e3a5dec 100644
--- a/src/ompl/geometric/planners/kpiece/src/KPIECE1.cpp
+++ b/src/ompl/geometric/planners/kpiece/src/KPIECE1.cpp
@@ -41,7 +41,7 @@
#include <cassert>
ompl::geometric::KPIECE1::KPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "KPIECE1"),
- disc_(boost::bind(&KPIECE1::freeMotion, this, _1))
+ disc_(std::bind(&KPIECE1::freeMotion, this, std::placeholders::_1))
{
specs_.approximateSolutions = true;
specs_.directed = true;
@@ -50,7 +50,7 @@ ompl::geometric::KPIECE1::KPIECE1(const base::SpaceInformationPtr &si) : base::P
failedExpansionScoreFactor_ = 0.5;
minValidPathFraction_ = 0.2;
maxDistance_ = 0.0;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
Planner::declareParam<double>("range", this, &KPIECE1::setRange, &KPIECE1::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &KPIECE1::setGoalBias, &KPIECE1::getGoalBias, "0.:.05:1.");
@@ -83,7 +83,7 @@ void ompl::geometric::KPIECE1::clear()
Planner::clear();
sampler_.reset();
disc_.clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::geometric::KPIECE1::freeMotion(Motion *motion)
@@ -120,8 +120,8 @@ ompl::base::PlannerStatus ompl::geometric::KPIECE1::solve(const base::PlannerTer
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), disc_.getMotionCount());
- Motion *solution = NULL;
- Motion *approxsol = NULL;
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
base::State *xstate = si_->allocState();
@@ -130,8 +130,8 @@ ompl::base::PlannerStatus ompl::geometric::KPIECE1::solve(const base::PlannerTer
disc_.countIteration();
/* Decide on a state to expand from */
- Motion *existing = NULL;
- Discretization<Motion>::Cell *ecell = NULL;
+ Motion *existing = nullptr;
+ Discretization<Motion>::Cell *ecell = nullptr;
disc_.selectMotion(existing, ecell);
assert(existing);
@@ -177,19 +177,19 @@ ompl::base::PlannerStatus ompl::geometric::KPIECE1::solve(const base::PlannerTer
bool solved = false;
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
diff --git a/src/ompl/geometric/planners/kpiece/src/LBKPIECE1.cpp b/src/ompl/geometric/planners/kpiece/src/LBKPIECE1.cpp
index 7c93599..b5f15f6 100644
--- a/src/ompl/geometric/planners/kpiece/src/LBKPIECE1.cpp
+++ b/src/ompl/geometric/planners/kpiece/src/LBKPIECE1.cpp
@@ -40,14 +40,14 @@
#include <cassert>
ompl::geometric::LBKPIECE1::LBKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "LBKPIECE1"),
- dStart_(boost::bind(&LBKPIECE1::freeMotion, this, _1)),
- dGoal_(boost::bind(&LBKPIECE1::freeMotion, this, _1))
+ dStart_(std::bind(&LBKPIECE1::freeMotion, this, std::placeholders::_1)),
+ dGoal_(std::bind(&LBKPIECE1::freeMotion, this, std::placeholders::_1))
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
minValidPathFraction_ = 0.5;
maxDistance_ = 0.0;
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
Planner::declareParam<double>("range", this, &LBKPIECE1::setRange, &LBKPIECE1::getRange, "0.:1.:10000");
Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction, "0.:.05:1.");
@@ -143,8 +143,8 @@ ompl::base::PlannerStatus ompl::geometric::LBKPIECE1::solve(const base::PlannerT
}
}
- Discretization<Motion>::Cell *ecell = NULL;
- Motion *existing = NULL;
+ Discretization<Motion>::Cell *ecell = nullptr;
+ Motion *existing = nullptr;
disc.selectMotion(existing, ecell);
assert(existing);
sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
@@ -184,14 +184,14 @@ ompl::base::PlannerStatus ompl::geometric::LBKPIECE1::solve(const base::PlannerT
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
- while (motion != NULL)
+ while (motion != nullptr)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
- while (connectOther != NULL)
+ while (connectOther != nullptr)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
@@ -231,7 +231,7 @@ bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motio
std::vector<Motion*> mpath;
/* construct the solution path */
- while (motion != NULL)
+ while (motion != nullptr)
{
mpath.push_back(motion);
motion = motion->parent;
@@ -294,7 +294,7 @@ void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Moti
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
- motion->children[i]->parent = NULL;
+ motion->children[i]->parent = nullptr;
removeMotion(disc, motion->children[i]);
}
@@ -316,14 +316,14 @@ void ompl::geometric::LBKPIECE1::clear()
sampler_.reset();
dStart_.clear();
dGoal_.clear();
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
}
void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
- dStart_.getPlannerData(data, 1, true, NULL);
- dGoal_.getPlannerData(data, 2, false, NULL);
+ dStart_.getPlannerData(data, 1, true, nullptr);
+ dGoal_.getPlannerData(data, 2, false, nullptr);
// Insert the edge connecting the two trees
data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
diff --git a/src/ompl/geometric/planners/pdst/PDST.h b/src/ompl/geometric/planners/pdst/PDST.h
index 954aab9..433d9bb 100755
--- a/src/ompl/geometric/planners/pdst/PDST.h
+++ b/src/ompl/geometric/planners/pdst/PDST.h
@@ -148,13 +148,13 @@ namespace ompl
public:
Motion(base::State *startState, base::State *endState, double priority, Motion *parent)
: startState_(startState), endState_(endState), priority_(priority),
- parent_(parent), cell_(NULL), heapElement_(NULL), isSplit_(false)
+ parent_(parent), cell_(nullptr), heapElement_(nullptr), isSplit_(false)
{
}
/// constructor for start states
Motion(base::State *state)
: startState_(state), endState_(state), priority_(0.),
- parent_(NULL), cell_(NULL), heapElement_(NULL), isSplit_(false)
+ parent_(nullptr), cell_(nullptr), heapElement_(nullptr), isSplit_(false)
{
}
/// The score is used to order motions in a priority queue.
@@ -197,7 +197,7 @@ namespace ompl
Cell(double volume, const ompl::base::RealVectorBounds &bounds,
unsigned int splitDimension = 0)
: volume_(volume), splitDimension_(splitDimension), splitValue_(0.0),
- left_(NULL), right_(NULL), bounds_(bounds)
+ left_(nullptr), right_(nullptr), bounds_(bounds)
{
}
@@ -217,7 +217,7 @@ namespace ompl
Cell* stab(const ompl::base::EuclideanProjection& projection) const
{
Cell *containingCell = const_cast<Cell*>(this);
- while (containingCell->left_ != NULL)
+ while (containingCell->left_ != nullptr)
{
if (projection[containingCell->splitDimension_] <= containingCell->splitValue_)
containingCell = containingCell->left_;
@@ -248,9 +248,9 @@ namespace ompl
unsigned int splitDimension_;
/// The midpoint between the bounds_ at the splitDimension_
double splitValue_;
- /// The left child cell (NULL for a leaf cell)
+ /// The left child cell (nullptr for a leaf cell)
Cell* left_;
- /// The right child cell (NULL for a leaf cell)
+ /// The right child cell (nullptr for a leaf cell)
Cell* right_;
/// A bounding box for this cell
ompl::base::RealVectorBounds bounds_;
@@ -270,7 +270,7 @@ namespace ompl
motion->heapElement_ = priorityQueue_.insert(motion);
}
/// \brief Select a state along motion and propagate a new motion from there.
- /// Return NULL if no valid motion could be generated starting at the
+ /// Return nullptr if no valid motion could be generated starting at the
/// selected state.
Motion* propagateFrom(Motion *motion, base::State*, base::State*);
diff --git a/src/ompl/geometric/planners/pdst/src/PDST.cpp b/src/ompl/geometric/planners/pdst/src/PDST.cpp
index da8ca1a..972b67b 100755
--- a/src/ompl/geometric/planners/pdst/src/PDST.cpp
+++ b/src/ompl/geometric/planners/pdst/src/PDST.cpp
@@ -38,8 +38,8 @@
#include "ompl/geometric/planners/pdst/PDST.h"
ompl::geometric::PDST::PDST(const base::SpaceInformationPtr &si)
- : base::Planner(si, "PDST"), bsp_(NULL), goalBias_(0.05),
- goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL)
+ : base::Planner(si, "PDST"), bsp_(nullptr), goalBias_(0.05),
+ goalSampler_(nullptr), iteration_(1), lastGoalMotion_(nullptr)
{
Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
}
@@ -74,7 +74,7 @@ ompl::base::PlannerStatus ompl::geometric::PDST::solve(const base::PlannerTermin
// generated an approximate or exact solution. If solve is being called for the first
// time then initializes hasSolution to false and isApproximate to true.
double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
- bool hasSolution = lastGoalMotion_ != NULL;
+ bool hasSolution = lastGoalMotion_ != nullptr;
bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
unsigned ndim = projectionEvaluator_->getDimension();
@@ -136,7 +136,7 @@ ompl::base::PlannerStatus ompl::geometric::PDST::solve(const base::PlannerTermin
addMotion(*m, cellSelected, tmpState1, tmpProj);
}
- if (lastGoalMotion_ != NULL)
+ if (lastGoalMotion_ != nullptr)
hasSolution = true;
// If a solution path has been computed, save it in the problem definition object.
@@ -240,7 +240,7 @@ void ompl::geometric::PDST::clear()
Planner::clear();
sampler_.reset();
iteration_ = 1;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
freeMemory();
bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
}
@@ -261,7 +261,7 @@ void ompl::geometric::PDST::freeMemory()
}
priorityQueue_.clear(); // clears the Element objects in the priority queue
delete bsp_;
- bsp_ = NULL;
+ bsp_ = nullptr;
}
void ompl::geometric::PDST::setup()
@@ -276,7 +276,7 @@ void ompl::geometric::PDST::setup()
if (bsp_)
delete bsp_;
bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::geometric::PDST::getPlannerData(ompl::base::PlannerData &data) const
@@ -287,7 +287,7 @@ void ompl::geometric::PDST::getPlannerData(ompl::base::PlannerData &data) const
priorityQueue_.getContent(motions);
// Add goal vertex
- if (lastGoalMotion_ != NULL)
+ if (lastGoalMotion_ != nullptr)
data.addGoalVertex(lastGoalMotion_->endState_);
for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
diff --git a/src/ompl/geometric/planners/prm/ConnectionStrategy.h b/src/ompl/geometric/planners/prm/ConnectionStrategy.h
index cc12106..88ee992 100644
--- a/src/ompl/geometric/planners/prm/ConnectionStrategy.h
+++ b/src/ompl/geometric/planners/prm/ConnectionStrategy.h
@@ -38,8 +38,8 @@
#define OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
#include "ompl/datastructures/NearestNeighbors.h"
-#include <boost/function.hpp>
-#include <boost/shared_ptr.hpp>
+#include <functional>
+#include <memory>
#include <boost/math/constants/constants.hpp>
#include <algorithm>
#include <vector>
@@ -61,7 +61,7 @@ namespace ompl
/** \brief Constructor takes the maximum number of nearest neighbors to return (\e k) and the
nearest neighbors datastruture to use (\e nn) */
KStrategy(const unsigned int k,
- const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) :
+ const std::shared_ptr< NearestNeighbors<Milestone> > &nn) :
k_(k), nn_(nn)
{
neighbors_.reserve(k_);
@@ -72,7 +72,7 @@ namespace ompl
}
/** \brief Set the nearest neighbors datastructure to use */
- void setNearestNeighbors(const boost::shared_ptr< NearestNeighbors<Milestone> > &nn)
+ void setNearestNeighbors(const std::shared_ptr< NearestNeighbors<Milestone> > &nn)
{
nn_ = nn;
}
@@ -92,7 +92,7 @@ namespace ompl
unsigned int k_;
/** \brief Nearest neighbors data structure */
- boost::shared_ptr< NearestNeighbors<Milestone> > nn_;
+ std::shared_ptr< NearestNeighbors<Milestone> > nn_;
/** \brief Scratch space for storing k-nearest neighbors */
std::vector<Milestone> neighbors_;
@@ -127,7 +127,7 @@ namespace ompl
class KStarStrategy : public KStrategy<Milestone>
{
public:
- typedef boost::function<unsigned int()> NumNeighborsFn;
+ typedef std::function<unsigned int()> NumNeighborsFn;
/**
* \brief Constructor
*
@@ -138,7 +138,7 @@ namespace ompl
* is valid for all problem instances.
*/
KStarStrategy(const NumNeighborsFn& n,
- const boost::shared_ptr< NearestNeighbors<Milestone> > &nn,
+ const std::shared_ptr< NearestNeighbors<Milestone> > &nn,
const unsigned int d = 1) :
KStrategy<Milestone>(n(), nn), n_(n),
kPRMConstant_(boost::math::constants::e<double>() + (boost::math::constants::e<double>() / (double)d))
@@ -177,7 +177,7 @@ namespace ompl
*/
KBoundedStrategy(const unsigned int k,
const double bound,
- const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) :
+ const std::shared_ptr< NearestNeighbors<Milestone> > &nn) :
KStrategy<Milestone>(k, nn), bound_(bound)
{
}
diff --git a/src/ompl/geometric/planners/prm/LazyPRM.h b/src/ompl/geometric/planners/prm/LazyPRM.h
index 861c31e..4bad149 100644
--- a/src/ompl/geometric/planners/prm/LazyPRM.h
+++ b/src/ompl/geometric/planners/prm/LazyPRM.h
@@ -41,7 +41,7 @@
#include "ompl/datastructures/NearestNeighbors.h"
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
-#include <boost/function.hpp>
+#include <functional>
#include <utility>
#include <vector>
#include <map>
@@ -127,18 +127,18 @@ namespace ompl
typedef boost::graph_traits<Graph>::edge_descriptor Edge;
/** @brief A nearest neighbors data structure for roadmap vertices. */
- typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
+ typedef std::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
/** @brief A function returning the milestones that should be
* attempted to connect to. */
- typedef boost::function<const std::vector<Vertex>&(const Vertex)> ConnectionStrategy;
+ typedef std::function<const std::vector<Vertex>&(const Vertex)> ConnectionStrategy;
/** @brief A function that can reject connections.
This is called after previous connections from the neighbor list
have been added to the roadmap.
*/
- typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
+ typedef std::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
/** \brief Constructor */
LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
@@ -160,7 +160,7 @@ namespace ompl
{
nn_.reset(new NN<Vertex>());
if (!userSetConnectionStrategy_)
- connectionStrategy_.clear();
+ connectionStrategy_ = ConnectionStrategy();
if (isSetup())
setup();
}
diff --git a/src/ompl/geometric/planners/prm/PRM.h b/src/ompl/geometric/planners/prm/PRM.h
index 160bdc0..5ec5dbf 100644
--- a/src/ompl/geometric/planners/prm/PRM.h
+++ b/src/ompl/geometric/planners/prm/PRM.h
@@ -42,8 +42,8 @@
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/pending/disjoint_sets.hpp>
-#include <boost/function.hpp>
-#include <boost/thread.hpp>
+#include <functional>
+#include <mutex>
#include <utility>
#include <vector>
#include <map>
@@ -127,18 +127,18 @@ namespace ompl
typedef boost::graph_traits<Graph>::edge_descriptor Edge;
/** @brief A nearest neighbors data structure for roadmap vertices. */
- typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
+ typedef std::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
/** @brief A function returning the milestones that should be
* attempted to connect to. */
- typedef boost::function<const std::vector<Vertex>&(const Vertex)> ConnectionStrategy;
+ typedef std::function<const std::vector<Vertex>&(const Vertex)> ConnectionStrategy;
/** @brief A function that can reject connections.
This is called after previous connections from the neighbor list
have been added to the roadmap.
*/
- typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
+ typedef std::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
/** \brief Constructor */
PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
@@ -243,7 +243,7 @@ namespace ompl
{
nn_.reset(new NN<Vertex>());
if (!userSetConnectionStrategy_)
- connectionStrategy_.clear();
+ connectionStrategy_ = ConnectionStrategy();
if (isSetup())
setup();
}
@@ -323,19 +323,19 @@ namespace ompl
// Planner progress property functions
std::string getIterationCount() const
{
- return boost::lexical_cast<std::string>(iterations_);
+ return std::to_string(iterations_);
}
std::string getBestCost() const
{
- return boost::lexical_cast<std::string>(bestCost_);
+ return std::to_string(bestCost_.value());
}
std::string getMilestoneCountString() const
{
- return boost::lexical_cast<std::string>(milestoneCount());
+ return std::to_string(milestoneCount());
}
std::string getEdgeCountString() const
{
- return boost::lexical_cast<std::string>(edgeCount());
+ return std::to_string(edgeCount());
}
/** \brief Flag indicating whether the default connection strategy is the Star strategy */
@@ -395,7 +395,7 @@ namespace ompl
bool addedNewSolution_;
/** \brief Mutex to guard access to the Graph member (g_) */
- mutable boost::mutex graphMutex_;
+ mutable std::mutex graphMutex_;
/** \brief Objective cost function for PRM graph edges */
base::OptimizationObjectivePtr opt_;
diff --git a/src/ompl/geometric/planners/prm/SPARS.h b/src/ompl/geometric/planners/prm/SPARS.h
index 21819e1..8fd52a4 100644
--- a/src/ompl/geometric/planners/prm/SPARS.h
+++ b/src/ompl/geometric/planners/prm/SPARS.h
@@ -43,12 +43,12 @@
#include "ompl/util/Time.h"
#include <boost/range/adaptor/map.hpp>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/pending/disjoint_sets.hpp>
-#include <boost/function.hpp>
-#include <boost/thread.hpp>
+#include <functional>
+#include <mutex>
#include <iostream>
#include <fstream>
#include <utility>
@@ -115,21 +115,11 @@ namespace ompl
typedef unsigned long int VertexIndexType;
/** \brief Hash for storing interface information. */
- typedef boost::unordered_map<VertexIndexType, std::set<VertexIndexType>, boost::hash<VertexIndexType> > InterfaceHash;
+ typedef std::unordered_map<VertexIndexType, std::set<VertexIndexType> > InterfaceHash;
/** \brief Internal representation of a dense path */
typedef std::deque<base::State*> DensePath;
- // The InterfaceHash structure is wrapped inside of this struct due to a compilation error on
- // GCC 4.6 with Boost 1.48. An implicit assignment operator overload does not compile with these
- // components, so an explicit overload is given here.
- // Remove this struct when the minimum Boost requirement is > v1.48.
- struct InterfaceHashStruct
- {
- InterfaceHashStruct& operator=(const InterfaceHashStruct &rhs) { interfaceHash = rhs.interfaceHash; return *this; }
- InterfaceHash interfaceHash;
- };
-
/**
@brief The constructed roadmap spanner.
@@ -149,7 +139,7 @@ namespace ompl
boost::property < boost::vertex_rank_t, VertexIndexType,
boost::property < vertex_color_t, GuardType,
boost::property < vertex_list_t, std::set<VertexIndexType>,
- boost::property < vertex_interface_list_t, InterfaceHashStruct > > > > > >,
+ boost::property < vertex_interface_list_t, InterfaceHash > > > > > >,
boost::property < boost::edge_weight_t, base::Cost >
> SpannerGraph;
@@ -160,7 +150,7 @@ namespace ompl
typedef boost::graph_traits<SpannerGraph>::edge_descriptor SparseEdge;
/** \brief Nearest neighbor structure which works over the SpannerGraph */
- typedef boost::shared_ptr< NearestNeighbors<SparseVertex> > SparseNeighbors;
+ typedef std::shared_ptr< NearestNeighbors<SparseVertex> > SparseNeighbors;
/**
@brief The underlying roadmap graph.
@@ -193,7 +183,7 @@ namespace ompl
typedef boost::graph_traits<DenseGraph>::edge_descriptor DenseEdge;
/** \brief Nearest neighbor structure which works over the DenseGraph */
- typedef boost::shared_ptr< NearestNeighbors<DenseVertex> > DenseNeighbors;
+ typedef std::shared_ptr< NearestNeighbors<DenseVertex> > DenseNeighbors;
/** \brief Constructor. */
SPARS(const base::SpaceInformationPtr &si);
@@ -242,7 +232,7 @@ namespace ompl
void setDenseNeighbors()
{
nn_.reset(new NN<DenseVertex>());
- connectionStrategy_.clear();
+ connectionStrategy_ = std::function<const std::vector<DenseVertex>&(const DenseVertex)>();
if (isSetup())
setup();
}
@@ -536,7 +526,7 @@ namespace ompl
sparseDJSets_;
/** \brief Function that returns the milestones to attempt connections with */
- boost::function<const std::vector<DenseVertex>&(const DenseVertex)> connectionStrategy_;
+ std::function<const std::vector<DenseVertex>&(const DenseVertex)> connectionStrategy_;
/** \brief A counter for the number of consecutive failed iterations of the algorithm */
unsigned int consecutiveFailures_;
@@ -566,7 +556,7 @@ namespace ompl
RNG rng_;
/** \brief Mutex to guard access to the graphs */
- mutable boost::mutex graphMutex_;
+ mutable std::mutex graphMutex_;
/** \brief Objective cost function for PRM graph edges */
base::OptimizationObjectivePtr opt_;
diff --git a/src/ompl/geometric/planners/prm/SPARStwo.h b/src/ompl/geometric/planners/prm/SPARStwo.h
index eb5d66a..69c2729 100644
--- a/src/ompl/geometric/planners/prm/SPARStwo.h
+++ b/src/ompl/geometric/planners/prm/SPARStwo.h
@@ -41,14 +41,15 @@
#include "ompl/datastructures/NearestNeighbors.h"
#include "ompl/geometric/PathSimplifier.h"
#include "ompl/util/Time.h"
+#include "ompl/util/Hash.h"
#include <boost/range/adaptor/map.hpp>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/pending/disjoint_sets.hpp>
-#include <boost/function.hpp>
-#include <boost/thread.hpp>
+#include <functional>
+#include <mutex>
#include <iostream>
#include <fstream>
#include <utility>
@@ -113,10 +114,10 @@ namespace ompl
/** \brief Constructor */
InterfaceData() :
- pointA_(NULL),
- pointB_(NULL),
- sigmaA_(NULL),
- sigmaB_(NULL),
+ pointA_(nullptr),
+ pointB_(nullptr),
+ sigmaA_(nullptr),
+ sigmaB_(nullptr),
d_(std::numeric_limits<double>::infinity())
{
}
@@ -127,22 +128,22 @@ namespace ompl
if (pointA_)
{
si->freeState(pointA_);
- pointA_ = NULL;
+ pointA_ = nullptr;
}
if (pointB_)
{
si->freeState(pointB_);
- pointB_ = NULL;
+ pointB_ = nullptr;
}
if (sigmaA_)
{
si->freeState(sigmaA_);
- sigmaA_ = NULL;
+ sigmaA_ = nullptr;
}
if (sigmaB_)
{
si->freeState(sigmaB_);
- sigmaB_ = NULL;
+ sigmaB_ = nullptr;
}
d_ = std::numeric_limits<double>::infinity();
}
@@ -179,17 +180,7 @@ namespace ompl
};
/** \brief the hash which maps pairs of neighbor points to pairs of states */
- typedef boost::unordered_map< VertexPair, InterfaceData, boost::hash< VertexPair > > InterfaceHash;
-
- // The InterfaceHash structure is wrapped inside of this struct due to a compilation error on
- // GCC 4.6 with Boost 1.48. An implicit assignment operator overload does not compile with these
- // components, so an explicit overload is given here.
- // Remove this struct when the minimum Boost requirement is > v1.48.
- struct InterfaceHashStruct
- {
- InterfaceHashStruct& operator=(const InterfaceHashStruct &rhs) { interfaceHash = rhs.interfaceHash; return *this; }
- InterfaceHash interfaceHash;
- };
+ typedef std::unordered_map<VertexPair, InterfaceData> InterfaceHash;
struct vertex_state_t {
typedef boost::vertex_property_tag kind;
@@ -224,7 +215,7 @@ namespace ompl
boost::property < boost::vertex_predecessor_t, VertexIndexType,
boost::property < boost::vertex_rank_t, VertexIndexType,
boost::property < vertex_color_t, GuardType,
- boost::property < vertex_interface_data_t, InterfaceHashStruct > > > > >,
+ boost::property < vertex_interface_data_t, InterfaceHash > > > > >,
boost::property < boost::edge_weight_t, base::Cost >
> Graph;
@@ -456,7 +447,7 @@ namespace ompl
base::StateSamplerPtr simpleSampler_;
/** \brief Nearest neighbors data structure */
- boost::shared_ptr< NearestNeighbors<Vertex> > nn_;
+ std::shared_ptr< NearestNeighbors<Vertex> > nn_;
/** \brief Connectivity graph */
Graph g_;
@@ -521,7 +512,7 @@ namespace ompl
double denseDelta_;
/** \brief Mutex to guard access to the Graph member (g_) */
- mutable boost::mutex graphMutex_;
+ mutable std::mutex graphMutex_;
/** \brief Objective cost function for PRM graph edges */
base::OptimizationObjectivePtr opt_;
diff --git a/src/ompl/geometric/planners/prm/src/GoalVisitor.hpp b/src/ompl/geometric/planners/prm/src/GoalVisitor.hpp
index 9d39448..f27026b 100644
--- a/src/ompl/geometric/planners/prm/src/GoalVisitor.hpp
+++ b/src/ompl/geometric/planners/prm/src/GoalVisitor.hpp
@@ -40,6 +40,8 @@
#ifndef OMPL_GEOMETRIC_PLANNERS_PRM_A_STAR_GOAL_VISITOR_
#define OMPL_GEOMETRIC_PLANNERS_PRM_A_STAR_GOAL_VISITOR_
+#include <boost/graph/astar_search.hpp>
+
namespace
{
struct AStarFoundGoal {}; // exception for termination
diff --git a/src/ompl/geometric/planners/prm/src/LazyPRM.cpp b/src/ompl/geometric/planners/prm/src/LazyPRM.cpp
index 3bb1ff6..4176da7 100644
--- a/src/ompl/geometric/planners/prm/src/LazyPRM.cpp
+++ b/src/ompl/geometric/planners/prm/src/LazyPRM.cpp
@@ -39,7 +39,6 @@
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/geometric/planners/prm/ConnectionStrategy.h"
#include "ompl/tools/config/SelfConfig.h"
-#include <boost/lambda/bind.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/graph/lookup_edge.hpp>
@@ -89,13 +88,13 @@ ompl::geometric::LazyPRM::LazyPRM(const base::SpaceInformationPtr &si, bool star
Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &LazyPRM::setMaxNearestNeighbors, std::string("8:1000"));
addPlannerProgressProperty("iterations INTEGER",
- boost::bind(&LazyPRM::getIterationCount, this));
+ std::bind(&LazyPRM::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
- boost::bind(&LazyPRM::getBestCost, this));
+ std::bind(&LazyPRM::getBestCost, this));
addPlannerProgressProperty("milestone count INTEGER",
- boost::bind(&LazyPRM::getMilestoneCountString, this));
+ std::bind(&LazyPRM::getMilestoneCountString, this));
addPlannerProgressProperty("edge count INTEGER",
- boost::bind(&LazyPRM::getEdgeCountString, this));
+ std::bind(&LazyPRM::getEdgeCountString, this));
}
ompl::geometric::LazyPRM::~LazyPRM()
@@ -111,17 +110,18 @@ void ompl::geometric::LazyPRM::setup()
if (!nn_)
{
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
- nn_->setDistanceFunction(boost::bind(&LazyPRM::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&LazyPRM::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
}
if (!connectionStrategy_)
{
if (starStrategy_)
- connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&LazyPRM::milestoneCount, this), nn_, si_->getStateDimension());
+ connectionStrategy_ = KStarStrategy<Vertex>(std::bind(&LazyPRM::milestoneCount, this), nn_, si_->getStateDimension());
else
connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
}
if (!connectionFilter_)
- connectionFilter_ = boost::lambda::constant(true);
+ connectionFilter_ = [] (const Vertex&, const Vertex&) { return true; };
// Setup optimization objective
//
@@ -152,7 +152,7 @@ void ompl::geometric::LazyPRM::setRange(double distance)
{
maxDistance_ = distance;
if (!userSetConnectionStrategy_)
- connectionStrategy_.clear();
+ connectionStrategy_ = ConnectionStrategy();
if (isSetup())
setup();
}
@@ -164,10 +164,11 @@ void ompl::geometric::LazyPRM::setMaxNearestNeighbors(unsigned int k)
if (!nn_)
{
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
- nn_->setDistanceFunction(boost::bind(&LazyPRM::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&LazyPRM::distanceFunction, this,
+ std::placeholders::_1, std::placeholders::_2));
}
if (!userSetConnectionStrategy_)
- connectionStrategy_.clear();
+ connectionStrategy_ = ConnectionStrategy();
if (isSetup())
setup();
}
@@ -419,12 +420,12 @@ ompl::base::PathPtr ompl::geometric::LazyPRM::constructSolution(const Vertex &st
{
// Consider using a persistent distance_map if it's slow
boost::astar_search(g_, start,
- boost::bind(&LazyPRM::costHeuristic, this, _1, goal),
+ std::bind(&LazyPRM::costHeuristic, this, std::placeholders::_1, goal),
boost::predecessor_map(prev).
- distance_compare(boost::bind(&base::OptimizationObjective::
- isCostBetterThan, opt_.get(), _1, _2)).
- distance_combine(boost::bind(&base::OptimizationObjective::
- combineCosts, opt_.get(), _1, _2)).
+ distance_compare(std::bind(&base::OptimizationObjective::
+ isCostBetterThan, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
+ distance_combine(std::bind(&base::OptimizationObjective::
+ combineCosts, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
distance_inf(opt_->infiniteCost()).
distance_zero(opt_->identityCost()).
visitor(AStarGoalVisitor<Vertex>(goal)));
diff --git a/src/ompl/geometric/planners/prm/src/PRM.cpp b/src/ompl/geometric/planners/prm/src/PRM.cpp
index 6cdfe27..0d4de37 100644
--- a/src/ompl/geometric/planners/prm/src/PRM.cpp
+++ b/src/ompl/geometric/planners/prm/src/PRM.cpp
@@ -41,12 +41,11 @@
#include "ompl/datastructures/PDF.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/tools/config/MagicConstants.h"
-#include <boost/lambda/bind.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/foreach.hpp>
-#include <boost/thread.hpp>
+#include <thread>
#include "GoalVisitor.hpp"
@@ -93,13 +92,13 @@ ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy
Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors, std::string("8:1000"));
addPlannerProgressProperty("iterations INTEGER",
- boost::bind(&PRM::getIterationCount, this));
+ std::bind(&PRM::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
- boost::bind(&PRM::getBestCost, this));
+ std::bind(&PRM::getBestCost, this));
addPlannerProgressProperty("milestone count INTEGER",
- boost::bind(&PRM::getMilestoneCountString, this));
+ std::bind(&PRM::getMilestoneCountString, this));
addPlannerProgressProperty("edge count INTEGER",
- boost::bind(&PRM::getEdgeCountString, this));
+ std::bind(&PRM::getEdgeCountString, this));
}
ompl::geometric::PRM::~PRM()
@@ -115,17 +114,17 @@ void ompl::geometric::PRM::setup()
specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
specs_.multithreaded = true;
- nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&PRM::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
if (!connectionStrategy_)
{
if (starStrategy_)
- connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
+ connectionStrategy_ = KStarStrategy<Vertex>(std::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
else
connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
}
if (!connectionFilter_)
- connectionFilter_ = boost::lambda::constant(true);
+ connectionFilter_ = [] (const Vertex&, const Vertex&) { return true; };
// Setup optimization objective
//
@@ -159,10 +158,10 @@ void ompl::geometric::PRM::setMaxNearestNeighbors(unsigned int k)
specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
specs_.multithreaded = true;
- nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&PRM::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
if (!userSetConnectionStrategy_)
- connectionStrategy_.clear();
+ connectionStrategy_ = ConnectionStrategy();
if (isSetup())
setup();
}
@@ -340,7 +339,7 @@ void ompl::geometric::PRM::checkForSolution(const base::PlannerTerminationCondit
addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
// Sleep for 1ms
if (!addedNewSolution_)
- boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
@@ -436,11 +435,11 @@ ompl::base::PlannerStatus ompl::geometric::PRM::solve(const base::PlannerTermina
// Reset addedNewSolution_ member and create solution checking thread
addedNewSolution_ = false;
base::PathPtr sol;
- boost::thread slnThread(boost::bind(&PRM::checkForSolution, this, ptc, boost::ref(sol)));
+ std::thread slnThread(std::bind(&PRM::checkForSolution, this, ptc, boost::ref(sol)));
// construct new planner termination condition that fires when the given ptc is true, or a solution is found
base::PlannerTerminationCondition ptcOrSolutionFound =
- base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&PRM::addedNewSolution, this)));
+ base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(std::bind(&PRM::addedNewSolution, this)));
constructRoadmap(ptcOrSolutionFound);
@@ -491,7 +490,7 @@ void ompl::geometric::PRM::constructRoadmap(const base::PlannerTerminationCondit
ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
Vertex m = boost::add_vertex(g_);
stateProperty_[m] = state;
@@ -537,19 +536,19 @@ bool ompl::geometric::PRM::sameComponent(Vertex m1, Vertex m2)
ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex &start, const Vertex &goal)
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
try
{
// Consider using a persistent distance_map if it's slow
boost::astar_search(g_, start,
- boost::bind(&PRM::costHeuristic, this, _1, goal),
+ std::bind(&PRM::costHeuristic, this, std::placeholders::_1, goal),
boost::predecessor_map(prev).
- distance_compare(boost::bind(&base::OptimizationObjective::
- isCostBetterThan, opt_.get(), _1, _2)).
- distance_combine(boost::bind(&base::OptimizationObjective::
- combineCosts, opt_.get(), _1, _2)).
+ distance_compare(std::bind(&base::OptimizationObjective::
+ isCostBetterThan, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
+ distance_combine(std::bind(&base::OptimizationObjective::
+ combineCosts, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
distance_inf(opt_->infiniteCost()).
distance_zero(opt_->identityCost()).
visitor(AStarGoalVisitor<Vertex>(goal)));
diff --git a/src/ompl/geometric/planners/prm/src/SPARS.cpp b/src/ompl/geometric/planners/prm/src/SPARS.cpp
index aa3f6d4..c7c2f49 100644
--- a/src/ompl/geometric/planners/prm/src/SPARS.cpp
+++ b/src/ompl/geometric/planners/prm/src/SPARS.cpp
@@ -40,7 +40,8 @@
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/tools/config/MagicConstants.h"
-#include <boost/bind.hpp>
+#include <functional>
+#include <thread>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
@@ -88,9 +89,9 @@ ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si) :
Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:3000");
addPlannerProgressProperty("iterations INTEGER",
- boost::bind(&SPARS::getIterationCount, this));
+ std::bind(&SPARS::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
- boost::bind(&SPARS::getBestCost, this));
+ std::bind(&SPARS::getBestCost, this));
}
ompl::geometric::SPARS::~SPARS()
@@ -103,12 +104,12 @@ void ompl::geometric::SPARS::setup()
Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(this));
- nn_->setDistanceFunction(boost::bind(&SPARS::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&SPARS::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
if (!snn_)
snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(this));
- snn_->setDistanceFunction(boost::bind(&SPARS::sparseDistanceFunction, this, _1, _2));
+ snn_->setDistanceFunction(std::bind(&SPARS::sparseDistanceFunction, this, std::placeholders::_1, std::placeholders::_2));
if (!connectionStrategy_)
- connectionStrategy_ = KStarStrategy<DenseVertex>(boost::bind(&SPARS::milestoneCount, this), nn_, si_->getStateDimension());
+ connectionStrategy_ = KStarStrategy<DenseVertex>(std::bind(&SPARS::milestoneCount, this), nn_, si_->getStateDimension());
double maxExt = si_->getMaximumExtent();
sparseDelta_ = sparseDeltaFraction_ * maxExt;
denseDelta_ = denseDeltaFraction_ * maxExt;
@@ -177,16 +178,16 @@ void ompl::geometric::SPARS::clear()
void ompl::geometric::SPARS::freeMemory()
{
foreach (DenseVertex v, boost::vertices(g_))
- if( stateProperty_[v] != NULL )
+ if( stateProperty_[v] != nullptr )
{
si_->freeState(stateProperty_[v]);
- stateProperty_[v] = NULL;
+ stateProperty_[v] = nullptr;
}
foreach (SparseVertex n, boost::vertices(s_))
- if( sparseStateProperty_[n] != NULL )
+ if( sparseStateProperty_[n] != nullptr )
{
si_->freeState(sparseStateProperty_[n]);
- sparseStateProperty_[n] = NULL;
+ sparseStateProperty_[n] = nullptr;
}
s_.clear();
g_.clear();
@@ -233,7 +234,7 @@ void ompl::geometric::SPARS::checkForSolution(const base::PlannerTerminationCond
addedSolution_ = haveSolution(startM_, goalM_, solution);
// Sleep for 1ms
if (!addedSolution_)
- boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
@@ -289,13 +290,13 @@ bool ompl::geometric::SPARS::reachedFailureLimit() const
void ompl::geometric::SPARS::checkQueryStateInitialization()
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
if (boost::num_vertices(g_) < 1)
{
sparseQueryVertex_ = boost::add_vertex(s_);
queryVertex_ = boost::add_vertex(g_);
- sparseStateProperty_[sparseQueryVertex_] = NULL;
- stateProperty_[queryVertex_] = NULL;
+ sparseStateProperty_[sparseQueryVertex_] = nullptr;
+ stateProperty_[queryVertex_] = nullptr;
}
}
@@ -356,12 +357,12 @@ ompl::base::PlannerStatus ompl::geometric::SPARS::solve(const base::PlannerTermi
resetFailures();
base::PathPtr sol;
base::PlannerTerminationCondition ptcOrFail =
- base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedFailureLimit, this)));
- boost::thread slnThread(boost::bind(&SPARS::checkForSolution, this, ptcOrFail, boost::ref(sol)));
+ base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(std::bind(&SPARS::reachedFailureLimit, this)));
+ std::thread slnThread(std::bind(&SPARS::checkForSolution, this, ptcOrFail, boost::ref(sol)));
// Construct planner termination condition which also takes maxFailures_ and addedSolution_ into account
base::PlannerTerminationCondition ptcOrStop =
- base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedTerminationCriterion, this)));
+ base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(std::bind(&SPARS::reachedTerminationCriterion, this)));
constructRoadmap(ptcOrStop);
// Ensure slnThread is ceased before exiting solve
@@ -384,7 +385,7 @@ void ompl::geometric::SPARS::constructRoadmap(const base::PlannerTerminationCond
{
resetFailures();
base::PlannerTerminationCondition ptcOrFail =
- base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedFailureLimit, this)));
+ base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(std::bind(&SPARS::reachedFailureLimit, this)));
constructRoadmap(ptcOrFail);
}
else
@@ -454,7 +455,7 @@ void ompl::geometric::SPARS::constructRoadmap(const base::PlannerTerminationCond
ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addMilestone(base::State *state)
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
DenseVertex m = boost::add_vertex(g_);
stateProperty_[m] = state;
@@ -494,7 +495,7 @@ ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addMilestone(base::S
ompl::geometric::SPARS::SparseVertex ompl::geometric::SPARS::addGuard(base::State *state, GuardType type)
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
SparseVertex v = boost::add_vertex(s_);
sparseStateProperty_[v] = state;
@@ -513,7 +514,7 @@ void ompl::geometric::SPARS::connectSparsePoints(SparseVertex v, SparseVertex vp
{
const base::Cost weight(costHeuristic(v, vp));
const SpannerGraph::edge_property_type properties(weight);
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
boost::add_edge(v, vp, properties, s_);
sparseDJSets_.union_set(v, vp);
}
@@ -522,7 +523,7 @@ void ompl::geometric::SPARS::connectDensePoints( DenseVertex v, DenseVertex vp )
{
const double weight = distanceFunction(v, vp);
const DenseGraph::edge_property_type properties(weight);
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
boost::add_edge(v, vp, properties, g_);
}
@@ -648,7 +649,7 @@ bool ompl::geometric::SPARS::checkAddPath(DenseVertex q, const std::vector<Dense
{
SparseVertex vpp = VPPs[j];
//For each q", which are stored interface nodes on v for i(vpp,v)
- foreach( DenseVertex qpp, interfaceListsProperty_[v].interfaceHash[vpp] )
+ foreach( DenseVertex qpp, interfaceListsProperty_[v][vpp] )
{
// check that representatives are consistent
assert(representativesProperty_[qpp] == v);
@@ -743,7 +744,7 @@ void ompl::geometric::SPARS::getSparseNeighbors(base::State *inState, std::vecto
graphNeighborhood.clear();
snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood);
- sparseStateProperty_[sparseQueryVertex_] = NULL;
+ sparseStateProperty_[sparseQueryVertex_] = nullptr;
}
void ompl::geometric::SPARS::filterVisibleNeighbors(base::State *inState, const std::vector<SparseVertex> &graphNeighborhood,
@@ -815,7 +816,7 @@ void ompl::geometric::SPARS::updateRepresentatives(SparseVertex v)
nn_->nearestR( queryVertex_, sparseDelta_ + denseDelta_, dense_points );
- stateProperty_[ queryVertex_ ] = NULL;
+ stateProperty_[ queryVertex_ ] = nullptr;
//For each of those points
for (std::size_t i = 0 ; i < dense_points.size() ; ++i)
@@ -876,7 +877,7 @@ void ompl::geometric::SPARS::addToRepresentatives(DenseVertex q, SparseVertex re
foreach( SparseVertex v, oreps )
{
assert(rep == representativesProperty_[q]);
- bool new_insert = interfaceListsProperty_[rep].interfaceHash[v].insert(q).second;
+ bool new_insert = interfaceListsProperty_[rep][v].insert(q).second;
if (!new_insert)
assert(false);
}
@@ -889,10 +890,10 @@ void ompl::geometric::SPARS::removeFromRepresentatives(DenseVertex q, SparseVert
nonInterfaceListsProperty_[rep].erase(q);
// From each of the interfaces
- foreach (SparseVertex vpp, interfaceListsProperty_[rep].interfaceHash | boost::adaptors::map_keys)
+ foreach (SparseVertex vpp, interfaceListsProperty_[rep] | boost::adaptors::map_keys)
{
// Remove this node from that list
- interfaceListsProperty_[rep].interfaceHash[vpp].erase( q );
+ interfaceListsProperty_[rep][vpp].erase( q );
}
}
@@ -909,7 +910,7 @@ void ompl::geometric::SPARS::computeX(SparseVertex v, SparseVertex vp, SparseVer
Xs.clear();
foreach( SparseVertex cx, boost::adjacent_vertices( vpp, s_ ) )
if( boost::edge( cx, v, s_ ).second && !boost::edge( cx, vp, s_ ).second )
- if (interfaceListsProperty_[vpp].interfaceHash[cx].size() > 0)
+ if (interfaceListsProperty_[vpp][cx].size() > 0)
Xs.push_back( cx );
Xs.push_back( vpp );
}
@@ -953,7 +954,7 @@ void ompl::geometric::SPARS::getInterfaceNeighborhood(DenseVertex q, std::vector
ompl::base::PathPtr ompl::geometric::SPARS::constructSolution(const SparseVertex start, const SparseVertex goal) const
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_));
@@ -961,12 +962,12 @@ ompl::base::PathPtr ompl::geometric::SPARS::constructSolution(const SparseVertex
{
// Consider using a persistent distance_map if it's slow
boost::astar_search(s_, start,
- boost::bind(&SPARS::costHeuristic, this, _1, goal),
+ std::bind(&SPARS::costHeuristic, this, std::placeholders::_1, goal),
boost::predecessor_map(prev).
- distance_compare(boost::bind(&base::OptimizationObjective::
- isCostBetterThan, opt_.get(), _1, _2)).
- distance_combine(boost::bind(&base::OptimizationObjective::
- combineCosts, opt_.get(), _1, _2)).
+ distance_compare(std::bind(&base::OptimizationObjective::
+ isCostBetterThan, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
+ distance_combine(std::bind(&base::OptimizationObjective::
+ combineCosts, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
distance_inf(opt_->infiniteCost()).
distance_zero(opt_->identityCost()).
visitor(AStarGoalVisitor<SparseVertex>(goal)));
@@ -999,7 +1000,7 @@ void ompl::geometric::SPARS::computeDensePath(const DenseVertex start, const Den
try
{
boost::astar_search(g_, start,
- boost::bind(&SPARS::distanceFunction, this, _1, goal),
+ std::bind(&SPARS::distanceFunction, this, std::placeholders::_1, goal),
boost::predecessor_map(prev).
visitor(AStarGoalVisitor<DenseVertex>(goal)));
}
diff --git a/src/ompl/geometric/planners/prm/src/SPARStwo.cpp b/src/ompl/geometric/planners/prm/src/SPARStwo.cpp
index 05254a8..d763e9a 100644
--- a/src/ompl/geometric/planners/prm/src/SPARStwo.cpp
+++ b/src/ompl/geometric/planners/prm/src/SPARStwo.cpp
@@ -39,11 +39,11 @@
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
#include "ompl/tools/config/SelfConfig.h"
-#include <boost/lambda/bind.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/foreach.hpp>
+#include <thread>
#include "GoalVisitor.hpp"
@@ -83,9 +83,9 @@ ompl::geometric::SPARStwo::SPARStwo(const base::SpaceInformationPtr &si) :
Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures, "100:10:3000");
addPlannerProgressProperty("iterations INTEGER",
- boost::bind(&SPARStwo::getIterationCount, this));
+ std::bind(&SPARStwo::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
- boost::bind(&SPARStwo::getBestCost, this));
+ std::bind(&SPARStwo::getBestCost, this));
}
ompl::geometric::SPARStwo::~SPARStwo()
@@ -98,7 +98,7 @@ void ompl::geometric::SPARStwo::setup()
Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
- nn_->setDistanceFunction(boost::bind(&SPARStwo::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&SPARStwo::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
double maxExt = si_->getMaximumExtent();
sparseDelta_ = sparseDeltaFraction_ * maxExt;
denseDelta_ = denseDeltaFraction_ * maxExt;
@@ -159,11 +159,11 @@ void ompl::geometric::SPARStwo::freeMemory()
foreach (Vertex v, boost::vertices(g_))
{
- foreach (InterfaceData &d, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_values)
+ foreach (InterfaceData &d, interfaceDataProperty_[v] | boost::adaptors::map_values)
d.clear(si_);
- if( stateProperty_[v] != NULL )
+ if( stateProperty_[v] != nullptr )
si_->freeState(stateProperty_[v]);
- stateProperty_[v] = NULL;
+ stateProperty_[v] = nullptr;
}
g_.clear();
@@ -229,7 +229,7 @@ void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationC
{
resetFailures();
base::PlannerTerminationCondition ptcOrFail =
- base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedFailureLimit, this)));
+ base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(std::bind(&SPARStwo::reachedFailureLimit, this)));
constructRoadmap(ptcOrFail);
}
else
@@ -295,11 +295,11 @@ void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationC
void ompl::geometric::SPARStwo::checkQueryStateInitialization()
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
if (boost::num_vertices(g_) < 1)
{
queryVertex_ = boost::add_vertex( g_ );
- stateProperty_[queryVertex_] = NULL;
+ stateProperty_[queryVertex_] = nullptr;
}
}
@@ -348,12 +348,12 @@ ompl::base::PlannerStatus ompl::geometric::SPARStwo::solve(const base::PlannerTe
resetFailures();
base::PathPtr sol;
base::PlannerTerminationCondition ptcOrFail =
- base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedFailureLimit, this)));
- boost::thread slnThread(boost::bind(&SPARStwo::checkForSolution, this, ptcOrFail, boost::ref(sol)));
+ base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(std::bind(&SPARStwo::reachedFailureLimit, this)));
+ std::thread slnThread(std::bind(&SPARStwo::checkForSolution, this, ptcOrFail, boost::ref(sol)));
//Construct planner termination condition which also takes M into account
base::PlannerTerminationCondition ptcOrStop =
- base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedTerminationCriterion, this)));
+ base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(std::bind(&SPARStwo::reachedTerminationCriterion, this)));
constructRoadmap(ptcOrStop);
// Ensure slnThread is ceased before exiting solve
@@ -385,7 +385,7 @@ void ompl::geometric::SPARStwo::checkForSolution(const base::PlannerTerminationC
addedSolution_ = haveSolution(startM_, goalM_, solution);
// Sleep for 1ms
if (!addedSolution_)
- boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
@@ -562,7 +562,7 @@ void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<
visibleNeighborhood.clear();
stateProperty_[ queryVertex_ ] = st;
nn_->nearestR( queryVertex_, sparseDelta_, graphNeighborhood);
- stateProperty_[ queryVertex_ ] = NULL;
+ stateProperty_[ queryVertex_ ] = nullptr;
//Now that we got the neighbors from the NN, we must remove any we can't see
for (std::size_t i = 0; i < graphNeighborhood.size() ; ++i )
@@ -589,7 +589,7 @@ ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::findGraphRepresenta
std::vector<Vertex> nbh;
stateProperty_[ queryVertex_ ] = st;
nn_->nearestR( queryVertex_, sparseDelta_, nbh);
- stateProperty_[queryVertex_] = NULL;
+ stateProperty_[queryVertex_] = nullptr;
Vertex result = boost::graph_traits<Graph>::null_vertex();
@@ -696,7 +696,7 @@ ompl::geometric::SPARStwo::VertexPair ompl::geometric::SPARStwo::index( Vertex v
ompl::geometric::SPARStwo::InterfaceData& ompl::geometric::SPARStwo::getData( Vertex v, Vertex vp, Vertex vpp )
{
- return interfaceDataProperty_[v].interfaceHash[index( vp, vpp )];
+ return interfaceDataProperty_[v][index( vp, vpp )];
}
void ompl::geometric::SPARStwo::distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)
@@ -706,12 +706,12 @@ void ompl::geometric::SPARStwo::distanceCheck(Vertex rep, const base::State *q,
if (r < rp) // FIRST points represent r (the guy discovered through sampling)
{
- if (d.pointA_ == NULL) // If the point we're considering replacing (P_v(r,.)) isn't there
+ if (d.pointA_ == nullptr) // If the point we're considering replacing (P_v(r,.)) isn't there
//Then we know we're doing better, so add it
d.setFirst(q, s, si_);
else //Otherwise, he is there,
{
- if (d.pointB_ == NULL) //But if the other guy doesn't exist, we can't compare.
+ if (d.pointB_ == nullptr) //But if the other guy doesn't exist, we can't compare.
{
//Should probably keep the one that is further away from rep? Not known what to do in this case.
// \todo: is this not part of the algorithm?
@@ -724,12 +724,12 @@ void ompl::geometric::SPARStwo::distanceCheck(Vertex rep, const base::State *q,
}
else // SECOND points represent r (the guy discovered through sampling)
{
- if (d.pointB_ == NULL) //If the point we're considering replacing (P_V(.,r)) isn't there...
+ if (d.pointB_ == nullptr) //If the point we're considering replacing (P_V(.,r)) isn't there...
//Then we must be doing better, so add it
d.setSecond(q, s, si_);
else //Otherwise, he is there
{
- if (d.pointA_ == NULL) //But if the other guy doesn't exist, we can't compare.
+ if (d.pointA_ == nullptr) //But if the other guy doesn't exist, we can't compare.
{
//Should we be doing something cool here?
}
@@ -741,7 +741,7 @@ void ompl::geometric::SPARStwo::distanceCheck(Vertex rep, const base::State *q,
}
// Lastly, save what we have discovered
- interfaceDataProperty_[rep].interfaceHash[index(r, rp)] = d;
+ interfaceDataProperty_[rep][index(r, rp)] = d;
}
void ompl::geometric::SPARStwo::abandonLists(base::State *st)
@@ -751,19 +751,19 @@ void ompl::geometric::SPARStwo::abandonLists(base::State *st)
std::vector< Vertex > hold;
nn_->nearestR( queryVertex_, sparseDelta_, hold );
- stateProperty_[queryVertex_] = NULL;
+ stateProperty_[queryVertex_] = nullptr;
//For each of the vertices
foreach (Vertex v, hold)
{
- foreach (VertexPair r, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_keys)
- interfaceDataProperty_[v].interfaceHash[r].clear(si_);
+ foreach (VertexPair r, interfaceDataProperty_[v] | boost::adaptors::map_keys)
+ interfaceDataProperty_[v][r].clear(si_);
}
}
ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::addGuard(base::State *state, GuardType type)
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
Vertex m = boost::add_vertex(g_);
stateProperty_[m] = state;
@@ -786,26 +786,26 @@ void ompl::geometric::SPARStwo::connectGuards(Vertex v, Vertex vp)
const base::Cost weight(costHeuristic(v, vp));
const Graph::edge_property_type properties(weight);
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
boost::add_edge(v, vp, properties, g_);
disjointSets_.union_set(v, vp);
}
ompl::base::PathPtr ompl::geometric::SPARStwo::constructSolution(const Vertex start, const Vertex goal) const
{
- boost::mutex::scoped_lock _(graphMutex_);
+ std::lock_guard<std::mutex> _(graphMutex_);
boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
try
{
boost::astar_search(g_, start,
- boost::bind(&SPARStwo::costHeuristic, this, _1, goal),
+ std::bind(&SPARStwo::costHeuristic, this, std::placeholders::_1, goal),
boost::predecessor_map(prev).
- distance_compare(boost::bind(&base::OptimizationObjective::
- isCostBetterThan, opt_.get(), _1, _2)).
- distance_combine(boost::bind(&base::OptimizationObjective::
- combineCosts, opt_.get(), _1, _2)).
+ distance_compare(std::bind(&base::OptimizationObjective::
+ isCostBetterThan, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
+ distance_combine(std::bind(&base::OptimizationObjective::
+ combineCosts, opt_.get(), std::placeholders::_1, std::placeholders::_2)).
distance_inf(opt_->infiniteCost()).
distance_zero(opt_->identityCost()).
visitor(AStarGoalVisitor<Vertex>(goal)));
@@ -882,4 +882,3 @@ ompl::base::Cost ompl::geometric::SPARStwo::costHeuristic(Vertex u, Vertex v) co
{
return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
}
-
diff --git a/src/ompl/geometric/planners/rrt/BiTRRT.h b/src/ompl/geometric/planners/rrt/BiTRRT.h
index 475c98b..c2d6c99 100644
--- a/src/ompl/geometric/planners/rrt/BiTRRT.h
+++ b/src/ompl/geometric/planners/rrt/BiTRRT.h
@@ -179,10 +179,10 @@ namespace ompl
public:
/// \brief Default constructor
- Motion() : state(NULL), parent(NULL), root(NULL) {}
+ Motion() : state(nullptr), parent(nullptr), root(nullptr) {}
/// \brief Constructor that allocates memory for the state
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL), root(NULL) {}
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr), root(nullptr) {}
~Motion() {}
@@ -207,11 +207,11 @@ namespace ompl
/// \brief The nearest-neighbors data structure that contains the
/// entire the tree of motions generated during planning.
- typedef boost::shared_ptr< NearestNeighbors<Motion*> > TreeData;
+ typedef std::shared_ptr< NearestNeighbors<Motion*> > TreeData;
/// \brief Add a state to the given tree. The motion created
/// is returned.
- Motion* addMotion(const base::State* state, TreeData& tree, Motion* parent = NULL);
+ Motion* addMotion(const base::State* state, TreeData& tree, Motion* parent = nullptr);
/// \brief Transition test that filters transitions based on the
/// motion cost. If the motion cost is near or below zero, the motion
diff --git a/src/ompl/geometric/planners/rrt/LBTRRT.h b/src/ompl/geometric/planners/rrt/LBTRRT.h
index 2f61bf4..70e65f6 100755
--- a/src/ompl/geometric/planners/rrt/LBTRRT.h
+++ b/src/ompl/geometric/planners/rrt/LBTRRT.h
@@ -146,11 +146,11 @@ namespace ompl
// Planner progress property functions
std::string getIterationCount() const
{
- return boost::lexical_cast<std::string>(iterations_);
+ return std::to_string(iterations_);
}
std::string getBestCost() const
{
- return boost::lexical_cast<std::string>(bestCost_);
+ return std::to_string(bestCost_);
}
protected:
@@ -163,13 +163,13 @@ namespace ompl
{
public:
- Motion() : state_(NULL), parentApx_(NULL), costApx_(0.0)
+ Motion() : state_(nullptr), parentApx_(nullptr), costApx_(0.0)
{
}
/** \brief Constructor that allocates memory for the state */
Motion(const base::SpaceInformationPtr &si)
- : state_(si->allocState()), parentApx_(NULL), costApx_(0.0)
+ : state_(si->allocState()), parentApx_(nullptr), costApx_(0.0)
{
}
@@ -224,7 +224,7 @@ namespace ompl
{
}
- bool operator() (const Motion *motionA, const Motion *motionB)
+ bool operator() (const Motion *motionA, const Motion *motionB) const
{
return motionA->costLb_ < motionB->costLb_;
}
@@ -276,7 +276,7 @@ namespace ompl
base::StateSamplerPtr sampler_;
/** \brief A nearest-neighbors datastructure containing the tree of motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
/** \brief A graph of motions Glb*/
DynamicSSSP lowerBoundGraph_;
diff --git a/src/ompl/geometric/planners/rrt/LazyLBTRRT.h b/src/ompl/geometric/planners/rrt/LazyLBTRRT.h
index 25b0265..f7cbf88 100644
--- a/src/ompl/geometric/planners/rrt/LazyLBTRRT.h
+++ b/src/ompl/geometric/planners/rrt/LazyLBTRRT.h
@@ -44,9 +44,9 @@
#include <fstream>
#include <vector>
+#include <tuple>
#include <cassert>
-#include <boost/tuple/tuple.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
@@ -127,11 +127,11 @@ namespace ompl
// Planner progress property functions
std::string getIterationCount() const
{
- return boost::lexical_cast<std::string>(iterations_);
+ return std::to_string(iterations_);
}
std::string getBestCost() const
{
- return boost::lexical_cast<std::string>(bestCost_);
+ return std::to_string(bestCost_);
}
protected:
@@ -140,7 +140,7 @@ namespace ompl
{
public:
- Motion(void) : state_(NULL)
+ Motion(void) : state_(nullptr)
{
}
@@ -279,7 +279,7 @@ namespace ompl
LPAstarLb_->removeEdge(b->id_, a->id_);
return;
}
- boost::tuple<Motion*, base::State*, double> rrtExtend(
+ std::tuple<Motion*, base::State*, double> rrtExtend(
const base::GoalSampleableRegion *goal_s, base::State *xstate,
Motion *rmotion, double &approxdif);
void rrt(const base::PlannerTerminationCondition &ptc,
@@ -300,7 +300,7 @@ namespace ompl
base::StateSamplerPtr sampler_;
/** \brief A nearest-neighbors datastructure containing the tree of motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
/** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
double goalBias_;
diff --git a/src/ompl/geometric/planners/rrt/LazyRRT.h b/src/ompl/geometric/planners/rrt/LazyRRT.h
index 7350ef3..9d4bcf5 100644
--- a/src/ompl/geometric/planners/rrt/LazyRRT.h
+++ b/src/ompl/geometric/planners/rrt/LazyRRT.h
@@ -142,12 +142,12 @@ namespace ompl
{
public:
- Motion() : state(NULL), parent(NULL), valid(false)
+ Motion() : state(nullptr), parent(nullptr), valid(false)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL), valid(false)
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr), valid(false)
{
}
@@ -184,7 +184,7 @@ namespace ompl
base::StateSamplerPtr sampler_;
/** \brief A nearest-neighbors datastructure containing the tree of motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
/** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
double goalBias_;
diff --git a/src/ompl/geometric/planners/rrt/RRT.h b/src/ompl/geometric/planners/rrt/RRT.h
index a7ffeb2..209040c 100644
--- a/src/ompl/geometric/planners/rrt/RRT.h
+++ b/src/ompl/geometric/planners/rrt/RRT.h
@@ -133,12 +133,12 @@ namespace ompl
{
public:
- Motion() : state(NULL), parent(NULL)
+ Motion() : state(nullptr), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
{
}
@@ -167,7 +167,7 @@ namespace ompl
base::StateSamplerPtr sampler_;
/** \brief A nearest-neighbors datastructure containing the tree of motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
/** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
double goalBias_;
diff --git a/src/ompl/geometric/planners/rrt/RRTConnect.h b/src/ompl/geometric/planners/rrt/RRTConnect.h
index 6a8990c..e21158d 100644
--- a/src/ompl/geometric/planners/rrt/RRTConnect.h
+++ b/src/ompl/geometric/planners/rrt/RRTConnect.h
@@ -49,7 +49,7 @@ namespace ompl
/**
@anchor gRRTC
@par Short description
- The basic idea is to grow to RRTs, one from the start and
+ The basic idea is to grow two RRTs, one from the start and
one from the goal, and attempt to connect them.
@par External documentation
J. Kuffner and S.M. LaValle, RRT-connect: An efficient approach to single-query path planning, in <em>Proc. 2000 IEEE Intl. Conf. on Robotics and Automation</em>, pp. 995–1001, Apr. 2000. DOI: [10.1109/ROBOT.2000.844730](http://dx.doi.org/10.1109/ROBOT.2000.844730)<br>
@@ -106,13 +106,13 @@ namespace ompl
{
public:
- Motion() : root(NULL), state(NULL), parent(NULL)
+ Motion() : root(nullptr), state(nullptr), parent(nullptr)
{
- parent = NULL;
- state = NULL;
+ parent = nullptr;
+ state = nullptr;
}
- Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : root(nullptr), state(si->allocState()), parent(nullptr)
{
}
@@ -127,7 +127,7 @@ namespace ompl
};
/** \brief A nearest-neighbor datastructure representing a tree of motions */
- typedef boost::shared_ptr< NearestNeighbors<Motion*> > TreeData;
+ typedef std::shared_ptr< NearestNeighbors<Motion*> > TreeData;
/** \brief Information attached to growing a tree of motions (used internally) */
struct TreeGrowingInfo
diff --git a/src/ompl/geometric/planners/rrt/RRTstar.h b/src/ompl/geometric/planners/rrt/RRTstar.h
index 87b7aa4..d19708a 100644
--- a/src/ompl/geometric/planners/rrt/RRTstar.h
+++ b/src/ompl/geometric/planners/rrt/RRTstar.h
@@ -310,7 +310,7 @@ namespace ompl
/** \brief Constructor that allocates memory for the state. This constructor automatically allocates memory for \e state, \e cost, and \e incCost */
Motion(const base::SpaceInformationPtr &si) :
state(si->allocState()),
- parent(NULL)
+ parent(nullptr)
{
}
@@ -400,7 +400,7 @@ namespace ompl
base::InformedSamplerPtr infSampler_;
/** \brief A nearest-neighbors datastructure containing the tree of motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
/** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
double goalBias_;
@@ -477,11 +477,11 @@ namespace ompl
// Planner progress property functions
std::string numIterationsProperty() const
{
- return boost::lexical_cast<std::string>(numIterations());
+ return std::to_string(numIterations());
}
std::string bestCostProperty() const
{
- return boost::lexical_cast<std::string>(bestCost());
+ return std::to_string(bestCost().value());
}
};
}
diff --git a/src/ompl/geometric/planners/rrt/TRRT.h b/src/ompl/geometric/planners/rrt/TRRT.h
index d0b3776..f97c763 100644
--- a/src/ompl/geometric/planners/rrt/TRRT.h
+++ b/src/ompl/geometric/planners/rrt/TRRT.h
@@ -221,12 +221,12 @@ namespace ompl
{
public:
- Motion() : state(NULL), parent(NULL)
+ Motion() : state(nullptr), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
{
}
@@ -266,7 +266,7 @@ namespace ompl
base::StateSamplerPtr sampler_;
/** \brief A nearest-neighbors datastructure containing the tree of motions */
- boost::shared_ptr< NearestNeighbors<Motion*> > nearestNeighbors_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nearestNeighbors_;
/** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
double goalBias_;
diff --git a/src/ompl/geometric/planners/rrt/VFRRT.h b/src/ompl/geometric/planners/rrt/VFRRT.h
new file mode 100644
index 0000000..3c626f9
--- /dev/null
+++ b/src/ompl/geometric/planners/rrt/VFRRT.h
@@ -0,0 +1,155 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Caleb Voss and Wilson Beebe
+* 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.
+*********************************************************************/
+
+/* Authors: Caleb Voss, Wilson Beebe */
+
+#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_VFRRT_
+#define OMPL_GEOMETRIC_PLANNERS_RRT_VFRRT_
+
+#include <limits>
+
+#include <eigen3/Eigen/Core>
+
+#include <ompl/geometric/planners/rrt/RRT.h>
+
+namespace ompl
+{
+ namespace geometric
+ {
+ /**
+ \anchor gVFRRT
+ \par Short description
+ Vector Field Rapidly-exploring Random Tree (VFRRT) is a tree-based
+ motion planner that tries to minimize the so-called upstream cost
+ of a path. The upstream cost is defined by an integral over a
+ user-defined vector field.
+
+ \par External documentation
+ I. Ko, B. Kim, and F. C. Park, Randomized path planning on vector fields, <em>Intl. J. of Robotics Research,</em> 33(13), pp. 1664–1682, 2014. DOI: [10.1177/0278364914545812](http://dx.doi.org/10.1177/0278364914545812)<br>
+
+ [[PDF]](http://robotics.snu.ac.kr/fcp/files/_pdf_files_publications/201411_Randomized%20path%20planning.pdf)
+
+ */
+ class VFRRT : public RRT
+ {
+ public:
+ typedef std::function<Eigen::VectorXd(const base::State *state)> VectorField;
+
+ /** Constructor. */
+ VFRRT(const base::SpaceInformationPtr &si, const VectorField &vf, double exploration,
+ double initial_lambda, unsigned int update_freq);
+
+ /** Destructor. */
+ virtual ~VFRRT();
+
+ /** Reset internal data. */
+ virtual void clear();
+
+ /** Make a Monte Carlo estimate for the mean vector norm in the field. */
+ double determineMeanNorm();
+
+ /** Use the vector field to alter the direction of a sample. */
+ Eigen::VectorXd getNewDirection(const base::State *qnear, const base::State *qrand);
+
+ /** Calculates the weight omega which can be used to compute alpha and beta. */
+ double biasedSampling (const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield,
+ double lambdaScale);
+
+ /**
+ * Every nth time this function is called (where the nth step is the update
+ * frequency given in the constructor) the value of lambda is updated and
+ * the counts of efficient and inefficient samples added to the tree are
+ * reset to 0. The measure for exploration inefficiency is also reset to 0.
+ */
+ void updateGain ();
+
+ /**
+ * Computes alpha and beta, using these values to produce the vector returned by
+ * getNewDirection. This produced vector can be used to determine the direction an
+ * added state should be to maximize the upstream criterion of the produced path.
+ */
+ Eigen::VectorXd computeAlphaBeta (double omega, const Eigen::VectorXd &vrand,
+ const Eigen::VectorXd &vfield);
+
+ /**
+ * This attempts to extend the tree from the motion m to a new motion
+ * in the direction specified by the vector v.
+ */
+ Motion *extendTree (Motion *m, base::State* rstate, const Eigen::VectorXd &v);
+ /**
+ * Updates measures for exploration efficiency if a given motion m is added to the
+ * nearest NearestNeighbors structure.
+ */
+ void updateExplorationEfficiency(Motion *m);
+
+ /** Solve the planning problem. */
+ virtual base::PlannerStatus solve (const base::PlannerTerminationCondition &ptc);
+
+ virtual void setup();
+
+ private:
+
+ /** Vector field for the environemnt. */
+ const VectorField vf_;
+
+ /** Number of efficient nodes. */
+ unsigned int efficientCount_;
+
+ /** Number of inefficient nodes. */
+ unsigned int inefficientCount_;
+
+ /** Current inefficiency. */
+ double explorationInefficiency_;
+
+ /** User-specified exploration parameter. */
+ double explorationSetting_;
+
+ /** Current lambda value. */
+ double lambda_;
+
+ /** The number of steps until lambda is updated and efficiency metrics are reset. */
+ unsigned int nth_step_;
+
+ /** Current number of steps since lambda was updated/initialized. */
+ unsigned int step_;
+
+ /** Average norm of vectors in the field. */
+ double meanNorm_;
+
+ /** Dimensionality of vector field */
+ unsigned int vfdim_;
+ };
+ }
+}
+#endif
diff --git a/src/ompl/geometric/planners/rrt/pRRT.h b/src/ompl/geometric/planners/rrt/pRRT.h
index 87229b3..045c854 100644
--- a/src/ompl/geometric/planners/rrt/pRRT.h
+++ b/src/ompl/geometric/planners/rrt/pRRT.h
@@ -40,7 +40,8 @@
#include "ompl/geometric/planners/PlannerIncludes.h"
#include "ompl/base/StateSamplerArray.h"
#include "ompl/datastructures/NearestNeighbors.h"
-#include <boost/thread/mutex.hpp>
+#include <thread>
+#include <mutex>
namespace ompl
{
@@ -137,11 +138,11 @@ namespace ompl
{
public:
- Motion() : state(NULL), parent(NULL)
+ Motion() : state(nullptr), parent(nullptr)
{
}
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
{
}
@@ -159,7 +160,7 @@ namespace ompl
Motion *solution;
Motion *approxsol;
double approxdif;
- boost::mutex lock;
+ std::mutex lock;
};
void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
@@ -171,8 +172,8 @@ namespace ompl
}
base::StateSamplerArray<base::StateSampler> samplerArray_;
- boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
- boost::mutex nnLock_;
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
+ std::mutex nnLock_;
unsigned int threadCount_;
diff --git a/src/ompl/geometric/planners/rrt/src/BiTRRT.cpp b/src/ompl/geometric/planners/rrt/src/BiTRRT.cpp
index d4db1be..7859582 100644
--- a/src/ompl/geometric/planners/rrt/src/BiTRRT.cpp
+++ b/src/ompl/geometric/planners/rrt/src/BiTRRT.cpp
@@ -48,7 +48,7 @@ ompl::geometric::BiTRRT::BiTRRT(const base::SpaceInformationPtr &si) : base::Pla
specs_.directed = true;
maxDistance_ = 0.0; // set in setup()
- connectionPoint_ = std::make_pair<Motion*, Motion*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<Motion*, Motion*>(nullptr, nullptr);
Planner::declareParam<double>("range", this, &BiTRRT::setRange, &BiTRRT::getRange, "0.:1.:10000.");
@@ -106,7 +106,7 @@ void ompl::geometric::BiTRRT::clear()
tStart_->clear();
if (tGoal_)
tGoal_->clear();
- connectionPoint_ = std::make_pair<Motion*, Motion*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<Motion*, Motion*>(nullptr, nullptr);
// TRRT specific variables
temp_ = initTemperature_;
@@ -133,8 +133,8 @@ void ompl::geometric::BiTRRT::setup()
tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
if (!tGoal_)
tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- tStart_->setDistanceFunction(boost::bind(&BiTRRT::distanceFunction, this, _1, _2));
- tGoal_->setDistanceFunction(boost::bind(&BiTRRT::distanceFunction, this, _1, _2));
+ tStart_->setDistanceFunction(std::bind(&BiTRRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
+ tGoal_->setDistanceFunction(std::bind(&BiTRRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
// Setup the optimization objective, if it isn't specified
if (!pdef_ || !pdef_->hasOptimizationObjective())
@@ -166,7 +166,7 @@ ompl::geometric::BiTRRT::Motion* ompl::geometric::BiTRRT::addMotion(const base::
si_->copyState(motion->state, state);
motion->cost = opt_->stateCost(motion->state);
motion->parent = parent;
- motion->root = parent ? parent->root : NULL;
+ motion->root = parent ? parent->root : nullptr;
if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
bestCost_ = motion->cost;
@@ -280,7 +280,7 @@ bool ompl::geometric::BiTRRT::connectTrees(Motion* nmotion, TreeData& tree, Moti
// extension into segments, just in case one piece fails
// the transition test
GrowResult result;
- Motion* next = NULL;
+ Motion* next = nullptr;
do
{
// Extend tree from nearest toward xmotion
@@ -415,7 +415,7 @@ ompl::base::PlannerStatus ompl::geometric::BiTRRT::solve(const base::PlannerTerm
// The trees have been connected. Construct the solution path
Motion *solution = connectionPoint_.first;
std::vector<Motion*> mpath1;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath1.push_back(solution);
solution = solution->parent;
@@ -423,7 +423,7 @@ ompl::base::PlannerStatus ompl::geometric::BiTRRT::solve(const base::PlannerTerm
solution = connectionPoint_.second;
std::vector<Motion*> mpath2;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath2.push_back(solution);
solution = solution->parent;
@@ -463,7 +463,7 @@ void ompl::geometric::BiTRRT::getPlannerData(base::PlannerData &data) const
tStart_->list(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state, 1));
else
{
@@ -477,7 +477,7 @@ void ompl::geometric::BiTRRT::getPlannerData(base::PlannerData &data) const
tGoal_->list(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motions[i]->state, 2));
else
{
@@ -488,5 +488,6 @@ void ompl::geometric::BiTRRT::getPlannerData(base::PlannerData &data) const
}
// Add the edge connecting the two trees
- data.addEdge(data.vertexIndex(connectionPoint_.first->state), data.vertexIndex(connectionPoint_.second->state));
+ if (connectionPoint_.first && connectionPoint_.second)
+ data.addEdge(data.vertexIndex(connectionPoint_.first->state), data.vertexIndex(connectionPoint_.second->state));
}
\ No newline at end of file
diff --git a/src/ompl/geometric/planners/rrt/src/InformedRRTstar.cpp b/src/ompl/geometric/planners/rrt/src/InformedRRTstar.cpp
index 52ee111..0c6f35b 100644
--- a/src/ompl/geometric/planners/rrt/src/InformedRRTstar.cpp
+++ b/src/ompl/geometric/planners/rrt/src/InformedRRTstar.cpp
@@ -43,11 +43,17 @@ ompl::geometric::InformedRRTstar::InformedRRTstar(const base::SpaceInformationPt
setName("InformedRRTstar");
//Configure RRTstar to be InformedRRT*:
+ setAdmissibleCostToCome(true);
setInformedSampling(true);
setTreePruning(true);
setPrunedMeasure(true);
+ //Disable conflicting options
+ setSampleRejection(false);
+ setNewStateRejection(false);
+
//Remove those parameters:
+ params_.remove("use_admissible_heuristic");
params_.remove("informed_sampling");
params_.remove("pruned_measure");
params_.remove("tree_pruning");
diff --git a/src/ompl/geometric/planners/rrt/src/LBTRRT.cpp b/src/ompl/geometric/planners/rrt/src/LBTRRT.cpp
index 040643c..6fc2dd3 100755
--- a/src/ompl/geometric/planners/rrt/src/LBTRRT.cpp
+++ b/src/ompl/geometric/planners/rrt/src/LBTRRT.cpp
@@ -46,7 +46,7 @@ ompl::geometric::LBTRRT::LBTRRT(const base::SpaceInformationPtr &si) :
goalBias_(0.05),
maxDistance_(0.0),
epsilon_(0.4),
- lastGoalMotion_(NULL),
+ lastGoalMotion_(nullptr),
iterations_(0)
{
specs_.approximateSolutions = true;
@@ -57,9 +57,9 @@ ompl::geometric::LBTRRT::LBTRRT(const base::SpaceInformationPtr &si) :
Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor, "0.:.1:10.");
addPlannerProgressProperty("iterations INTEGER",
- boost::bind(&LBTRRT::getIterationCount, this));
+ std::bind(&LBTRRT::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
- boost::bind(&LBTRRT::getBestCost, this));
+ std::bind(&LBTRRT::getBestCost, this));
}
ompl::geometric::LBTRRT::~LBTRRT()
@@ -74,7 +74,7 @@ void ompl::geometric::LBTRRT::clear()
freeMemory();
if (nn_)
nn_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
iterations_ = 0;
bestCost_ = std::numeric_limits<double>::infinity();
@@ -88,7 +88,7 @@ void ompl::geometric::LBTRRT::setup()
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&LBTRRT::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&LBTRRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::LBTRRT::freeMemory()
@@ -146,7 +146,7 @@ ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerm
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
Motion *solution = lastGoalMotion_;
- Motion *approxSol = NULL;
+ Motion *approxSol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
// e*(1+1/d) K-nearest constant, as used in RRT*
double k_rrg = boost::math::constants::e<double>() +
@@ -249,7 +249,7 @@ ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerm
approxSol = motion;
}
- if (solution != NULL && bestCost_ != solution->costApx_)
+ if (solution != nullptr && bestCost_ != solution->costApx_)
{
OMPL_INFORM("%s: approximation cost = %g", getName().c_str(),
solution->costApx_);
@@ -261,19 +261,19 @@ ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerm
bool solved = false;
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxSol;
approximate = true;
}
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parentApx_;
@@ -395,7 +395,7 @@ void ompl::geometric::LBTRRT::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parentApx_ == NULL)
+ if (motions[i]->parentApx_ == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state_));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parentApx_->state_),
diff --git a/src/ompl/geometric/planners/rrt/src/LazyLBTRRT.cpp b/src/ompl/geometric/planners/rrt/src/LazyLBTRRT.cpp
index 447a4d0..cb6a60a 100644
--- a/src/ompl/geometric/planners/rrt/src/LazyLBTRRT.cpp
+++ b/src/ompl/geometric/planners/rrt/src/LazyLBTRRT.cpp
@@ -52,10 +52,10 @@ ompl::geometric::LazyLBTRRT::LazyLBTRRT(const base::SpaceInformationPtr &si) :
goalBias_(0.05),
maxDistance_(0.0),
epsilon_(0.4),
- lastGoalMotion_(NULL),
- goalMotion_(NULL),
- LPAstarApx_(NULL),
- LPAstarLb_(NULL),
+ lastGoalMotion_(nullptr),
+ goalMotion_(nullptr),
+ LPAstarApx_(nullptr),
+ LPAstarLb_(nullptr),
iterations_(0)
{
specs_.approximateSolutions = true;
@@ -66,9 +66,9 @@ ompl::geometric::LazyLBTRRT::LazyLBTRRT(const base::SpaceInformationPtr &si) :
Planner::declareParam<double>("epsilon", this, &LazyLBTRRT::setApproximationFactor, &LazyLBTRRT::getApproximationFactor, "0.:.1:10.");
addPlannerProgressProperty("iterations INTEGER",
- boost::bind(&LazyLBTRRT::getIterationCount, this));
+ std::bind(&LazyLBTRRT::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
- boost::bind(&LazyLBTRRT::getBestCost, this));
+ std::bind(&LazyLBTRRT::getBestCost, this));
}
@@ -86,7 +86,7 @@ void ompl::geometric::LazyLBTRRT::clear(void)
nn_->clear();
graphLb_.clear();
graphApx_.clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
iterations_ = 0;
bestCost_ = std::numeric_limits<double>::infinity();
@@ -100,9 +100,9 @@ void ompl::geometric::LazyLBTRRT::setup(void)
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(
+ nn_->setDistanceFunction(std::bind(
(double(LazyLBTRRT::*)(const Motion*, const Motion*) const)
- &LazyLBTRRT::distanceFunction, this, _1, _2));
+ &LazyLBTRRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::LazyLBTRRT::freeMemory(void)
@@ -199,13 +199,13 @@ ompl::base::PlannerStatus ompl::geometric::LazyLBTRRT::solve(const base::Planner
////////////////////////////////////////////
while (ptc() == false)
{
- boost::tuple<Motion*, base::State*, double> res = rrtExtend(goal_s, xstate, rmotion, approxdif);
- Motion * nmotion= boost::get<0>(res);
- base::State *dstate = boost::get<1>(res);
- double d = boost::get<2>(res);
+ std::tuple<Motion*, base::State*, double> res = rrtExtend(goal_s, xstate, rmotion, approxdif);
+ Motion * nmotion= std::get<0>(res);
+ base::State *dstate = std::get<1>(res);
+ double d = std::get<2>(res);
iterations_++;
- if (dstate != NULL)
+ if (dstate != nullptr)
{
/* create a motion */
Motion* motion = createMotion(goal_s, dstate);
@@ -260,7 +260,7 @@ ompl::base::PlannerStatus ompl::geometric::LazyLBTRRT::solve(const base::Planner
return base::PlannerStatus(solved, !solved);
}
-boost::tuple<ompl::geometric::LazyLBTRRT::Motion*, ompl::base::State*, double>
+std::tuple<ompl::geometric::LazyLBTRRT::Motion*, ompl::base::State*, double>
ompl::geometric::LazyLBTRRT::rrtExtend(const base::GoalSampleableRegion* goal_s,
base::State *xstate, Motion *rmotion, double &approxdif)
{
@@ -280,7 +280,7 @@ ompl::geometric::LazyLBTRRT::rrtExtend(const base::GoalSampleableRegion* goal_s,
}
if (checkMotion(nmotion->state_, dstate) == false)
- return boost::make_tuple((Motion*)NULL, (base::State*)NULL, 0.0);
+ return std::make_tuple((Motion*)nullptr, (base::State*)nullptr, 0.0);
// motion is valid
double dist = 0.0;
@@ -294,7 +294,7 @@ ompl::geometric::LazyLBTRRT::rrtExtend(const base::GoalSampleableRegion* goal_s,
approxdif = dist;
}
- return boost::make_tuple(nmotion, dstate, d);
+ return std::make_tuple(nmotion, dstate, d);
}
void ompl::geometric::LazyLBTRRT::rrt(const base::PlannerTerminationCondition &ptc,
@@ -302,13 +302,13 @@ void ompl::geometric::LazyLBTRRT::rrt(const base::PlannerTerminationCondition &p
{
while (ptc() == false)
{
- boost::tuple<Motion*, base::State*, double> res = rrtExtend(goal_s, xstate, rmotion, approxdif);
- Motion* nmotion = boost::get<0>(res);
- base::State *dstate = boost::get<1>(res);
- double d = boost::get<2>(res);
+ std::tuple<Motion*, base::State*, double> res = rrtExtend(goal_s, xstate, rmotion, approxdif);
+ Motion* nmotion = std::get<0>(res);
+ base::State *dstate = std::get<1>(res);
+ double d = std::get<2>(res);
iterations_++;
- if (dstate != NULL)
+ if (dstate != nullptr)
{
/* create a motion */
Motion* motion = createMotion(goal_s, dstate);
diff --git a/src/ompl/geometric/planners/rrt/src/LazyRRT.cpp b/src/ompl/geometric/planners/rrt/src/LazyRRT.cpp
index af4dfbc..cc93110 100644
--- a/src/ompl/geometric/planners/rrt/src/LazyRRT.cpp
+++ b/src/ompl/geometric/planners/rrt/src/LazyRRT.cpp
@@ -44,7 +44,7 @@ ompl::geometric::LazyRRT::LazyRRT(const base::SpaceInformationPtr &si) : base::P
specs_.directed = true;
goalBias_ = 0.05;
maxDistance_ = 0.0;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
Planner::declareParam<double>("range", this, &LazyRRT::setRange, &LazyRRT::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &LazyRRT::setGoalBias, &LazyRRT::getGoalBias, "0.:.05:1.");
@@ -63,7 +63,7 @@ void ompl::geometric::LazyRRT::setup()
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&LazyRRT::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&LazyRRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::LazyRRT::clear()
@@ -73,7 +73,7 @@ void ompl::geometric::LazyRRT::clear()
freeMemory();
if (nn_)
nn_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::geometric::LazyRRT::freeMemory()
@@ -116,7 +116,7 @@ ompl::base::PlannerStatus ompl::geometric::LazyRRT::solve(const base::PlannerTer
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
- Motion *solution = NULL;
+ Motion *solution = nullptr;
double distsol = -1.0;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
@@ -163,7 +163,7 @@ ompl::base::PlannerStatus ompl::geometric::LazyRRT::solve(const base::PlannerTer
// Check that the solution is valid:
// construct the solution path
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -179,7 +179,7 @@ ompl::base::PlannerStatus ompl::geometric::LazyRRT::solve(const base::PlannerTer
{
removeMotion(mpath[i]);
solutionFound = false;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
}
@@ -223,7 +223,7 @@ void ompl::geometric::LazyRRT::removeMotion(Motion *motion)
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
- motion->children[i]->parent = NULL;
+ motion->children[i]->parent = nullptr;
removeMotion(motion->children[i]);
}
@@ -245,10 +245,10 @@ void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
- data.addEdge(base::PlannerDataVertex(motions[i]->parent ? motions[i]->parent->state : NULL),
+ data.addEdge(base::PlannerDataVertex(motions[i]->parent ? motions[i]->parent->state : nullptr),
base::PlannerDataVertex(motions[i]->state));
data.tagState(motions[i]->state, motions[i]->valid ? 1 : 0);
diff --git a/src/ompl/geometric/planners/rrt/src/RRT.cpp b/src/ompl/geometric/planners/rrt/src/RRT.cpp
index d97673e..12423a8 100644
--- a/src/ompl/geometric/planners/rrt/src/RRT.cpp
+++ b/src/ompl/geometric/planners/rrt/src/RRT.cpp
@@ -46,7 +46,7 @@ ompl::geometric::RRT::RRT(const base::SpaceInformationPtr &si) : base::Planner(s
goalBias_ = 0.05;
maxDistance_ = 0.0;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
Planner::declareParam<double>("range", this, &RRT::setRange, &RRT::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias, "0.:.05:1.");
@@ -64,7 +64,7 @@ void ompl::geometric::RRT::clear()
freeMemory();
if (nn_)
nn_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::geometric::RRT::setup()
@@ -75,7 +75,7 @@ void ompl::geometric::RRT::setup()
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&RRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::RRT::freeMemory()
@@ -117,8 +117,8 @@ ompl::base::PlannerStatus ompl::geometric::RRT::solve(const base::PlannerTermina
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
- Motion *solution = NULL;
- Motion *approxsol = NULL;
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
@@ -171,19 +171,19 @@ ompl::base::PlannerStatus ompl::geometric::RRT::solve(const base::PlannerTermina
bool solved = false;
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -220,7 +220,7 @@ void ompl::geometric::RRT::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
diff --git a/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp b/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
index 1986c6f..a68fa3d 100644
--- a/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
+++ b/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
@@ -46,7 +46,7 @@ ompl::geometric::RRTConnect::RRTConnect(const base::SpaceInformationPtr &si) : b
maxDistance_ = 0.0;
Planner::declareParam<double>("range", this, &RRTConnect::setRange, &RRTConnect::getRange, "0.:1.:10000.");
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
}
ompl::geometric::RRTConnect::~RRTConnect()
@@ -64,8 +64,8 @@ void ompl::geometric::RRTConnect::setup()
tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
if (!tGoal_)
tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
- tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
+ tStart_->setDistanceFunction(std::bind(&RRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
+ tGoal_->setDistanceFunction(std::bind(&RRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::RRTConnect::freeMemory()
@@ -104,7 +104,7 @@ void ompl::geometric::RRTConnect::clear()
tStart_->clear();
if (tGoal_)
tGoal_->clear();
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
}
ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
@@ -245,7 +245,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::Planner
if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
{
// it must be the case that either the start tree or the goal tree has made some progress
- // so one of the parents is not NULL. We go one step 'back' to avoid having a duplicate state
+ // so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
// on the solution path
if (startMotion->parent)
startMotion = startMotion->parent;
@@ -257,7 +257,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::Planner
/* construct the solution path */
Motion *solution = startMotion;
std::vector<Motion*> mpath1;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath1.push_back(solution);
solution = solution->parent;
@@ -265,7 +265,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::Planner
solution = goalMotion;
std::vector<Motion*> mpath2;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath2.push_back(solution);
solution = solution->parent;
@@ -304,7 +304,7 @@ void ompl::geometric::RRTConnect::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state, 1));
else
{
@@ -319,7 +319,7 @@ void ompl::geometric::RRTConnect::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motions[i]->state, 2));
else
{
diff --git a/src/ompl/geometric/planners/rrt/src/RRTstar.cpp b/src/ompl/geometric/planners/rrt/src/RRTstar.cpp
index 913cd07..653aa2b 100644
--- a/src/ompl/geometric/planners/rrt/src/RRTstar.cpp
+++ b/src/ompl/geometric/planners/rrt/src/RRTstar.cpp
@@ -46,7 +46,6 @@
#include <algorithm>
#include <limits>
#include <boost/math/constants/constants.hpp>
-#include <boost/make_shared.hpp>
#include <vector>
ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) :
@@ -58,7 +57,7 @@ ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) :
k_rrg_(0u),
r_rrg_(0.0),
delayCC_(true),
- lastGoalMotion_(NULL),
+ lastGoalMotion_(nullptr),
useTreePruning_(false),
pruneThreshold_(0.05),
usePrunedMeasure_(false),
@@ -92,9 +91,9 @@ ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) :
Planner::declareParam<bool>("number_sampling_attempts", this, &RRTstar::setNumSamplingAttempts, &RRTstar::getNumSamplingAttempts, "10:10:100000");
addPlannerProgressProperty("iterations INTEGER",
- boost::bind(&RRTstar::numIterationsProperty, this));
+ std::bind(&RRTstar::numIterationsProperty, this));
addPlannerProgressProperty("best cost REAL",
- boost::bind(&RRTstar::bestCostProperty, this));
+ std::bind(&RRTstar::bestCostProperty, this));
}
ompl::geometric::RRTstar::~RRTstar()
@@ -114,7 +113,7 @@ void ompl::geometric::RRTstar::setup()
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&RRTstar::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
// Setup optimization objective
//
@@ -161,7 +160,7 @@ void ompl::geometric::RRTstar::clear()
if (nn_)
nn_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
goalMotions_.clear();
startMotions_.clear();
@@ -220,7 +219,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer
Motion *solution = lastGoalMotion_;
- Motion *approximation = NULL;
+ Motion *approximation = nullptr;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
@@ -504,7 +503,7 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer
Motion *intermediate_solution = solution->parent; // Do not include goal state to simplify code.
//Push back until we find the start, but not the start itself
- while (intermediate_solution->parent != NULL)
+ while (intermediate_solution->parent != nullptr)
{
spath.push_back(intermediate_solution->state);
intermediate_solution = intermediate_solution->parent;
@@ -528,19 +527,19 @@ ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTer
break;
}
- bool approximate = (solution == NULL);
+ bool approximate = (solution == nullptr);
bool addedSolution = false;
if (approximate)
solution = approximation;
else
lastGoalMotion_ = solution;
- if (solution != NULL)
+ if (solution != nullptr)
{
ptc.terminate();
// construct the solution path
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -640,7 +639,7 @@ void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
for (std::size_t i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
@@ -1027,7 +1026,7 @@ void ompl::geometric::RRTstar::allocSampler()
{
// We are explicitly using rejection sampling.
OMPL_INFORM("%s: Using rejection sampling.", getName().c_str());
- infSampler_ = boost::make_shared<base::RejectionInfSampler>(pdef_, numSampleAttempts_);
+ infSampler_ = std::make_shared<base::RejectionInfSampler>(pdef_, numSampleAttempts_);
}
else
{
diff --git a/src/ompl/geometric/planners/rrt/src/TRRT.cpp b/src/ompl/geometric/planners/rrt/src/TRRT.cpp
index ee8c5b2..ffa5623 100644
--- a/src/ompl/geometric/planners/rrt/src/TRRT.cpp
+++ b/src/ompl/geometric/planners/rrt/src/TRRT.cpp
@@ -49,7 +49,7 @@ ompl::geometric::TRRT::TRRT(const base::SpaceInformationPtr &si) : base::Planner
goalBias_ = 0.05;
maxDistance_ = 0.0; // set in setup()
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
Planner::declareParam<double>("range", this, &TRRT::setRange, &TRRT::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &TRRT::setGoalBias, &TRRT::getGoalBias, "0.:.05:1.");
@@ -80,7 +80,7 @@ void ompl::geometric::TRRT::clear()
freeMemory();
if (nearestNeighbors_)
nearestNeighbors_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
// Clear TRRT specific variables ---------------------------------------------------------
temp_ = initTemperature_;
@@ -122,7 +122,7 @@ void ompl::geometric::TRRT::setup()
nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
// Set the distance function
- nearestNeighbors_->setDistanceFunction(boost::bind(&TRRT::distanceFunction, this, _1, _2));
+ nearestNeighbors_->setDistanceFunction(std::bind(&TRRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
// Setup TRRT specific variables ---------------------------------------------------------
temp_ = initTemperature_;
@@ -196,9 +196,9 @@ ompl::geometric::TRRT::solve(const base::PlannerTerminationCondition &plannerTer
// Solver variables ------------------------------------------------------------------------------------
// the final solution
- Motion *solution = NULL;
+ Motion *solution = nullptr;
// the approximate solution, returned if no final solution found
- Motion *approxSolution = NULL;
+ Motion *approxSolution = nullptr;
// track the distance from goal to closest solution yet found
double approxDifference = std::numeric_limits<double>::infinity();
@@ -323,20 +323,20 @@ ompl::geometric::TRRT::solve(const base::PlannerTerminationCondition &plannerTer
bool approximate = false;
// Substitute an empty solution with the best approximation
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxSolution;
approximate = true;
}
// Generate solution path for real/approx solution
- if (solution != NULL)
+ if (solution != nullptr)
{
lastGoalMotion_ = solution;
// construct the solution path
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -376,7 +376,7 @@ void ompl::geometric::TRRT::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
diff --git a/src/ompl/geometric/planners/rrt/src/VFRRT.cpp b/src/ompl/geometric/planners/rrt/src/VFRRT.cpp
new file mode 100644
index 0000000..3b57296
--- /dev/null
+++ b/src/ompl/geometric/planners/rrt/src/VFRRT.cpp
@@ -0,0 +1,293 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Caleb Voss and Wilson Beebe
+* 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.
+*********************************************************************/
+
+/* Authors: Caleb Voss, Wilson Beebe */
+
+#include "ompl/geometric/planners/rrt/VFRRT.h"
+#include "ompl/base/goals/GoalSampleableRegion.h"
+
+namespace ompl
+{
+ namespace magic
+ {
+ /// Number of sampler to determine mean vector field norm in \ref gVFRRT
+ static const unsigned int VFRRT_MEAN_NORM_SAMPLES = 1000;
+ }
+}
+
+ompl::geometric::VFRRT::VFRRT(const base::SpaceInformationPtr &si, const VectorField &vf,
+ double exploration, double initial_lambda, unsigned int update_freq)
+ : RRT(si), vf_(vf), efficientCount_(0), inefficientCount_(0), explorationInefficiency_(0.),
+ explorationSetting_(exploration), lambda_(initial_lambda),
+ nth_step_(update_freq), step_(0), meanNorm_(0.), vfdim_(0)
+{
+ setName("VFRRT");
+ maxDistance_ = si->getStateValidityCheckingResolution();
+}
+
+ompl::geometric::VFRRT::~VFRRT()
+{
+}
+
+void ompl::geometric::VFRRT::clear()
+{
+ RRT::clear();
+ efficientCount_ = 0;
+ inefficientCount_ = 0;
+ explorationInefficiency_ = 0.;
+ step_ = 0;
+}
+
+void ompl::geometric::VFRRT::setup()
+{
+ RRT::setup();
+ vfdim_ = si_->getStateSpace()->getValueLocations().size();
+}
+
+double ompl::geometric::VFRRT::determineMeanNorm()
+{
+ ompl::base::State *rstate = si_->allocState();
+ double sum = 0.;
+ for (unsigned int i = 0; i < magic::VFRRT_MEAN_NORM_SAMPLES; i++)
+ {
+ sampler_->sampleUniform(rstate);
+ sum += vf_(rstate).norm();
+ }
+ si_->freeState(rstate);
+ return sum / magic::VFRRT_MEAN_NORM_SAMPLES;
+}
+
+Eigen::VectorXd ompl::geometric::VFRRT::getNewDirection(const base::State *qnear, const base::State *qrand)
+{
+ // Set vrand to be the normalized vector from qnear to qrand
+ Eigen::VectorXd vrand(vfdim_);
+ for (unsigned int i = 0; i < vfdim_; i++)
+ vrand[i] = *si_->getStateSpace()->getValueAddressAtIndex(qrand, i)
+ - *si_->getStateSpace()->getValueAddressAtIndex(qnear, i);
+ vrand /= si_->distance(qnear, qrand);
+
+ // Get the vector at qnear, and normalize
+ Eigen::VectorXd vfield = vf_(qnear);
+ const double lambdaScale = vfield.norm();
+ // In the case where there is no vector field present, vfield.norm() == 0,
+ // return the direction of the random state.
+ if (lambdaScale < std::numeric_limits<float>::epsilon())
+ return vrand;
+ vfield /= lambdaScale;
+ // Sample a weight from the distribution
+ const double omega = biasedSampling(vrand, vfield, lambdaScale);
+ // Determine updated direction
+ return computeAlphaBeta(omega, vrand, vfield);
+}
+
+double ompl::geometric::VFRRT::biasedSampling(const Eigen::VectorXd &vrand,
+ const Eigen::VectorXd &vfield, double lambdaScale)
+{
+ double sigma = .25 * (vrand - vfield).squaredNorm();
+ updateGain();
+ double scaledLambda = lambda_ * lambdaScale / meanNorm_;
+ double phi = scaledLambda / (1. - std::exp(-2. * scaledLambda));
+ double z = - std::log(1. - sigma * scaledLambda / phi) / scaledLambda;
+ return std::sqrt(2. * z);
+}
+
+void ompl::geometric::VFRRT::updateGain()
+{
+ if (step_ == nth_step_)
+ {
+ lambda_ = lambda_ * (1 - explorationInefficiency_ + explorationSetting_);
+ efficientCount_ = inefficientCount_ = 0;
+ explorationInefficiency_ = 0;
+ step_ = 0;
+ }
+ else
+ step_++;
+}
+
+Eigen::VectorXd ompl::geometric::VFRRT::computeAlphaBeta(
+ double omega, const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield)
+{
+ double w2 = omega * omega;
+ double c = vfield.dot(vrand);
+ double cc_1 = c * c - 1.;
+ double root = std::sqrt(cc_1 * w2 * (w2 - 4.));
+ double beta = -root / (2. * cc_1);
+ double sign = (beta < 0.) ? -1. : 1.;
+ beta *= sign;
+ double alpha = (sign * c * root + cc_1 * (2. - w2)) / (2. * cc_1);
+ return alpha * vfield + beta * vrand;
+}
+
+ompl::geometric::VFRRT::Motion *ompl::geometric::VFRRT::extendTree(
+ Motion *m, base::State* rstate, const Eigen::VectorXd &v)
+{
+ base::State* newState = si_->allocState();
+ si_->copyState(newState, m->state);
+
+ double d = si_->distance(m->state, rstate);
+ if (d > maxDistance_)
+ d = maxDistance_;
+
+ const base::StateSpacePtr &space = si_->getStateSpace();
+ for (unsigned int i = 0; i < vfdim_; i++)
+ *space->getValueAddressAtIndex(newState, i) += d * v[i];
+ if (!v.hasNaN() && si_->checkMotion(m->state, newState))
+ {
+ Motion *motion = new Motion(si_);
+ motion->state = newState;
+ motion->parent = m;
+ updateExplorationEfficiency(motion);
+ nn_->add(motion);
+ return motion;
+ }
+ else
+ {
+ si_->freeState(newState);
+ inefficientCount_++;
+ return nullptr;
+ }
+}
+
+void ompl::geometric::VFRRT::updateExplorationEfficiency(Motion *m)
+{
+ Motion *near = nn_->nearest(m);
+ if (distanceFunction(m, near) < si_->getStateValidityCheckingResolution())
+ inefficientCount_++;
+ else
+ efficientCount_++;
+ explorationInefficiency_ = inefficientCount_ / (double)(efficientCount_ + inefficientCount_);
+}
+
+ompl::base::PlannerStatus ompl::geometric::VFRRT::solve(const base::PlannerTerminationCondition &ptc)
+{
+ checkValidity();
+ base::Goal *goal = pdef_->getGoal().get();
+ base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
+
+ if (!sampler_)
+ sampler_ = si_->allocStateSampler();
+
+ meanNorm_ = determineMeanNorm();
+
+ while (const base::State *st = pis_.nextStart())
+ {
+ Motion *motion = new Motion(si_);
+ si_->copyState(motion->state, st);
+ nn_->add(motion);
+ }
+
+ if (nn_->size() == 0)
+ {
+ OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
+ return base::PlannerStatus::INVALID_START;
+ }
+
+ OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
+
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
+ double approxdif = std::numeric_limits<double>::infinity();
+ Motion *rmotion = new Motion(si_);
+ base::State *rstate = rmotion->state;
+ base::State *xstate = si_->allocState();
+
+ while (ptc == false)
+ {
+ // Sample random state (with goal biasing)
+ if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
+ goal_s->sampleGoal(rstate);
+ else
+ sampler_->sampleUniform(rstate);
+
+ // Find closest state in the tree
+ Motion *nmotion = nn_->nearest(rmotion);
+
+ // Modify direction based on vector field before extending
+ Motion *motion = extendTree(nmotion, rstate, getNewDirection(nmotion->state, rstate));
+ if (!motion)
+ continue;
+
+ // Check if we can connect to the goal
+ double dist = 0;
+ bool sat = goal->isSatisfied(motion->state, &dist);
+ if (sat)
+ {
+ approxdif = dist;
+ solution = motion;
+ break;
+ }
+ if (dist < approxdif)
+ {
+ approxdif = dist;
+ approxsol = motion;
+ }
+ }
+
+ bool solved = false;
+ bool approximate = false;
+ if (solution == nullptr)
+ {
+ solution = approxsol;
+ approximate = true;
+ }
+
+ if (solution != nullptr)
+ {
+ lastGoalMotion_ = solution;
+
+ // Construct the solution path
+ std::vector<Motion*> mpath;
+ while (solution != nullptr)
+ {
+ mpath.push_back(solution);
+ solution = solution->parent;
+ }
+
+ // Set the solution path
+ PathGeometric *path = new PathGeometric(si_);
+ for (int i = mpath.size() - 1 ; i >= 0 ; --i)
+ path->append(mpath[i]->state);
+ pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, name_);
+ solved = true;
+ }
+
+ si_->freeState(xstate);
+ if (rmotion->state)
+ si_->freeState(rmotion->state);
+ delete rmotion;
+
+ OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
+
+ return base::PlannerStatus(solved, approximate);
+}
diff --git a/src/ompl/geometric/planners/rrt/src/pRRT.cpp b/src/ompl/geometric/planners/rrt/src/pRRT.cpp
index a71d0c3..0f2e554 100644
--- a/src/ompl/geometric/planners/rrt/src/pRRT.cpp
+++ b/src/ompl/geometric/planners/rrt/src/pRRT.cpp
@@ -37,7 +37,6 @@
#include "ompl/geometric/planners/rrt/pRRT.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
-#include <boost/thread/thread.hpp>
#include <limits>
ompl::geometric::pRRT::pRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "pRRT"),
@@ -50,7 +49,7 @@ ompl::geometric::pRRT::pRRT(const base::SpaceInformationPtr &si) : base::Planner
setThreadCount(2);
goalBias_ = 0.05;
maxDistance_ = 0.0;
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
Planner::declareParam<double>("range", this, &pRRT::setRange, &pRRT::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &pRRT::setGoalBias, &pRRT::getGoalBias, "0.:.05:1.");
@@ -70,7 +69,7 @@ void ompl::geometric::pRRT::setup()
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
- nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&pRRT::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::pRRT::clear()
@@ -80,7 +79,7 @@ void ompl::geometric::pRRT::clear()
freeMemory();
if (nn_)
nn_->clear();
- lastGoalMotion_ = NULL;
+ lastGoalMotion_ = nullptr;
}
void ompl::geometric::pRRT::freeMemory()
@@ -108,7 +107,7 @@ void ompl::geometric::pRRT::threadSolve(unsigned int tid, const base::PlannerTer
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
- while (sol->solution == NULL && ptc == false)
+ while (sol->solution == nullptr && ptc == false)
{
/* sample random state (with goal biasing) */
if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
@@ -200,13 +199,13 @@ ompl::base::PlannerStatus ompl::geometric::pRRT::solve(const base::PlannerTermin
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
SolutionInfo sol;
- sol.solution = NULL;
- sol.approxsol = NULL;
+ sol.solution = nullptr;
+ sol.approxsol = nullptr;
sol.approxdif = std::numeric_limits<double>::infinity();
- std::vector<boost::thread*> th(threadCount_);
+ std::vector<std::thread*> th(threadCount_);
for (unsigned int i = 0 ; i < threadCount_ ; ++i)
- th[i] = new boost::thread(boost::bind(&pRRT::threadSolve, this, i, ptc, &sol));
+ th[i] = new std::thread(std::bind(&pRRT::threadSolve, this, i, ptc, &sol));
for (unsigned int i = 0 ; i < threadCount_ ; ++i)
{
th[i]->join();
@@ -215,19 +214,19 @@ ompl::base::PlannerStatus ompl::geometric::pRRT::solve(const base::PlannerTermin
bool solved = false;
bool approximate = false;
- if (sol.solution == NULL)
+ if (sol.solution == nullptr)
{
sol.solution = sol.approxsol;
approximate = true;
}
- if (sol.solution != NULL)
+ if (sol.solution != nullptr)
{
lastGoalMotion_ = sol.solution;
/* construct the solution path */
std::vector<Motion*> mpath;
- while (sol.solution != NULL)
+ while (sol.solution != nullptr)
{
mpath.push_back(sol.solution);
sol.solution = sol.solution->parent;
@@ -260,7 +259,7 @@ void ompl::geometric::pRRT::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
- if (motions[i]->parent == NULL)
+ if (motions[i]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
diff --git a/src/ompl/geometric/planners/sbl/SBL.h b/src/ompl/geometric/planners/sbl/SBL.h
index a7646a0..df3db65 100644
--- a/src/ompl/geometric/planners/sbl/SBL.h
+++ b/src/ompl/geometric/planners/sbl/SBL.h
@@ -151,12 +151,12 @@ namespace ompl
public:
/** \brief Default constructor. Allocates no memory */
- Motion() : root(NULL), state(NULL), parent(NULL), valid(false)
+ Motion() : root(nullptr), state(nullptr), parent(nullptr), valid(false)
{
}
/** \brief Constructor that allocates storage for a state */
- Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
+ Motion(const base::SpaceInformationPtr &si) : root(nullptr), state(si->allocState()), parent(nullptr), valid(false)
{
}
diff --git a/src/ompl/geometric/planners/sbl/pSBL.h b/src/ompl/geometric/planners/sbl/pSBL.h
index 944f532..0051cb1 100644
--- a/src/ompl/geometric/planners/sbl/pSBL.h
+++ b/src/ompl/geometric/planners/sbl/pSBL.h
@@ -42,7 +42,8 @@
#include "ompl/base/StateSamplerArray.h"
#include "ompl/datastructures/Grid.h"
#include "ompl/datastructures/PDF.h"
-#include <boost/thread/mutex.hpp>
+#include <thread>
+#include <mutex>
#include <vector>
namespace ompl
@@ -160,11 +161,11 @@ namespace ompl
{
public:
- Motion() : root(NULL), state(NULL), parent(NULL), valid(false)
+ Motion() : root(nullptr), state(nullptr), parent(nullptr), valid(false)
{
}
- Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
+ Motion(const base::SpaceInformationPtr &si) : root(nullptr), state(si->allocState()), parent(nullptr), valid(false)
{
}
@@ -177,7 +178,7 @@ namespace ompl
Motion *parent;
bool valid;
std::vector<Motion*> children;
- boost::mutex lock;
+ std::mutex lock;
};
/** \brief A struct containing an array of motions and a corresponding PDF element */
@@ -220,14 +221,14 @@ namespace ompl
Grid<MotionInfo> grid;
unsigned int size;
CellPDF pdf;
- boost::mutex lock;
+ std::mutex lock;
};
struct SolutionInfo
{
std::vector<Motion*> solution;
bool found;
- boost::mutex lock;
+ std::mutex lock;
};
struct PendingRemoveMotion
@@ -239,7 +240,7 @@ namespace ompl
struct MotionsToBeRemoved
{
std::vector<PendingRemoveMotion> motions;
- boost::mutex lock;
+ std::mutex lock;
};
void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
@@ -266,8 +267,8 @@ namespace ompl
TreeData tGoal_;
MotionsToBeRemoved removeList_;
- boost::mutex loopLock_;
- boost::mutex loopLockCounter_;
+ std::mutex loopLock_;
+ std::mutex loopLockCounter_;
unsigned int loopCounter_;
double maxDistance_;
diff --git a/src/ompl/geometric/planners/sbl/src/SBL.cpp b/src/ompl/geometric/planners/sbl/src/SBL.cpp
index ad7822d..e6c058b 100644
--- a/src/ompl/geometric/planners/sbl/src/SBL.cpp
+++ b/src/ompl/geometric/planners/sbl/src/SBL.cpp
@@ -44,7 +44,7 @@ ompl::geometric::SBL::SBL(const base::SpaceInformationPtr &si) : base::Planner(s
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
maxDistance_ = 0.0;
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
Planner::declareParam<double>("range", this, &SBL::setRange, &SBL::getRange, "0.:1.:10000.");
}
@@ -211,14 +211,14 @@ bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &o
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
- while (motion != NULL)
+ while (motion != nullptr)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
- while (connectOther != NULL)
+ while (connectOther != nullptr)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
@@ -243,7 +243,7 @@ bool ompl::geometric::SBL::isPathValid(TreeData &tree, Motion *motion)
std::vector<Motion*> mpath;
/* construct the solution path */
- while (motion != NULL)
+ while (motion != nullptr)
{
mpath.push_back(motion);
motion = motion->parent;
@@ -267,7 +267,7 @@ bool ompl::geometric::SBL::isPathValid(TreeData &tree, Motion *motion)
ompl::geometric::SBL::Motion* ompl::geometric::SBL::selectMotion(TreeData &tree)
{
GridCell* cell = tree.pdf.sample(rng_.uniform01());
- return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
+ return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : nullptr;
}
void ompl::geometric::SBL::removeMotion(TreeData &tree, Motion *motion)
@@ -317,7 +317,7 @@ void ompl::geometric::SBL::removeMotion(TreeData &tree, Motion *motion)
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
- motion->children[i]->parent = NULL;
+ motion->children[i]->parent = nullptr;
removeMotion(tree, motion->children[i]);
}
@@ -361,7 +361,7 @@ void ompl::geometric::SBL::clear()
tGoal_.grid.clear();
tGoal_.size = 0;
tGoal_.pdf.clear();
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
}
void ompl::geometric::SBL::getPlannerData(base::PlannerData &data) const
@@ -373,7 +373,7 @@ void ompl::geometric::SBL::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
- if (motions[i][j]->parent == NULL)
+ if (motions[i][j]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
else
data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
@@ -383,7 +383,7 @@ void ompl::geometric::SBL::getPlannerData(base::PlannerData &data) const
tGoal_.grid.getContent(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
- if (motions[i][j]->parent == NULL)
+ if (motions[i][j]->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
else
// The edges in the goal tree are reversed so that they are in the same direction as start tree
diff --git a/src/ompl/geometric/planners/sbl/src/pSBL.cpp b/src/ompl/geometric/planners/sbl/src/pSBL.cpp
index 22c9cdd..ec8eaa5 100644
--- a/src/ompl/geometric/planners/sbl/src/pSBL.cpp
+++ b/src/ompl/geometric/planners/sbl/src/pSBL.cpp
@@ -37,7 +37,6 @@
#include "ompl/geometric/planners/sbl/pSBL.h"
#include "ompl/base/goals/GoalState.h"
#include "ompl/tools/config/SelfConfig.h"
-#include <boost/thread.hpp>
#include <limits>
#include <cassert>
@@ -48,7 +47,7 @@ ompl::geometric::pSBL::pSBL(const base::SpaceInformationPtr &si) : base::Planner
specs_.multithreaded = true;
maxDistance_ = 0.0;
setThreadCount(2);
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
Planner::declareParam<double>("range", this, &pSBL::setRange, &pSBL::getRange, "0.:1.:10000.");
Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount, "1:64");
@@ -87,7 +86,7 @@ void ompl::geometric::pSBL::clear()
tGoal_.pdf.clear();
removeList_.motions.clear();
- connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
+ connectionPoint_ = std::make_pair<base::State*, base::State*>(nullptr, nullptr);
}
void ompl::geometric::pSBL::freeGridMotions(Grid<MotionInfo> &grid)
@@ -244,9 +243,9 @@ ompl::base::PlannerStatus ompl::geometric::pSBL::solve(const base::PlannerTermin
sol.found = false;
loopCounter_ = 0;
- std::vector<boost::thread*> th(threadCount_);
+ std::vector<std::thread*> th(threadCount_);
for (unsigned int i = 0 ; i < threadCount_ ; ++i)
- th[i] = new boost::thread(boost::bind(&pSBL::threadSolve, this, i, ptc, &sol));
+ th[i] = new std::thread(std::bind(&pSBL::threadSolve, this, i, ptc, &sol));
for (unsigned int i = 0 ; i < threadCount_ ; ++i)
{
th[i]->join();
@@ -297,14 +296,14 @@ bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree,
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
- while (motion != NULL)
+ while (motion != nullptr)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
- while (connectOther != NULL)
+ while (connectOther != nullptr)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
@@ -332,7 +331,7 @@ bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
std::vector<Motion*> mpath;
/* construct the solution path */
- while (motion != NULL)
+ while (motion != nullptr)
{
mpath.push_back(motion);
motion = motion->parent;
@@ -370,7 +369,7 @@ ompl::geometric::pSBL::Motion* ompl::geometric::pSBL::selectMotion(RNG &rng, Tre
{
tree.lock.lock ();
GridCell* cell = tree.pdf.sample(rng.uniform01());
- Motion *result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : NULL;
+ Motion *result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : nullptr;
tree.lock.unlock ();
return result;
}
@@ -419,7 +418,7 @@ void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::ma
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
- motion->children[i]->parent = NULL;
+ motion->children[i]->parent = nullptr;
removeMotion(tree, motion->children[i], seen);
}
@@ -459,7 +458,7 @@ void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
- if (motions[i][j]->parent == NULL)
+ if (motions[i][j]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
else
data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
@@ -469,7 +468,7 @@ void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const
tGoal_.grid.getContent(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
- if (motions[i][j]->parent == NULL)
+ if (motions[i][j]->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
else
// The edges in the goal tree are reversed so that they are in the same direction as start tree
diff --git a/src/ompl/geometric/planners/sst/SST.h b/src/ompl/geometric/planners/sst/SST.h
new file mode 100644
index 0000000..a230b83
--- /dev/null
+++ b/src/ompl/geometric/planners/sst/SST.h
@@ -0,0 +1,299 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Rutgers the State University of New Jersey, New Brunswick
+* 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 Rutgers University 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.
+*********************************************************************/
+
+/* Authors: Zakary Littlefield */
+
+#ifndef OMPL_GEOMETRIC_PLANNERS_SST_SST_
+#define OMPL_GEOMETRIC_PLANNERS_SST_SST_
+
+#include "ompl/geometric/planners/PlannerIncludes.h"
+#include "ompl/datastructures/NearestNeighbors.h"
+
+namespace ompl
+{
+ namespace geometric
+ {
+ /**
+ @anchor gSST
+ @par Short description
+ \ref gSST "SST" (Stable Sparse RRT) is an asymptotically near-optimal incremental
+ sampling-based motion planning algorithm. It is recommended for geometric problems
+ to use an alternative method that makes use of a steering function. Using SST for
+ geometric problems does not take advantage of this function.
+ @par External documentation
+ Yanbo Li, Zakary Littlefield, Kostas E. Bekris, Sampling-based
+ Asymptotically Optimal Sampling-based Kinodynamic Planning.
+ [[PDF]](http://arxiv.org/abs/1407.2896)
+ */
+ class SST : public base::Planner
+ {
+ public:
+
+ /** \brief Constructor */
+ SST(const base::SpaceInformationPtr &si);
+
+ virtual ~SST();
+
+ virtual void setup();
+
+ /** \brief Continue solving for some amount of time. Return true if solution was found. */
+ virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
+
+ virtual void getPlannerData(base::PlannerData &data) const;
+
+ /** \brief Clear datastructures. Call this function if the
+ input data to the planner has changed and you do not
+ want to continue planning */
+ virtual void clear();
+
+ /** In the process of randomly selecting states in the state
+ space to attempt to go towards, the algorithm may in fact
+ choose the actual goal state, if it knows it, with some
+ probability. This probability is a real number between 0.0
+ and 1.0; its value should usually be around 0.05 and
+ should not be too large. It is probably a good idea to use
+ the default value. */
+ void setGoalBias(double goalBias)
+ {
+ goalBias_ = goalBias;
+ }
+
+ /** \brief Get the goal bias the planner is using */
+ double getGoalBias() const
+ {
+ return goalBias_;
+ }
+
+
+ /** \brief Set the range the planner is supposed to use.
+
+ This parameter greatly influences the runtime of the
+ algorithm. It represents the maximum length of a
+ motion to be added in the tree of motions. */
+ void setRange(double distance)
+ {
+ maxDistance_ = distance;
+ }
+
+ /** \brief Get the range the planner is using */
+ double getRange() const
+ {
+ return maxDistance_;
+ }
+
+ /**
+ \brief Set the radius for selecting nodes relative to random sample.
+
+ This radius is used to mimic behavior of RRT* in that it promotes
+ extending from nodes with good path cost from the root of the tree.
+ Making this radius larger will provide higher quality paths, but has two
+ major drawbacks; exploration will occur much more slowly and exploration
+ around the boundary of the state space may become impossible. */
+ void setSelectionRadius(double selectionRadius)
+ {
+ selectionRadius_ = selectionRadius;
+ }
+
+ /** \brief Get the selection radius the planner is using */
+ double getSelectionRadius() const
+ {
+ return selectionRadius_;
+ }
+
+
+ /**
+ \brief Set the radius for pruning nodes.
+
+ This is the radius used to surround nodes in the witness set.
+ Within this radius around a state in the witness set, only one
+ active tree node can exist. This limits the size of the tree and
+ forces computation to focus on low path costs nodes. If this value
+ is too large, narrow passages will be impossible to traverse. In addition,
+ children nodes may be removed if they are not at least this distance away
+ from their parent nodes.*/
+ void setPruningRadius(double pruningRadius)
+ {
+ pruningRadius_ = pruningRadius;
+ }
+
+ /** \brief Get the pruning radius the planner is using */
+ double getPruningRadius() const
+ {
+ return pruningRadius_;
+ }
+
+ /** \brief Set a different nearest neighbors datastructure */
+ template<template<typename T> class NN>
+ void setNearestNeighbors()
+ {
+ nn_.reset(new NN<Motion*>());
+ witnesses_.reset(new NN<Motion*>());
+ }
+
+
+ protected:
+
+
+ /** \brief Representation of a motion
+
+ This only contains pointers to parent motions as we
+ only need to go backwards in the tree. */
+ class Motion
+ {
+ public:
+
+ Motion() : accCost_(0), state_(nullptr), parent_(nullptr), numChildren_(0), inactive_(false)
+ {
+ }
+
+ /** \brief Constructor that allocates memory for the state and the control */
+ Motion(const base::SpaceInformationPtr& si) : accCost_(0), state_(si->allocState()), parent_(nullptr), numChildren_(0), inactive_(false)
+ {
+ }
+
+ virtual ~Motion()
+ {
+ }
+
+ virtual base::State* getState() const
+ {
+ return state_;
+ }
+ virtual Motion* getParent() const
+ {
+ return parent_;
+ }
+ base::Cost accCost_;
+
+ /** \brief The state contained by the motion */
+ base::State *state_;
+
+ /** \brief The parent motion in the exploration tree */
+ Motion *parent_;
+
+ /** \brief Number of children */
+ unsigned numChildren_;
+
+ /** \brief If inactive, this node is not considered for selection.*/
+ bool inactive_;
+
+
+ };
+
+ class Witness : public Motion
+ {
+ public:
+
+ Witness() : Motion(), rep_(nullptr)
+ {
+ }
+
+ Witness(const base::SpaceInformationPtr& si) : Motion(si), rep_(nullptr)
+ {
+ }
+ virtual base::State* getState() const
+ {
+ return rep_->state_;
+ }
+ virtual Motion* getParent() const
+ {
+ return rep_->parent_;
+ }
+
+ void linkRep(Motion *lRep)
+ {
+ rep_ = lRep;
+ }
+
+ /** \brief The node in the tree that is within the pruning radius.*/
+ Motion* rep_;
+ };
+
+
+ /** \brief Finds the best node in the tree withing the selection radius around a random sample.*/
+ Motion* selectNode(Motion *sample);
+
+ /** \brief Find the closest witness node to a newly generated potential node.*/
+ Witness* findClosestWitness(Motion *node);
+
+ /** \brief Randomly propagate a new edge.*/
+ base::State* monteCarloProp(Motion *m);
+
+ /** \brief Free the memory allocated by this planner */
+ void freeMemory();
+
+ /** \brief Compute distance between motions (actually distance between contained states) */
+ double distanceFunction(const Motion *a, const Motion *b) const
+ {
+ return si_->distance(a->state_, b->state_);
+ }
+
+ /** \brief State sampler */
+ base::StateSamplerPtr sampler_;
+
+ /** \brief A nearest-neighbors datastructure containing the tree of motions */
+ std::shared_ptr< NearestNeighbors<Motion*> > nn_;
+
+ /** \brief A nearest-neighbors datastructure containing the tree of witness motions */
+ std::shared_ptr< NearestNeighbors<Motion*> > witnesses_;
+
+ /** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
+ double goalBias_;
+
+ /** \brief The maximum length of a motion to be added to a tree */
+ double maxDistance_;
+
+ /** \brief The radius for determining the node selected for extension. */
+ double selectionRadius_;
+
+ /** \brief The radius for determining the size of the pruning region. */
+ double pruningRadius_;
+
+ /** \brief The random number generator */
+ RNG rng_;
+
+ /** \brief The best solution we found so far. */
+ std::vector<base::State*> prevSolution_;
+
+ /** \brief The best solution cost we found so far. */
+ base::Cost prevSolutionCost_;
+
+ /** \brief The optimization objective. */
+ base::OptimizationObjectivePtr opt_;
+
+ };
+ }
+}
+
+#endif
\ No newline at end of file
diff --git a/src/ompl/geometric/planners/sst/src/SST.cpp b/src/ompl/geometric/planners/sst/src/SST.cpp
new file mode 100644
index 0000000..7ed9468
--- /dev/null
+++ b/src/ompl/geometric/planners/sst/src/SST.cpp
@@ -0,0 +1,423 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, Rutgers the State University of New Jersey, New Brunswick
+* 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 Rutgers University 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.
+*********************************************************************/
+
+/* Authors: Zakary Littlefield */
+
+#include "ompl/geometric/planners/sst/SST.h"
+#include "ompl/base/goals/GoalSampleableRegion.h"
+#include "ompl/base/objectives/MinimaxObjective.h"
+#include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
+#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
+#include "ompl/tools/config/SelfConfig.h"
+#include <limits>
+
+ompl::geometric::SST::SST(const base::SpaceInformationPtr &si) : base::Planner(si, "SST")
+{
+ specs_.approximateSolutions = true;
+ specs_.directed = true;
+ prevSolution_.clear();
+
+ goalBias_ = 0.05;
+ selectionRadius_ = 5.0;
+ pruningRadius_ = 3.0;
+ maxDistance_ = 5.0;
+
+ Planner::declareParam<double>("range", this, &SST::setRange, &SST::getRange, ".1:.1:100");
+ Planner::declareParam<double>("goal_bias", this, &SST::setGoalBias, &SST::getGoalBias, "0.:.05:1.");
+ Planner::declareParam<double>("selection_radius", this, &SST::setSelectionRadius, &SST::getSelectionRadius, "0.:.1:100");
+ Planner::declareParam<double>("pruning_radius", this, &SST::setPruningRadius, &SST::getPruningRadius, "0.:.1:100");
+}
+
+ompl::geometric::SST::~SST()
+{
+ freeMemory();
+}
+
+void ompl::geometric::SST::setup()
+{
+ base::Planner::setup();
+ if (!nn_)
+ nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
+ nn_->setDistanceFunction(std::bind(&SST::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
+ if (!witnesses_)
+ witnesses_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
+ witnesses_->setDistanceFunction(std::bind(&SST::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
+
+ if (pdef_)
+ {
+ if (pdef_->hasOptimizationObjective())
+ {
+ opt_ = pdef_->getOptimizationObjective();
+ if (dynamic_cast<base::MaximizeMinClearanceObjective*>(opt_.get()) || dynamic_cast<base::MinimaxObjective*>(opt_.get()))
+ OMPL_WARN("%s: Asymptotic near-optimality has only been proven with Lipschitz continuous cost functions w.r.t. state and control. This optimization objective will result in undefined behavior", getName().c_str());
+ }
+ else
+ {
+ OMPL_WARN("%s: No optimization object set. Using path length", getName().c_str());
+ opt_.reset(new base::PathLengthOptimizationObjective(si_));
+ pdef_->setOptimizationObjective(opt_);
+ }
+ }
+
+ prevSolutionCost_ = opt_->infiniteCost();
+
+}
+
+void ompl::geometric::SST::clear()
+{
+ Planner::clear();
+ sampler_.reset();
+ freeMemory();
+ if (nn_)
+ nn_->clear();
+ if (witnesses_)
+ witnesses_->clear();
+ prevSolutionCost_ = opt_->infiniteCost();
+}
+
+void ompl::geometric::SST::freeMemory()
+{
+ if (nn_)
+ {
+ std::vector<Motion*> motions;
+ nn_->list(motions);
+ for (unsigned int i = 0 ; i < motions.size() ; ++i)
+ {
+ if (motions[i]->state_)
+ si_->freeState(motions[i]->state_);
+ delete motions[i];
+ }
+ }
+ if (witnesses_)
+ {
+ std::vector<Motion*> witnesses;
+ witnesses_->list(witnesses);
+ for (unsigned int i = 0 ; i < witnesses.size() ; ++i)
+ {
+ if (witnesses[i]->state_)
+ si_->freeState(witnesses[i]->state_);
+ delete witnesses[i];
+ }
+ }
+
+ for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
+ {
+ if (prevSolution_[i])
+ si_->freeState(prevSolution_[i]);
+ }
+ prevSolution_.clear();
+}
+
+ompl::geometric::SST::Motion* ompl::geometric::SST::selectNode(ompl::geometric::SST::Motion *sample)
+{
+ std::vector<Motion*> ret;
+ Motion* selected = nullptr;
+ base::Cost bestCost = opt_->infiniteCost();
+ nn_->nearestR(sample, selectionRadius_, ret);
+ for (unsigned int i = 0; i < ret.size(); i++)
+ {
+ if (!ret[i]->inactive_ && opt_->isCostBetterThan(ret[i]->accCost_, bestCost))
+ {
+ bestCost = ret[i]->accCost_;
+ selected = ret[i];
+ }
+ }
+ if(selected==nullptr)
+ {
+ int k = 1;
+ while (selected == nullptr)
+ {
+ nn_->nearestK(sample,k,ret);
+ for (unsigned int i = 0; i < ret.size() && selected == nullptr; i++)
+ if(!ret[i]->inactive_)
+ selected = ret[i];
+ k += 5;
+ }
+ }
+ return selected;
+}
+
+ompl::geometric::SST::Witness* ompl::geometric::SST::findClosestWitness(ompl::geometric::SST::Motion *node)
+{
+ if(witnesses_->size() > 0)
+ {
+ Witness *closest = static_cast<Witness*>(witnesses_->nearest(node));
+ if(distanceFunction(closest, node) > pruningRadius_)
+ {
+ closest = new Witness(si_);
+ closest->linkRep(node);
+ si_->copyState(closest->state_, node->state_);
+ witnesses_->add(closest);
+ }
+ return closest;
+ }
+ else
+ {
+ Witness *closest = new Witness(si_);
+ closest->linkRep(node);
+ si_->copyState(closest->state_, node->state_);
+ witnesses_->add(closest);
+ return closest;
+ }
+}
+
+
+ompl::base::State* ompl::geometric::SST::monteCarloProp(Motion *m)
+{
+ //sample random point to serve as a direction
+ base::State *xstate = si_->allocState();
+ sampler_->sampleUniform(xstate);
+
+ //sample length of step from (0 - maxDistance_]
+ double step = rng_.uniformReal(0, maxDistance_);
+
+ //take a step of length step towards the random state
+ double d = si_->distance(m->state_, xstate);
+ si_->getStateSpace()->interpolate(m->state_, xstate, step / d, xstate);
+
+ return xstate;
+}
+
+ompl::base::PlannerStatus ompl::geometric::SST::solve(const base::PlannerTerminationCondition &ptc)
+{
+ checkValidity();
+ base::Goal *goal = pdef_->getGoal().get();
+ base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
+
+ while (const base::State *st = pis_.nextStart())
+ {
+ Motion *motion = new Motion(si_);
+ si_->copyState(motion->state_, st);
+ nn_->add(motion);
+ motion->accCost_ = opt_->identityCost();
+ findClosestWitness(motion);
+ }
+
+ if (nn_->size() == 0)
+ {
+ OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
+ return base::PlannerStatus::INVALID_START;
+ }
+
+ if (!sampler_)
+ sampler_ = si_->allocStateSampler();
+
+ OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
+
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
+ double approxdif = std::numeric_limits<double>::infinity();
+ bool sufficientlyShort = false;
+ Motion *rmotion = new Motion(si_);
+ base::State *rstate = rmotion->state_;
+ base::State *xstate = si_->allocState();
+
+ unsigned iterations = 0;
+
+ while (ptc == false)
+ {
+ /* sample random state (with goal biasing) */
+ bool attemptToReachGoal = (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample());
+ if (attemptToReachGoal)
+ goal_s->sampleGoal(rstate);
+ else
+ sampler_->sampleUniform(rstate);
+
+ /* find closest state in the tree */
+ Motion *nmotion = selectNode(rmotion);
+
+ base::State *dstate = rstate;
+ double d = si_->distance(nmotion->state_, rstate);
+
+ attemptToReachGoal = rng_.uniform01() < .5;
+ if (attemptToReachGoal)
+ {
+ if (d > maxDistance_)
+ {
+ si_->getStateSpace()->interpolate(nmotion->state_, rstate, maxDistance_ / d, xstate);
+ dstate = xstate;
+ }
+ }
+ else
+ {
+ dstate = monteCarloProp(nmotion);
+ }
+
+
+ si_->copyState(rstate, dstate);
+
+ if (si_->checkMotion(nmotion->state_, rstate))
+ {
+ base::Cost incCost = opt_->motionCost(nmotion->state_, rstate);
+ base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
+ Witness* closestWitness = findClosestWitness(rmotion);
+
+ if (closestWitness->rep_ == rmotion || opt_->isCostBetterThan(cost, closestWitness->rep_->accCost_))
+ {
+ Motion* oldRep = closestWitness->rep_;
+ /* create a motion */
+ Motion *motion = new Motion(si_);
+ motion->accCost_ = cost;
+ si_->copyState(motion->state_, rstate);
+
+ if (!attemptToReachGoal)
+ si_->freeState(dstate);
+ motion->parent_ = nmotion;
+ nmotion->numChildren_++;
+ closestWitness->linkRep(motion);
+
+ nn_->add(motion);
+ double dist = 0.0;
+ bool solv = goal->isSatisfied(motion->state_, &dist);
+ if (solv && opt_->isCostBetterThan(motion->accCost_,prevSolutionCost_))
+ {
+ approxdif = dist;
+ solution = motion;
+
+ for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
+ if (prevSolution_[i])
+ si_->freeState(prevSolution_[i]);
+ prevSolution_.clear();
+ Motion* solTrav = solution;
+ while (solTrav!=nullptr)
+ {
+ prevSolution_.push_back(si_->cloneState(solTrav->state_) );
+ solTrav = solTrav->parent_;
+ }
+ prevSolutionCost_ = solution->accCost_;
+
+ OMPL_INFORM("Found solution with cost %.2f",solution->accCost_.value());
+ sufficientlyShort = opt_->isSatisfied(solution->accCost_);
+ if (sufficientlyShort)
+ {
+ break;
+ }
+ }
+ if (solution==nullptr && dist < approxdif)
+ {
+ approxdif = dist;
+ approxsol = motion;
+
+ for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
+ {
+ if (prevSolution_[i])
+ si_->freeState(prevSolution_[i]);
+ }
+ prevSolution_.clear();
+ Motion *solTrav = approxsol;
+ while (solTrav!=nullptr)
+ {
+ prevSolution_.push_back(si_->cloneState(solTrav->state_) );
+ solTrav = solTrav->parent_;
+ }
+ }
+
+ if(oldRep != rmotion)
+ {
+ oldRep->inactive_ = true;
+ nn_->remove(oldRep);
+ while (oldRep->inactive_ && oldRep->numChildren_==0)
+ {
+ if (oldRep->state_)
+ si_->freeState(oldRep->state_);
+ oldRep->state_=nullptr;
+ oldRep->parent_->numChildren_--;
+ Motion* oldRepParent = oldRep->parent_;
+ delete oldRep;
+ oldRep = oldRepParent;
+ }
+ }
+
+ }
+ }
+ iterations++;
+ }
+
+ bool solved = false;
+ bool approximate = false;
+ if (solution == nullptr)
+ {
+ solution = approxsol;
+ approximate = true;
+ }
+
+ if (solution != nullptr)
+ {
+ /* set the solution path */
+ PathGeometric *path = new PathGeometric(si_);
+ for (int i = prevSolution_.size() - 1 ; i >= 0 ; --i)
+ path->append(prevSolution_[i]);
+ solved = true;
+ pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
+ }
+
+ si_->freeState(xstate);
+ if (rmotion->state_)
+ si_->freeState(rmotion->state_);
+ rmotion->state_=nullptr;
+ delete rmotion;
+
+ OMPL_INFORM("%s: Created %u states in %u iterations", getName().c_str(), nn_->size(),iterations);
+
+ return base::PlannerStatus(solved, approximate);
+}
+
+void ompl::geometric::SST::getPlannerData(base::PlannerData &data) const
+{
+ Planner::getPlannerData(data);
+
+ std::vector<Motion*> motions;
+ std::vector<Motion*> allMotions;
+ if (nn_)
+ nn_->list(motions);
+
+ for (unsigned i=0; i<motions.size(); i++)
+ if(motions[i]->numChildren_ == 0)
+ allMotions.push_back(motions[i]);
+ for(unsigned i=0;i <allMotions.size(); i++)
+ if(allMotions[i]->getParent() != nullptr)
+ allMotions.push_back(allMotions[i]->getParent());
+
+ if (prevSolution_.size()!=0)
+ data.addGoalVertex(base::PlannerDataVertex(prevSolution_[0]));
+
+ for (unsigned int i = 0 ; i < allMotions.size() ; ++i)
+ {
+ if (allMotions[i]->getParent() == nullptr)
+ data.addStartVertex(base::PlannerDataVertex(allMotions[i]->getState()));
+ else
+ data.addEdge(base::PlannerDataVertex(allMotions[i]->getParent()->getState()),
+ base::PlannerDataVertex(allMotions[i]->getState()));
+ }
+}
diff --git a/src/ompl/geometric/planners/stride/STRIDE.h b/src/ompl/geometric/planners/stride/STRIDE.h
index 187f8d0..5562e5a 100644
--- a/src/ompl/geometric/planners/stride/STRIDE.h
+++ b/src/ompl/geometric/planners/stride/STRIDE.h
@@ -41,7 +41,7 @@
#include "ompl/geometric/planners/PlannerIncludes.h"
#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/datastructures/PDF.h"
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <boost/scoped_ptr.hpp>
#include <vector>
@@ -242,12 +242,12 @@ namespace ompl
class Motion
{
public:
- Motion() : state(NULL), parent(NULL)
+ Motion() : state(nullptr), parent(nullptr)
{
}
/** \brief Constructor that allocates memory for the state */
- Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
+ Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
{
}
diff --git a/src/ompl/geometric/planners/stride/src/STRIDE.cpp b/src/ompl/geometric/planners/stride/src/STRIDE.cpp
index 6877457..a1bb342 100644
--- a/src/ompl/geometric/planners/stride/src/STRIDE.cpp
+++ b/src/ompl/geometric/planners/stride/src/STRIDE.cpp
@@ -87,9 +87,9 @@ void ompl::geometric::STRIDE::setupTree()
{
tree_.reset(new NearestNeighborsGNAT<Motion*>(degree_, minDegree_, maxDegree_, maxNumPtsPerLeaf_, estimatedDimension_));
if (useProjectedDistance_)
- tree_->setDistanceFunction(boost::bind(&STRIDE::projectedDistanceFunction, this, _1, _2));
+ tree_->setDistanceFunction(std::bind(&STRIDE::projectedDistanceFunction, this, std::placeholders::_1, std::placeholders::_2));
else
- tree_->setDistanceFunction(boost::bind(&STRIDE::distanceFunction, this, _1, _2));
+ tree_->setDistanceFunction(std::bind(&STRIDE::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void ompl::geometric::STRIDE::clear()
@@ -140,8 +140,8 @@ ompl::base::PlannerStatus ompl::geometric::STRIDE::solve(const base::PlannerTerm
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_->size());
- Motion *solution = NULL;
- Motion *approxsol = NULL;
+ Motion *solution = nullptr;
+ Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
base::State *xstate = si_->allocState();
@@ -187,17 +187,17 @@ ompl::base::PlannerStatus ompl::geometric::STRIDE::solve(const base::PlannerTerm
bool solved = false;
bool approximate = false;
- if (solution == NULL)
+ if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
- if (solution != NULL)
+ if (solution != nullptr)
{
/* construct the solution path */
std::vector<Motion*> mpath;
- while (solution != NULL)
+ while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
@@ -236,7 +236,7 @@ void ompl::geometric::STRIDE::getPlannerData(base::PlannerData &data) const
tree_->list(motions);
for (std::vector<Motion*>::iterator it=motions.begin(); it!=motions.end(); it++)
{
- if((*it)->parent == NULL)
+ if((*it)->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex((*it)->state,1));
else
data.addEdge(base::PlannerDataVertex((*it)->parent->state,1),base::PlannerDataVertex((*it)->state,1));
diff --git a/src/ompl/geometric/src/GeneticSearch.cpp b/src/ompl/geometric/src/GeneticSearch.cpp
index ba6d654..f10dd58 100644
--- a/src/ompl/geometric/src/GeneticSearch.cpp
+++ b/src/ompl/geometric/src/GeneticSearch.cpp
@@ -279,7 +279,7 @@ void ompl::geometric::GeneticSearch::tryToImprove(const base::GoalRegion &goal,
hc_.tryToImprove(goal, state, dist, &distance);
hc_.tryToImprove(goal, state, dist / 3.0, &distance);
hc_.tryToImprove(goal, state, dist / 10.0, &distance);
- OMPL_DEBUG("Improvement took %u ms", (time::now() - start).total_milliseconds());
+ OMPL_DEBUG("Improvement took %u ms", std::chrono::duration_cast<std::chrono::milliseconds>(time::now() - start).count());
OMPL_DEBUG("Distance to goal after improvement: %g", distance);
}
diff --git a/src/ompl/geometric/src/PathGeometric.cpp b/src/ompl/geometric/src/PathGeometric.cpp
index 1400845..d64c76c 100644
--- a/src/ompl/geometric/src/PathGeometric.cpp
+++ b/src/ompl/geometric/src/PathGeometric.cpp
@@ -154,6 +154,10 @@ double ompl::geometric::PathGeometric::smoothness() const
bool ompl::geometric::PathGeometric::check() const
{
+ // make sure state validity checker is set
+ if (!si_->isSetup())
+ si_->setup();
+
bool result = true;
if (states_.size() > 0)
{
@@ -206,8 +210,8 @@ std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned in
if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1 - 1]))
return std::make_pair(false, false);
- base::State *temp = NULL;
- base::UniformValidStateSampler *uvss = NULL;
+ base::State *temp = nullptr;
+ base::UniformValidStateSampler *uvss = nullptr;
bool result = true;
for (int i = 1 ; i < n1 ; ++i)
@@ -267,7 +271,7 @@ std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned in
// free potentially allocated memory
if (temp)
si_->freeState(temp);
- bool originalValid = uvss == NULL;
+ bool originalValid = uvss == nullptr;
if (uvss)
delete uvss;
diff --git a/src/ompl/geometric/src/PathHybridization.cpp b/src/ompl/geometric/src/PathHybridization.cpp
index 5c18ac1..76405a4 100644
--- a/src/ompl/geometric/src/PathHybridization.cpp
+++ b/src/ompl/geometric/src/PathHybridization.cpp
@@ -52,9 +52,9 @@ ompl::geometric::PathHybridization::PathHybridization(const base::SpaceInformati
name_("PathHybridization")
{
root_ = boost::add_vertex(g_);
- stateProperty_[root_] = NULL;
+ stateProperty_[root_] = nullptr;
goal_ = boost::add_vertex(g_);
- stateProperty_[goal_] = NULL;
+ stateProperty_[goal_] = nullptr;
}
ompl::geometric::PathHybridization::~PathHybridization()
@@ -68,9 +68,9 @@ void ompl::geometric::PathHybridization::clear()
g_.clear();
root_ = boost::add_vertex(g_);
- stateProperty_[root_] = NULL;
+ stateProperty_[root_] = nullptr;
goal_ = boost::add_vertex(g_);
- stateProperty_[goal_] = NULL;
+ stateProperty_[goal_] = nullptr;
}
void ompl::geometric::PathHybridization::print(std::ostream &out) const
diff --git a/src/ompl/geometric/src/PathSimplifier.cpp b/src/ompl/geometric/src/PathSimplifier.cpp
index 44f83f9..fb4a851 100644
--- a/src/ompl/geometric/src/PathSimplifier.cpp
+++ b/src/ompl/geometric/src/PathSimplifier.cpp
@@ -46,7 +46,7 @@ ompl::geometric::PathSimplifier::PathSimplifier(const base::SpaceInformationPtr
{
if (goal)
{
- gsr_ = boost::dynamic_pointer_cast<base::GoalSampleableRegion>(goal);
+ gsr_ = std::dynamic_pointer_cast<base::GoalSampleableRegion>(goal);
if (!gsr_)
OMPL_WARN("%s: Goal could not be cast to GoalSampleableRegion. Goal simplification will not be performed.", __FUNCTION__);
}
@@ -203,7 +203,7 @@ bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned
for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
{
// Sample a random point anywhere along the path
- base::State *s0 = NULL;
+ base::State *s0 = nullptr;
int index0 = -1;
double t0 = 0.0;
double p0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
@@ -221,7 +221,7 @@ bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned
}
// Sample a random point within rd distance of the previously sampled point
- base::State *s1 = NULL;
+ base::State *s1 = nullptr;
int index1 = -1;
double t1 = 0.0;
double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back())); // sample a random point (p1) near p0
diff --git a/src/ompl/tools/benchmark/Benchmark.h b/src/ompl/tools/benchmark/Benchmark.h
index 6dbca24..f220d7c 100644
--- a/src/ompl/tools/benchmark/Benchmark.h
+++ b/src/ompl/tools/benchmark/Benchmark.h
@@ -82,10 +82,10 @@ namespace ompl
typedef std::vector<std::map<std::string, std::string> > RunProgressData;
/** \brief Signature of function that can be called before a planner execution is started */
- typedef boost::function<void(const base::PlannerPtr&)> PreSetupEvent;
+ typedef std::function<void(const base::PlannerPtr&)> PreSetupEvent;
/** \brief Signature of function that can be called after a planner execution is completed */
- typedef boost::function<void(const base::PlannerPtr&, RunProperties&)> PostSetupEvent;
+ typedef std::function<void(const base::PlannerPtr&, RunProperties&)> PostSetupEvent;
/** \brief The data collected after running a planner multiple times */
struct PlannerExperiment
@@ -197,13 +197,13 @@ namespace ompl
};
/** \brief Constructor needs the SimpleSetup instance needed for planning. Optionally, the experiment name (\e name) can be specified */
- Benchmark(geometric::SimpleSetup &setup, const std::string &name = std::string()) : gsetup_(&setup), csetup_(NULL)
+ Benchmark(geometric::SimpleSetup &setup, const std::string &name = std::string()) : gsetup_(&setup), csetup_(nullptr)
{
exp_.name = name;
}
/** \brief Constructor needs the SimpleSetup instance needed for planning. Optionally, the experiment name (\e name) can be specified */
- Benchmark(control::SimpleSetup &setup, const std::string &name = std::string()) : gsetup_(NULL), csetup_(&setup)
+ Benchmark(control::SimpleSetup &setup, const std::string &name = std::string()) : gsetup_(nullptr), csetup_(&setup)
{
exp_.name = name;
}
diff --git a/src/ompl/tools/benchmark/src/Benchmark.cpp b/src/ompl/tools/benchmark/src/Benchmark.cpp
index 8506dda..5eef514 100644
--- a/src/ompl/tools/benchmark/src/Benchmark.cpp
+++ b/src/ompl/tools/benchmark/src/Benchmark.cpp
@@ -39,9 +39,10 @@
#include "ompl/util/Time.h"
#include "ompl/config.h"
#include <boost/scoped_ptr.hpp>
-#include <boost/lexical_cast.hpp>
#include <boost/progress.hpp>
-#include <boost/thread.hpp>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
#include <fstream>
#include <sstream>
@@ -53,13 +54,13 @@ namespace ompl
/** \brief Propose a name for a file in which results should be saved, based on the date and hostname of the experiment */
static std::string getResultsFilename(const Benchmark::CompleteExperiment &exp)
{
- return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".log";
+ return "ompl_" + exp.host + "_" + time::as_string(exp.startTime) + ".log";
}
/** \brief Propose a name for a file in which console output should be saved, based on the date and hostname of the experiment */
static std::string getConsoleFilename(const Benchmark::CompleteExperiment &exp)
{
- return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".console";
+ return "ompl_" + exp.host + "_" + time::as_string(exp.startTime) + ".console";
}
static bool terminationCondition(const machine::MemUsage_t maxMem, const time::point &endTime)
@@ -80,43 +81,37 @@ namespace ompl
void run(const base::PlannerPtr &planner, const machine::MemUsage_t memStart, const machine::MemUsage_t maxMem, const double maxTime, const double timeBetweenUpdates)
{
- if (!useThreads_)
- {
+ // if (!useThreads_)
+ // {
runThread(planner, memStart + maxMem, time::seconds(maxTime), time::seconds(timeBetweenUpdates));
- return;
- }
+ // return;
+ // }
- boost::thread t(boost::bind(&RunPlanner::runThread, this, planner, memStart + maxMem, time::seconds(maxTime), time::seconds(timeBetweenUpdates)));
+ // std::thread t(std::bind(&RunPlanner::runThread, this, planner, memStart + maxMem, time::seconds(maxTime), time::seconds(timeBetweenUpdates)));
// allow 25% more time than originally specified, in order to detect planner termination
-#if BOOST_VERSION < 105000
- // For older versions of boost, we have to use this
- // deprecated form of the timed join
- if (!t.timed_join(time::seconds(maxTime * 1.25)))
-#else
- if (!t.try_join_for(boost::chrono::duration<double>(maxTime * 1.25)))
-#endif
- {
- status_ = base::PlannerStatus::CRASH;
-
- std::stringstream es;
- es << "Planner " << benchmark_->getStatus().activePlanner << " did not complete run " << benchmark_->getStatus().activeRun
- << " within the specified amount of time (possible crash). Attempting to force termination of planning thread ..." << std::endl;
- std::cerr << es.str();
- OMPL_ERROR(es.str().c_str());
-
- t.interrupt();
- t.join();
-
- std::string m = "Planning thread cancelled";
- std::cerr << m << std::endl;
- OMPL_ERROR(m.c_str());
- }
-
- if (memStart < memUsed_)
- memUsed_ -= memStart;
- else
- memUsed_ = 0;
+ // if (!t.try_join_for(time::seconds(maxTime * 1.25)))
+ // {
+ // status_ = base::PlannerStatus::CRASH;
+ //
+ // std::stringstream es;
+ // es << "Planner " << benchmark_->getStatus().activePlanner << " did not complete run " << benchmark_->getStatus().activeRun
+ // << " within the specified amount of time (possible crash). Attempting to force termination of planning thread ..." << std::endl;
+ // std::cerr << es.str();
+ // OMPL_ERROR(es.str().c_str());
+ //
+ // t.interrupt();
+ // t.join();
+ //
+ // std::string m = "Planning thread cancelled";
+ // std::cerr << m << std::endl;
+ // OMPL_ERROR(m.c_str());
+ // }
+
+ // if (memStart < memUsed_)
+ // memUsed_ -= memStart;
+ // else
+ // memUsed_ = 0;
}
double getTimeUsed() const
@@ -147,7 +142,7 @@ namespace ompl
try
{
- base::PlannerTerminationConditionFn ptc = boost::bind(&terminationCondition, maxMem, time::now() + maxDuration);
+ base::PlannerTerminationConditionFn ptc = std::bind(&terminationCondition, maxMem, time::now() + maxDuration);
solved_ = false;
// Only launch the planner progress property
// collector if there is any data for it to report
@@ -156,9 +151,9 @@ namespace ompl
// always gets taken before planner even starts;
// might be worth adding a short wait time before
// collector begins sampling
- boost::scoped_ptr<boost::thread> t;
+ boost::scoped_ptr<std::thread> t;
if (planner->getPlannerProgressProperties().size() > 0)
- t.reset(new boost::thread(boost::bind(&RunPlanner::collectProgressProperties, this,
+ t.reset(new std::thread(std::bind(&RunPlanner::collectProgressProperties, this,
planner->getPlannerProgressProperties(),
timeBetweenUpdates)));
status_ = planner->solve(ptc, 0.1);
@@ -187,16 +182,15 @@ namespace ompl
{
time::point timeStart = time::now();
- boost::unique_lock<boost::mutex> ulock(solvedFlag_);
+ std::unique_lock<std::mutex> ulock(solvedFlag_);
while (!solved_)
{
- if (solvedCondition_.timed_wait(ulock, time::now() + timePerUpdate))
+ if (solvedCondition_.wait_for(ulock, timePerUpdate) == std::cv_status::no_timeout)
return;
else
{
double timeInSeconds = time::seconds(time::now() - timeStart);
- std::string timeStamp =
- boost::lexical_cast<std::string>(timeInSeconds);
+ std::string timeStamp = std::to_string(timeInSeconds);
std::map<std::string, std::string> data;
data["time REAL"] = timeStamp;
for (base::Planner::PlannerProgressProperties::const_iterator item = properties.begin();
@@ -219,8 +213,8 @@ namespace ompl
// variables needed for progress property collection
bool solved_;
- boost::mutex solvedFlag_;
- boost::condition_variable solvedCondition_;
+ std::mutex solvedFlag_;
+ std::condition_variable solvedCondition_;
};
}
@@ -276,7 +270,7 @@ bool ompl::tools::Benchmark::saveResultsToStream(std::ostream &out) const
out << it->first << " = " << it->second << std::endl;
out << "Running on " << (exp_.host.empty() ? "UNKNOWN" : exp_.host) << std::endl;
- out << "Starting at " << boost::posix_time::to_iso_extended_string(exp_.startTime) << std::endl;
+ out << "Starting at " << time::as_string(exp_.startTime) << std::endl;
out << "<<<|" << std::endl << exp_.setupInfo << "|>>>" << std::endl;
out << "<<<|" << std::endl << exp_.cpuInfo << "|>>>" << std::endl;
@@ -588,35 +582,35 @@ void ompl::tools::Benchmark::benchmark(const Request &req)
{
RunProperties run;
- run["time REAL"] = boost::lexical_cast<std::string>(rp.getTimeUsed());
- run["memory REAL"] = boost::lexical_cast<std::string>((double)rp.getMemUsed() / (1024.0 * 1024.0));
- run["status ENUM"] = boost::lexical_cast<std::string>((int)static_cast<base::PlannerStatus::StatusType>(rp.getStatus()));
+ run["time REAL"] = std::to_string(rp.getTimeUsed());
+ run["memory REAL"] = std::to_string((double)rp.getMemUsed() / (1024.0 * 1024.0));
+ run["status ENUM"] = std::to_string((int)static_cast<base::PlannerStatus::StatusType>(rp.getStatus()));
if (gsetup_)
{
- run["solved BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->haveExactSolutionPath());
- run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
+ run["solved BOOLEAN"] = std::to_string(gsetup_->haveExactSolutionPath());
+ run["valid segment fraction REAL"] = std::to_string(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
}
else
{
- run["solved BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->haveExactSolutionPath());
- run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
+ run["solved BOOLEAN"] = std::to_string(csetup_->haveExactSolutionPath());
+ run["valid segment fraction REAL"] = std::to_string(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
}
if (solved)
{
if (gsetup_)
{
- run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->hasApproximateSolution());
- run["solution difference REAL"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->getSolutionDifference());
- run["solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
- run["solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
- run["solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
- run["solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
- run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
+ run["approximate solution BOOLEAN"] = std::to_string(gsetup_->getProblemDefinition()->hasApproximateSolution());
+ run["solution difference REAL"] = std::to_string(gsetup_->getProblemDefinition()->getSolutionDifference());
+ run["solution length REAL"] = std::to_string(gsetup_->getSolutionPath().length());
+ run["solution smoothness REAL"] = std::to_string(gsetup_->getSolutionPath().smoothness());
+ run["solution clearance REAL"] = std::to_string(gsetup_->getSolutionPath().clearance());
+ run["solution segments INTEGER"] = std::to_string(gsetup_->getSolutionPath().getStateCount() - 1);
+ run["correct solution BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
unsigned int factor = gsetup_->getStateSpace()->getValidSegmentCountFactor();
gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
- run["correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
+ run["correct solution strict BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
if (req.simplify)
@@ -625,32 +619,32 @@ void ompl::tools::Benchmark::benchmark(const Request &req)
time::point timeStart = time::now();
gsetup_->simplifySolution();
double timeUsed = time::seconds(time::now() - timeStart);
- run["simplification time REAL"] = boost::lexical_cast<std::string>(timeUsed);
- run["simplified solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
- run["simplified solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
- run["simplified solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
- run["simplified solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
- run["simplified correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
+ run["simplification time REAL"] = std::to_string(timeUsed);
+ run["simplified solution length REAL"] = std::to_string(gsetup_->getSolutionPath().length());
+ run["simplified solution smoothness REAL"] = std::to_string(gsetup_->getSolutionPath().smoothness());
+ run["simplified solution clearance REAL"] = std::to_string(gsetup_->getSolutionPath().clearance());
+ run["simplified solution segments INTEGER"] = std::to_string(gsetup_->getSolutionPath().getStateCount() - 1);
+ run["simplified correct solution BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
- run["simplified correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
+ run["simplified correct solution strict BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
}
}
else
{
- run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->hasApproximateSolution());
- run["solution difference REAL"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->getSolutionDifference());
- run["solution length REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().length());
- run["solution clearance REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().asGeometric().clearance());
- run["solution segments INTEGER"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().getControlCount());
- run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().check());
+ run["approximate solution BOOLEAN"] = std::to_string(csetup_->getProblemDefinition()->hasApproximateSolution());
+ run["solution difference REAL"] = std::to_string(csetup_->getProblemDefinition()->getSolutionDifference());
+ run["solution length REAL"] = std::to_string(csetup_->getSolutionPath().length());
+ run["solution clearance REAL"] = std::to_string(csetup_->getSolutionPath().asGeometric().clearance());
+ run["solution segments INTEGER"] = std::to_string(csetup_->getSolutionPath().getControlCount());
+ run["correct solution BOOLEAN"] = std::to_string(csetup_->getSolutionPath().check());
}
}
base::PlannerData pd (gsetup_ ? gsetup_->getSpaceInformation() : csetup_->getSpaceInformation());
planners_[i]->getPlannerData(pd);
- run["graph states INTEGER"] = boost::lexical_cast<std::string>(pd.numVertices());
- run["graph motions INTEGER"] = boost::lexical_cast<std::string>(pd.numEdges());
+ run["graph states INTEGER"] = std::to_string(pd.numVertices());
+ run["graph motions INTEGER"] = std::to_string(pd.numEdges());
for (std::map<std::string, std::string>::const_iterator it = pd.properties.begin() ; it != pd.properties.end() ; ++it)
run[it->first] = it->second;
diff --git a/src/ompl/tools/benchmark/src/MachineSpecs.cpp b/src/ompl/tools/benchmark/src/MachineSpecs.cpp
index 9b12cf0..b06d9a5 100644
--- a/src/ompl/tools/benchmark/src/MachineSpecs.cpp
+++ b/src/ompl/tools/benchmark/src/MachineSpecs.cpp
@@ -60,7 +60,7 @@ ompl::machine::MemUsage_t getProcessMemoryUsageAux()
ompl::machine::MemUsage_t result = 0;
- if (NULL != hProcess)
+ if (nullptr != hProcess)
{
if ( GetProcessMemoryInfo( hProcess, &pmc, sizeof(pmc)) )
result = pmc.WorkingSetSize;
@@ -76,7 +76,7 @@ std::string getCPUInfoAux()
char buffer[BUF_SIZE];
std::stringstream result;
FILE *cmdPipe = _popen("wmic cpu list full", "rt");
- if (cmdPipe != NULL)
+ if (cmdPipe != nullptr)
{
while (fgets(buffer, BUF_SIZE, cmdPipe))
result << buffer;
@@ -121,7 +121,7 @@ std::string getCPUInfoAux()
char buffer[BUF_SIZE];
std::stringstream result;
FILE *cmdPipe = popen("sysctl hw", "r");
- if (cmdPipe != NULL)
+ if (cmdPipe != nullptr)
{
while (fgets(buffer, BUF_SIZE, cmdPipe))
result << buffer;
@@ -182,7 +182,7 @@ std::string getCPUInfoAux()
char buffer[BUF_SIZE];
std::stringstream result;
FILE *cmdPipe = popen("lscpu", "r");
- if (cmdPipe != NULL)
+ if (cmdPipe != nullptr)
{
while (fgets(buffer, BUF_SIZE, cmdPipe))
result << buffer;
diff --git a/src/ompl/tools/config/SelfConfig.h b/src/ompl/tools/config/SelfConfig.h
index fb1bb7d..609886c 100644
--- a/src/ompl/tools/config/SelfConfig.h
+++ b/src/ompl/tools/config/SelfConfig.h
@@ -44,6 +44,7 @@
#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
#include "ompl/datastructures/NearestNeighborsGNAT.h"
#include "ompl/datastructures/NearestNeighborsGNATNoThreadSafety.h"
+#include <mutex>
#include <iostream>
#include <string>
@@ -116,6 +117,7 @@ namespace ompl
SelfConfigImpl *impl_;
std::string context_;
+ static std::mutex staticConstructorLock_;
/// @endcond
};
}
diff --git a/src/ompl/tools/config/src/SelfConfig.cpp b/src/ompl/tools/config/src/SelfConfig.cpp
index 0a148b7..00cbe31 100644
--- a/src/ompl/tools/config/src/SelfConfig.cpp
+++ b/src/ompl/tools/config/src/SelfConfig.cpp
@@ -43,9 +43,7 @@
#include "ompl/control/planners/rrt/RRT.h"
#include "ompl/control/planners/kpiece/KPIECE1.h"
#include "ompl/util/Console.h"
-#include <boost/thread/mutex.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
+#include <memory>
#include <algorithm>
#include <limits>
#include <cmath>
@@ -161,28 +159,28 @@ namespace ompl
// we store weak pointers so that the SpaceInformation instances are not kept in
// memory until termination of the program due to the use of a static ConfigMap below
- boost::weak_ptr<base::SpaceInformation> wsi_;
+ std::weak_ptr<base::SpaceInformation> wsi_;
double probabilityOfValidState_;
double averageValidMotionLength_;
- boost::mutex lock_;
+ std::mutex lock_;
};
}
}
+std::mutex ompl::tools::SelfConfig::staticConstructorLock_;
/// @endcond
ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) :
context_(context.empty() ? "" : context + ": ")
{
- typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;
+ typedef std::map<base::SpaceInformation*, std::shared_ptr<SelfConfigImpl> > ConfigMap;
- static ConfigMap SMAP;
- static boost::mutex LOCK;
+ std::unique_lock<std::mutex> smLock(staticConstructorLock_);
- boost::mutex::scoped_lock smLock(LOCK);
+ static ConfigMap SMAP;
// clean expired entries from the map
ConfigMap::iterator dit = SMAP.begin();
@@ -213,37 +211,37 @@ ompl::tools::SelfConfig::~SelfConfig()
double ompl::tools::SelfConfig::getProbabilityOfValidState()
{
- boost::mutex::scoped_lock iLock(impl_->lock_);
+ std::lock_guard<std::mutex> iLock(impl_->lock_);
return impl_->getProbabilityOfValidState();
}
double ompl::tools::SelfConfig::getAverageValidMotionLength()
{
- boost::mutex::scoped_lock iLock(impl_->lock_);
+ std::lock_guard<std::mutex> iLock(impl_->lock_);
return impl_->getAverageValidMotionLength();
}
void ompl::tools::SelfConfig::configureValidStateSamplingAttempts(unsigned int &attempts)
{
- boost::mutex::scoped_lock iLock(impl_->lock_);
+ std::lock_guard<std::mutex> iLock(impl_->lock_);
impl_->configureValidStateSamplingAttempts(attempts);
}
void ompl::tools::SelfConfig::configurePlannerRange(double &range)
{
- boost::mutex::scoped_lock iLock(impl_->lock_);
+ std::lock_guard<std::mutex> iLock(impl_->lock_);
impl_->configurePlannerRange(range, context_);
}
void ompl::tools::SelfConfig::configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
{
- boost::mutex::scoped_lock iLock(impl_->lock_);
+ std::lock_guard<std::mutex> iLock(impl_->lock_);
return impl_->configureProjectionEvaluator(proj, context_);
}
void ompl::tools::SelfConfig::print(std::ostream &out) const
{
- boost::mutex::scoped_lock iLock(impl_->lock_);
+ std::lock_guard<std::mutex> iLock(impl_->lock_);
impl_->print(out);
}
@@ -254,21 +252,23 @@ ompl::base::PlannerPtr ompl::tools::SelfConfig::getDefaultPlanner(const base::Go
throw Exception("Unable to allocate default planner for unspecified goal definition");
base::SpaceInformationPtr si(goal->getSpaceInformation());
- control::SpaceInformationPtr siC(boost::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si));
+ const base::StateSpacePtr &space(si->getStateSpace());
+ control::SpaceInformationPtr siC(std::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si));
if (siC) // kinodynamic planning
{
// if we have a default projection
- if (siC->getStateSpace()->hasDefaultProjection())
+ if (space->hasDefaultProjection())
planner = base::PlannerPtr(new control::KPIECE1(siC));
// otherwise use a single-tree planner
else
planner = base::PlannerPtr(new control::RRT(siC));
}
- // if we can sample the goal region, use a bi-directional planner
- else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
+ // if we can sample the goal region and interpolation between states is symmetric,
+ // use a bi-directional planner
+ else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION) && space->hasSymmetricInterpolate())
{
// if we have a default projection
- if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
+ if (space->hasDefaultProjection())
planner = base::PlannerPtr(new geometric::LBKPIECE1(goal->getSpaceInformation()));
else
planner = base::PlannerPtr(new geometric::RRTConnect(goal->getSpaceInformation()));
@@ -277,7 +277,7 @@ ompl::base::PlannerPtr ompl::tools::SelfConfig::getDefaultPlanner(const base::Go
else
{
// if we have a default projection
- if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
+ if (space->hasDefaultProjection())
planner = base::PlannerPtr(new geometric::KPIECE1(goal->getSpaceInformation()));
else
planner = base::PlannerPtr(new geometric::RRT(goal->getSpaceInformation()));
diff --git a/src/ompl/tools/debug/PlannerMonitor.h b/src/ompl/tools/debug/PlannerMonitor.h
index a4182b2..5559e63 100644
--- a/src/ompl/tools/debug/PlannerMonitor.h
+++ b/src/ompl/tools/debug/PlannerMonitor.h
@@ -39,8 +39,7 @@
#include <iostream>
#include <boost/scoped_ptr.hpp>
-#include <boost/thread.hpp>
-#include <boost/noncopyable.hpp>
+#include <thread>
#include "ompl/base/Planner.h"
@@ -50,9 +49,12 @@ namespace ompl
{
/** \brief Monitor the properties a planner exposes, as the planner is running.
Dump the planner properties to a stream, periodically. */
- class PlannerMonitor : private boost::noncopyable
+ class PlannerMonitor
{
public:
+ // non-copyright
+ PlannerMonitor(const PlannerMonitor&) = delete;
+ PlannerMonitor& operator=(const PlannerMonitor&) = delete;
/** \brief Monitor a planner instance, and dump its properties to a specified stream, periodically.
@@ -84,7 +86,7 @@ namespace ompl
std::ostream &out_;
double period_;
bool shouldMonitor_;
- boost::scoped_ptr<boost::thread> monitorThread_;
+ boost::scoped_ptr<std::thread> monitorThread_;
};
}
}
diff --git a/src/ompl/tools/debug/Profiler.h b/src/ompl/tools/debug/Profiler.h
index 5d31438..3ff5dfa 100644
--- a/src/ompl/tools/debug/Profiler.h
+++ b/src/ompl/tools/debug/Profiler.h
@@ -56,8 +56,8 @@
#include <map>
#include <string>
#include <iostream>
-#include <boost/thread.hpp>
-#include <boost/noncopyable.hpp>
+#include <thread>
+#include <mutex>
#include "ompl/util/Time.h"
@@ -72,9 +72,12 @@ namespace ompl
external profiling tools in that it allows the user to count
time spent in various bits of code (sub-function granularity)
or count how many times certain pieces of code are executed.*/
- class Profiler : private boost::noncopyable
+ class Profiler
{
public:
+ // non-copyable
+ Profiler(const Profiler&) = delete;
+ Profiler& operator=(const Profiler&) = delete;
/** \brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope. */
class ScopedBlock
@@ -244,7 +247,7 @@ namespace ompl
/** \brief Information about time spent in a section of the code */
struct TimeInfo
{
- TimeInfo() : total(0, 0, 0, 0), shortest(boost::posix_time::pos_infin), longest(boost::posix_time::neg_infin), parts(0)
+ TimeInfo() : total(time::seconds(0.)), shortest(time::duration::max()), longest(time::duration::min()), parts(0)
{
}
@@ -310,8 +313,8 @@ namespace ompl
void printThreadInfo(std::ostream &out, const PerThread &data);
- boost::mutex lock_;
- std::map<boost::thread::id, PerThread> data_;
+ std::mutex lock_;
+ std::map<std::thread::id, PerThread> data_;
TimeInfo tinfo_;
bool running_;
bool printOnDestroy_;
diff --git a/src/ompl/tools/debug/src/PlannerMonitor.cpp b/src/ompl/tools/debug/src/PlannerMonitor.cpp
index b222c49..44544a3 100644
--- a/src/ompl/tools/debug/src/PlannerMonitor.cpp
+++ b/src/ompl/tools/debug/src/PlannerMonitor.cpp
@@ -37,19 +37,19 @@
#include "ompl/tools/debug/PlannerMonitor.h"
#include "ompl/util/Time.h"
#include <limits>
-#include <boost/bind.hpp>
+#include <functional>
void ompl::tools::PlannerMonitor::startMonitor()
{
- if (monitorThread_)
+ if (monitorThread_)
return;
shouldMonitor_ = true;
- monitorThread_.reset(new boost::thread(boost::bind(&PlannerMonitor::threadFunction, this)));
+ monitorThread_.reset(new std::thread(std::bind(&PlannerMonitor::threadFunction, this)));
}
void ompl::tools::PlannerMonitor::stopMonitor()
{
- if (!monitorThread_)
+ if (!monitorThread_)
return;
shouldMonitor_ = false;
monitorThread_->join();
@@ -58,7 +58,7 @@ void ompl::tools::PlannerMonitor::stopMonitor()
void ompl::tools::PlannerMonitor::threadFunction()
{
- time::point startTime = time::now();
+ time::point startTime = time::now();
time::point lastOutputTime = startTime;
while (shouldMonitor_)
@@ -66,7 +66,7 @@ void ompl::tools::PlannerMonitor::threadFunction()
double timeSinceOutput = time::seconds(time::now() - lastOutputTime);
if (timeSinceOutput < period_)
{
- boost::this_thread::sleep(time::seconds(0.01));
+ std::this_thread::sleep_for(time::seconds(0.01));
continue;
}
out_.seekp(0);
@@ -85,6 +85,6 @@ void ompl::tools::PlannerMonitor::threadFunction()
out_ << std::endl;
out_.flush();
lastOutputTime = time::now();
- boost::this_thread::sleep(time::seconds(0.01));
+ std::this_thread::sleep_for(time::seconds(0.01));
}
}
diff --git a/src/ompl/tools/debug/src/Profiler.cpp b/src/ompl/tools/debug/src/Profiler.cpp
index 33a78ba..59c48ca 100644
--- a/src/ompl/tools/debug/src/Profiler.cpp
+++ b/src/ompl/tools/debug/src/Profiler.cpp
@@ -36,6 +36,7 @@
/** \author Ioan Sucan */
#include "ompl/tools/debug/Profiler.h"
+#include <cmath>
ompl::tools::Profiler& ompl::tools::Profiler::Instance()
{
@@ -85,14 +86,14 @@ void ompl::tools::Profiler::clear()
void ompl::tools::Profiler::event(const std::string &name, const unsigned int times)
{
lock_.lock();
- data_[boost::this_thread::get_id()].events[name] += times;
+ data_[std::this_thread::get_id()].events[name] += times;
lock_.unlock();
}
void ompl::tools::Profiler::average(const std::string &name, const double value)
{
lock_.lock();
- AvgInfo &a = data_[boost::this_thread::get_id()].avg[name];
+ AvgInfo &a = data_[std::this_thread::get_id()].avg[name];
a.total += value;
a.totalSqr += value*value;
a.parts++;
@@ -102,14 +103,14 @@ void ompl::tools::Profiler::average(const std::string &name, const double value)
void ompl::tools::Profiler::begin(const std::string &name)
{
lock_.lock();
- data_[boost::this_thread::get_id()].time[name].set();
+ data_[std::this_thread::get_id()].time[name].set();
lock_.unlock();
}
void ompl::tools::Profiler::end(const std::string &name)
{
lock_.lock();
- data_[boost::this_thread::get_id()].time[name].update();
+ data_[std::this_thread::get_id()].time[name].update();
lock_.unlock();
}
@@ -125,7 +126,7 @@ void ompl::tools::Profiler::status(std::ostream &out, bool merge)
if (merge)
{
PerThread combined;
- for (std::map<boost::thread::id, PerThread>::const_iterator it = data_.begin() ; it != data_.end() ; ++it)
+ for (std::map<std::thread::id, PerThread>::const_iterator it = data_.begin() ; it != data_.end() ; ++it)
{
for (std::map<std::string, unsigned long int>::const_iterator iev = it->second.events.begin() ; iev != it->second.events.end(); ++iev)
combined.events[iev->first] += iev->second;
@@ -149,7 +150,7 @@ void ompl::tools::Profiler::status(std::ostream &out, bool merge)
printThreadInfo(out, combined);
}
else
- for (std::map<boost::thread::id, PerThread>::const_iterator it = data_.begin() ; it != data_.end() ; ++it)
+ for (std::map<std::thread::id, PerThread>::const_iterator it = data_.begin() ; it != data_.end() ; ++it)
{
out << "Thread " << it->first << ":" << std::endl;
printThreadInfo(out, it->second);
@@ -228,7 +229,7 @@ void ompl::tools::Profiler::printThreadInfo(std::ostream &out, const PerThread &
{
const AvgInfo &a = data.avg.find(avg[i].name)->second;
out << avg[i].name << ": " << avg[i].value << " (stddev = " <<
- sqrt(fabs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl;
+ std::sqrt(std::abs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl;
}
std::vector<dataDoubleVal> time;
diff --git a/src/ompl/tools/experience/ExperienceSetup.h b/src/ompl/tools/experience/ExperienceSetup.h
index 47090ea..405b01a 100644
--- a/src/ompl/tools/experience/ExperienceSetup.h
+++ b/src/ompl/tools/experience/ExperienceSetup.h
@@ -51,7 +51,7 @@ namespace ompl
/// @endcond
/** \class ompl::geometric::ExperienceSetupPtr
- \brief A boost shared pointer wrapper for ompl::geometric::ExperienceSetup */
+ \brief A shared pointer wrapper for ompl::geometric::ExperienceSetup */
/** \brief Create the set of classes typically needed to solve a
geometric problem */
diff --git a/src/ompl/tools/lightning/DynamicTimeWarp.h b/src/ompl/tools/lightning/DynamicTimeWarp.h
index bd169ee..0220917 100644
--- a/src/ompl/tools/lightning/DynamicTimeWarp.h
+++ b/src/ompl/tools/lightning/DynamicTimeWarp.h
@@ -56,7 +56,7 @@ OMPL_CLASS_FORWARD(DynamicTimeWarp);
/// @endcond
/** \class ompl::geometric::DynamicTimeWarpPtr
- \brief A boost shared pointer wrapper for ompl::tools::DynamicTimeWarp */
+ \brief A shared pointer wrapper for ompl::tools::DynamicTimeWarp */
class DynamicTimeWarp
{
diff --git a/src/ompl/tools/lightning/Lightning.h b/src/ompl/tools/lightning/Lightning.h
index 7c11f87..ea4754f 100644
--- a/src/ompl/tools/lightning/Lightning.h
+++ b/src/ompl/tools/lightning/Lightning.h
@@ -92,7 +92,7 @@ namespace ompl
/// @endcond
/** \class ompl::geometric::LightningPtr
- \brief A boost shared pointer wrapper for ompl::tools::Lightning */
+ \brief A shared pointer wrapper for ompl::tools::Lightning */
/** \brief Built off of SimpleSetup but provides support for planning from experience */
class Lightning : public ompl::tools::ExperienceSetup
diff --git a/src/ompl/tools/lightning/LightningDB.h b/src/ompl/tools/lightning/LightningDB.h
index f614deb..0022d05 100644
--- a/src/ompl/tools/lightning/LightningDB.h
+++ b/src/ompl/tools/lightning/LightningDB.h
@@ -67,7 +67,7 @@ namespace ompl
/// @endcond
/** \class ompl::geometric::LightningDBPtr
- \brief A boost shared pointer wrapper for ompl::tools::LightningDB */
+ \brief A shared pointer wrapper for ompl::tools::LightningDB */
/** \brief Save and load entire paths from file */
class LightningDB
@@ -163,7 +163,7 @@ namespace ompl
ompl::base::PlannerDataStorage plannerDataStorage_;
// A nearest-neighbors datastructure containing the tree of start/goal states combined
- boost::shared_ptr< NearestNeighbors<ompl::base::PlannerDataPtr> > nn_;
+ std::shared_ptr< NearestNeighbors<ompl::base::PlannerDataPtr> > nn_;
// Reusable plannerData instance for filling in start and goal and performing searches on the tree
ompl::base::PlannerDataPtr nnSearchKey_;
diff --git a/src/ompl/tools/lightning/src/LightningDB.cpp b/src/ompl/tools/lightning/src/LightningDB.cpp
index 150a015..84db566 100644
--- a/src/ompl/tools/lightning/src/LightningDB.cpp
+++ b/src/ompl/tools/lightning/src/LightningDB.cpp
@@ -53,7 +53,7 @@ ompl::tools::LightningDB::LightningDB(const base::StateSpacePtr &space)
nn_.reset(new ompl::NearestNeighborsSqrtApprox<ompl::base::PlannerDataPtr>());
// Use our custom distance function for nearest neighbor tree
- nn_->setDistanceFunction(boost::bind(&ompl::tools::LightningDB::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&ompl::tools::LightningDB::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
// Load the PlannerData instance to be used for searching
nnSearchKey_.reset(new ompl::base::PlannerData(si_));
diff --git a/src/ompl/tools/multiplan/ParallelPlan.h b/src/ompl/tools/multiplan/ParallelPlan.h
index 3dc20f9..66a5324 100644
--- a/src/ompl/tools/multiplan/ParallelPlan.h
+++ b/src/ompl/tools/multiplan/ParallelPlan.h
@@ -39,7 +39,7 @@
#include "ompl/base/Planner.h"
#include "ompl/geometric/PathGeometric.h"
-#include <boost/thread.hpp>
+#include <mutex>
namespace ompl
{
@@ -130,7 +130,7 @@ namespace ompl
geometric::PathHybridizationPtr phybrid_;
/** \brief Lock for phybrid_ */
- boost::mutex phlock_;
+ std::mutex phlock_;
private:
@@ -138,7 +138,7 @@ namespace ompl
unsigned int foundSolCount_;
/** \brief Lock for phybrid_ */
- boost::mutex foundSolCountLock_;
+ std::mutex foundSolCountLock_;
};
}
diff --git a/src/ompl/tools/multiplan/src/ParallelPlan.cpp b/src/ompl/tools/multiplan/src/ParallelPlan.cpp
index 26ce504..c39c93c 100644
--- a/src/ompl/tools/multiplan/src/ParallelPlan.cpp
+++ b/src/ompl/tools/multiplan/src/ParallelPlan.cpp
@@ -36,6 +36,7 @@
#include "ompl/tools/multiplan/ParallelPlan.h"
#include "ompl/geometric/PathHybridization.h"
+#include <thread>
ompl::tools::ParallelPlan::ParallelPlan(const base::ProblemDefinitionPtr &pdef) :
pdef_(pdef), phybrid_(new geometric::PathHybridization(pdef->getSpaceInformation()))
@@ -96,15 +97,15 @@ ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(const base::PlannerTe
foundSolCount_ = 0;
time::point start = time::now();
- std::vector<boost::thread*> threads(planners_.size());
+ std::vector<std::thread*> threads(planners_.size());
// Decide if we are combining solutions or just taking the first one
if (hybridize)
for (std::size_t i = 0 ; i < threads.size() ; ++i)
- threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveMore, this, planners_[i].get(), minSolCount, maxSolCount, &ptc));
+ threads[i] = new std::thread(std::bind(&ParallelPlan::solveMore, this, planners_[i].get(), minSolCount, maxSolCount, &ptc));
else
for (std::size_t i = 0 ; i < threads.size() ; ++i)
- threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveOne, this, planners_[i].get(), minSolCount, &ptc));
+ threads[i] = new std::thread(std::bind(&ParallelPlan::solveOne, this, planners_[i].get(), minSolCount, &ptc));
for (std::size_t i = 0 ; i < threads.size() ; ++i)
{
@@ -169,7 +170,7 @@ void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t mi
const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
- boost::mutex::scoped_lock slock(phlock_);
+ std::lock_guard<std::mutex> slock(phlock_);
start = time::now();
unsigned int attempts = 0;
for (std::size_t i = 0 ; i < paths.size() ; ++i)
diff --git a/src/ompl/tools/thunder/SPARSdb.h b/src/ompl/tools/thunder/SPARSdb.h
index ecdc7b5..db58700 100644
--- a/src/ompl/tools/thunder/SPARSdb.h
+++ b/src/ompl/tools/thunder/SPARSdb.h
@@ -37,13 +37,14 @@
#ifndef OMPL_TOOLS_THUNDER_SPARS_DB_
#define OMPL_TOOLS_THUNDER_SPARS_DB_
-#include <ompl/geometric/planners/PlannerIncludes.h>
-#include <ompl/datastructures/NearestNeighbors.h>
-#include <ompl/geometric/PathSimplifier.h>
-#include <ompl/util/Time.h>
+#include "ompl/geometric/planners/PlannerIncludes.h"
+#include "ompl/datastructures/NearestNeighbors.h"
+#include "ompl/geometric/PathSimplifier.h"
+#include "ompl/util/Time.h"
+#include "ompl/util/Hash.h"
#include <boost/range/adaptor/map.hpp>
-#include <boost/unordered_map.hpp>
+#include <unordered_map>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/filtered_graph.hpp>
@@ -52,8 +53,8 @@
#include <boost/graph/connected_components.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/pending/disjoint_sets.hpp>
-#include <boost/function.hpp>
-#include <boost/thread.hpp>
+#include <functional>
+#include <thread>
#include <iostream>
#include <fstream>
#include <utility>
@@ -126,10 +127,10 @@ namespace ompl
/** \brief Constructor */
InterfaceData() :
- pointA_(NULL),
- pointB_(NULL),
- sigmaA_(NULL),
- sigmaB_(NULL),
+ pointA_(nullptr),
+ pointB_(nullptr),
+ sigmaA_(nullptr),
+ sigmaB_(nullptr),
d_(std::numeric_limits<double>::infinity())
{
}
@@ -140,22 +141,22 @@ namespace ompl
if (pointA_)
{
si->freeState(pointA_);
- pointA_ = NULL;
+ pointA_ = nullptr;
}
if (pointB_)
{
si->freeState(pointB_);
- pointB_ = NULL;
+ pointB_ = nullptr;
}
if (sigmaA_)
{
si->freeState(sigmaA_);
- sigmaA_ = NULL;
+ sigmaA_ = nullptr;
}
if (sigmaB_)
{
si->freeState(sigmaB_);
- sigmaB_ = NULL;
+ sigmaB_ = nullptr;
}
d_ = std::numeric_limits<double>::infinity();
}
@@ -192,7 +193,7 @@ namespace ompl
};
/** \brief the hash which maps pairs of neighbor points to pairs of states */
- typedef boost::unordered_map< VertexPair, InterfaceData, boost::hash< VertexPair > > InterfaceHash;
+ typedef std::unordered_map<VertexPair, InterfaceData> InterfaceHash;
////////////////////////////////////////////////////////////////////////////////////////
// The InterfaceHash structure is wrapped inside of this struct due to a compilation error on
@@ -714,7 +715,7 @@ namespace ompl
base::ValidStateSamplerPtr sampler_;
/** \brief Nearest neighbors data structure */
- boost::shared_ptr< NearestNeighbors<Vertex> > nn_;
+ std::shared_ptr< NearestNeighbors<Vertex> > nn_;
/** \brief Connectivity graph */
Graph g_;
diff --git a/src/ompl/tools/thunder/Thunder.h b/src/ompl/tools/thunder/Thunder.h
index 6bba44b..c3b4c29 100644
--- a/src/ompl/tools/thunder/Thunder.h
+++ b/src/ompl/tools/thunder/Thunder.h
@@ -79,7 +79,7 @@ namespace ompl
/// @endcond
/** \class ompl::geometric::ThunderPtr
- \brief A boost shared pointer wrapper for ompl::tools::Thunder */
+ \brief A shared pointer wrapper for ompl::tools::Thunder */
/** \brief Built off of SimpleSetup but provides support for planning from experience */
class Thunder : public ompl::tools::ExperienceSetup
@@ -199,7 +199,7 @@ namespace ompl
ompl::tools::ThunderDBPtr experienceDB_;
/** \brief Accumulated experiences to be later added to experience database */
- std::vector<ompl::geometric::PathGeometric> QueuedSolutionPaths_;
+ std::vector<ompl::geometric::PathGeometric> queuedSolutionPaths_;
}; // end of class Thunder
diff --git a/src/ompl/tools/thunder/ThunderDB.h b/src/ompl/tools/thunder/ThunderDB.h
index 03a73c8..f751f18 100644
--- a/src/ompl/tools/thunder/ThunderDB.h
+++ b/src/ompl/tools/thunder/ThunderDB.h
@@ -62,10 +62,10 @@ namespace ompl
OMPL_CLASS_FORWARD(ThunderDB);
/// @endcond
- typedef boost::shared_ptr<ompl::geometric::SPARSdb> SPARSdbPtr;
+ typedef std::shared_ptr<ompl::geometric::SPARSdb> SPARSdbPtr;
/** \class ompl::geometric::ThunderDBPtr
- \brief A boost shared pointer wrapper for ompl::tools::ThunderDB */
+ \brief A shared pointer wrapper for ompl::tools::ThunderDB */
/** \brief Save and load entire paths from file */
class ThunderDB
diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp
index 2f64cec..7939e46 100644
--- a/src/ompl/tools/thunder/src/SPARSdb.cpp
+++ b/src/ompl/tools/thunder/src/SPARSdb.cpp
@@ -40,7 +40,6 @@
#include <ompl/base/goals/GoalSampleableRegion.h>
#include <ompl/tools/config/SelfConfig.h>
#include <ompl/util/Console.h>
-#include <boost/lambda/bind.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
@@ -145,7 +144,7 @@ void ompl::geometric::SPARSdb::setup()
Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
- nn_->setDistanceFunction(boost::bind(&SPARSdb::distanceFunction, this, _1, _2));
+ nn_->setDistanceFunction(std::bind(&SPARSdb::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
double maxExt = si_->getMaximumExtent();
sparseDelta_ = sparseDeltaFraction_ * maxExt;
denseDelta_ = denseDeltaFraction_ * maxExt;
@@ -187,9 +186,9 @@ void ompl::geometric::SPARSdb::freeMemory()
{
foreach (InterfaceData &d, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_values)
d.clear(si_);
- if( stateProperty_[v] != NULL )
+ if( stateProperty_[v] != nullptr )
si_->freeState(stateProperty_[v]);
- stateProperty_[v] = NULL;
+ stateProperty_[v] = nullptr;
}
g_.clear();
@@ -237,7 +236,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int nearestK, const base::State*
}
if (!candidateSolution.path_)
{
- OMPL_ERROR("getSimilarPaths(): SPARSdb returned solution is NULL");
+ OMPL_ERROR("getSimilarPaths(): SPARSdb returned solution is nullptr");
return false;
}
@@ -434,7 +433,7 @@ bool ompl::geometric::SPARSdb::constructSolution(const Vertex start, const Verte
{
boost::astar_search(g_, // graph
start, // start state
- boost::bind(&SPARSdb::distanceFunction, this, _1, goal), // the heuristic
+ std::bind(&SPARSdb::distanceFunction, this, std::placeholders::_1, goal), // the heuristic
// ability to disable edges (set cost to inifinity):
boost::weight_map(edgeWeightMap(g_, edgeCollisionStateProperty_)).
predecessor_map(vertexPredecessors).
@@ -809,7 +808,7 @@ bool ompl::geometric::SPARSdb::addPathToRoadmap(const base::PlannerTerminationCo
for (std::size_t i = 1; i < solutionPath.getStateCount(); ++i) // skip 0 because start already added
{
// Check if we've already used this id
- if (i == addedStateIDs[usedIDTracker])
+ if (usedIDTracker < addedStateIDs.size() && i == addedStateIDs[usedIDTracker])
{
// skip this id
usedIDTracker ++;
@@ -1053,7 +1052,7 @@ void ompl::geometric::SPARSdb::checkQueryStateInitialization()
if (boost::num_vertices(g_) < 1)
{
queryVertex_ = boost::add_vertex( g_ );
- stateProperty_[queryVertex_] = NULL;
+ stateProperty_[queryVertex_] = nullptr;
}
}
@@ -1282,7 +1281,7 @@ void ompl::geometric::SPARSdb::findGraphNeighbors(base::State *st, std::vector<V
nn_->nearestR( queryVertex_, sparseDelta_, graphNeighborhood);
if (verbose_ && false)
OMPL_INFORM("Finding nearest nodes in NN tree within radius %f", sparseDelta_);
- stateProperty_[ queryVertex_ ] = NULL;
+ stateProperty_[ queryVertex_ ] = nullptr;
//Now that we got the neighbors from the NN, we must remove any we can't see
for (std::size_t i = 0; i < graphNeighborhood.size() ; ++i )
@@ -1318,7 +1317,7 @@ bool ompl::geometric::SPARSdb::findGraphNeighbors(const base::State *state, std:
if (graphNeighborhood.size() > 0)
break;
}
- stateProperty_[ queryVertex_ ] = NULL;
+ stateProperty_[ queryVertex_ ] = nullptr;
// Check if no neighbors found
if (!graphNeighborhood.size())
@@ -1347,7 +1346,7 @@ ompl::geometric::SPARSdb::Vertex ompl::geometric::SPARSdb::findGraphRepresentati
std::vector<Vertex> nbh;
stateProperty_[ queryVertex_ ] = st;
nn_->nearestR( queryVertex_, sparseDelta_, nbh);
- stateProperty_[queryVertex_] = NULL;
+ stateProperty_[queryVertex_] = nullptr;
if (verbose_)
OMPL_INFORM(" ------- findGraphRepresentative found %d nearest neighbors of distance %f",
@@ -1540,12 +1539,12 @@ void ompl::geometric::SPARSdb::distanceCheck(Vertex rep, const base::State *q, V
if (r < rp) // FIRST points represent r (the guy discovered through sampling)
{
- if (d.pointA_ == NULL) // If the point we're considering replacing (P_v(r,.)) isn't there
+ if (d.pointA_ == nullptr) // If the point we're considering replacing (P_v(r,.)) isn't there
//Then we know we're doing better, so add it
d.setFirst(q, s, si_);
else //Otherwise, he is there,
{
- if (d.pointB_ == NULL) //But if the other guy doesn't exist, we can't compare.
+ if (d.pointB_ == nullptr) //But if the other guy doesn't exist, we can't compare.
{
//Should probably keep the one that is further away from rep? Not known what to do in this case.
// TODO: is this not part of the algorithm?
@@ -1558,12 +1557,12 @@ void ompl::geometric::SPARSdb::distanceCheck(Vertex rep, const base::State *q, V
}
else // SECOND points represent r (the guy discovered through sampling)
{
- if (d.pointB_ == NULL) //If the point we're considering replacing (P_V(.,r)) isn't there...
+ if (d.pointB_ == nullptr) //If the point we're considering replacing (P_V(.,r)) isn't there...
//Then we must be doing better, so add it
d.setSecond(q, s, si_);
else //Otherwise, he is there
{
- if (d.pointA_ == NULL) //But if the other guy doesn't exist, we can't compare.
+ if (d.pointA_ == nullptr) //But if the other guy doesn't exist, we can't compare.
{
//Should we be doing something cool here?
}
@@ -1585,7 +1584,7 @@ void ompl::geometric::SPARSdb::abandonLists(base::State *st)
std::vector< Vertex > hold;
nn_->nearestR( queryVertex_, sparseDelta_, hold );
- stateProperty_[queryVertex_] = NULL;
+ stateProperty_[queryVertex_] = nullptr;
//For each of the vertices
foreach (Vertex v, hold)
diff --git a/src/ompl/tools/thunder/src/Thunder.cpp b/src/ompl/tools/thunder/src/Thunder.cpp
index 03345ba..b28c352 100644
--- a/src/ompl/tools/thunder/src/Thunder.cpp
+++ b/src/ompl/tools/thunder/src/Thunder.cpp
@@ -209,7 +209,7 @@ ompl::base::PlannerStatus ompl::tools::Thunder::solve(const base::PlannerTermina
time::point start = time::now();
// Warn if there are queued paths that have not been added to the experience database
- if (!QueuedSolutionPaths_.empty())
+ if (!queuedSolutionPaths_.empty())
{
OMPL_WARN("Previous solved paths are currently uninserted into the experience database and are in the post-proccessing queue");
}
@@ -331,7 +331,7 @@ ompl::base::PlannerStatus ompl::tools::Thunder::solve(const base::PlannerTermina
stats_.numSolutionsFromRecallSaved_++;
// Queue the solution path for future insertion into experience database (post-processing)
- QueuedSolutionPaths_.push_back(solutionPath);
+ queuedSolutionPaths_.push_back(solutionPath);
// Logging
log.insertion_failed = 0; // TODO this is wrong logging data
@@ -376,7 +376,7 @@ ompl::base::PlannerStatus ompl::tools::Thunder::solve(const base::PlannerTermina
log.is_saved = "saving";
// Queue the solution path for future insertion into experience database (post-processing)
- QueuedSolutionPaths_.push_back(solutionPath);
+ queuedSolutionPaths_.push_back(solutionPath);
log.insertion_failed = 0; // TODO fix this wrong logging info
}
@@ -525,18 +525,18 @@ bool ompl::tools::Thunder::doPostProcessing()
{
OMPL_INFORM("Performing post-processing");
- for (std::size_t i = 0; i < QueuedSolutionPaths_.size(); ++i)
+ for (std::size_t i = 0; i < queuedSolutionPaths_.size(); ++i)
{
// Time to add a path to experience database
double insertionTime;
- experienceDB_->addPath(QueuedSolutionPaths_[i], insertionTime);
+ experienceDB_->addPath(queuedSolutionPaths_[i], insertionTime);
OMPL_INFORM("Finished inserting experience path in %f seconds", insertionTime);
stats_.totalInsertionTime_ += insertionTime; // used for averaging
}
// Remove all inserted paths from the queue
- QueuedSolutionPaths_.clear();
+ queuedSolutionPaths_.clear();
return true;
}
diff --git a/src/ompl/util/ClassForward.h b/src/ompl/util/ClassForward.h
index 8d33e15..acf74ce 100644
--- a/src/ompl/util/ClassForward.h
+++ b/src/ompl/util/ClassForward.h
@@ -37,13 +37,13 @@
#ifndef OMPL_UTIL_CLASS_FORWARD_
#define OMPL_UTIL_CLASS_FORWARD_
-#include <boost/shared_ptr.hpp>
+#include <memory>
/** \brief Macro that defines a forward declaration for a class, and
shared pointers to the class. For example OMPL_CLASS_FORWARD(MyType);
will produce type definitions for MyType and MyTypePtr. */
#define OMPL_CLASS_FORWARD(C) \
class C; \
- typedef boost::shared_ptr<C> C##Ptr
+ typedef std::shared_ptr<C> C##Ptr
#endif
diff --git a/src/ompl/util/Console.h b/src/ompl/util/Console.h
index cbd01d4..fa6d61a 100644
--- a/src/ompl/util/Console.h
+++ b/src/ompl/util/Console.h
@@ -150,7 +150,7 @@ namespace ompl
};
- /** \brief This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(NULL) */
+ /** \brief This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(nullptr) */
void noOutputHandler();
/** \brief Restore the output handler that was previously in use (if any) */
@@ -159,7 +159,7 @@ namespace ompl
/** \brief Specify the instance of the OutputHandler to use. By default, this is OutputHandlerSTD */
void useOutputHandler(OutputHandler *oh);
- /** \brief Get the instance of the OutputHandler currently used. This is NULL in case there is no output handler. */
+ /** \brief Get the instance of the OutputHandler currently used. This is nullptr in case there is no output handler. */
OutputHandler* getOutputHandler();
/** \brief Set the minimum level of logging data to output. Messages
diff --git a/py-bindings/ompl_py_control.h b/src/ompl/util/Hash.h
similarity index 70%
copy from py-bindings/ompl_py_control.h
copy to src/ompl/util/Hash.h
index dbd3d28..b55421f 100644
--- a/py-bindings/ompl_py_control.h
+++ b/src/ompl/util/Hash.h
@@ -1,7 +1,7 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
-* Copyright (c) 2010, Rice University
+* Copyright (c) 2016, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@@ -34,25 +34,38 @@
/* Author: Mark Moll */
-#ifndef PY_BINDINGS_OMPL_PY_CONTROL_
-#define PY_BINDINGS_OMPL_PY_CONTROL_
-
-#include "ompl/control/ODESolver.h"
-#include "py_boost_function.hpp"
+#ifndef OMPL_UTIL_HASH_
+#define OMPL_UTIL_HASH_
+#include <functional>
+#include <type_traits>
+#include <utility>
namespace ompl
{
- namespace control
+ // copied from <boost/functional/hash.hpp>
+ template <class T>
+ inline void hash_combine(std::size_t& seed, const T& v)
+ {
+ std::hash<T> hasher;
+ seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+ }
+}
+
+namespace std
+{
+ template <class U, class V>
+ struct hash<std::pair<U,V> >
{
-// Boost.OdeInt needs Boost version >= 1.44
-#if BOOST_VERSION >= 104400
- inline int dummyODESolverSize()
+ typedef std::pair<U,V> argument_type;
+ typedef std::size_t result_type;
+ result_type operator()(argument_type const& p) const
{
- return sizeof(ODEBasicSolver<>) + sizeof(ODEErrorSolver<>) + sizeof(ODEAdaptiveSolver<>);
+ result_type h = std::hash<typename std::remove_cv<U>::type>()(p.first);
+ ompl::hash_combine(h, p.second);
+ return h;
}
-#endif
- }
+ };
}
#endif
diff --git a/src/ompl/util/ProlateHyperspheroid.h b/src/ompl/util/ProlateHyperspheroid.h
index 9dda18f..772e1ae 100644
--- a/src/ompl/util/ProlateHyperspheroid.h
+++ b/src/ompl/util/ProlateHyperspheroid.h
@@ -42,7 +42,7 @@
#error The ProlateHyperspheroid class uses Eigen3, which was not detected at build time.
#endif
-#include <boost/shared_ptr.hpp>
+#include <memory>
//For ease-of-use shared_ptr definition
#include <ompl/util/ClassForward.h>
@@ -106,7 +106,7 @@ namespace ompl
struct PhsData;
/** \brief A shared pointer to the actual data of a ProlateHyperspheroid. Used to hide Eigen from the header. */
- boost::shared_ptr<PhsData> dataPtr_;
+ std::shared_ptr<PhsData> dataPtr_;
// Functions
/** \brief Calculate the rotation from the PHS frame to the world frame via singular-value decomposition using the transverse symmetry of the PHS. */
diff --git a/src/ompl/util/RandomNumbers.h b/src/ompl/util/RandomNumbers.h
index 9951d93..373f3e0 100644
--- a/src/ompl/util/RandomNumbers.h
+++ b/src/ompl/util/RandomNumbers.h
@@ -37,12 +37,10 @@
#ifndef OMPL_UTIL_RANDOM_NUMBERS_
#define OMPL_UTIL_RANDOM_NUMBERS_
-#include <boost/shared_ptr.hpp>
-#include <boost/random/mersenne_twister.hpp>
-#include <boost/random/uniform_real.hpp>
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
+#include <memory>
+#include <random>
#include <cassert>
+#include <cstdint>
#include "ompl/config.h"
#if OMPL_HAVE_EIGEN3
@@ -65,19 +63,19 @@ namespace ompl
RNG();
/** \brief Constructor. Set to the specified instance seed. */
- RNG(boost::uint32_t localSeed);
+ RNG(std::uint_fast32_t localSeed);
/** \brief Generate a random real between 0 and 1 */
double uniform01()
{
- return uni_();
+ return uniDist_(generator_);
}
/** \brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound) */
double uniformReal(double lower_bound, double upper_bound)
{
assert(lower_bound <= upper_bound);
- return (upper_bound - lower_bound) * uni_() + lower_bound;
+ return (upper_bound - lower_bound) * uniDist_(generator_) + lower_bound;
}
/** \brief Generate a random integer within given bounds: [\e lower_bound, \e upper_bound] */
@@ -90,19 +88,19 @@ namespace ompl
/** \brief Generate a random boolean */
bool uniformBool()
{
- return uni_() <= 0.5;
+ return uniDist_(generator_) <= 0.5;
}
/** \brief Generate a random real using a normal distribution with mean 0 and variance 1 */
double gaussian01()
{
- return normal_();
+ return normalDist_(generator_);
}
/** \brief Generate a random real using a normal distribution with given mean and variance */
double gaussian(double mean, double stddev)
{
- return normal_() * stddev + mean;
+ return normalDist_(generator_) * stddev + mean;
}
/** \brief Generate a random real using a half-normal distribution. The value is within specified bounds [\e
@@ -125,22 +123,22 @@ namespace ompl
/** \brief Set the seed used to generate the seeds of each RNG instance. Use this
function to ensure the same sequence of random numbers is generated across multiple instances of RNG. */
- static void setSeed(boost::uint32_t seed);
+ static void setSeed(std::uint_fast32_t seed);
/** \brief Get the seed used to generate the seeds of each RNG instance.
Passing the returned value to setSeed() at a subsequent execution of the code will ensure deterministic
(repeatable) behaviour across multiple instances of RNG. Useful for debugging. */
- static boost::uint32_t getSeed();
+ static std::uint_fast32_t getSeed();
/** \brief Set the seed used for the instance of a RNG. Use this function to ensure that an instance of
an RNG generates the same deterministic sequence of numbers. This function resets the member generators*/
- void setLocalSeed(boost::uint32_t localSeed);
+ void setLocalSeed(std::uint_fast32_t localSeed);
/** \brief Get the seed used for the instance of a RNG. Passing the returned value to the setInstanceSeed()
of another RNG will assure that the two objects generate the same sequence of numbers.
Useful for comparing different settings of a planner while maintaining the same stochastic behaviour,
assuming that every "random" decision made by the planner is made from the same RNG. */
- boost::uint32_t getLocalSeed() const
+ std::uint_fast32_t getLocalSeed() const
{
return localSeed_;
}
@@ -161,7 +159,7 @@ namespace ompl
DOI: <a href="http://dx.doi.org/10.1109/IROS.2014.6942976">10.1109/IROS.2014.6942976</a>.
<a href="http://www.youtube.com/watch?v=d7dX5MvDYTc">Illustration video</a>.
<a href="http://www.youtube.com/watch?v=nsl-5MZfwu4">Short description video</a>. */
- void uniformProlateHyperspheroidSurface(const boost::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[]);
+ void uniformProlateHyperspheroidSurface(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[]);
/** \brief Uniform random sampling of a prolate hyperspheroid, a special symmetric type of
n-dimensional ellipse. The return variable \e value is expected to already exist.
@@ -172,7 +170,7 @@ namespace ompl
DOI: <a href="http://dx.doi.org/10.1109/IROS.2014.6942976">10.1109/IROS.2014.6942976</a>.
<a href="http://www.youtube.com/watch?v=d7dX5MvDYTc">Illustration video</a>.
<a href="http://www.youtube.com/watch?v=nsl-5MZfwu4">Short description video</a>. */
- void uniformProlateHyperspheroid(const boost::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[]);
+ void uniformProlateHyperspheroid(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[]);
#endif
private:
@@ -180,15 +178,12 @@ namespace ompl
class SphericalData;
/** \brief The seed used for the instance of a RNG */
- boost::uint32_t localSeed_;
- boost::mt19937 generator_;
- boost::uniform_real<> uniDist_;
- boost::normal_distribution<> normalDist_;
- // Variate generators must be reset when the seed changes
- boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni_;
- boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > normal_;
+ std::uint_fast32_t localSeed_;
+ std::mt19937 generator_;
+ std::uniform_real_distribution<> uniDist_;
+ std::normal_distribution<> normalDist_;
//A structure holding boost::uniform_on_sphere distributions and the associated boost::variate_generators for various dimension
- boost::shared_ptr<SphericalData> sphericalDataPtr_;
+ std::shared_ptr<SphericalData> sphericalDataPtr_;
};
}
diff --git a/src/ompl/util/Time.h b/src/ompl/util/Time.h
index eebccbf..da1b7d1 100644
--- a/src/ompl/util/Time.h
+++ b/src/ompl/util/Time.h
@@ -37,7 +37,23 @@
#ifndef OMPL_UTIL_TIME_
#define OMPL_UTIL_TIME_
-#include <boost/date_time/posix_time/posix_time.hpp>
+#include <chrono>
+#include <ctime>
+#include <iomanip>
+#include <sstream>
+
+// std::put_time is not implemented in GCC 4.x
+#if defined (__GNUC__) && (__GNUC__ < 5)
+namespace std
+{
+ inline std::string put_time(const std::tm* tmb, const char* fmt)
+ {
+ char mbstr[100];
+ std::strftime(mbstr, sizeof(mbstr), fmt, tmb);
+ return string(mbstr);
+ }
+}
+#endif
namespace ompl
{
@@ -47,15 +63,15 @@ namespace ompl
{
/** \brief Representation of a point in time */
- typedef boost::posix_time::ptime point;
+ typedef std::chrono::system_clock::time_point point;
/** \brief Representation of a time duration */
- typedef boost::posix_time::time_duration duration;
+ typedef std::chrono::system_clock::duration duration;
/** \brief Get the current time point */
inline point now()
{
- return boost::posix_time::microsec_clock::universal_time();
+ return std::chrono::system_clock::now();
}
/** \brief Return the time duration representing a given number of seconds */
@@ -63,13 +79,22 @@ namespace ompl
{
long s = (long)sec;
long us = (long)((sec - (double)s) * 1000000);
- return boost::posix_time::seconds(s) + boost::posix_time::microseconds(us);
+ return std::chrono::seconds(s) + std::chrono::microseconds(us);
}
/** \brief Return the number of seconds that a time duration represents */
inline double seconds(const duration &d)
{
- return (double)d.total_microseconds() / 1000000.0;
+ return std::chrono::duration<double>(d).count();
+ }
+
+ /** \brief Return string representation of point in time */
+ inline std::string as_string(const point& p)
+ {
+ std::time_t pt = std::chrono::system_clock::to_time_t(p);
+ std::stringstream ss;
+ ss << std::put_time(std::localtime(&pt), "%F %T");
+ return ss.str();
}
}
diff --git a/src/ompl/util/src/Console.cpp b/src/ompl/util/src/Console.cpp
index 1cf80a2..b562ffd 100644
--- a/src/ompl/util/src/Console.cpp
+++ b/src/ompl/util/src/Console.cpp
@@ -35,7 +35,7 @@
/* Author: Ioan Sucan */
#include "ompl/util/Console.h"
-#include <boost/thread/mutex.hpp>
+#include <mutex>
#include <iostream>
#include <cstdio>
#include <cstdarg>
@@ -71,7 +71,7 @@ struct DefaultOutputHandler
ompl::msg::OutputHandler *output_handler_;
ompl::msg::OutputHandler *previous_output_handler_;
ompl::msg::LogLevel logLevel_;
- boost::mutex lock_; // it is likely the outputhandler does some I/O, so we serialize it
+ std::mutex lock_; // it is likely the outputhandler does some I/O, so we serialize it
};
// we use this function because we want to handle static initialization correctly
@@ -86,7 +86,7 @@ static DefaultOutputHandler* getDOH()
#define USE_DOH \
DefaultOutputHandler *doh = getDOH(); \
- boost::mutex::scoped_lock slock(doh->lock_)
+ std::lock_guard<std::mutex> slock(doh->lock_)
#define MAX_BUFFER_SIZE 1024
@@ -96,7 +96,7 @@ void ompl::msg::noOutputHandler()
{
USE_DOH;
doh->previous_output_handler_ = doh->output_handler_;
- doh->output_handler_ = NULL;
+ doh->output_handler_ = nullptr;
}
void ompl::msg::restorePreviousOutputHandler()
diff --git a/src/ompl/util/src/GeometricEquations.cpp b/src/ompl/util/src/GeometricEquations.cpp
index fc82440..7e9817f 100644
--- a/src/ompl/util/src/GeometricEquations.cpp
+++ b/src/ompl/util/src/GeometricEquations.cpp
@@ -37,21 +37,24 @@
// This file's header
#include "ompl/util/GeometricEquations.h"
-// For pre C++ 11 gamma function
-#include <boost/math/special_functions/gamma.hpp>
+// For gamma function
+#include <cmath>
+
+// For pi definition
+#include <boost/math/constants/constants.hpp>
// OMPL exceptions
#include "ompl/util/Exception.h"
double ompl::nBallMeasure(unsigned int N, double r)
{
- return std::pow(std::sqrt(boost::math::constants::pi<double>()) * r, static_cast<double>(N)) / boost::math::tgamma(static_cast<double>(N)/2.0 + 1.0);
+ return std::pow(std::sqrt(boost::math::constants::pi<double>()) * r, static_cast<double>(N)) / std::tgamma(static_cast<double>(N)/2.0 + 1.0);
}
double ompl::unitNBallMeasure(unsigned int N)
{
// This is the radius version with r removed (as it is 1) for efficiency
- return std::pow(std::sqrt(boost::math::constants::pi<double>()), static_cast<double>(N)) / boost::math::tgamma(static_cast<double>(N)/2.0 + 1.0);
+ return std::pow(std::sqrt(boost::math::constants::pi<double>()), static_cast<double>(N)) / std::tgamma(static_cast<double>(N)/2.0 + 1.0);
}
double ompl::prolateHyperspheroidMeasure(unsigned int N, double dFoci, double dTransverse)
diff --git a/src/ompl/util/src/PPM.cpp b/src/ompl/util/src/PPM.cpp
index 3d005e7..a1553a0 100644
--- a/src/ompl/util/src/PPM.cpp
+++ b/src/ompl/util/src/PPM.cpp
@@ -37,7 +37,6 @@
#include "ompl/util/PPM.h"
#include "ompl/util/Exception.h"
#include <cstdio>
-#include <boost/lexical_cast.hpp>
ompl::PPM::PPM() : width_(0), height_(0)
{
@@ -82,9 +81,9 @@ void ompl::PPM::loadFile(const char *filename)
void ompl::PPM::saveFile(const char *filename)
{
if (pixels_.size() != width_ * height_)
- throw Exception("Number of pixels is " + boost::lexical_cast<std::string>(pixels_.size()) +
+ throw Exception("Number of pixels is " + std::to_string(pixels_.size()) +
" but the set width and height require " +
- boost::lexical_cast<std::string>(width_ * height_) + " pixels.");
+ std::to_string(width_ * height_) + " pixels.");
FILE *fp;
fp = fopen(filename, "wb");
if (!fp)
diff --git a/src/ompl/util/src/ProlateHyperspheroid.cpp b/src/ompl/util/src/ProlateHyperspheroid.cpp
index 028a265..3138566 100644
--- a/src/ompl/util/src/ProlateHyperspheroid.cpp
+++ b/src/ompl/util/src/ProlateHyperspheroid.cpp
@@ -43,8 +43,8 @@
// For geometric equations like prolateHyperspheroidMeasure
#include "ompl/util/GeometricEquations.h"
-// For boost::make_shared
-#include <boost/make_shared.hpp>
+// For std::make_shared
+#include <memory>
// Eigen core:
#include <Eigen/Core>
@@ -82,7 +82,7 @@ struct ompl::ProlateHyperspheroid::PhsData
ompl::ProlateHyperspheroid::ProlateHyperspheroid(unsigned int n, const double focus1[], const double focus2[])
- : dataPtr_ (boost::make_shared<PhsData>())
+ : dataPtr_ (std::make_shared<PhsData>())
{
//Initialize the data:
dataPtr_->dim_ = n;
diff --git a/src/ompl/util/src/RandomNumbers.cpp b/src/ompl/util/src/RandomNumbers.cpp
index d6f9533..71f697a 100644
--- a/src/ompl/util/src/RandomNumbers.cpp
+++ b/src/ompl/util/src/RandomNumbers.cpp
@@ -40,15 +40,12 @@
#include "ompl/util/RandomNumbers.h"
#include "ompl/util/Exception.h"
#include "ompl/util/Console.h"
-#include <boost/random/lagged_fibonacci.hpp>
-#include <boost/random/uniform_int.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/once.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
+#include <mutex>
+#include <memory>
#include <boost/math/constants/constants.hpp>
#include <boost/scoped_ptr.hpp>
-#include <boost/make_shared.hpp>
#include <boost/random/uniform_on_sphere.hpp>
+#include <boost/random/variate_generator.hpp>
// For boost::numeric::ublas::shallow_array_adaptor:
#include <boost/numeric/ublas/vector.hpp>
@@ -63,23 +60,23 @@ namespace
public:
RNGSeedGenerator() :
someSeedsGenerated_(false),
- firstSeed_((boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
- boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds()),
+ firstSeed_(std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::system_clock::now() -
+ std::chrono::system_clock::time_point::min()).count()),
sGen_(firstSeed_),
- sDist_(1, 1000000000),
- s_(sGen_, sDist_)
+ sDist_(1, 1000000000)
{
}
- boost::uint32_t firstSeed()
+ std::uint_fast32_t firstSeed()
{
- boost::mutex::scoped_lock slock(rngMutex_);
+ std::lock_guard<std::mutex> slock(rngMutex_);
return firstSeed_;
}
- void setSeed(boost::uint32_t seed)
+ void setSeed(std::uint_fast32_t seed)
{
- boost::mutex::scoped_lock slock(rngMutex_);
+ std::lock_guard<std::mutex> slock(rngMutex_);
if (seed > 0)
{
if (someSeedsGenerated_)
@@ -108,23 +105,22 @@ namespace
sGen_.seed(seed);
}
- boost::uint32_t nextSeed()
+ std::uint_fast32_t nextSeed()
{
- boost::mutex::scoped_lock slock(rngMutex_);
+ std::lock_guard<std::mutex> slock(rngMutex_);
someSeedsGenerated_ = true;
- return s_();
+ return sDist_(sGen_);
}
private:
- bool someSeedsGenerated_;
- boost::uint32_t firstSeed_;
- boost::mutex rngMutex_;
- boost::lagged_fibonacci607 sGen_;
- boost::uniform_int<> sDist_;
- boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s_;
+ bool someSeedsGenerated_;
+ std::uint_fast32_t firstSeed_;
+ std::mutex rngMutex_;
+ std::ranlux24_base sGen_;
+ std::uniform_int_distribution<> sDist_;
};
- static boost::once_flag g_once = BOOST_ONCE_INIT;
+ static std::once_flag g_once;
static boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;
void initRNGSeedGenerator()
@@ -134,7 +130,7 @@ namespace
RNGSeedGenerator& getRNGSeedGenerator()
{
- boost::call_once(&initRNGSeedGenerator, g_once);
+ std::call_once(g_once, &initRNGSeedGenerator);
return *g_RNGSeedGenerator;
}
} // namespace
@@ -152,17 +148,11 @@ public:
/** \brief The uniform_on_sphere distribution type. */
typedef boost::uniform_on_sphere<double, container_type_t> spherical_dist_t;
- /** \brief A shared pointer to the distribution */
- typedef boost::shared_ptr<spherical_dist_t> spherical_dist_ptr_t;
-
/** \brief The resulting variate generator type. */
- typedef boost::variate_generator<boost::mt19937*, spherical_dist_t > variate_generator_t;
-
- /** \brief A shared pointer to the variate generator */
- typedef boost::shared_ptr<variate_generator_t> variate_generator_ptr_t;
+ typedef boost::variate_generator<std::mt19937*, spherical_dist_t > variate_generator_t;
/** \brief Constructor */
- SphericalData(boost::mt19937* generatorPtr) : generatorPtr_(generatorPtr)
+ SphericalData(std::mt19937* generatorPtr) : generatorPtr_(generatorPtr)
{
};
@@ -197,13 +187,14 @@ public:
private:
/** \brief The pair of distribution and variate generator. */
- typedef std::pair<spherical_dist_ptr_t, variate_generator_ptr_t> dist_gen_pair_t;
+ typedef std::pair<std::shared_ptr<spherical_dist_t>,
+ std::shared_ptr<variate_generator_t> > dist_gen_pair_t;
/** \brief A vector distribution and variate generators (as pointers) indexed on dimension. */
- std::vector<dist_gen_pair_t> dimVector_;
+ std::vector<dist_gen_pair_t> dimVector_;
/** \brief A pointer to the generator owned by the outer class. Needed for creating new variate_generators */
- boost::mt19937* generatorPtr_;
+ std::mt19937* generatorPtr_;
/** \brief Grow the vector until it contains an (empty) entry for the specified dimension. */
void growVector(unsigned int dim)
@@ -220,25 +211,25 @@ private:
void allocateDimension(unsigned int dim)
{
// Only do this if unallocated, so check that:
- if (dimVector_.at(dim).first == NULL)
+ if (dimVector_.at(dim).first == nullptr)
{
// It is not allocated, so....
// First construct the distribution
- dimVector_.at(dim).first = boost::make_shared<spherical_dist_t> (dim);
+ dimVector_.at(dim).first = std::make_shared<spherical_dist_t> (dim);
// Then the variate generator
- dimVector_.at(dim).second = boost::make_shared<variate_generator_t> (generatorPtr_, *dimVector_.at(dim).first);
+ dimVector_.at(dim).second = std::make_shared<variate_generator_t> (generatorPtr_, *dimVector_.at(dim).first);
}
//No else, the pointer is already allocated.
};
};
/// @endcond
-boost::uint32_t ompl::RNG::getSeed()
+std::uint_fast32_t ompl::RNG::getSeed()
{
return getRNGSeedGenerator().firstSeed();
}
-void ompl::RNG::setSeed(boost::uint32_t seed)
+void ompl::RNG::setSeed(std::uint_fast32_t seed)
{
getRNGSeedGenerator().setSeed(seed);
}
@@ -248,24 +239,20 @@ ompl::RNG::RNG() :
generator_(localSeed_),
uniDist_(0, 1),
normalDist_(0, 1),
- uni_(generator_, uniDist_),
- normal_(generator_, normalDist_),
- sphericalDataPtr_(boost::make_shared<SphericalData> (&generator_))
+ sphericalDataPtr_(std ::make_shared<SphericalData> (&generator_))
{
}
-ompl::RNG::RNG(boost::uint32_t localSeed) :
+ompl::RNG::RNG(std::uint_fast32_t localSeed) :
localSeed_(localSeed),
generator_(localSeed_),
uniDist_(0, 1),
normalDist_(0, 1),
- uni_(generator_, uniDist_),
- normal_(generator_, normalDist_),
- sphericalDataPtr_(boost::make_shared<SphericalData> (&generator_))
+ sphericalDataPtr_(std::make_shared<SphericalData> (&generator_))
{
}
-void ompl::RNG::setLocalSeed(boost::uint32_t localSeed)
+void ompl::RNG::setLocalSeed(std::uint_fast32_t localSeed)
{
// Store the seed
localSeed_ = localSeed;
@@ -274,8 +261,8 @@ void ompl::RNG::setLocalSeed(boost::uint32_t localSeed)
generator_.seed(localSeed_);
// Reset the distributions used by the variate generators, as they can cache values
- uni_.distribution().reset();
- normal_.distribution().reset();
+ uniDist_.reset();
+ normalDist_.reset();
sphericalDataPtr_->reset();
}
@@ -302,9 +289,9 @@ int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
// pg. 124-132
void ompl::RNG::quaternion(double value[4])
{
- double x0 = uni_();
+ double x0 = uniDist_(generator_);
double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
- double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
+ double t1 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_), t2 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_);
double c1 = cos(t1), s1 = sin(t1);
double c2 = cos(t2), s2 = sin(t2);
value[0] = s1 * r1;
@@ -316,9 +303,9 @@ void ompl::RNG::quaternion(double value[4])
// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
void ompl::RNG::eulerRPY(double value[3])
{
- value[0] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
- value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
- value[2] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
+ value[0] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
+ value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - boost::math::constants::pi<double>() / 2.0;
+ value[2] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
}
@@ -348,7 +335,7 @@ void ompl::RNG::uniformInBall(double r, unsigned int n, double value[])
}
#if OMPL_HAVE_EIGEN3
-void ompl::RNG::uniformProlateHyperspheroidSurface(const boost::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
+void ompl::RNG::uniformProlateHyperspheroidSurface(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
{
// Variables
// The spherical point as a std::vector
@@ -361,7 +348,7 @@ void ompl::RNG::uniformProlateHyperspheroidSurface(const boost::shared_ptr<const
phsPtr->transform(&sphere[0], value);
}
-void ompl::RNG::uniformProlateHyperspheroid(const boost::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
+void ompl::RNG::uniformProlateHyperspheroid(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
{
// Variables
// The spherical point as a std::vector
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 7b20653..f18a96a 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,69 +1,73 @@
-# configure location of resources
-file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/resources" TEST_RESOURCES_DIR)
-if(WIN32)
- # Correct directory separator for Windows
- string(REPLACE "\\" "\\\\" TEST_RESOURCES_DIR "${TEST_RESOURCES_DIR}")
-endif(WIN32)
-configure_file("${TEST_RESOURCES_DIR}/config.h.in" "${TEST_RESOURCES_DIR}/config.h")
+option(OMPL_BUILD_TESTS "Build OMPL tests" ON)
-# Disable teamcity reporting for tests by default
-option(OMPL_TESTS_TEAMCITY "Enable unit test reporting to TeamCity" OFF)
-file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" CURRENT_DIR)
-configure_file("${CURRENT_DIR}/BoostTestTeamCityReporter.h.in" "${CURRENT_DIR}/BoostTestTeamCityReporter.h")
+if (OMPL_BUILD_TESTS)
+ # configure location of resources
+ file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/resources" TEST_RESOURCES_DIR)
+ if(WIN32)
+ # Correct directory separator for Windows
+ string(REPLACE "\\" "\\\\" TEST_RESOURCES_DIR "${TEST_RESOURCES_DIR}")
+ endif(WIN32)
+ configure_file("${TEST_RESOURCES_DIR}/config.h.in" "${TEST_RESOURCES_DIR}/config.h")
-# Test datastructures implementation
-add_ompl_test(test_heap datastructures/heap.cpp)
-add_ompl_test(test_grid datastructures/grid.cpp)
-add_ompl_test(test_gridb datastructures/gridb.cpp)
-add_ompl_test(test_nearestneighbors datastructures/nearestneighbors.cpp)
-add_ompl_test(test_pdf datastructures/pdf.cpp)
+ # Disable teamcity reporting for tests by default
+ option(OMPL_TESTS_TEAMCITY "Enable unit test reporting to TeamCity" OFF)
+ file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" CURRENT_DIR)
+ configure_file("${CURRENT_DIR}/BoostTestTeamCityReporter.h.in" "${CURRENT_DIR}/BoostTestTeamCityReporter.h")
-# Test utilities
-add_ompl_test(test_random util/random/random.cpp)
-add_ompl_test(test_machine_specs benchmark/machine_specs.cpp)
+ # Test datastructures implementation
+ add_ompl_test(test_heap datastructures/heap.cpp)
+ add_ompl_test(test_grid datastructures/grid.cpp)
+ add_ompl_test(test_gridb datastructures/gridb.cpp)
+ add_ompl_test(test_nearestneighbors datastructures/nearestneighbors.cpp)
+ add_ompl_test(test_pdf datastructures/pdf.cpp)
-# Test base code
-add_ompl_test(test_state_operations base/state_operations.cpp)
-add_ompl_test(test_state_spaces base/state_spaces.cpp)
-add_ompl_test(test_state_storage base/state_storage.cpp)
-add_ompl_test(test_ptc base/ptc.cpp)
-add_ompl_test(test_planner_data base/planner_data.cpp)
+ # Test utilities
+ add_ompl_test(test_random util/random/random.cpp)
+ add_ompl_test(test_machine_specs benchmark/machine_specs.cpp)
-# Test kinematic motion planners in 2D environments
-add_ompl_test(test_2denvs_geometric geometric/2d/2denvs.cpp)
-add_ompl_test(test_2dmap_geometric_simple geometric/2d/2dmap_simple.cpp)
-add_ompl_test(test_2dmap_ik geometric/2d/2dmap_ik.cpp)
-add_ompl_test(test_2dcircles_opt_geometric geometric/2d/2dcircles_optimize.cpp)
+ # Test base code
+ add_ompl_test(test_state_operations base/state_operations.cpp)
+ add_ompl_test(test_state_spaces base/state_spaces.cpp)
+ add_ompl_test(test_state_storage base/state_storage.cpp)
+ add_ompl_test(test_ptc base/ptc.cpp)
+ add_ompl_test(test_planner_data base/planner_data.cpp)
-# Test planning with controls on a 2D map
-add_ompl_test(test_2dmap_control control/2dmap/2dmap.cpp)
-add_ompl_test(test_planner_data_control control/planner_data.cpp)
+ # Test kinematic motion planners in 2D environments
+ add_ompl_test(test_2denvs_geometric geometric/2d/2denvs.cpp)
+ add_ompl_test(test_2dmap_geometric_simple geometric/2d/2dmap_simple.cpp)
+ add_ompl_test(test_2dmap_ik geometric/2d/2dmap_ik.cpp)
+ add_ompl_test(test_2dcircles_opt_geometric geometric/2d/2dcircles_optimize.cpp)
-# Test planning via MORSE extension
-if(OMPL_EXTENSION_MORSE)
- add_ompl_test(test_morse_extension extensions/morse/morse_plan.cpp)
-endif(OMPL_EXTENSION_MORSE)
+ # Test planning with controls on a 2D map
+ add_ompl_test(test_2dmap_control control/2dmap/2dmap.cpp)
+ add_ompl_test(test_planner_data_control control/planner_data.cpp)
-# Python unit tests
-if(PYTHON_FOUND AND OMPL_BUILD_PYTESTS AND EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/../py-bindings/bindings")
- add_ompl_python_test(util/test_util.py)
- add_ompl_python_test(base/test_base.py)
- add_ompl_python_test(geometric/test_geometric.py)
- add_ompl_python_test(geometric/test_geometric_compoundstate.py)
- add_ompl_python_test(control/test_control.py)
+ # Test planning via MORSE extension
+ if(OMPL_EXTENSION_MORSE)
+ add_ompl_test(test_morse_extension extensions/morse/morse_plan.cpp)
+ endif(OMPL_EXTENSION_MORSE)
- # test the python function to boost::function conversion utility functions
- include_directories(${PYTHON_INCLUDE_DIRS})
- add_library(py_boost_function MODULE util/test_py_boost_function.cpp)
- target_link_libraries(py_boost_function ompl ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARIES})
- if(WIN32)
- set_target_properties(py_boost_function PROPERTIES OUTPUT_NAME py_boost_function SUFFIX .pyd)
- endif(WIN32)
- add_custom_command(TARGET py_boost_function POST_BUILD
- COMMAND "${CMAKE_COMMAND}" -E copy "$<TARGET_FILE:py_boost_function>"
- "${CMAKE_CURRENT_SOURCE_DIR}/util"
- WORKING_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
- add_ompl_python_test(util/test_py_boost_function.py)
-endif()
+ # Python unit tests
+ if(PYTHON_FOUND AND OMPL_BUILD_PYTESTS AND EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/../py-bindings/bindings")
+ add_ompl_python_test(util/test_util.py)
+ add_ompl_python_test(base/test_base.py)
+ add_ompl_python_test(geometric/test_geometric.py)
+ add_ompl_python_test(geometric/test_geometric_compoundstate.py)
+ add_ompl_python_test(control/test_control.py)
+
+ # test the python function to std::function conversion utility functions
+ include_directories(${PYTHON_INCLUDE_DIRS})
+ add_library(py_std_function MODULE util/test_py_std_function.cpp)
+ target_link_libraries(py_std_function ompl ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARIES})
+ if(WIN32)
+ set_target_properties(py_std_function PROPERTIES OUTPUT_NAME py_std_function SUFFIX .pyd)
+ endif(WIN32)
+ add_custom_command(TARGET py_std_function POST_BUILD
+ COMMAND "${CMAKE_COMMAND}" -E copy "$<TARGET_FILE:py_std_function>"
+ "${CMAKE_CURRENT_SOURCE_DIR}/util"
+ WORKING_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
+ add_ompl_python_test(util/test_py_std_function.py)
+ endif()
-add_subdirectory(regression_tests)
+ add_subdirectory(regression_tests)
+endif()
diff --git a/tests/base/StateSpaceTest.h b/tests/base/StateSpaceTest.h
index e33e8df..da89ceb 100644
--- a/tests/base/StateSpaceTest.h
+++ b/tests/base/StateSpaceTest.h
@@ -114,12 +114,25 @@ namespace ompl
BOOST_OMPL_EXPECT_NEAR(s2.distance(s3), 0.0, eps_);
}
}
+
+ /** \brief Test that states are correctly cloned*/
+ void testCloneState(void)
+ {
+ base::ScopedState<> source(space_);
+ source.random();
+ const base::State* clonedState = space_->cloneState(source.get());
+ //Make sure the cloned state is actually a new state in memory.
+ BOOST_CHECK(clonedState != source.get());
+ //Make sure the states are the same.
+ BOOST_CHECK(space_->equalStates(clonedState, source.get()));
+ }
/** \brief Call all tests for the state space */
void test(void)
{
testDistance();
testInterpolation();
+ testCloneState();
}
private:
diff --git a/tests/base/ptc.cpp b/tests/base/ptc.cpp
index 06ccffb..1745b7b 100644
--- a/tests/base/ptc.cpp
+++ b/tests/base/ptc.cpp
@@ -36,8 +36,8 @@
#define BOOST_TEST_MODULE "PlannerTerminationCondition"
#include <boost/test/unit_test.hpp>
-#include <boost/thread.hpp>
#include <iostream>
+#include <thread>
#include "ompl/base/PlannerTerminationCondition.h"
#include "ompl/util/Time.h"
@@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE(TestSimpleTermination)
const base::PlannerTerminationCondition &ptc = base::timedPlannerTerminationCondition(dt);
BOOST_CHECK(ptc == false);
BOOST_CHECK(ptc() == false);
- boost::this_thread::sleep(ompl::time::seconds(dt + 0.01));
+ std::this_thread::sleep_for(ompl::time::seconds(dt + 0.01));
BOOST_CHECK(ptc == true);
BOOST_CHECK(ptc() == true);
@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE(TestThreadedTermination)
const base::PlannerTerminationCondition &ptc = base::timedPlannerTerminationCondition(dt, interval);
BOOST_CHECK(ptc == false);
BOOST_CHECK(ptc() == false);
- boost::this_thread::sleep(ompl::time::seconds(dt + interval * 3.0));
+ std::this_thread::sleep_for(ompl::time::seconds(dt + interval * 3.0));
BOOST_CHECK(ptc == true);
BOOST_CHECK(ptc() == true);
diff --git a/tests/base/state_operations.cpp b/tests/base/state_operations.cpp
index decd1bf..66c153b 100644
--- a/tests/base/state_operations.cpp
+++ b/tests/base/state_operations.cpp
@@ -36,7 +36,7 @@
#define BOOST_TEST_MODULE "State"
#include <boost/test/unit_test.hpp>
-#include <boost/thread.hpp>
+#include <thread>
#include <iostream>
#include "ompl/base/ScopedState.h"
@@ -274,9 +274,9 @@ BOOST_AUTO_TEST_CASE(AllocationWithThreads)
si.setup();
const int NT = 10;
ompl::time::point start = ompl::time::now();
- std::vector<boost::thread*> threads;
+ std::vector<std::thread*> threads;
for (int i = 0 ; i < NT ; ++i)
- threads.push_back(new boost::thread(boost::bind(&randomizedAllocator, &si)));
+ threads.push_back(new std::thread(std::bind(&randomizedAllocator, &si)));
for (int i = 0 ; i < NT ; ++i)
{
threads[i]->join();
diff --git a/tests/base/state_spaces.cpp b/tests/base/state_spaces.cpp
index 2b676eb..f87e447 100644
--- a/tests/base/state_spaces.cpp
+++ b/tests/base/state_spaces.cpp
@@ -225,7 +225,7 @@ BOOST_AUTO_TEST_CASE(SO3_Simple)
s2.random();
base::SpaceInformation si(m);
- si.setStateValidityChecker(boost::bind(&isValid, _1));
+ si.setStateValidityChecker(std::bind(&isValid, std::placeholders::_1));
si.setup();
std::vector<base::State*> states;
diff --git a/tests/datastructures/nearestneighbors.cpp b/tests/datastructures/nearestneighbors.cpp
index 74af45a..8f24cdb 100644
--- a/tests/datastructures/nearestneighbors.cpp
+++ b/tests/datastructures/nearestneighbors.cpp
@@ -38,7 +38,7 @@
#include <boost/test/unit_test.hpp>
#include <algorithm>
-#include <boost/unordered_set.hpp>
+#include <unordered_set>
#include "ompl/config.h"
#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
@@ -125,8 +125,8 @@ void stateSpaceTest(base::StateSpace& space, NearestNeighbors<base::State*>& pro
NearestNeighborsLinear<base::State*> proximityLinear;
base::State* s;
- proximity.setDistanceFunction(boost::bind(&base::StateSpace::distance, &space, _1, _2));
- proximityLinear.setDistanceFunction(boost::bind(&base::StateSpace::distance, &space, _1, _2));
+ proximity.setDistanceFunction(std::bind(&base::StateSpace::distance, &space, std::placeholders::_1, std::placeholders::_2));
+ proximityLinear.setDistanceFunction(std::bind(&base::StateSpace::distance, &space, std::placeholders::_1, std::placeholders::_2));
for(i=0; i<n; ++i)
{
@@ -210,13 +210,13 @@ void randomAccessPatternTest(base::StateSpace& space, NearestNeighbors<base::Sta
base::StateSamplerPtr sampler(space.allocStateSampler());
std::vector<base::State*> nghbr, nghbrGroundTruth;
NearestNeighborsLinear<base::State*> proximityLinear;
- boost::unordered_set<base::State*> states;
- boost::unordered_set<base::State*>::iterator it;
+ std::unordered_set<base::State*> states;
+ std::unordered_set<base::State*>::iterator it;
base::State* s;
double r;
- proximity.setDistanceFunction(boost::bind(&base::StateSpace::distance, &space, _1, _2));
- proximityLinear.setDistanceFunction(boost::bind(&base::StateSpace::distance, &space, _1, _2));
+ proximity.setDistanceFunction(std::bind(&base::StateSpace::distance, &space, std::placeholders::_1, std::placeholders::_2));
+ proximityLinear.setDistanceFunction(std::bind(&base::StateSpace::distance, &space, std::placeholders::_1, std::placeholders::_2));
for (i=0; i<m; ++i)
{
diff --git a/tests/extensions/morse/morse_plan.cpp b/tests/extensions/morse/morse_plan.cpp
index 8a250bd..96e536b 100644
--- a/tests/extensions/morse/morse_plan.cpp
+++ b/tests/extensions/morse/morse_plan.cpp
@@ -76,7 +76,7 @@ public:
quat->y = 0.0;
quat->z = 0.0;
}
-
+
}
void writeState(const base::State *state)
{
@@ -144,13 +144,13 @@ int main()
abounds[4] = -6;
abounds[5] = 6;
base::MorseEnvironmentPtr env(new MyEnvironment(2, 2, cbounds, pbounds, lbounds, abounds));
-
+
control::SimpleSetupPtr ss(new control::MorseSimpleSetup(env));
-
+
base::GoalPtr g(new MyGoal(ss->getSpaceInformation()));
-
+
ss->setGoal(g);
-
+
ss->solve(1.0);
return 0;
diff --git a/tests/geometric/2d/2DmapSetup1.h b/tests/geometric/2d/2DmapSetup1.h
index dc4ae19..ab252b3 100644
--- a/tests/geometric/2d/2DmapSetup1.h
+++ b/tests/geometric/2d/2DmapSetup1.h
@@ -126,7 +126,7 @@ namespace ompl
bounds.high[0] = (double)env_.height - 0.000000001;
getStateSpace()->as<base::CompoundStateSpace>()->as<StateSpace2DMap1>(1)->setBounds(bounds);
- setStateValidityChecker(boost::bind(&isValidFn2DMap1, &env_.grid, _1));
+ setStateValidityChecker(std::bind(&isValidFn2DMap1, &env_.grid, std::placeholders::_1));
base::ScopedState<base::CompoundStateSpace> state(getSpaceInformation());
state->as<base::RealVectorStateSpace::StateType>(0)->values[0] = env_.start.first;
diff --git a/tests/geometric/2d/2denvs.cpp b/tests/geometric/2d/2denvs.cpp
index 955ec3b..6fea497 100644
--- a/tests/geometric/2d/2denvs.cpp
+++ b/tests/geometric/2d/2denvs.cpp
@@ -55,6 +55,8 @@
#include "ompl/geometric/planners/rrt/LazyRRT.h"
#include "ompl/geometric/planners/pdst/PDST.h"
#include "ompl/geometric/planners/est/EST.h"
+#include "ompl/geometric/planners/est/BiEST.h"
+#include "ompl/geometric/planners/est/ProjEST.h"
#include "ompl/geometric/planners/stride/STRIDE.h"
#include "ompl/geometric/planners/prm/PRM.h"
#include "ompl/geometric/planners/prm/PRMstar.h"
@@ -433,6 +435,32 @@ protected:
{
geometric::EST *est = new geometric::EST(si);
est->setRange(10.0);
+ return base::PlannerPtr(est);
+ }
+
+};
+
+class BiESTTest : public TestPlanner
+{
+protected:
+
+ base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si)
+ {
+ geometric::BiEST *est = new geometric::BiEST(si);
+ est->setRange(10.0);
+ return base::PlannerPtr(est);
+ }
+
+};
+
+class ProjESTTest : public TestPlanner
+{
+protected:
+
+ base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si)
+ {
+ geometric::ProjEST *est = new geometric::ProjEST(si);
+ est->setRange(10.0);
std::vector<double> cdim;
cdim.push_back(1);
@@ -741,7 +769,7 @@ OMPL_PLANNER_TEST(TRRT, 95.0, 0.01)
OMPL_PLANNER_TEST(PDST, 95.0, 0.03)
-OMPL_PLANNER_TEST(pSBL, 95.0, 0.02)
+OMPL_PLANNER_TEST(pSBL, 95.0, 0.04)
OMPL_PLANNER_TEST(SBL, 95.0, 0.02)
OMPL_PLANNER_TEST(KPIECE1, 95.0, 0.01)
diff --git a/tests/geometric/test_geometric.py b/tests/geometric/test_geometric.py
index 69febaf..0e951ab 100755
--- a/tests/geometric/test_geometric.py
+++ b/tests/geometric/test_geometric.py
@@ -261,6 +261,18 @@ class ESTTest(TestPlanner):
def newplanner(self, si):
planner = og.EST(si)
planner.setRange(10.0)
+ return planner
+
+class BiESTTest(TestPlanner):
+ def newplanner(self, si):
+ planner = og.BiEST(si)
+ planner.setRange(10.0)
+ return planner
+
+class ProjESTTest(TestPlanner):
+ def newplanner(self, si):
+ planner = og.ProjEST(si)
+ planner.setRange(10.0)
projection = ou.vectorUint()
projection.extend([0, 1])
cdim = ou.vectorDouble()
diff --git a/tests/regression_tests/RegressionTest.cpp b/tests/regression_tests/RegressionTest.cpp
index e97e7b1..1a25de9 100644
--- a/tests/regression_tests/RegressionTest.cpp
+++ b/tests/regression_tests/RegressionTest.cpp
@@ -58,7 +58,6 @@
#include "ompl/geometric/planners/rrt/RRTConnect.h"
#include "ompl/geometric/planners/rrt/pRRT.h"
#include "ompl/geometric/planners/rrt/LazyRRT.h"
-#include "ompl/geometric/planners/est/EST.h"
#include "ompl/geometric/planners/prm/PRM.h"
#if OMPL_VERSION_VALUE >= 12000
@@ -76,6 +75,12 @@
#include "ompl/geometric/planners/stride/STRIDE.h"
#endif
+#if OMPL_VERSION_VALUE >= 1002000
+#include "ompl/geometric/planners/est/EST.h"
+#include "ompl/geometric/planners/est/BiEST.h"
+#include "ompl/geometric/planners/est/ProjEST.h"
+#endif
+
using namespace ompl;
#if OMPL_VERSION_VALUE > 9000
@@ -121,18 +126,18 @@ void addAllPlanners(Benchmark &b, geometric::SimpleSetup &ss)
// Setup a problem from the known set of problems included with the regression tests.
template<unsigned int PROBLEM>
-boost::shared_ptr<geometric::SimpleSetup> setupProblem()
+std::shared_ptr<geometric::SimpleSetup> setupProblem()
{
if (PROBLEM == CIRCLES_ID)
return setupCirclesProblem(0);
fprintf(stderr, "Unknown problem '%d'", PROBLEM);
- return boost::shared_ptr<geometric::SimpleSetup>();
+ return std::shared_ptr<geometric::SimpleSetup>();
}
template<unsigned int PROBLEM>
void runProblem(double runtime_limit, double memory_limit, int run_count)
{
- boost::shared_ptr<geometric::SimpleSetup> ss = setupProblem<PROBLEM>();
+ std::shared_ptr<geometric::SimpleSetup> ss = setupProblem<PROBLEM>();
if (ss)
{
const std::string exp_name = problemName<PROBLEM>();
diff --git a/tests/regression_tests/RegressionTestCirclesProblem.inl.h b/tests/regression_tests/RegressionTestCirclesProblem.inl.h
index 10bf49f..ddf95a9 100644
--- a/tests/regression_tests/RegressionTestCirclesProblem.inl.h
+++ b/tests/regression_tests/RegressionTestCirclesProblem.inl.h
@@ -5,7 +5,7 @@ template<>
std::string problemName<CIRCLES_ID>() { return "circles"; }
// Setup for the Circles problem, part of the OMPL test suite.
-static boost::shared_ptr<geometric::SimpleSetup> setupCirclesProblem(unsigned int query_index) {
+static std::shared_ptr<geometric::SimpleSetup> setupCirclesProblem(unsigned int query_index) {
boost::filesystem::path path(TEST_RESOURCES_DIR);
Circles2D circles;
@@ -23,12 +23,12 @@ static boost::shared_ptr<geometric::SimpleSetup> setupCirclesProblem(unsigned in
geometric::SimpleSetup *raw_ss = new geometric::SimpleSetup(si);
#endif
- boost::shared_ptr<geometric::SimpleSetup> ss(raw_ss);
+ std::shared_ptr<geometric::SimpleSetup> ss(raw_ss);
base::ScopedState<> start(ss->getSpaceInformation());
base::ScopedState<> goal(ss->getSpaceInformation());
if (query_index >= circles.getQueryCount())
- return boost::shared_ptr<geometric::SimpleSetup>();
+ return std::shared_ptr<geometric::SimpleSetup>();
const Circles2D::Query &q = circles.getQuery(query_index);
start[0] = q.startX_;
start[1] = q.startY_;
@@ -57,6 +57,22 @@ void addPlanner<geometric::EST, CIRCLES_ID>(Benchmark &benchmark, const base::Sp
{
geometric::EST *est = new geometric::EST(si);
est->setRange(10.0);
+ benchmark.addPlanner(base::PlannerPtr(est));
+}
+
+template<>
+void addPlanner<geometric::BiEST, CIRCLES_ID>(Benchmark &benchmark, const base::SpaceInformationPtr &si)
+{
+ geometric::BiEST *est = new geometric::BiEST(si);
+ est->setRange(10.0);
+ benchmark.addPlanner(base::PlannerPtr(est));
+}
+
+template<>
+void addPlanner<geometric::ProjEST, CIRCLES_ID>(Benchmark &benchmark, const base::SpaceInformationPtr &si)
+{
+ geometric::ProjEST *est = new geometric::ProjEST(si);
+ est->setRange(10.0);
est->setProjectionEvaluator(getCirclesProjEvaluator(si));
benchmark.addPlanner(base::PlannerPtr(est));
}
diff --git a/tests/regression_tests/VERSIONS b/tests/regression_tests/VERSIONS
index 95ce0f3..4edead7 100644
--- a/tests/regression_tests/VERSIONS
+++ b/tests/regression_tests/VERSIONS
@@ -12,4 +12,7 @@
0.14.0
0.14.1
0.14.2
+1.0.0
+1.1.0
+1.2.0
tip
diff --git a/tests/util/random/random.cpp b/tests/util/random/random.cpp
index 66dedba..8a1f5f6 100644
--- a/tests/util/random/random.cpp
+++ b/tests/util/random/random.cpp
@@ -43,8 +43,6 @@
#include <cmath>
#include <vector>
#include <cstdio>
-// For boost::make_shared
-#include <boost/make_shared.hpp>
using namespace ompl;
@@ -330,7 +328,7 @@ BOOST_AUTO_TEST_CASE(SamplePhsSurface)
}
// Create the PHS object
- phsPtr = boost::make_shared<ompl::ProlateHyperspheroid>(dim, &v1[0], &v2[0]);
+ phsPtr = std::make_shared<ompl::ProlateHyperspheroid>(dim, &v1[0], &v2[0]);
// Pick a random transverse diameter
tDiameter = rng.uniformReal(1.01*phsPtr->getMinTransverseDiameter(), 2.5*phsPtr->getMinTransverseDiameter());
@@ -385,7 +383,7 @@ BOOST_AUTO_TEST_CASE(SampleInPhs)
}
// Create the PHS object
- phsPtr = boost::make_shared<ompl::ProlateHyperspheroid>(dim, &v1[0], &v2[0]);
+ phsPtr = std::make_shared<ompl::ProlateHyperspheroid>(dim, &v1[0], &v2[0]);
// Pick a random transverse diameter
tDiameter = rng.uniformReal(1.1*phsPtr->getMinTransverseDiameter(), 2.5*phsPtr->getMinTransverseDiameter());
@@ -404,7 +402,8 @@ BOOST_AUTO_TEST_CASE(SampleInPhs)
rng.uniformProlateHyperspheroid(phsPtr, &xRand[0]);
// Check that the point lies within the shape
- BOOST_CHECK_GE(phsPtr->getPathLength(&xRand[0]), phsPtr->getMinTransverseDiameter());
+ BOOST_CHECK_GE(phsPtr->getPathLength(&xRand[0]) + std::numeric_limits<float>::epsilon(),
+ phsPtr->getMinTransverseDiameter());
BOOST_CHECK_LT(phsPtr->getPathLength(&xRand[0]), tDiameter);
}
}
diff --git a/tests/util/test_py_boost_function.cpp b/tests/util/test_py_std_function.cpp
similarity index 86%
rename from tests/util/test_py_boost_function.cpp
rename to tests/util/test_py_std_function.cpp
index 87723b6..ba2ed6c 100644
--- a/tests/util/test_py_boost_function.cpp
+++ b/tests/util/test_py_std_function.cpp
@@ -35,8 +35,8 @@
/* Author: Mark Moll */
#include <boost/python.hpp>
-#include <boost/shared_ptr.hpp>
-#include "../../py-bindings/py_boost_function.hpp"
+#include <memory>
+#include "../../py-bindings/py_std_function.hpp"
namespace bp = boost::python;
@@ -58,14 +58,14 @@ IntClass intClassFun2(const IntClass& i, int j) { IntClass i2(i); i2.value = j;
IntClass intClassFun3(IntClass& i, int& j) { i.value = j; return i; }
void intClassFun4(IntClass* i, int j) { i->value = j; }
-boost::function<IntClass(IntClass,int)> intClassFun0_obj(intClassFun0);
-boost::function<IntClass(IntClass&,int)> intClassFun1_obj(intClassFun1);
-boost::function<IntClass(const IntClass&,int)> intClassFun2_obj(intClassFun2);
-boost::function<IntClass(IntClass&,int&)> intClassFun3_obj(intClassFun3);
-boost::function<void(IntClass*,int)> intClassFun4_obj(intClassFun4);
+std::function<IntClass(IntClass,int)> intClassFun0_obj(intClassFun0);
+std::function<IntClass(IntClass&,int)> intClassFun1_obj(intClassFun1);
+std::function<IntClass(const IntClass&,int)> intClassFun2_obj(intClassFun2);
+std::function<IntClass(IntClass&,int&)> intClassFun3_obj(intClassFun3);
+std::function<void(IntClass*,int)> intClassFun4_obj(intClassFun4);
-BOOST_PYTHON_MODULE(py_boost_function)
+BOOST_PYTHON_MODULE(py_std_function)
{
bp::class_<IntClass>("IntClass", bp::init<int>())
.def_readwrite("value", &IntClass::value);
diff --git a/tests/util/test_py_boost_function.py b/tests/util/test_py_std_function.py
similarity index 96%
rename from tests/util/test_py_boost_function.py
rename to tests/util/test_py_std_function.py
index c4200ad..7ea8c8f 100755
--- a/tests/util/test_py_boost_function.py
+++ b/tests/util/test_py_std_function.py
@@ -37,7 +37,7 @@
# Author: Mark Moll
import unittest
-from py_boost_function import *
+from py_std_function import *
def myIntFun0(i, j):
i.value = j
@@ -51,7 +51,7 @@ def myIntFun1(i, j):
def myIntFun2(i, j):
i.value = j
-class TestPyBoostFunction(unittest.TestCase):
+class TestPyStdFunction(unittest.TestCase):
def testIntClassFunObj(self):
i = IntClass(0)
j = intClassFun0_obj(i, 1)
@@ -96,7 +96,7 @@ class TestPyBoostFunction(unittest.TestCase):
def suite():
- suites = (unittest.makeSuite(TestPyBoostFunction,'test'))
+ suites = (unittest.makeSuite(TestPyStdFunction,'test'))
return unittest.TestSuite(suites)
if __name__ == '__main__':
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ompl.git
More information about the debian-science-commits
mailing list