Example code for ROS2 foxy

[devvaibhav455]

Hi,

I want to use OMPL RRT/ RRT*/ any planner to find the path for my robot. I am using ROS2 foxy on Ubuntu 20.04.05 LTS focal on amd64 architecture. I have a robot and its occupancy grid message (nav_msgs::msg::OccupancyGrid) published over a topic. I could not find any example on OMPL's website to implement this. Luckily I found your code but having difficulty understanding it mostly because of my lack of proficiency in understanding C++ codes when multiple includes are done. I know you must be very busy with your work and life but would it be possible for you to port this code to ROS2 or if not, help explain with help of block diagrams/ simple paragraphs? I want to understand the flow of execution.

Thanks in advance, Dev

[dominikbelter]

Dear Dev, I'm going to migrate to ROS2 Humble and port this code in March 2023. Maybe I'll have some time to do this earlier. It shouldn't be difficult. Most of the work is related to launch scripts and a new way of creating publishers and subscribers in ROS. The part of the code related to OMPL should remain unchanged. Best, Dominik

[devvaibhav455]

Dear Dominik,

I understood the working of the code. Thanks for putting it all together.

I wanted to know that for isStateValid function, if I want to make it work with ROS occupancy grid message, do I need to write something on my own for the comparison or does OMPL have something implemented already?

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.

[dominikbelter]

isStateValid method checks the motion constraints. You can check there if the cell below the robot is occupied:

/// check if the current state is valid
bool isStateValid(const ob::State *state){
    // get x coord of the robot
    const auto *coordX =
            state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(0);
    // get y coord of the robot
    const auto *coordY =
            state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
    // compute robot position in the map frame
    double xInMap = coordX->values[0] - occupancyMap.info.origin.position.x;
    double yInMap = coordY->values[0] - occupancyMap.info.origin.position.y;
    // determine index of the cell
    int cellX = int(xInMap/occupancyMap.info.resolution);
    int cellY = int(yInMap/occupancyMap.info.resolution);
    // check occupancy
    if (cellX>0&&cellY>0&&cellX<occupancyMap.info.width&&cellY<occupancyMap.info.height){
        if (occupancyMap.data[cellX+cellY*occupancyMap.info.width]<50&&occupancyMap.data[cellX+cellY*occupancyMap.info.width]>=0)
            return true;
        else
            return false;
    }
    else
        return false;
    //! Your code goes above
    return true;
} 

Obviously you should modify the code if your robot is larger than a single cell.

[dominikbelter]

Dear Dev, current "main" branch contains code that works in ROS2 Humble. Best, Dominik

[devvaibhav455]

Thanks, Dominik, Will try to run it when I get a chance.