1 | bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value) |
convexFillCells
这个函数用于获取多边形边缘及内部cell1
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
69
70
71
72
73void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
// we need a minimum polygon of a triangle
if (polygon.size() < 3)
return;
// first get the cells that make up the outline of the polygon
//首先获得轮廓点之间连线的列表，存放在polygon_cells中
polygonOutlineCells(polygon, polygon_cells);
// quick bubble sort to sort points by x
MapLocation swap;
unsigned int i = 0;
// 对边上的cell点的x做排序，使其按x坐标升序排列
while (i < polygon_cells.size() - 1)
{
if (polygon_cells[i].x > polygon_cells[i + 1].x)
{
swap = polygon_cells[i];
polygon_cells[i] = polygon_cells[i + 1];
polygon_cells[i + 1] = swap;
if (i > 0)
--i;
}
else
++i;
}
/* 遍历所有x，对每个相同的x，检查y，获得y最大和最小的polygon cell，
将范围内的所有cell填充进polygon_cells，获得多边形边缘及内部的所有cell */
i = 0;
MapLocation min_pt;
MapLocation max_pt;
unsigned int min_x = polygon_cells[0].x;
unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
// walk through each column and mark cells inside the polygon
for (unsigned int x = min_x; x <= max_x; ++x)
{
if (i >= polygon_cells.size() - 1)
break;
if (polygon_cells[i].y < polygon_cells[i + 1].y)
{
min_pt = polygon_cells[i];
max_pt = polygon_cells[i + 1];
}
else
{
min_pt = polygon_cells[i + 1];
max_pt = polygon_cells[i];
}
i += 2;
while (i < polygon_cells.size() && polygon_cells[i].x == x)
{
if (polygon_cells[i].y < min_pt.y)
min_pt = polygon_cells[i];
else if (polygon_cells[i].y > max_pt.y)
max_pt = polygon_cells[i];
++i;
}
MapLocation pt;
// loop though cells in the column
for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
{
pt.x = x;
pt.y = y;
polygon_cells.push_back(pt);
}
}
}
polygonOutlineCells
循环调用raytraceLine函数，不断获取相邻之间的连线，最终组成多边形边上的cell，需要注意的是需要将最后一点和第一点连接起来，形成闭合。1
2
3
4
5
6
7
8
9
10
11
12
13
14void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
for (unsigned int i = 0; i < polygon.size() - 1; ++i)
{
raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
}
if (!polygon.empty())
{
unsigned int last_index = polygon.size() - 1;
// we also need to close the polygon by going from the last point to the first
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
}
}
raytraceLine
找到两点连线上的cell。对于离散的平面点，指定两个点，这个函数可以找到两个点之间的其他点，使得这些中间组成一个尽可能趋近直线的点集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
31template<class ActionType> inline void raytraceLine(ActionType at,
unsigned int x0, unsigned int y0,
unsigned int x1, unsigned int y1,
unsigned int max_length = UINT_MAX )
{
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = sign(dx);
int offset_dy = sign(dy) * size_x_;
unsigned int offset = y0 * size_x_ + x0;
// we need to chose how much to scale our dominant dimension, based on the maximum length of the line
double dist = hypot(dx, dy);
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
// if x is dominant
if (abs_dx >= abs_dy)
{
int error_y = abs_dx / 2;
bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
return;
}
// otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}
