Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Simplify RRT Connect #6

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Simplify RRT Connect #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8f10359
Avoid obstacles
americast Sep 30, 2017
28bf0eb
Simplification
americast Sep 30, 2017
574d789
Match original RRT with modified class
americast Sep 30, 2017
3e21029
Avoid obstacles & simplify path for simple RRT
americast Sep 30, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 8 additions & 3 deletions include/RRT.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,10 @@ namespace rrt
Utils::Point<T> startPoint;
Utils::Point<T> endPoint;
double stepLength;
double obstacleradius;
std::deque<Utils::Point<T> > pathPoints;
std::vector<Utils::Point<T> > finalPath;
std::vector<Utils::Point<T> > ObstaclePoints;
int maxIterations;
std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree;
std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree1;
Expand All @@ -35,7 +38,7 @@ namespace rrt
}

virtual bool plan();
virtual std::deque<Utils::Point<T> > getPointsOnPath();
virtual std::vector<Utils::Point<T> > getPointsOnPath();

virtual void setEndPoints(Utils::Point<T> start, Utils::Point<T> end);
virtual void setCheckPointFunction(bool (*f)(Utils::Point<T>));
Expand All @@ -44,22 +47,24 @@ namespace rrt
virtual void setHalfDimensions(double x,double y);
virtual void setBiasParameter(unsigned int);
virtual void setMaxIterations(int);
virtual void setObstacleRadius(int);
//TODO : To be implemented in the derived classes
// virtual void interpolate();
// virtual void fitVelocityProfile();
// virtual void pruneTree();

private:
bool (*userCheck)(Utils::Point<T>);
bool checkPoint(Utils::Point<T> pt);
bool checkPoint(Utils::Point<T>, Utils::Point<T>);
Utils::Point<T> generatePoint();
Utils::Point<T> generateBiasedPoint(int);
void growTree(Utils::Point<T>);
void growTree(std::pair<Utils::Point<T>,int>,Utils::Point<T>);
std::pair<Utils::Point<T>, int> findClosestNode(Utils::Point<T>);
Utils::Point<T> getParent(Utils::Point<T>);
std::pair <Utils::Point <T>,Utils::Point <T> > treeComplete(int*);
void generatePath(Utils::Point<T> first,Utils::Point<T> last);
double dist(Utils::Point<T> a,Utils::Point<T> b);
bool obstacle_here(int, int);
};
}

Expand Down
132 changes: 119 additions & 13 deletions include/RRTConnect_implementation.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,24 @@ namespace rrt
}

template <class T>
std::deque<Utils::Point<T> > RRT<T>::getPointsOnPath()
std::vector<Utils::Point<T> > RRT<T>::getPointsOnPath()
{
return pathPoints;
Utils::Point<T> parent = pathPoints[0];
finalPath.push_back(parent);
for (int i=1; i<pathPoints.size();i++)
{
Utils::Point<T> now = pathPoints[1];
if(checkPoint(parent,now)!=true)
{
parent = pathPoints[i-1];
i--;
if (i!=0)
finalPath.push_back(parent);
}

}
finalPath.push_back(pathPoints[pathPoints.size()-1]);
return finalPath;
}

template <class T>
Expand All @@ -42,6 +57,12 @@ namespace rrt
maxIterations=value;
}

template <class T>
void RRT<T>::setObstacleRadius(int value)
{
obstacleradius=value;
}

