1 Star 0 Fork 2

陈狗翔 / PRM-US-path-planning

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
PRM_USS.cpp 7.79 KB
Copy Edit Web IDE Raw Blame History
陈狗翔 authored 2018-01-24 22:02 . PRM US path planning
#include "PRM_USS.h"
#include <stdint.h>
#include "Astar.h"
#include <fstream>
bool PRM_map::if_line(const point a,const point b)
{
int ** walkability = buff;
line.clear();
int dx=abs(a.x-b.x);
int dy=abs(a.y-b.y);
double k;
if (dx != 0)
{
k=double(b.y-a.y)/double(b.x-a.x);
}
else if (dy ==0)
{
k=1;
}
else
{
k=99999;
}
point temp;
if (dx>=dy)
{
if (a.x<b.x)
{
temp =a;
}
else
temp=b;
for (int i=1;i<dx+1;i++)
{
int x=temp.x+i;
int y=temp.y+k*i;
point pt(x,y);
line.push_back(pt);
}
}
else
{
if (a.y<b.y)
{
temp =a;
}
else
temp=b;
for (int i=1;i<dy+1;i++)
{
int x=temp.x+i/k;
int y=temp.y+i;
point pt(x,y);
line.push_back(pt);
}
}
int len=line.size();
for (int i=0;i<len;i++)
{
if (1 == walkability[line[i].x][line[i].y])
{
return false;
}
}
return true;
}
bool PRM_map::collisionCheck(int x,int y)
{
using namespace cv;
bool ok=false;
int xm=sizeX/steplength +1;
int ym=sizeY/steplength;
if ( !(x<sizeX && x>=0 && y<sizeY && y>=0 ) )
{
return false;
}
if (buff[x][y] == 1)
{
p_node *p=new p_node(point(x,y));
allnode.push_back(p);
return false;
}
p_node *p=new p_node(point(x,y));
allnode.push_back(p);
if (x-steplength>=0 && y-steplength >=0)
{
point p1(x,y);
point p2(x-steplength,y-steplength);
if (if_line(p1,p2))
{
int i=(y-steplength) / steplength * xm + (x-steplength) /steplength ;
/*
// if ( ! (p2 == allnode[i]->pose))
// {
// test++;
// std::cout<<p2.x<<","<<p2.y<<" and "<<allnode[i]->pose.x<<","<<allnode[i]->pose.y<<std::endl;
// }
*/
p->neighbors.push_back(allnode[i]);
allnode[i]->neighbors.push_back(p);
circle( img,Point(p1.x,p1.y), 1 , Scalar(255,10,0), 1, 1, 0 );
circle( img,Point(p2.x,p2.y), 1 , Scalar(255,10,0), 1, 1, 0 );
for (int i=0;i<line.size();i++)
{
circle( img,Point(line[i].x,line[i].y), 0.1, Scalar(0,10,255), 0.1, 1, 0 );
}
}
ok=true;
}
if (x-steplength >=0)
{
point p1(x,y);
point p2(x-steplength,y);
if (if_line(p1,p2))
{
int i=(y) / steplength * xm + (x-steplength) /steplength ;
/*
// if ( ! (p2 == allnode[i]->pose))
// {
// test++;
// std::cout<<p2.x<<","<<p2.y<<" and "<<allnode[i]->pose.x<<","<<allnode[i]->pose.y<<std::endl;
// }
*/
p->neighbors.push_back(allnode[i]);
allnode[i]->neighbors.push_back(p);
circle( img,Point(p1.x,p1.y), 1 , Scalar(255,10,0), 1, 1, 0 );
circle( img,Point(p2.x,p2.y), 1 , Scalar(255,10,0), 1, 1, 0 );
for (int i=0;i<line.size();i++)
{
circle( img,Point(line[i].x,line[i].y), 0.1, Scalar(0,10,255), 0.1, 1, 0 );
}
}
ok=true;
}
if (y-steplength >=0)
{
point p1(x,y);
point p2(x,y-steplength);
if (if_line(p1,p2))
{
int i=(y-steplength) / steplength * xm + (x) /steplength ;
/*
// if ( ! (p2 == allnode[i]->pose))
// {
// test++;
// std::cout<<p2.x<<","<<p2.y<<" and "<<allnode[i]->pose.x<<","<<allnode[i]->pose.y<<std::endl;
// }
*/
p->neighbors.push_back(allnode[i]);
allnode[i]->neighbors.push_back(p);
circle( img,Point(p1.x,p1.y), 1 , Scalar(255,10,0), 1, 1, 0 );
circle( img,Point(p2.x,p2.y), 1 , Scalar(255,10,0), 1, 1, 0 );
for (int i=0;i<line.size();i++)
{
circle( img,Point(line[i].x,line[i].y), 0.1, Scalar(0,10,255), 0.1, 1, 0 );
}
}
ok=true;
}
if (x+steplength < sizeX && y-steplength >=0)
{
point p1(x,y);
point p2(x+steplength,y-steplength);
if (if_line(p1,p2))
{
int i=(y-steplength) / steplength * xm + (x+steplength) /steplength ;
/*
// if ( ! (p2 == allnode[i]->pose))
// {
// test++;
// std::cout<<p2.x<<","<<p2.y<<" and "<<allnode[i]->pose.x<<","<<allnode[i]->pose.y<<std::endl;
// }
*/
p->neighbors.push_back(allnode[i]);
allnode[i]->neighbors.push_back(p);
circle( img,Point(p1.x,p1.y), 1 , Scalar(255,10,0), 1, 1, 0 );
circle( img,Point(p2.x,p2.y), 1 , Scalar(255,10,0), 1, 1, 0 );
for (int i=0;i<line.size();i++)
{
circle( img,Point(line[i].x,line[i].y), 0.1, Scalar(0,10,255), 0.1, 1, 0 );
}
}
ok=true;
}
return ok;
}
int PRM_map::getDistance(const point p1,const point p2)
{
return (int)sqrt(double((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)));
}
double PRM_map::getDistance(int x1,int y1,int x2,int y2)
{
return (double)sqrt(double((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)));
}
bool PRM_map::getPath( point start_, point goal_ )
{
using namespace cv;
Astar pathfinder;
p_node * s=NULL;
p_node * g=NULL;
int dis=99999;
for (int i=0;i<allnode.size();i++)
{
int d = getDistance(allnode[i]->pose,goal_);
if (dis > d )
{
dis = d;
g=allnode[i];
}
}
dis=99999;
for (int i=0;i<allnode.size();i++)
{
int d = getDistance(allnode[i]->pose,start_);
if (dis > d )
{
dis = d;
s=allnode[i];
}
}
//std::cout<<test<<std::endl;
if ( (g==NULL) || !(if_line(g->pose,goal_)) )
{
std::cout<<"path find failed!"<<std::endl;
return false;
}
//circle( img,Point(goal_.x,goal_.y), 1, Scalar(0,0,0), 3, 1, 0 );
for (int i=0;i<line.size();i++)
{
//circle( img,Point(line[i].x,line[i].y), 1, Scalar(255,10,255), 3, 1, 0 );
}
//cv::line(img,Point(line[0].x,line[0].y),Point(line[line.size()-1].x,line[line.size()-1].y),Scalar(255,0,0),2);
if ( (s==NULL) || !(if_line(s->pose,start_)) )
{
std::cout<<"path find failed!"<<std::endl;
return false;
}
//circle( img,Point(start_.x,start_.y), 1, Scalar(0,0,0), 3, 1, 0 );
for (int i=0;i<line.size();i++)
{
//circle( img,Point(line[i].x,line[i].y), 1, Scalar(255,10,255), 3, 1, 0 );
}
//cv::line(img,Point(line[0].x,line[0].y),Point(line[line.size()-1].x,line[line.size()-1].y),Scalar(255,0,0),2);
pathfinder.getPath(s,g);
if (pathfinder.path.size() == 0)
{
std::cout<<"path find failed!"<<std::endl;
return false;
}
else
{
//circle( img,Point(pathfinder.path[0]->pose.x,pathfinder.path[0]->pose.y), 3, Scalar(0,0,0), 3, 1, 0 );
//circle( img,Point(pathfinder.path[pathfinder.path.size()-1]->pose.x,pathfinder.path[pathfinder.path.size()-1]->pose.y), 3, Scalar(0,0,0), 3, 1, 0 );
double distan = 0.0;
for (int i=0;i<pathfinder.path.size()-1;i++)
{
path.push_back(pathfinder.path[i]);
cv::line(img,Point(pathfinder.path[i]->pose.x,pathfinder.path[i]->pose.y),Point(pathfinder.path[i+1]->pose.x,pathfinder.path[i+1]->pose.y),Scalar(255,0,0),2);
distan += getDistance(pathfinder.path[i]->pose.x,pathfinder.path[i]->pose.y,pathfinder.path[i+1]->pose.x,pathfinder.path[i+1]->pose.y);
}
std::cout << " total distance : " << distan << std::endl;
return true;
}
}
void PRM_map::showResult(const char * filename)
{
cv::namedWindow("map");
cv::imshow("map",img);
cv::imwrite("map.jpg",img);
cv::waitKey();
using namespace cv;
using namespace std;
// namedWindow("map");
//imshow("map",img);
//imwrite("map_RSS.jpg",img);
ofstream out;
out.open(filename,ios::out);
if (out.is_open())
{
//char a;
//in>>a;
for (int i=0;i<path.size();i++)
{
out<<path[i]->pose.x<<" "<<path[i]->pose.y<<"\n";
}
out.close();
}
}
PRM_map::PRM_map(const char * filename):map(filename),steplength(20) //steplength ķֶ
{
img=cv::imread(filename);
test=0;
if (isCreated = map.transformMapToGrid())
{
sizeX=map.getSizeX();
sizeY=map.getSizeY();
if (sizeX % steplength == 0 && sizeX > 0)
{
sizeX --;
}
if (sizeY %steplength ==0 && sizeY >0)
{
sizeY --;
}
/* graph = new int *[sizeX / steplength +1 ];
for (int i=0;i<sizeX / steplength +1;i++)
{
graph[i]=new int [sizeY / steplength +1 ];
}
*/
buff=map.getGridMap2D();
int xt=0;
int yt=0;
while( (yt ) < sizeY)
{
xt=0;
while((xt ) < sizeX)
{
//graph[xt / steplength][yt / steplength]=buff[xt][yt];
if (collisionCheck(xt,yt) )
{
//graph[xt / steplength][yt / steplength] = 0;
}
else
{
}
//graph[xt / steplength][yt / steplength] = 1;
xt+= steplength;
}
yt+= steplength;
}
}
}

Comment ( 0 )

Sign in for post a comment

C++
1
https://gitee.com/ChenGouXiang/PRM-US-path-planning.git
git@gitee.com:ChenGouXiang/PRM-US-path-planning.git
ChenGouXiang
PRM-US-path-planning
PRM-US-path-planning
master

Search