Use ROS2 nav_msgs/OccupancyGrid msg to plan path using OMPL's RRT* implementation

asked 2022-12-09 12:10:46 -0500

dvy gravatar image

updated 2022-12-09 13:11:54 -0500


System Details: ROS2 foxy on Ubuntu 20.04 focal on amd64 architecture.

I have created my node which can publish a nav_msgs/OccupancyGrid message on the topic /map which takes care of the free and occupied cells. My idea is to use this information to plan the path of a robot by providing it with a start and end goal location using a sample-based planner (which I think RRT* should work well). For this, I have created a C++ subscriber for the map topic and I want to do further processing inside the callback function. For this, I tried to look upon some example codes on RRT (any similar planner algorithm's OMPL's implementation) and came across these two links but have a few questions as explained below.

References which I am using: (This example uses the Without ompl::geometric::SimpleSetup implementation in the kavrakilab's link)

From the ompl_example_2d repo, I understand the following:

1) ompl_example_2d_node runs first which instantiates an object of the class Planner2D. Then the constructor gets executed which calls the configure function. 2) Whenever the node listens to the /map topic, the callback function mapCallback gets executed, the message gets stored in globalMap, and it is passed as an argument to the planPath function. 3) In ompl_example_2d.cpp where the function planPath is defined, this message again gets stored as occupancyMap. Fine.

I know the theoretical basics of RRT/ similar algorithms. But don't know how are they using the isStateValid function. The kavraki lab's link mentions below.

And a state validity checking function defined like this: bool isStateValid(const ob::State *state)

From this, I build up an understanding that the function isStateValid should have an input state which should check something like: Is the current state which is being sampled overlapping with any obstacle or not? Is it feasible to move the robot there or not?

But in the code, no argument is provided to the isStateValid function. Is it even valid? But it should be because the authors have also written like this. Is it a fancy way to do stuff in C++? What is this way called? What should I read to understand it better?

// search space information
auto si(std::make_shared<ompl::base::SpaceInformation>(space));
// define state checking callback

Now the task is to compare the state with the occupancy grid.

My question:
1) For this, do I need to write something on my own for the comparison or does OMPL have something implemented already?
2) Is there any way to handle the robot's footprint/ inflation radius while comparing and planning the path? I am asking this question because I imagine a corner scenario.

Let's say that the algorithm picks up a state in 2-D (R^2) that is close to an obstacle but it's not overlapping with it. If this state turns out to be a part of the solved path ...

edit retag flag offensive close merge delete