Ask Your Question
0

How to write a code with OMPL -Part1-

asked 2014-06-10 00:28:44 -0500

Ken_in_JAPAN gravatar image

updated 2014-10-24 13:52:43 -0500

ahendrix gravatar image

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.

edit retag flag offensive close merge delete

Comments

Anyone is about to tell me to study C++.

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-10 13:28:51 -0500 )edit

Have you declared "costmap_ros_" variable? I guess that variable is a class variable.

alfa_80 gravatar image alfa_80  ( 2014-06-10 14:06:13 -0500 )edit

Thanks @alfa_80, I don't understand costmap_2d. I know that It consists of Layered costmap. But I don't know how to take out cost from costmap. I'm checking it.

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-10 14:33:55 -0500 )edit

I quote a sample code GeomPlanningSE2_PRM.cpp on the web( http://robotics.naist.jp/edu/text/?Robotics%2FOMPL#OMPLProgramming ). isStateValid in this cpp just sets free space and obstacle region. That means this doesn't have a function to check if free space is.

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-10 19:30:04 -0500 )edit

I guess that isStateValid() is necessary to store information of costmap. I need to know the way to transfer from costmap to statespace of OMPL format.

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-10 20:28:19 -0500 )edit

I deleted my answer, because anyone denied my answer. I know the reason that I intend to use a code with a bug. I want to know an essential answer to move a turtlebot with OMPL.

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-15 11:37:49 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2014-06-10 14:11:26 -0500

osrf gravatar image

Your reference to costmap_ros_ variable has not been declared. The format suggests it's a member variable, so if you're moving it to a different class that may be a problem.

edit flag offensive delete link more

Comments

Thanks @osrf, I replaced isStateValid( ) to ompl_planner_rrt::OMPLPlannerRRT::isStateValid( ). So, error message has gone out.

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-10 14:30:30 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-06-10 00:28:44 -0500

Seen: 846 times

Last updated: Jun 18 '14