Use ROS2 nav_msgs/OccupancyGrid msg to plan path using OMPL's RRT* implementation
Hi,
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:
https://ompl.kavrakilab.org/geometricPlanningSE3.html
https://github.com/dominikbelter/omplexample2d (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
si->setStateValidityChecker(isStateValid);
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, the robot will move over there. But since, the robot is not point size, it has the possibility to collide with the obstacle because of its size/ geometry.
Thanks in advance,
Dev
Asked by dvy on 2022-12-09 13:10:46 UTC
Comments