Set state space bounds changes

se2_planning/include/se2_planning/OmplPlanner.hpp

@@ -33,6 +33,8 @@ class OmplPlanner : public Planner {
   void unlockStateValidator() final;
   bool isLocked() const final;
 
+  virtual void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds);
+
   void setMaxPlanningDuration(double T);
   void getOmplPath(ompl::geometric::PathGeometric* omplPath) const;
   void getOmplInterpolatedPath(ompl::geometric::PathGeometric* omplPath, double spatialResolution) const;
@@ -41,16 +43,15 @@ class OmplPlanner : public Planner {
   void getInterpolatedPath(Path* interpolatedPath, unsigned int numPoints) const;
   ompl::geometric::SimpleSetupPtr getSimpleSetup() const;
   void setOmplPlanner(ompl::base::PlannerPtr planner);
-  virtual void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds);
 
  protected:
-  virtual void initializeStateSpace() = 0;
   virtual bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const = 0;
   virtual ompl::base::ScopedStatePtr convert(const State& state) const = 0;
   virtual void convert(const ompl::base::ScopedStatePtr omplState, State* state) const = 0;
   virtual void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const = 0;
 
   ompl::base::StateSpacePtr stateSpace_;
+  std::unique_ptr<ompl::base::RealVectorBounds> bounds_;
   ompl::geometric::SimpleSetupPtr simpleSetup_;
   ompl::base::ScopedStatePtr startState_, goalState_;
   std::unique_ptr<ompl::geometric::PathGeometric> path_, interpolatedPath_;

se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp

@@ -35,10 +35,14 @@ struct ReedsSheppPath : public Path {
 
 struct OmplReedsSheppPlannerParameters {
   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";
@@ -54,21 +58,19 @@ class OmplReedsSheppPlanner final : public OmplPlanner {
   bool initialize() final;
   bool plan() final;
   void setParameters(const OmplReedsSheppPlannerParameters& parameters);
-  void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override;
+  void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override;
   bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const;
   const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const;
 
  private:
   void createDefaultStateSpace();
-  void initializeStateSpace() final;
-  void setStateSpaceBoundaries();
   bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const final;
   ompl::base::ScopedStatePtr convert(const State& state) const final;
   void convert(const ompl::base::ScopedStatePtr omplState, State* state) const final;
   void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const final;
   int getDistanceSignAt(const ompl::geometric::PathGeometric& path, unsigned int id) const;
 
-  std::unique_ptr<ompl::base::RealVectorBounds> bounds_;
+  // TODO(christoph): Can we move bounds_ to OmplPlanner? Can we remove it altogether?
   const int reedsSheppStateSpaceDim_ = 2;
   OmplReedsSheppPlannerParameters parameters_;
 };

se2_planning/src/OmplPlanner.cpp

@@ -86,7 +86,6 @@ bool OmplPlanner::reset() {
 }
 
 bool OmplPlanner::initialize() {
-  initializeStateSpace();
   if (stateSpace_ == nullptr) {
     std::cerr << "OmplPlanner:: state space is nullptr" << std::endl;
     return false;
@@ -100,8 +99,13 @@ bool OmplPlanner::initialize() {
   return true;
 }
 
-void OmplPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) {
-  throw std::runtime_error("Not implemented");
+void OmplPlanner::setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) {
+  for (size_t idx = 0; idx < bounds.low.size(); idx++) {
+    bounds_->low[idx] = bounds.low[idx];
+  }
+  for (size_t idx = 0; idx < bounds.high.size(); idx++) {
+    bounds_->high[idx] = bounds.high[idx];
+  }
 }
 
 void OmplPlanner::setMaxPlanningDuration(double T) {

se2_planning/src/OmplReedsSheppPlanner.cpp

@@ -42,52 +42,33 @@ bool OmplReedsSheppPlanner::initialize() {
   return true;
 }
 
-void OmplReedsSheppPlanner::initializeStateSpace() {
-  // todo maybe not really needed
-  createDefaultStateSpace();
-}
-
 void OmplReedsSheppPlanner::createDefaultStateSpace() {
+  // Allocate abstracted objects
   stateSpace_.reset(new ompl::base::ReedsSheppStateSpace(parameters_.turningRadius_));
   bounds_ = std::make_unique<ompl::base::RealVectorBounds>(reedsSheppStateSpaceDim_);
-  setStateSpaceBoundaries();
+  // Initialize bounds with parameters
+  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);
 }
 bool OmplReedsSheppPlanner::plan() {
-  initialize();
-  ompl::base::StateSpacePtr space = simpleSetup_->getStateSpace();
-  auto bounds = space->as<ompl::base::SE2StateSpace>()->getBounds();
-  //  std::cout << "OmplReedsSheppPlanner: Planner state space bounds: x = [" << bounds.low[0] << ", " << bounds.high[0] << "], y=["
-  //            << bounds.low[1] << ", " << bounds.high[1] << "]" << std::endl;
-
-  bool result = BASE::plan();
+  // Call to initialize is required s.t. OMPL planner is updated with latest state space boundaries
+  //  initialize();
+  const bool result = BASE::plan();
   *interpolatedPath_ = interpolatePath(*path_, parameters_.pathSpatialResolution_);
   return result;
 }
-void OmplReedsSheppPlanner::setStateSpaceBoundaries() {
-  bounds_->low[0] = parameters_.xLowerBound_;
-  bounds_->low[1] = parameters_.yLowerBound_;
-  bounds_->high[0] = parameters_.xUpperBound_;
-  bounds_->high[1] = parameters_.yUpperBound_;
-  stateSpace_->as<ompl::base::SE2StateSpace>()->setBounds(*bounds_);
-  //  std::cout << "OmplReedsSheppPlanner: Set state space bounds: x = [" << bounds_->low[0] << ", " << bounds_->high[0] << "], y=["
-  //            << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl;
-}
+
 const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundaries() const {
   return stateSpace_->as<ompl::base::SE2StateSpace>()->getBounds();
 }
 
-void OmplReedsSheppPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) {
-  bounds_->low[0] = bounds.low[0];
-  bounds_->low[1] = bounds.low[1];
-  bounds_->high[0] = bounds.high[0];
-  bounds_->high[1] = bounds.high[1];
-  parameters_.xLowerBound_ = bounds_->low[0];
-  parameters_.yLowerBound_ = bounds_->low[1];
-  parameters_.xUpperBound_ = bounds_->high[0];
-  parameters_.yUpperBound_ = bounds_->high[1];
-  stateSpace_->as<ompl::base::SE2StateSpace>()->setBounds(*bounds_);
-  //  std::cout << "OmplReedsSheppPlanner: Update state space bounds: x = [" << bounds_->low[0] << ", " << bounds_->high[0] << "], y=["
-  //            << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl;
+void OmplReedsSheppPlanner::setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) {
+  BASE::setStateSpaceBoundaries(bounds);
+  stateSpace_->as<ompl::base::SE2StateSpace>()->setBounds(bounds);
 }
 
 bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const {
@@ -101,6 +82,7 @@ bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::Re
 }
 
 bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const {
+  (void)si;
   const ReedsSheppState rsState = se2_planning::convert(state);
   return stateValidator_->isStateValid(rsState);
 }

se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp

@@ -57,13 +57,13 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
 
   // Block update of state validator obstacle map during planning
   planner_->lockStateValidator();
+
   // TODO move from se2_planner_node here (introduces dependency but makes state space update more consistent:
   //  adapt state space boundaries from grid map
   // planner_->getStateValidator().as<GridMapLazyStateValidator>
 
   // Adapt state space boundaries (larger than grid map, state validity checking assumes area out of bounds to be
   // traversable) to contain initial and goal state otherwise RRTstar fails
-  // TODO expose stateSpaceBoundsMargin_ as param => depends on footprint size?
   // TODO move to OMPLReedsSheppPlanner.cpp?
   const double stateSpaceBoundsMargin_ = parameters_.stateSpaceBoundsMargin_;
   if (!planner_->as<OmplReedsSheppPlanner>()->satisfiesStateSpaceBoundaries(start)) {
@@ -74,7 +74,7 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
     newBounds.high[1] = std::max(bounds.high[1], start.y_ + stateSpaceBoundsMargin_);
     newBounds.low[0] = std::min(bounds.low[0], start.x_ - stateSpaceBoundsMargin_);
     newBounds.low[1] = std::min(bounds.low[1], start.y_ - stateSpaceBoundsMargin_);
-    planner_->as<OmplReedsSheppPlanner>()->updateStateSpaceBoundaries(newBounds);
+    planner_->as<OmplReedsSheppPlanner>()->setStateSpaceBoundaries(newBounds);
   }
   if (!planner_->as<OmplReedsSheppPlanner>()->satisfiesStateSpaceBoundaries(goal)) {
     ROS_DEBUG("Goal state not in grid map. Enlarge state space boundaries.");
@@ -84,10 +84,9 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
     newBounds.high[1] = std::max(bounds.high[1], goal.y_ + stateSpaceBoundsMargin_);
     newBounds.low[0] = std::min(bounds.low[0], goal.x_ - stateSpaceBoundsMargin_);
     newBounds.low[1] = std::min(bounds.low[1], goal.y_ - stateSpaceBoundsMargin_);
-    planner_->as<OmplReedsSheppPlanner>()->updateStateSpaceBoundaries(newBounds);
+    planner_->as<OmplReedsSheppPlanner>()->setStateSpaceBoundaries(newBounds);
   }
 
-  // TODO move to detach? Better to publish this info for debugging before checking validity of states.
   publishStartState();
   publishGoalState();
   publishStateSpaceBoundaryMarker();

se2_planning_ros/src/se2_planner_node.cpp

@@ -24,24 +24,20 @@ void gridMapCallback(const grid_map_msgs::GridMap& msg) {
   // Update planner bounds when new map is published, placed here due to class structure
   grid_map::GridMap map;
   grid_map::GridMapRosConverter::fromMessage(msg, map);
+
   // Grid map is symmetric around position
-  grid_map::Position mapPosition;
-  mapPosition = map.getPosition();
-  grid_map::Length mapLength;
-  mapLength = map.getLength();
+  const grid_map::Position mapPosition = map.getPosition();
+  const grid_map::Length mapLength = map.getLength();
   ompl::base::RealVectorBounds bounds(2);
   bounds.low[0] = mapPosition.x() - mapLength.x() / 2.0;
   bounds.low[1] = mapPosition.y() - mapLength.y() / 2.0;
   bounds.high[0] = mapPosition.x() + mapLength.x() / 2.0;
   bounds.high[1] = mapPosition.y() + mapLength.y() / 2.0;
-
-  if (!planner->isLocked()) {
-    planner->updateStateSpaceBoundaries(bounds);
-    ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", "
-                                                      << mapLength.y());
-  } else {
-    ROS_DEBUG_STREAM("OMPL State Space Update: Planner is locked. Not updating state space bounds.");
-  }
+  planner->lockStateValidator();
+  planner->setStateSpaceBoundaries(bounds);
+  planner->unlockStateValidator();
+  ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", "
+                                                    << mapLength.y());
 }
 
 int main(int argc, char** argv) {