template <class T>
bool RRT<T>:: plan()
{
Expand All @@ -50,9 +71,10 @@ namespace rrt
int check=0;
tree1.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (startPoint,startPoint));
tree2.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (endPoint,endPoint));
std::pair < Utils::Point<T> , int > both;
while( check < maxIterations)
{
//std::cout<<"In Planning Loop"<<std::endl;
// std::cout<<"In Planning Loop"<<std::endl;
Utils::Point<T> next;
int flag[]={0};
std::pair <Utils::Point<T>, Utils::Point<T> > mid = treeComplete(flag);
Expand All @@ -68,24 +90,29 @@ namespace rrt
}
else if(count%biasParameter==0)
{
//std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
// std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
count=0;
do{
next= generateBiasedPoint(which);
which*=-1;
}while(checkPoint(next)!=true);
both= findClosestNode(next);
// std::cout<<"Case 1: "<<checkPoint(both.first, next);

}while(checkPoint(both.first, next)!=true);
//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
}
else
{
//std::cout<<"Adding next point to tree!!"<<std::endl;
// std::cout<<"Adding next point to tree!!"<<std::endl;
do{
next = generatePoint();
}while(checkPoint(next)!=true);
both= findClosestNode(next);
// std::cout<<"Case 2: "<<checkPoint(both.first, next);
}while(checkPoint(both.first,next)!=true);
//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
}
//std::cout<<" Growing Tree next : "<<next.x<<","<<next.y<<std::endl;
growTree(next);
growTree(both,next);
count++;
check++;
//std::cout<<"check= "<<check<<", count= "<<count<<std::endl;
Expand All @@ -95,19 +122,18 @@ namespace rrt
}

template <class T>
void RRT<T>::growTree(Utils::Point<T> next)
void RRT<T>::growTree(std::pair < Utils::Point<T>, int > both,Utils::Point<T> next)
{
//growing the tree by adding node
//std::cout<<"finding parent in tree of size = "<<tree.size()<<std::endl;
std::pair < Utils::Point<T> , int > both= findClosestNode(next);
// std::pair < Utils::Point<T> , int > both= findClosestNode(next);
Utils::Point<T> parent=both.first;
int which = both.second;
//std::cout<<"current : "<<next.x<<","<<next.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;
if (which)
tree2.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (next,parent));
else
tree1.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (next,parent));

//std::cout<<"Tree grown"<<std::endl;
//add pruning code to keep size of tree under control
}
Expand Down Expand Up @@ -199,10 +225,90 @@ namespace rrt
return next;
}

// float dist(int x1, int y1, int x2, int y2)
// {
// return sqrt(pow((x1-x2),2)+pow((y1-y2),2));
// }

template <class T>
bool RRT<T>::checkPoint(Utils::Point<T> next)
bool RRT<T>::obstacle_here(int x, int y)
{
return userCheck(next);
for (int i=0;i<ObstaclePoints.size();i++)
{
Utils::Point<T> pratham;
pratham.x = x; pratham.y = y;
Utils::Point<T> dwitiya;
dwitiya.x = ObstaclePoints[i].x; dwitiya.y = ObstaclePoints[i].y;
if (dist(pratham,dwitiya)<obstacleradius)
return true;
}
return false;
}

template <class T>
bool RRT<T>::checkPoint(Utils::Point<T> parent, Utils::Point<T> next)
{
int x1=parent.x, x2=next.x, y1=parent.y,y2=next.y;
float x=x1, count;
try
{
// std::cout<<"\nIn try ";
// std::cout<<x1<<std::endl;
// std::cout<<x2<<std::endl;
// std::cout<<y1<<std::endl;
// std::cout<<y2<<std::endl;

int m=float(y2-y1)/(x2-x1);
if (m==0) throw 20;
int c=y2-m*x2;
count = fabs(1.0/m);
// while(1)
if (count>1) count=1;
if (count<-1) count=-1;
if (x2<x1) count*=-1;
// std::cout<<"\nm is "<<m<<" and count is: "<<count<<"\n";
while (1)
{
x+=count;
int y=m*x+c;
if ((count>0 and x>=x2) || (count<0 and x<=x2))
{
// std::cout<<"Return true from try\n";
return true;
}
else
{
// std::cout<<"\nm is "<<m<<" and count is: "<<count<<"\n";
// std::cout<<std::endl<<"x: "<<x<<" x2: "<<x2<<std::endl;
// std::cout<<"count: "<<count<<std::endl;
}
if (obstacle_here(x,y))
{
// std::cout<<"Return false from try\n";
return false;
}
}
}
catch(int e)
{
count=1;
int y=y1;
if (y2<y1) count*=-1;
while (1)
{
y+=count;
if ((count>0 and y>=y2) || (count<0 and y<=y2))
{
// std::cout<<"Return true from catch\n";
return true;
}
if (obstacle_here(x,y))
{
// std::cout<<"Return false from catch\n";
return false;
}
}
}
}

template <class T>
Expand Down
Loading