Imported Upstream version 1.3.0+ds1

parent 071eb95f
---
BasedOnStyle: Google
AccessModifierOffset: -4
ConstructorInitializerIndentWidth: 2
AlignEscapedNewlinesLeft: false
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AllowShortFunctionsOnASingleLine: None
AllowShortLoopsOnASingleLine: false
AlwaysBreakTemplateDeclarations: true
AlwaysBreakBeforeMultilineStrings: false
BreakBeforeBinaryOperators: false
BreakBeforeTernaryOperators: false
BreakConstructorInitializersBeforeComma: true
BinPackParameters: true
ColumnLimit: 120
ConstructorInitializerAllOnOneLineOrOnePerLine: true
PointerAlignment: Right
DerivePointerBinding: false
ExperimentalAutoDetectBinPacking: false
IndentCaseLabels: true
MaxEmptyLinesToKeep: 1
NamespaceIndentation: All
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 60
PenaltyBreakString: 1
PenaltyBreakFirstLessLess: 1000
PenaltyExcessCharacter: 1000
PenaltyReturnTypeOnItsOwnLine: 90
PointerBindsToType: false
SpacesBeforeTrailingComments: 2
Cpp11BracedListStyle: true
Standard: Cpp11
IndentWidth: 4
TabWidth: 4
UseTab: Never
BreakBeforeBraces: Allman
IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false
SpacesInAngles: false
SpaceInEmptyParentheses: false
SpacesInCStyleCastParentheses: false
SpaceAfterControlStatementKeyword: true
SpaceBeforeAssignmentOperators: true
ContinuationIndentWidth: 4
...
......@@ -26,4 +26,6 @@ tests/BoostTestTeamCityReporter.h
tests/resources/config.h
_symlinks.sh
package.xml
.dir-locals.el
\ No newline at end of file
.dir-locals.el
*~
install-ompl-ubuntu.sh
cache:
apt: true
directories:
- ${HOME}/castxml
- /usr/local
language: cpp
matrix:
include:
# On Linux, exclude test_machine_specs; started to fail when we switched to g++-5
- os: linux
dist: trusty
sudo: required
compiler: gcc
env: PYTHONPATH=/usr/local/lib/python2.7/dist-packages
env: PYTHONPATH=/usr/local/lib/python2.7/dist-packages PATH=${PATH}:${HOME}/castxml/bin JOBS=4 TEST_EXCLUDE='test_machine_spec'
addons:
apt:
packages:
- libboost1.55-all-dev
- python-dev
- libode-dev
- libeigen3-dev
- python-pip
cache:
apt: true
directories:
- ${HOME}/castxml
# On OS X, exclude test_machine_specs; fails due to "lazy" memory freeing in Release mode
# (test passes in Debug builds).
# On OS X, exclude test_random; fails due to bug in Boost.Random in 1.56.
- os: osx
osx_image: xcode7.2
compiler: clang
env: JOBS=2 TEST_EXCLUDE='test_(machine_specs|random)'
cache:
apt: true
directories:
- ${HOME}/castxml
- /usr/local
- os: linux
sudo: required
services:
- docker
env: DOCKERFILE="debian-jessie" JOBS=4
- os: linux
sudo: required
services:
- docker
env: DOCKERFILE="ubuntu-xenial" JOBS=4 TEST_EXCLUDE='test_(machine_specs|random)'
allow_failures:
- os: osx
addons:
apt:
packages:
- libboost-all-dev
- python-dev
- libode-dev
- libeigen3-dev
- python-pip
install:
- if [ "$TRAVIS_OS_NAME" = "osx" ]; then
- if [ -n "$DOCKERFILE" ]; then
docker build -t "$DOCKERFILE" -f "scripts/docker/$DOCKERFILE" .;
fi
- if [ -z "$DOCKERFILE" -a "$TRAVIS_OS_NAME" = "osx" ]; then
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
fi
- if [ -z "$DOCKERFILE" -a "$TRAVIS_OS_NAME" = "linux" ]; then
sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test;
sudo apt-get -y update;
sudo apt-get -y install g++-5;
export CXX=g++-5;
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
- sudo -H pip -v install pygccxml https://bitbucket.org/ompl/pyplusplus/get/1.7.0.tar.gz
script:
# Create build directory
- mkdir -p build
- cd build
# Configure
- 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
# Run unit tests
# On OS X, exclude test_machine_specs; fails due to "lazy" memory freeing in Release mode (test passes in Debug builds).
# On OS X, exclude test_random; fails due to bug in Boost.Random in 1.56.
- if [ "$TRAVIS_OS_NAME" = "osx" ]; then
ctest -E 'test_(machine_specs|random)';
- if [ -n "$DOCKERFILE" ]; then
docker run "$DOCKERFILE" /bin/sh -c "mkdir /root/ompl/build && cd /root/ompl/build && cmake -DOMPL_REGISTRATION=OFF -DCMAKE_INSTALL_PREFIX=tmp .. && make -j $JOBS && ctest -E '$TEST_EXCLUDE'";
else
ctest;
mkdir -p build &&
cd build &&
cmake -DOMPL_REGISTRATION=OFF -DCMAKE_INSTALL_PREFIX=tmp .. &&
make -j $JOBS &&
ctest -E "$TEST_EXCLUDE";
fi
# TIMES OUT BECAUSE IT DOES NOT PRODUCE OUTPUT FOR MORE THAN 10 MINUTES
# generate python bindings
# make -j $JOBS update_bindings
......@@ -48,6 +48,13 @@ set(OMPL_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src")
set(OMPL_DEMO_INSTALL_DIR "share/ompl${OMPL_INSTALL_SUFFIX}/demos"
CACHE STRING "Relative path to directory where demos will be installed")
# Add support in Boost::Python for std::shared_ptr
# This is a hack that replaces boost::shared_ptr related code with std::shared_ptr.
# Proper support for std::shared_ptr was added in Boost 1.63.
if(Boost_VERSION VERSION_LESS "106300")
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/src/external")
endif()
include_directories("${OMPL_INCLUDE_DIR}")
......@@ -62,7 +69,7 @@ else(IS_ICPC)
add_definitions(-DBOOST_TEST_DYN_LINK)
endif(IS_ICPC)
find_package(Boost 1.54 COMPONENTS serialization filesystem system program_options unit_test_framework REQUIRED)
find_package(Boost 1.54 COMPONENTS serialization filesystem system program_options REQUIRED)
include_directories(SYSTEM ${Boost_INCLUDE_DIR})
# on OS X we need to check whether to use libc++ or libstdc++ with clang++
......@@ -121,6 +128,7 @@ find_package(flann 1.8.3 QUIET)
if (FLANN_FOUND)
set(OMPL_HAVE_FLANN 1)
include_directories(SYSTEM "${FLANN_INCLUDE_DIRS}")
link_directories(${FLANN_LIBRARY_DIRS})
endif()
# R is needed for running Planner Arena locally
......
......@@ -19,7 +19,12 @@ if (CASTXML)
endif()
endif()
set(CASTXMLCONFIG "[gccxml]\nxml_generator=castxml\nxml_generator_path=${CASTXML}\ncompiler=${CASTXMLCOMPILER}\ncompiler_path=${CMAKE_CXX_COMPILER}\n")
set(CASTXMLCONFIG "[xml_generator]
xml_generator=castxml
xml_generator_path=${CASTXML}
compiler=${CASTXMLCOMPILER}
compiler_path=${CMAKE_CXX_COMPILER}
")
set(_candidate_include_path
"${OMPL_INCLUDE_DIR}"
......
......@@ -2,13 +2,11 @@
include(FindPackageHandleStandardArgs)
find_path(FLANN_INCLUDE_DIR flann.hpp PATH_SUFFIXES flann)
if (FLANN_INCLUDE_DIR)
file(READ "${FLANN_INCLUDE_DIR}/config.h" FLANN_CONFIG)
string(REGEX REPLACE ".*FLANN_VERSION_ \"([0-9.]+)\".*" "\\1" FLANN_VERSION ${FLANN_CONFIG})
if(NOT FLANN_VERSION VERSION_LESS flann_FIND_VERSION)
string(REGEX REPLACE "/flann$" "" FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR})
find_package(PkgConfig)
if(PKGCONFIG_FOUND)
pkg_check_modules(FLANN flann)
if(FLANN_LIBRARIES AND NOT FLANN_INCLUDE_DIRS)
set(FLANN_INCLUDE_DIRS "/usr/include")
endif()
endif()
find_package_handle_standard_args(flann DEFAULT_MSG FLANN_INCLUDE_DIRS)
find_package_handle_standard_args(FLANN DEFAULT_MSG FLANN_LIBRARIES FLANN_INCLUDE_DIRS)
# set the version in a way CMake can use
set(OMPL_MAJOR_VERSION 1)
set(OMPL_MINOR_VERSION 2)
set(OMPL_PATCH_VERSION 1)
set(OMPL_MINOR_VERSION 3)
set(OMPL_PATCH_VERSION 0)
set(OMPL_VERSION "${OMPL_MAJOR_VERSION}.${OMPL_MINOR_VERSION}.${OMPL_PATCH_VERSION}")
# increment this when we have ABI changes
set(OMPL_ABI_VERSION 12)
set(OMPL_ABI_VERSION 13)
......@@ -58,7 +58,7 @@ 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 time ${PYTHON_EXEC}
COMMAND ${PYTHON_EXEC}
"${CMAKE_CURRENT_SOURCE_DIR}/generate_bindings.py" "${module}"
"2>&1" | tee "${CMAKE_BINARY_DIR}/pyplusplus_${module}.log"
COMMAND ${CMAKE_COMMAND} ${CMAKE_BINARY_DIR}
......
......@@ -48,11 +48,6 @@ namespace og = ompl::geometric;
namespace ot = ompl::tools;
namespace po = boost::program_options;
ompl::base::OptimizationObjectivePtr getPathLengthObjective(const ompl::base::SpaceInformationPtr& si)
{
return ompl::base::OptimizationObjectivePtr(new ompl::base::PathLengthOptimizationObjective(si));
}
bool isStateValid(double radiusSquared, const ob::State *state)
{
const ob::SE2StateSpace::StateType *s = state->as<ob::SE2StateSpace::StateType>();
......@@ -69,7 +64,7 @@ int main(int argc, char **argv)
int distance, gridLimit, runCount;
double obstacleRadius, turningRadius, runtimeLimit;
ob::StateSpacePtr space(new ob::SE2StateSpace());
auto space(std::make_shared<ob::SE2StateSpace>());
po::options_description desc("Options");
......@@ -97,23 +92,28 @@ int main(int argc, char **argv)
}
if (vm.count("dubins"))
space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius));
space = std::make_shared<ob::DubinsStateSpace>(turningRadius);
if (vm.count("dubinssym"))
space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius, true));
space = std::make_shared<ob::DubinsStateSpace>(turningRadius, true);
if (vm.count("reedsshepp"))
space = ob::StateSpacePtr(new ob::ReedsSheppStateSpace(turningRadius));
space = std::make_shared<ob::ReedsSheppStateSpace>(turningRadius);
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-.5 * gridLimit);
bounds.setHigh(.5 * gridLimit);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
space->setBounds(bounds);
// define a simple setup class
og::SimpleSetup ss(space);
// set state validity checking for this space
ss.setStateValidityChecker(std::bind(&isStateValid, obstacleRadius*obstacleRadius, std::placeholders::_1));
double radiusSquared = obstacleRadius * obstacleRadius;
ss.setStateValidityChecker(
[radiusSquared](const ob::State *state)
{
return isStateValid(radiusSquared, state);
});
// define start & goal states
ob::ScopedState<ob::SE2StateSpace> start(space), goal(space);
......@@ -125,15 +125,16 @@ int main(int argc, char **argv)
// setting collision checking resolution to 0.05 (absolute)
ss.getSpaceInformation()->setStateValidityCheckingResolution(0.05 / gridLimit);
ss.getProblemDefinition()->setOptimizationObjective(getPathLengthObjective(ss.getSpaceInformation()));
ss.getProblemDefinition()->setOptimizationObjective(
std::make_shared<ompl::base::PathLengthOptimizationObjective>(ss.getSpaceInformation()));
// by default, use the Benchmark class
double memoryLimit = 4096;
ot::Benchmark::Request request(runtimeLimit, memoryLimit, runCount);
ot::Benchmark b(ss, "CircleGrid");
b.addPlanner(ob::PlannerPtr(new og::RRTstar(ss.getSpaceInformation())));
b.addPlanner(ob::PlannerPtr(new og::CForest(ss.getSpaceInformation())));
b.addPlanner(std::make_shared<og::RRTstar>(ss.getSpaceInformation()));
b.addPlanner(std::make_shared<og::CForest>(ss.getSpaceInformation()));
b.benchmark(request);
b.saveResultsToFile("circleGrid.log");
......
......@@ -31,6 +31,8 @@ if (OMPL_BUILD_DEMOS)
add_ompl_demo(demo_PlannerProgressProperties PlannerProgressProperties.cpp)
add_ompl_demo(demo_CForestCircleGridBenchmark CForestCircleGridBenchmark.cpp)
add_ompl_demo(demo_Diagonal Diagonal.cpp)
if(OMPL_HAVE_EIGEN3)
add_ompl_demo(demo_VectorFieldConservative VFRRT/VectorFieldConservative.cpp)
add_ompl_demo(demo_VectorFieldNonconservative VFRRT/VectorFieldNonconservative.cpp)
......
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2016, Georgia Institute of Technology
* 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: Florian Hauer */
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTXstatic.h>
#include <ompl/geometric/planners/rrt/RRTsharp.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/tools/benchmark/Benchmark.h>
#include <boost/format.hpp>
#include <boost/math/constants/constants.hpp>
#include <fstream>
class ValidityChecker : public ompl::base::StateValidityChecker
{
public:
ValidityChecker(const ompl::base::SpaceInformationPtr &si) : ompl::base::StateValidityChecker(si)
{
}
bool isValid(const ompl::base::State *state) const override
{
const auto *state2D = state->as<ompl::base::RealVectorStateSpace::StateType>();
double sum = 0;
for (unsigned i = 0; i < si_->getStateSpace()->getDimension(); ++i)
sum += state2D->values[i] * state2D->values[i];
return sqrt(sum) > 0.1;
}
};
int main(int argc, char **argv)
{
if (argc != 2)
{
std::cout << "Usage: " << argv[0] << " dimensionOfTheProblem" << std::endl;
exit(0);
}
int dim = atoi(argv[1]);
auto space(std::make_shared<ompl::base::RealVectorStateSpace>(dim));
ompl::geometric::SimpleSetup ss(space);
const ompl::base::SpaceInformationPtr &si = ss.getSpaceInformation();
space->setBounds(-1, 1);
ss.setStateValidityChecker(std::make_shared<ValidityChecker>(si));
ompl::base::ScopedState<> start(space), goal(space);
for (int i = 0; i < dim; ++i)
{
start[i] = -1;
goal[i] = 1;
}
ss.setStartAndGoalStates(start, goal);
// by default, use the Benchmark class
double runtime_limit = 5, memory_limit = 1024;
int run_count = 100;
ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.05, true, true, false, false);
ompl::tools::Benchmark b(ss, "Diagonal");
double range = 0.1 * sqrt(dim);
auto lengthObj(std::make_shared<ompl::base::PathLengthOptimizationObjective>(si));
ompl::base::OptimizationObjectivePtr oop((0.5 / sqrt(dim)) * lengthObj);
ss.setOptimizationObjective(oop);
bool knn = true;
auto rrtstar(std::make_shared<ompl::geometric::RRTstar>(si));
rrtstar->setName("RRT*");
rrtstar->setDelayCC(true);
// rrtstar->setFocusSearch(true);
rrtstar->setRange(range);
rrtstar->setKNearest(knn);
b.addPlanner(rrtstar);
auto rrtsh(std::make_shared<ompl::geometric::RRTsharp>(si));
rrtsh->setRange(range);
rrtsh->setKNearest(knn);
b.addPlanner(rrtsh);
/*auto rrtsh3(std::make_shared<ompl::geometric::RRTsharp>(si));
rrtsh3->setName("RRT#v3");
rrtsh3->setRange(range);
rrtsh3->setKNearest(knn);
rrtsh3->setVariant(3);
b.addPlanner(rrtsh3);
auto rrtsh2(std::make_shared<ompl::geometric::RRTsharp>(si));
rrtsh2->setName("RRT#v2");
rrtsh2->setRange(range);
rrtsh2->setKNearest(knn);
rrtsh2->setVariant(2);
b.addPlanner(rrtsh2);*/
auto rrtX1(std::make_shared<ompl::geometric::RRTXstatic>(si));
rrtX1->setName("RRTX0.1");
rrtX1->setEpsilon(0.1);
rrtX1->setRange(range);
// rrtX1->setVariant(3);
rrtX1->setKNearest(knn);
b.addPlanner(rrtX1);
auto rrtX2(std::make_shared<ompl::geometric::RRTXstatic>(si));
rrtX2->setName("RRTX0.01");
rrtX2->setEpsilon(0.01);
rrtX2->setRange(range);
// rrtX2->setVariant(3);
rrtX2->setKNearest(knn);
b.addPlanner(rrtX2);
auto rrtX3(std::make_shared<ompl::geometric::RRTXstatic>(si));
rrtX3->setName("RRTX0.001");
rrtX3->setEpsilon(0.001);
rrtX3->setRange(range);
// rrtX3->setVariant(3);
rrtX3->setKNearest(knn);
b.addPlanner(rrtX3);
b.benchmark(request);
b.saveResultsToFile(boost::str(boost::format("Diagonal.log")).c_str());
exit(0);
}
......@@ -61,7 +61,7 @@ bool isStateValidHard(const ob::SpaceInformation *si, const ob::State *state)
return si->satisfiesBounds(state);
}
void plan(ob::StateSpacePtr space, bool easy)
void plan(const ob::StateSpacePtr& space, bool easy)
{
ob::ScopedState<> start(space), goal(space);
ob::RealVectorBounds bounds(2);
......@@ -79,9 +79,12 @@ void plan(ob::StateSpacePtr space, bool easy)
og::SimpleSetup ss(space);
// set state validity checking for this space
ob::SpaceInformationPtr si(ss.getSpaceInformation());
ss.setStateValidityChecker(std::bind(
easy ? &isStateValidEasy : &isStateValidHard, si.get(), std::placeholders::_1));