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 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();
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 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 otheruavs,UAV* currentUAV,og::RRTstar* planner, const ompl::base::State *state)
{
// Cast state type and set values to compare against
const ompl::base::SE2StateSpace::StateType *s = state->as();
double x=s->getX(), y=s->getY();
// Fetch parent motion
og::RRTstar::Motion* parent = planner->getparent();
int newindex = (parent == NULL) ? 1 : parent->node_index();
// 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 capoints = curruav->avoidancepoints;
// Initiate time index
int index = 0;
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);
if (!currresult)
{
currresult = (abs(index-newindex) > TIME_INDEX);
}
result = currresult && result;
index++;
}
}
return result;
}
And the Index Finding function (within the 'motion' class in RRTstar header)
int node_index()
{
int index;
if (parent == NULL)
{
index = 0;
}
else
{
index = 1 + parent->node_index();
}
return index+1;
}
↧