Question about collision avoidance in OMPL motion planning library

edit

Hello!

I am pretty new to the object-oriented C++ scene, but currently I am working on a project that has us planning paths for multiple UAVs on a single field. We are supposed to have the UAVs reach waypoints without running into one another. I am currently utilizing the RRT* Planner to draw waypoint routes from the UAV's starting position to the goal within the field.

I was wondering, however, if anyone here had experience with trying to implement collision avoidance in a real time environment within the OMPL library. I am having a heck of a time trying to figure out how to pass parameters between the RRT* class and my own code.

Currently my state validity checking function looks like this

    bool isStateValid(std::vector<UAV*> otheruavs,UAV* currentUAV, const ompl::base::State *state)
{
    // Cast state type and set values to compare against
    const ompl::base::SE2StateSpace::StateType *s = state->as<ompl::base::SE2StateSpace::StateType>();
    double x=s->getX(), y=s->getY();

    // set boolean accumulation variable
    bool result = true;

    // Fetch Array
    // Compare against array
    for (unsigned int i=0; i < otheruavs.size(); i++)
    {
    UAV* curruav = otheruavs.at(i);
    std::vector<Point> capoints = curruav->avoidancepoints;
        for(unsigned int j=0; j < capoints.size(); j++)
        {
        Point currpoint = capoints.at(j);
        double currx = currpoint.getX();
        double curry = currpoint.getY();

        double dist = sqrt((x-currx)*(x-currx)+(y-curry)*(y-curry));
        bool currresult = (dist > BUFFER_ZONE);

        result = currresult && result;
        }
    }

    return result;
}

Basically it takes the current UAV and the other UAVs within the field. It runs through the other UAV's avoidance points and makes sure that it is not placing a point within a certain range of any of the other uav's.

I was hoping to somehow implement a timing function into this code, however, so that the position being verified is only checked against those positions of the other uavs around the same time index. I cannot figure out how to do this, though, with the setup of the OMPL framework. I realize this is sort of vague, but I don't know how to explain this further. If anyone thinks they can help with some more information, I'd be glad to give whatever you may want to see.

Another issue is to do with bearings. I would love to be able to limit the path so that it took into account the optimal turning angle of the UAV. Any insight on how to do this?

((I'm sorry that this isn't really ROS related, but it is being implemented within ROS, and I did not know where else to ask this question ><))

EDIT: Answered my own question! I introduced a new parameter to the RRTstar class named 'parent' and set the parent of whatever motion I am checking as that parameter. I then iterate through the parents of the motion to count what it's index is within the tree. It was a fairly easy fix ^^. New, fixed code

bool isStateValid(std::vector<UAV*> otheruavs,UAV* currentUAV,og::RRTstar* planner ...