How to write a code with OMPL -Part1-

edit

I might put a similar question on this web. As writing a subject, I want to understand how to write a code with OMPL to move a turtlebot. Now, I'm writing the code. I think global_planner with OMPL needs next steps.

1.isStateValid()
isStateValid() is a function to check if a position putting a sampler on a map is free space.
isStateValid() is a function to store information like free space and obstacle region.

2.makePlan()
makePlan() includes ob::PlannerPtr planner( og::RRT(ss.getSpaceInformation())), setStateValidityChecker() and so on.

On this article the contents I want to ask is following. isStateValid has to receive state of OMPL format instead of costmap, so my code is wrong. As I explain above, costmap has many information about an obstacle. So I think that I should use a costmap. How should I edit this code? or Could anyone tell me more information if I'm not correct?

   bool isStateValid(costmap_2d::Costmap2DROS* costmap_ros){
   costmap_ros_ = costmap_ros;
   costmap_ = costmap_ros_->getCostmap();

   if(costmap_ > 253)
        return false;
    return true;
  }

Thank you in advance!

Error message

/home/ken/catkin_ws/src/navigation/ompl_planner_rrt/src/ompl_planner_rrt.cpp: In function ‘bool ompl_planner_rrt::isStateValid(costmap_2d::Costmap2DROS*)’:
/home/ken/catkin_ws/src/navigation/ompl_planner_rrt/src/ompl_planner_rrt.cpp:62:3: error: ‘costmap_ros_’ was not declared in this scope

I edited ompl_planner_rrt.h like a following.

namespace ompl_planner_rrt{
    class OMPLPlannerRRT : public nav_core::BaseGlobalPlanner {
       public:    
           OMPLPlannerRRT();
           OMPLPlannerRRT(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

           void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

           bool makePlan(const geometry_msgs::PoseStamped& start,
                  const geometry_msgs::PoseStamped& goal, 
                  std::vector<geometry_msgs::posestamped>& plan);
           ~OMPLPlannerRRT();
       private:
           double footprintCost(double x_i, double y_i, double theta_i);
           bool isStateValid(const ompl::base::State *state);
           bool interpolatePathPose2D(std::vector<geometry_msgs::pose2d>& path);
           void publishPlan(std::vector<geometry_msgs::posestamped> path);
           void OMPLStateSE2ToROSPose2D(const ompl::base::State* ompl_state, geometry_msgs::Pose2D& pose2D);
           void OMPLScopedStateSE2ToROSPose2D(const ompl::base::ScopedState<> scoped_state, geometry_msgs::Pose2D& pose2D);
           void ROSPose2DToOMPLScopedStateSE2(ompl::base::ScopedState<>& scoped_state, const geometry_msgs::Pose2D pose2D);  
           void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D);
           void Pose2DToPose(geometry_msgs::Pose& pose, const geometry_msgs::Pose2D pose2D);
    };
};

Linking CXX shared library /home/ken/catkin_ws/devel/lib/libompl_planner_rrt.so
[100%] Built target ompl_planner_rrt

But, I have no confidence if my program works normally.