Ask Your Question
0

Question about collision avoidance in OMPL motion planning library

asked 2012-06-29 09:45:03 -0500

megan_starr9 gravatar image

updated 2014-01-28 17:12:51 -0500

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Hey, figured it out! This can be closed ^^

megan_starr9 gravatar imagemegan_starr9 ( 2012-07-02 09:38:38 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2012-10-25 05:07:19 -0500

isucan gravatar image

I presume the issue has been resolved in the meantime, but if not, please send this question on the OMPL mailing list.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2012-06-29 09:45:03 -0500

Seen: 960 times

Last updated: Oct 25 '12