1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
| double planner::distance(double x1,double y1,double x2,double y2)
{
return sqrt( (x1-x2) * (x1-x2) + (y1-y2) * (y1-y2) );
}
bool planner::isAroundFree(unsigned int mx, unsigned int my)
{
if(mx <= 1 || my <= 1 || mx >= this->costmap_->getSizeInCellsX()-1 || my >= this->costmap_->getSizeInCellsY()-1)
return false;
int x,y;
for(int i=-1;i<=1;i++)
{
for(int j=-1;j<=1;j++)
{
x = static_cast<int>(mx) + i;
y = static_cast<int>(my) + j;
if(this->costmap_->getCost(static_cast<unsigned int>(x),static_cast<unsigned int>(y)) != costmap_2d::FREE_SPACE)
return false;
}
}
return true;
}
void planner::getNearFreePoint(const geometry_msgs::PoseStamped in,
geometry_msgs::PoseStamped& out,
double tolerance)
{
out = in;
unsigned int grid_size = static_cast<unsigned int>(tolerance/costmap_->getResolution() + 0.5);
if(grid_size<1)
{
out = in;
return;
}
unsigned int mx0,my0;
if(costmap_->worldToMap(in.pose.position.x,in.pose.position.y,mx0,my0))
{
if(this->isAroundFree(mx0,my0))
return;
unsigned int minx,maxx,miny,maxy;
double wx = 0.0,wy = 0.0;
double min_move_cost = 10000000.0;
minx = mx0-grid_size>0?mx0-grid_size:0;
maxx = mx0+grid_size<costmap_->getSizeInCellsX()?mx0+grid_size:costmap_->getSizeInCellsX();
miny = my0-grid_size>0?my0-grid_size:0;
maxy = my0+grid_size<costmap_->getSizeInCellsY()?my0+grid_size:costmap_->getSizeInCellsY();
for(unsigned int i=minx;i<=maxx;i++)
{
for(unsigned int j=miny;j<=maxy;j++)
{
costmap_->mapToWorld(i,j,wx,wy);
double current_move_cost = this->distance(in.pose.position.x,in.pose.position.y,wx,wy);
if(!this->isAroundFree(i,j) || current_move_cost > tolerance)
continue;
if(min_move_cost > current_move_cost)
{
min_move_cost = current_move_cost;
out.pose.position.x = wx;
out.pose.position.y = wy;
}
}
}
}
}
|
