coverage
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
zone_map.py
[詳解]
1
"""清掃ゾーン内セルの地図占有率をROS非依存で求める。
2
3
``coverage_bridge`` が ``/map`` (OccupancyGrid) 受信時に使い、
4
静的障害物と重なるゾーンの実行要求を事前に拒否する。
5
座標単位は [m]、セル値は ``-1: unknown、0: free、1..100: occupied度``。
6
"""
7
8
from
__future__
import
annotations
9
10
from
dataclasses
import
dataclass
11
from
typing
import
Sequence
12
13
Point2D = tuple[float, float]
14
15
OCCUPIED_THRESHOLD = 65
16
"""この値以上を占有セルとみなす。地図のoccupied_thresh(0.65)に合わせる。"""
17
18
19
@dataclass(frozen=True)
20
class
ZoneOccupancy
:
21
"""ゾーン内セルの内訳。"""
22
23
inside_cells: int
24
"""中心がゾーン内にあるセル数。"""
25
26
occupied_ratio: float
27
"""占有セルの割合 [0.0, 1.0]。"""
28
29
unknown_ratio: float
30
"""unknownセルの割合 [0.0, 1.0]。unknownは許容し拒否理由にしない。"""
31
32
33
def
point_in_polygon
(x: float, y: float, polygon: Sequence[Point2D]) -> bool:
34
"""レイキャスト法で点の内外を判定する。辺上は内外いずれか(実装依存)。"""
35
inside =
False
36
n = len(polygon)
37
for
i
in
range(n):
38
x0, y0 = polygon[i]
39
x1, y1 = polygon[(i + 1) % n]
40
if
(y0 > y) != (y1 > y):
41
x_cross = x0 + (y - y0) * (x1 - x0) / (y1 - y0)
42
if
x < x_cross:
43
inside =
not
inside
44
return
inside
45
46
47
def
zone_occupancy
(
48
width: int,
49
height: int,
50
resolution: float,
51
origin_x: float,
52
origin_y: float,
53
data: Sequence[int],
54
polygon: Sequence[Point2D],
55
) -> ZoneOccupancy:
56
"""ゾーン内セルを数え、占有率とunknown率を返す。
57
58
Args:
59
width: 地図の幅 [cell]。
60
height: 地図の高さ [cell]。
61
resolution: 解像度 [m/cell]。
62
origin_x: 地図原点x [m]。
63
origin_y: 地図原点y [m]。
64
data: row-majorのセル値列。
65
polygon: ゾーン頂点列 [m](開路・閉路どちらも可)。
66
67
Returns:
68
ゾーン内セルが無い場合は全て0の :class:`ZoneOccupancy`。
69
"""
70
if
not
polygon
or
resolution <= 0:
71
return
ZoneOccupancy
(0, 0.0, 0.0)
72
xs = [p[0]
for
p
in
polygon]
73
ys = [p[1]
for
p
in
polygon]
74
cx0 = max(0, int((min(xs) - origin_x) / resolution))
75
cx1 = min(width - 1, int((max(xs) - origin_x) / resolution))
76
cy0 = max(0, int((min(ys) - origin_y) / resolution))
77
cy1 = min(height - 1, int((max(ys) - origin_y) / resolution))
78
79
inside = occupied = unknown = 0
80
for
cy
in
range(cy0, cy1 + 1):
81
for
cx
in
range(cx0, cx1 + 1):
82
x = origin_x + (cx + 0.5) * resolution
83
y = origin_y + (cy + 0.5) * resolution
84
if
not
point_in_polygon
(x, y, polygon):
85
continue
86
inside += 1
87
v = data[cy * width + cx]
88
if
v < 0:
89
unknown += 1
90
elif
v >= OCCUPIED_THRESHOLD:
91
occupied += 1
92
if
inside == 0:
93
return
ZoneOccupancy
(0, 0.0, 0.0)
94
return
ZoneOccupancy
(
95
inside_cells=inside,
96
occupied_ratio=occupied / inside,
97
unknown_ratio=unknown / inside,
98
)
coverage.zone_map.ZoneOccupancy
ゾーン内セルの内訳。
定義
zone_map.py:20
coverage.zone_map.zone_occupancy
ZoneOccupancy zone_occupancy(int width, int height, float resolution, float origin_x, float origin_y, Sequence[int] data, Sequence[Point2D] polygon)
ゾーン内セルを数え、占有率とunknown率を返す。
定義
zone_map.py:55
coverage.zone_map.point_in_polygon
bool point_in_polygon(float x, float y, Sequence[Point2D] polygon)
レイキャスト法で点の内外を判定する。辺上は内外いずれか(実装依存)。
定義
zone_map.py:33
coverage
zone_map.py
構築:
1.18.0