Integrate new map for state space boundaries update, adapt tests

car_demo/car_demo/config/reeds_shepp_planner_ros.yaml

@@ -1,9 +1,6 @@
 
 state_space:
-  x_lower: -100.0
-  x_upper: 100.0
-  y_lower: -100.0
-  y_upper: 100.0
+  boundaries_margin: 3.0
   turning_radius: 10.0
 
 state_validator_ros:

se2_planning/include/se2_planning/OmplPlanner.hpp

@@ -31,7 +31,11 @@ class OmplPlanner : public Planner {
   const StateValidator& getStateValidator() const final;
   void lockStateValidator() final;
   void unlockStateValidator() final;
-  bool isLocked() const final;
+
+  void setMap(std::unique_ptr<Map> Map) final;
+  const Map& getMap() const final;
+  void lockMap() final;
+  void unlockMap() final;
 
   virtual void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds);
   virtual const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const;
@@ -57,6 +61,7 @@ class OmplPlanner : public Planner {
   ompl::base::ScopedStatePtr startState_, goalState_;
   std::unique_ptr<ompl::geometric::PathGeometric> path_, interpolatedPath_;
   std::unique_ptr<StateValidator> stateValidator_;
+  std::unique_ptr<Map> map_;
 
  private:
   double maxPlanningDuration_ = 1.0;

se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp

@@ -34,15 +34,8 @@ struct ReedsSheppPath : public Path {
 };
 
 struct OmplReedsSheppPlannerParameters {
+  double boundariesMargin_ = 1.0;
   double turningRadius_ = 10.0;
-
-  // TODO(christoph): Do we need these parameters? Should be provided by the map. If no map available, no planning
-  //  possible...?
-  double xLowerBound_ = -1000.0;
-  double xUpperBound_ = 1000.0;
-  double yLowerBound_ = -1000.0;
-  double yUpperBound_ = 1000.0;
-
   double pathSpatialResolution_ = 0.05;
   double maxPlanningTime_ = 1.0;
   std::string omplPlannerName_ = "RRTstar";
@@ -61,6 +54,7 @@ class OmplReedsSheppPlanner final : public OmplPlanner {
   void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override;
   const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const override;
   bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const;
+  void extendStateSpaceBoundaries(const se2_planning::ReedsSheppState& state, const double margin);
 
  private:
   void createDefaultStateSpace();

se2_planning/include/se2_planning/Planner.hpp

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include "se2_planning/Map.hpp"
 #include "se2_planning/State.hpp"
 #include "se2_planning/StateValidator.hpp"
 
@@ -25,14 +26,24 @@ class Planner {
 
   virtual bool reset() { throw std::runtime_error("Planner: reset() not implemented"); }
   virtual bool initialize() { throw std::runtime_error("Planner: initialize() not implemented"); }
-  virtual void getStartingState(State* startingState) const { throw std::runtime_error("Planner: getStartingState() not implemented"); }
-  virtual void getGoalState(State* goalState) const { throw std::runtime_error("Planner: getGoalState() not implemented"); }
+  virtual void getStartingState(State* startingState) const {
+    (void)startingState;
+    throw std::runtime_error("Planner: getStartingState() not implemented");
+  }
+  virtual void getGoalState(State* goalState) const {
+    (void)goalState;
+    throw std::runtime_error("Planner: getGoalState() not implemented");
+  }
 
   virtual void setStateValidator(std::unique_ptr<StateValidator> stateValidator) = 0;
   virtual const StateValidator& getStateValidator() const { throw std::runtime_error("Planner: getStateValidator() not implemented"); };
   virtual void lockStateValidator() = 0;
   virtual void unlockStateValidator() = 0;
-  virtual bool isLocked() const = 0;
+
+  virtual void setMap(std::unique_ptr<Map> Map) = 0;
+  virtual const Map& getMap() const { throw std::runtime_error("Planner: getMap() not implemented"); };
+  virtual void lockMap() = 0;
+  virtual void unlockMap() = 0;
 
   template <class T>
   const T* as() const {

se2_planning/include/se2_planning/StateValidator.hpp

@@ -8,21 +8,23 @@
 #pragma once
 #include "se2_planning/State.hpp"
 
+#include <boost/thread/mutex.hpp>
+
 namespace se2_planning {
 
 class StateValidator {
  public:
   StateValidator() = default;
   virtual ~StateValidator() = default;
+
   virtual bool isStateValid(const State& state) const = 0;
   virtual void initialize();
   virtual bool isInitialized() const;
-  virtual bool isLocked() const;
   virtual void lock();
   virtual void unlock();
 
  private:
-  bool isLocked_ = false;
+  boost::mutex mtx_;
 };
 
 class SE2stateValidator : public StateValidator {

se2_planning/src/OmplPlanner.cpp

@@ -56,8 +56,20 @@ void OmplPlanner::unlockStateValidator() {
   stateValidator_->unlock();
 }
 
-bool OmplPlanner::isLocked() const {
-  return stateValidator_->isLocked();
+void OmplPlanner::setMap(std::unique_ptr<Map> Map) {
+  map_ = std::move(Map);
+}
+
+const Map& OmplPlanner::getMap() const {
+  return *map_;
+}
+
+void OmplPlanner::lockMap() {
+  map_->lock();
+}
+
+void OmplPlanner::unlockMap() {
+  map_->unlock();
 }
 
 bool OmplPlanner::plan() {

se2_planning/src/OmplReedsSheppPlanner.cpp

@@ -46,17 +46,38 @@ void OmplReedsSheppPlanner::createDefaultStateSpace() {
   // Allocate abstracted objects
   stateSpace_.reset(new ompl::base::ReedsSheppStateSpace(parameters_.turningRadius_));
   bounds_ = std::make_unique<ompl::base::RealVectorBounds>(reedsSheppStateSpaceDim_);
-  // Initialize bounds with parameters
+  // Initialize bounds to zero
   ompl::base::RealVectorBounds bounds(reedsSheppStateSpaceDim_);
-  bounds.low[0] = parameters_.xLowerBound_;
-  bounds.low[1] = parameters_.yLowerBound_;
-  bounds.high[0] = parameters_.xUpperBound_;
-  bounds.high[1] = parameters_.yUpperBound_;
   setStateSpaceBoundaries(bounds);
 }
+
+void OmplReedsSheppPlanner::extendStateSpaceBoundaries(const se2_planning::ReedsSheppState& state, const double margin) {
+  const auto bounds = getStateSpaceBoundaries();
+  ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_);
+  newBounds.high[0] = std::max(bounds.high[0], state.x_ + margin);
+  newBounds.high[1] = std::max(bounds.high[1], state.y_ + margin);
+  newBounds.low[0] = std::min(bounds.low[0], state.x_ - margin);
+  newBounds.low[1] = std::min(bounds.low[1], state.y_ - margin);
+  setStateSpaceBoundaries(newBounds);
+}
+
 bool OmplReedsSheppPlanner::plan() {
-  // Call to initialize is required s.t. OMPL planner is updated with latest state space boundaries
-  //  initialize();
+  // Update state space boundaries with values from map
+  const auto bounds = map_->getBounds();
+  ompl::base::RealVectorBounds realVectorBounds(reedsSheppStateSpaceDim_);
+  realVectorBounds.low[0] = bounds.low_.x_;
+  realVectorBounds.low[1] = bounds.low_.y_;
+  realVectorBounds.high[0] = bounds.high_.x_;
+  realVectorBounds.high[1] = bounds.high_.y_;
+  setStateSpaceBoundaries(realVectorBounds);
+  // Extend state space boundaries from the map to always include start and goal with an additional margin
+  ReedsSheppState startState{};
+  getStartingState(&startState);
+  extendStateSpaceBoundaries(startState, parameters_.boundariesMargin_);
+  ReedsSheppState goalState{};
+  getGoalState(&goalState);
+  extendStateSpaceBoundaries(goalState, parameters_.boundariesMargin_);
+  // Plan
   const bool result = BASE::plan();
   *interpolatedPath_ = interpolatePath(*path_, parameters_.pathSpatialResolution_);
   return result;

se2_planning/src/StateValidator.cpp

@@ -20,15 +20,11 @@ bool SE2stateValidator::isStateValid(const State& state) const {
   return true;
 }
 
-bool StateValidator::isLocked() const {
-  return isLocked_;
-}
-
 void StateValidator::lock() {
-  isLocked_ = true;
+  mtx_.lock();
 }
 
 void StateValidator::unlock() {
-  isLocked_ = false;
+  mtx_.unlock();
 }
 }  // namespace se2_planning

se2_planning/src/planning_benchmark.cpp

@@ -11,6 +11,7 @@
 #include "grid_map_core/iterators/GridMapIterator.hpp"
 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
 #include "se2_planning/GridMapLazyStateValidator.hpp"
+#include "se2_planning/OccupancyMap.hpp"
 #include "se2_planning/OmplReedsSheppPlanner.hpp"
 #include "test_helpers.hpp"
 
@@ -30,13 +31,6 @@ void setCostThreshold(se2_planning::OmplReedsSheppPlanner* planner) {
   planner->getSimpleSetup()->setOptimizationObjective(optimizationObjective);
 }
 
-void createRectangularStateSpace(double stateBound, se2_planning::OmplReedsSheppPlannerParameters* parameters) {
-  parameters->xLowerBound_ = -stateBound;
-  parameters->xUpperBound_ = stateBound;
-  parameters->yLowerBound_ = -stateBound;
-  parameters->yUpperBound_ = stateBound;
-}
-
 void runPlanner(se2_planning::OmplReedsSheppPlanner& planner) {
   const se2_planning::ReedsSheppState start(0.0, -10.0, 0.0);
   const se2_planning::ReedsSheppState goal(0.0, 10.0, 0.0);
@@ -99,27 +93,26 @@ void runLazyRandomEarlyStoppingValidator(se2_planning::OmplReedsSheppPlanner& pl
   runPlanner(planner);
 }
 
-int main(int argc, char** argv) {
+int main() {
   namespace test = se2_planning_test;
   ompl::msg::setLogLevel(ompl::msg::LogLevel::LOG_NONE);
-  const std::string testLayer = "occupancy";
   // create environment
   grid_map::GridMap gridMap;
   gridMap.setGeometry(grid_map::Length(40.0, 40.0), 0.1);
-  gridMap.add(testLayer, 0.0);
+  gridMap.add(benchmarkLayer, 0.0);
   std::function<bool(double, double)> isAnObstacle = [](double x, double y) {
     return test::isInsideRectangle(x, y, -10.0, 0.0, 40.0, 2.0) || test::isInsideRectangle(x, y, 5.0, 5.0, 5.0, 5.0);
   };
-  test::addObstacles(isAnObstacle, testLayer, &gridMap);
+  test::addObstacles(isAnObstacle, benchmarkLayer, &gridMap);
 
   // setup planner
-  se2_planning::OmplReedsSheppPlannerParameters parameters;
-  parameters.maxPlanningTime_ = 10.0;
-  parameters.turningRadius_ = 1.0;
-  createRectangularStateSpace(20.0, &parameters);
+  se2_planning::OmplReedsSheppPlannerParameters parameters = se2_planning_test::createDefaultParams();
   se2_planning::OmplReedsSheppPlanner planner;
   planner.setParameters(parameters);
   planner.initialize();
+  auto map = std::make_unique<se2_planning::OccupancyMap>();
+  map->setGridMap(gridMap, benchmarkLayer);
+  planner.setMap(std::move(map));
   setCostThreshold(&planner);
 
   runNormalValidator(planner, gridMap);

se2_planning/test/OmplReedsSheppPlannerTest.cpp

@@ -26,8 +26,12 @@ TEST(Planning, OmplReedsSheppPlanner)
   ompl::RNG::setSeed(seed);
   ompl::msg::setLogLevel(ompl::msg::LogLevel::LOG_NONE);
 
-  se2_planning::OmplReedsSheppPlannerParameters parameters =
-      createRectangularStateSpaceWithDefaultParams(10.0);
+  se2_planning::OmplReedsSheppPlannerParameters parameters = createDefaultParams();
+
+  std::function<bool(double, double)> noObstacles = [](double x, double y) {
+    return false;
+  };
+  grid_map::GridMap map = createGridMap(20, 20, 0.1, noObstacles)
 
   se2_planning::OmplReedsSheppPlanner planner;
   setupPlanner(parameters, &planner);

se2_planning/test/test_helpers.cpp

@@ -60,6 +60,18 @@ se2_planning::SE2state randomState(const grid_map::GridMap &gm, double margin)
   return s;
 }
 
+se2_planning::ReedsSheppState randomReedsSheppState(const grid_map::GridMap &gm, double margin)
+{
+  se2_planning::ReedsSheppState s;
+  const double l = gm.getLength()(0) / 2.0 - margin;
+  const double w = gm.getLength()(1) / 2.0 - margin;
+  s.x_ = randomNumber(-l, l);
+  s.y_ = randomNumber(-w, w);
+  s.yaw_ = randomNumber(-M_PI, M_PI);
+  return s;
+}
+
+
 grid_map::GridMap createGridMap(double length, double width, double resolution,
                                 std::function<bool(double, double)> isAnObstacle)
 {
@@ -93,16 +105,6 @@ bool isStartAndGoalStateOK(const se2_planning::ReedsSheppPath &path,
   return firstState == start && lastState == goal;
 }
 
-se2_planning::ReedsSheppState randomState(
-    const se2_planning::OmplReedsSheppPlannerParameters &parameters)
-{
-  se2_planning::ReedsSheppState s;
-  s.x_ = randomNumber(0.8 * parameters.xLowerBound_, 0.8 * parameters.xUpperBound_);
-  s.y_ = randomNumber(0.8 * parameters.yLowerBound_, 0.8 * parameters.yUpperBound_);
-  s.yaw_ = randomNumber(-M_PI, M_PI);
-  return s;
-}
-
 void setCostThreshold(se2_planning::OmplReedsSheppPlanner *planner)
 {
   auto si = planner->getSimpleSetup()->getSpaceInformation();
@@ -112,17 +114,12 @@ void setCostThreshold(se2_planning::OmplReedsSheppPlanner *planner)
   planner->getSimpleSetup()->setOptimizationObjective(optimizationObjective);
 }
 
-se2_planning::OmplReedsSheppPlannerParameters createRectangularStateSpaceWithDefaultParams(
-    double stateBound)
-{
-  se2_planning::OmplReedsSheppPlannerParameters parameters;
-  parameters.xLowerBound_ = -stateBound;
-  parameters.xUpperBound_ = stateBound;
-  parameters.yLowerBound_ = -stateBound;
-  parameters.yUpperBound_ = stateBound;
-  parameters.maxPlanningTime_ = 10.0;
-  parameters.turningRadius_ = 1.0;
-  return parameters;
+se2_planning::OmplReedsSheppPlannerParameters createDefaultParams(){
+  se2_planning::OmplReedsSheppPlannerParameters params{};
+  params.maxPlanningTime_ = 10.0;
+  params.turningRadius_ = 1.0;
+  params.boundariesMargin_ = 1.0;
+  return params;
 }
 
 void setupPlanner(const se2_planning::OmplReedsSheppPlannerParameters &parameters,
@@ -135,10 +132,10 @@ void setupPlanner(const se2_planning::OmplReedsSheppPlannerParameters &parameter
 
 bool simplePlanBetweenRandomStartAndGoalTest(
     se2_planning::OmplReedsSheppPlanner &planner,
-    const se2_planning::OmplReedsSheppPlannerParameters &parameters)
+    const se2_planning::OmplReedsSheppPlannerParameters &parameters, const grid_map::GridMap &map)
 {
-  const auto start = randomState(parameters);
-  const auto goal = randomState(parameters);
+  const se2_planning::ReedsSheppState start = randomReedsSheppState(map, parameters.boundariesMargin_);
+  const se2_planning::ReedsSheppState goal = randomReedsSheppState(map, parameters.boundariesMargin_);
   planner.setStartingState(start);
   planner.setGoalState(goal);
   const bool status = planner.plan();

se2_planning/test/test_helpers.hpp

@@ -26,6 +26,7 @@ void addObstacles(std::function<bool(double, double)> isObstacle, const std::str
                   grid_map::GridMap *map);
 bool isInsideRectangle(double _x, double _y, double x0, double y0, double xLength, double yLength);
 se2_planning::SE2state randomState(const grid_map::GridMap &gm, double margin);
+se2_planning::ReedsSheppState randomReedsSheppState(const grid_map::GridMap &gm, double margin);
 grid_map::GridMap createGridMap(double length, double width, double resolution,
                                 std::function<bool(double, double)> isAnObstacle);
 
@@ -36,19 +37,15 @@ bool isStartAndGoalStateOK(const se2_planning::ReedsSheppPath &path,
                            const se2_planning::ReedsSheppState &start,
                            const se2_planning::ReedsSheppState &goal);
 
-se2_planning::ReedsSheppState randomState(
-    const se2_planning::OmplReedsSheppPlannerParameters &parameters);
-
 void setCostThreshold(se2_planning::OmplReedsSheppPlanner *planner);
 
-se2_planning::OmplReedsSheppPlannerParameters createRectangularStateSpaceWithDefaultParams(
-    double stateBound);
+se2_planning::OmplReedsSheppPlannerParameters createDefaultParams();
 
 void setupPlanner(const se2_planning::OmplReedsSheppPlannerParameters &parameters,
                   se2_planning::OmplReedsSheppPlanner *planner);
 
 bool simplePlanBetweenRandomStartAndGoalTest(
     se2_planning::OmplReedsSheppPlanner &planner,
-    const se2_planning::OmplReedsSheppPlannerParameters &parameters);
+    const se2_planning::OmplReedsSheppPlannerParameters &parameters, const grid_map::GridMap &map);
 
 } /* namespace se2_planning_test */

se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml

@@ -1,9 +1,6 @@
 
 state_space:
-  x_lower: -1000.0
-  x_upper: 1000.0
-  y_lower: -1000.0
-  y_upper: 1000.0
+  boundaries_margin: 1.0
   turning_radius: 10.0
 
 state_validator_ros: