coverage
読み取り中…
検索中…
一致する文字列を見つけられません
zone_map.py
[詳解]
1"""清掃ゾーン内セルの地図占有率をROS非依存で求める。
2
3``coverage_bridge`` が ``/map`` (OccupancyGrid) 受信時に使い、
4静的障害物と重なるゾーンの実行要求を事前に拒否する。
5座標単位は [m]、セル値は ``-1: unknown、0: free、1..100: occupied度``。
6"""
7
8from __future__ import annotations
9
10from dataclasses import dataclass
11from typing import Sequence
12
13Point2D = tuple[float, float]
14
15OCCUPIED_THRESHOLD = 65
16"""この値以上を占有セルとみなす。地図のoccupied_thresh(0.65)に合わせる。"""
17
18
19@dataclass(frozen=True)
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
33def 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
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 )
ゾーン内セルの内訳。
ZoneOccupancy zone_occupancy(int width, int height, float resolution, float origin_x, float origin_y, Sequence[int] data, Sequence[Point2D] polygon)
ゾーン内セルを数え、占有率とunknown率を返す。
bool point_in_polygon(float x, float y, Sequence[Point2D] polygon)
レイキャスト法で点の内外を判定する。辺上は内外いずれか(実装依存)。