Move StateValidator functions to Planner interface

se2_planning/include/se2_planning/OmplPlanner.hpp

@@ -27,6 +27,12 @@ class OmplPlanner : public Planner {
   void getStartingState(State* startingState) const final;
   void getGoalState(State* goalState) const final;
 
+  void setStateValidator(std::unique_ptr<StateValidator> stateValidator) final;
+  const StateValidator& getStateValidator() const final;
+  void lockStateValidator() final;
+  void unlockStateValidator() final;
+  bool isLocked() const final;
+
   void setMaxPlanningDuration(double T);
   void getOmplPath(ompl::geometric::PathGeometric* omplPath) const;
   void getOmplInterpolatedPath(ompl::geometric::PathGeometric* omplPath, double spatialResolution) const;
@@ -48,6 +54,7 @@ class OmplPlanner : public Planner {
   ompl::geometric::SimpleSetupPtr simpleSetup_;
   ompl::base::ScopedStatePtr startState_, goalState_;
   std::unique_ptr<ompl::geometric::PathGeometric> path_, interpolatedPath_;
+  std::unique_ptr<StateValidator> stateValidator_;
 
  private:
   double maxPlanningDuration_ = 1.0;

se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp

@@ -54,11 +54,6 @@ class OmplReedsSheppPlanner final : public OmplPlanner {
   bool initialize() final;
   bool plan() final;
   void setParameters(const OmplReedsSheppPlannerParameters& parameters);
-  void setStateValidator(std::unique_ptr<StateValidator> stateValidator);
-  const StateValidator& getStateValidator() const;
-  void lockStateValidator();
-  void unlockStateValidator();
-  bool isLocked() const;
   void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override;
   bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const;
   const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const;
@@ -76,7 +71,6 @@ class OmplReedsSheppPlanner final : public OmplPlanner {
   std::unique_ptr<ompl::base::RealVectorBounds> bounds_;
   const int reedsSheppStateSpaceDim_ = 2;
   OmplReedsSheppPlannerParameters parameters_;
-  std::unique_ptr<StateValidator> stateValidator_;
 };
 
 std::string toString(ReedsSheppPathSegment::Direction direction);

se2_planning/include/se2_planning/Planner.hpp

@@ -8,6 +8,7 @@
 #pragma once
 
 #include "se2_planning/State.hpp"
+#include "se2_planning/StateValidator.hpp"
 
 #include <boost/concept_check.hpp>
 namespace se2_planning {
@@ -27,6 +28,12 @@ class Planner {
   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 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;
+
   template <class T>
   const T* as() const {
     BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));

se2_planning/src/OmplPlanner.cpp

@@ -40,10 +40,30 @@ void OmplPlanner::getGoalState(State* goalState) const {
   convert(goalState_, goalState);
 }
 
+void OmplPlanner::setStateValidator(std::unique_ptr<StateValidator> stateValidator) {
+  stateValidator_ = std::move(stateValidator);
+}
+
+const StateValidator& OmplPlanner::getStateValidator() const {
+  return *stateValidator_;
+}
+
+void OmplPlanner::lockStateValidator() {
+  stateValidator_->lock();
+}
+
+void OmplPlanner::unlockStateValidator() {
+  stateValidator_->unlock();
+}
+
+bool OmplPlanner::isLocked() const {
+  return stateValidator_->isLocked();
+}
+
 bool OmplPlanner::plan() {
   simpleSetup_->clear();
   simpleSetup_->setStartAndGoalStates(*startState_, *goalState_);
-  // TODO see https://ompl.kavrakilab.org/genericPlanning.html for continue planning
+  // TODO see https://ompl.kavrakilab.org/genericPlanning.html on how to continue planning
   //   The ompl::base::Planner::solve() method can be called repeatedly with different
   //   allowed time durations until a solution is found. The planning process continues
   //   with the available data structures when sequential calls to ompl::base::Planner::solve() are made.

se2_planning/src/OmplReedsSheppPlanner.cpp

@@ -100,26 +100,6 @@ bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::Re
   return stateSpace_->satisfiesBounds(s);
 }
 
-void OmplReedsSheppPlanner::setStateValidator(std::unique_ptr<StateValidator> stateValidator) {
-  stateValidator_ = std::move(stateValidator);
-}
-
-const StateValidator& OmplReedsSheppPlanner::getStateValidator() const {
-  return *stateValidator_;
-}
-
-void OmplReedsSheppPlanner::lockStateValidator() {
-  stateValidator_->lock();
-}
-
-void OmplReedsSheppPlanner::unlockStateValidator() {
-  stateValidator_->unlock();
-}
-
-bool OmplReedsSheppPlanner::isLocked() const {
-  return stateValidator_->isLocked();
-}
-
 bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const {
   const ReedsSheppState rsState = se2_planning::convert(state);
   return stateValidator_->isStateValid(rsState);

se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp

@@ -24,6 +24,12 @@ class PlannerRos : public Planner {
   void getStartingState(State* startingState) const override;
   void getGoalState(State* goalState) const override;
 
+  void setStateValidator(std::unique_ptr<StateValidator> stateValidator) override;
+  const StateValidator& getStateValidator() const override;
+  void lockStateValidator() override;
+  void unlockStateValidator() override;
+  bool isLocked() const override;
+
   void setPlanningStrategy(std::shared_ptr<Planner> planner);
   virtual void publishPath() const;
   virtual void publishPathNavMsgs() const;

se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp

@@ -41,7 +41,7 @@ bool OmplReedsSheppPlannerRos::plan() {
 
 bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, PlanningService::Response& res) {
   // TODO Grid map in state validator is currently initialized to some default values, this check is therefore useless
-  if (!planner_->as<OmplReedsSheppPlanner>()->getStateValidator().isInitialized()) {
+  if (!planner_->getStateValidator().isInitialized()) {
     ROS_WARN_STREAM("State validator has not been initialized yet. Abort planning.");
     res.status = false;
     return true;
@@ -55,10 +55,10 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
   setGoalState(goal);
 
   // Block update of state validator obstacle map during planning
-  planner_->as<OmplReedsSheppPlanner>()->lockStateValidator();
+  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_->as<OmplReedsSheppPlanner>()->getStateValidator().as<GridMapLazyStateValidator>
+  // 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
@@ -93,19 +93,16 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
 
   //  Use state validator only after lock mutex is active and state space is updated
   //  Checks only for non-traversable terrain not for state space bounds
-  if (!planner_->as<OmplReedsSheppPlanner>()->getStateValidator().isStateValid(start)) {
+  if (!planner_->getStateValidator().isStateValid(start)) {
     ROS_WARN_STREAM("Start state (x: " << start.x_ << ", y: " << start.y_ << ", yaw: " << start.yaw_
                                        << ") not valid. Start planning anyway.");
   }
-  if (!planner_->as<OmplReedsSheppPlanner>()->getStateValidator().isStateValid(goal)) {
+  if (!planner_->getStateValidator().isStateValid(goal)) {
     ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Start planning anyway.");
-    //    ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Abort planning.");
-    //    planner_->as<OmplReedsSheppPlanner>()->unlockStateValidator();
-    //    return false;
   }
 
   bool result = plan();
-  planner_->as<OmplReedsSheppPlanner>()->unlockStateValidator();
+  planner_->unlockStateValidator();
 
   res.status = result;
 

se2_planning_ros/src/PlannerRos.cpp

@@ -67,4 +67,24 @@ void PlannerRos::getGoalState(State* goalState) const {
   planner_->getGoalState(goalState);
 }
 
+void PlannerRos::setStateValidator(std::unique_ptr<StateValidator> stateValidator) {
+  planner_->setStateValidator(std::move(stateValidator));
+}
+
+const StateValidator& PlannerRos::getStateValidator() const {
+  return planner_->getStateValidator();
+}
+
+void PlannerRos::lockStateValidator() {
+  planner_->lockStateValidator();
+}
+
+void PlannerRos::unlockStateValidator() {
+  planner_->unlockStateValidator();
+}
+
+bool PlannerRos::isLocked() const {
+  return planner_->isLocked();
+}
+
 }  // namespace se2_planning