ble_server
読み取り中…
検索中…
一致する文字列を見つけられません
test_converter.py
[詳解]
1"""JSONペイロードの検証とROSメッセージへの変換契約を検証する。
2
3Dispatcher経由の振り分け(cleaning_zone / initial_pose)と、
4各コンバーターの検証ルールを対象とする。ROS導入済み環境で実行すること。
5"""
6
7import json
8import math
9
10from ble_server.converter import (
11 DispatcherConverter,
12 JsonInitialPoseConverter,
13 JsonPolygonConverter,
14 _has_self_intersection,
15 _signed_area,
16 build_default_dispatcher,
17)
18
19
20def _dispatch(payload: dict, **kwargs):
21 dispatcher = build_default_dispatcher(**kwargs)
22 return dispatcher.dispatch(json.dumps(payload).encode("utf-8"))
23
24
25def _square_zone(**overrides):
26 data = {
27 "type": "cleaning_zone",
28 "frame_id": "map",
29 "polygon": [
30 {"x": 0.0, "y": 0.0},
31 {"x": 2.0, "y": 0.0},
32 {"x": 2.0, "y": 1.0},
33 {"x": 0.0, "y": 1.0},
34 ],
35 }
36 data.update(overrides)
37 return data
38
39
40# --- dispatcher ---
41
43 dispatcher = build_default_dispatcher()
44 assert set(dispatcher.registered_types) == {"cleaning_zone", "initial_pose"}
45
46
48 assert _dispatch(_square_zone(type="other")) is None
49
50
52 data = _square_zone()
53 del data["type"]
54 dispatcher = build_default_dispatcher()
55 assert dispatcher.dispatch(json.dumps(data).encode("utf-8")) is None
56
57
59 dispatcher = build_default_dispatcher()
60 assert dispatcher.dispatch(b"") is None
61 assert dispatcher.dispatch(b"not-json") is None
62 assert dispatcher.dispatch(json.dumps([1, 2]).encode("utf-8")) is None
63
64
65# --- cleaning_zone ---
66
68 result = _dispatch(_square_zone())
69 assert result is not None
70 assert result.topic == "/cleaning_zone"
71 assert result.message.header.frame_id == "map"
72 assert len(result.message.polygon.points) == 4
73 assert result.message.polygon.points[0].x == 0.0
74 assert result.message.polygon.points[1].x == 2.0
75
76
78 data = _square_zone(polygon=[{"x": 0.0, "y": 0.0}, {"x": 1.0, "y": 1.0}])
79 assert _dispatch(data) is None
80
81
83 dispatcher = build_default_dispatcher()
84 payload = b'{"type": "cleaning_zone", "polygon": [{"x": NaN, "y": 1.0}, {"x": 2.0, "y": 1.0}, {"x": 2.0, "y": 2.0}]}'
85 assert dispatcher.dispatch(payload) is None
86
87
89 data = _square_zone(polygon=[
90 {"x": 0.0, "y": 0.0},
91 {"x": 99999.0, "y": 0.0},
92 {"x": 0.0, "y": 10.0},
93 ])
94 assert _dispatch(data, max_coord_abs_val=100.0) is None
95
96
98 data = _square_zone(polygon=[
99 {"x": 0.0, "y": 0.0},
100 {"x": 2.0, "y": 0.0},
101 {"x": 2.0, "y": 0.0},
102 {"x": 0.0, "y": 1.0},
103 ])
104 assert _dispatch(data) is None
105
106
108 # 閉路表現(末尾が始点と一致)は正規化され、開路4頂点として配信される。
109 data = _square_zone(polygon=[
110 {"x": 0.0, "y": 0.0},
111 {"x": 2.0, "y": 0.0},
112 {"x": 2.0, "y": 1.0},
113 {"x": 0.0, "y": 1.0},
114 {"x": 0.0, "y": 0.0},
115 ])
116 result = _dispatch(data)
117 assert result is not None
118 assert len(result.message.polygon.points) == 4
119
120
122 data = _square_zone(polygon=[
123 {"x": 0.0, "y": 0.0},
124 {"x": 2.0, "y": 0.0},
125 {"x": 2.0, "y": 0.0},
126 {"x": 0.0, "y": 1.0},
127 {"x": 0.0, "y": 0.0},
128 ])
129 assert _dispatch(data) is None
130
131
133 data = _square_zone(polygon=[
134 {"x": 0.0, "y": 0.0},
135 {"x": 0.001, "y": 0.0},
136 {"x": 0.0, "y": 0.001},
137 ])
138 assert _dispatch(data) is None
139
140
142 data = _square_zone(polygon=[
143 {"x": 0.0, "y": 0.0},
144 {"x": 2.0, "y": 2.0},
145 {"x": 0.0, "y": 2.0},
146 {"x": 2.0, "y": 0.0},
147 ])
148 assert _dispatch(data) is None
149
150
152 result = _dispatch(_square_zone(), cleaning_zone_topic="/zone2")
153 assert result is not None
154 assert result.topic == "/zone2"
155
156
157# --- initial_pose ---
158
160 result = _dispatch({"type": "initial_pose", "frame_id": "map",
161 "x": 1.0, "y": 2.0, "yaw": 0.0})
162 assert result is not None
163 assert result.topic == "/initialpose"
164 msg = result.message
165 assert msg.header.frame_id == "map"
166 assert msg.pose.pose.position.x == 1.0
167 assert msg.pose.pose.position.y == 2.0
168 # yaw=0 -> 単位クォータニオン
169 assert msg.pose.pose.orientation.z == 0.0
170 assert msg.pose.pose.orientation.w == 1.0
171
172
174 converter = JsonInitialPoseConverter()
175 result = converter.convert({"type": "initial_pose", "x": 0.0, "y": 0.0,
176 "yaw": math.pi / 2})
177 assert result is not None
178 q = result.message.pose.pose.orientation
179 assert q.z == math.sin(math.pi / 4)
180 assert q.w == math.cos(math.pi / 4)
181
182
184 converter = JsonInitialPoseConverter(default_frame_id="map")
185 result = converter.convert({"type": "initial_pose", "x": 1.0, "y": 2.0})
186 assert result is not None
187 assert result.message.header.frame_id == "map"
188 assert result.message.pose.pose.orientation.w == 1.0
189
190
192 result = _dispatch({"type": "initial_pose", "x": 1.0, "y": 1.0},
193 initial_pose_topic="/ble_initial_pose")
194 assert result is not None
195 assert result.topic == "/ble_initial_pose"
196
197
199 converter = JsonInitialPoseConverter()
200 # x欠落
201 assert converter.convert({"type": "initial_pose", "y": 1.0}) is None
202 # NaN / Inf
203 assert converter.convert({"type": "initial_pose", "x": float("nan"), "y": 0.0}) is None
204 assert converter.convert({"type": "initial_pose", "x": 0.0, "y": float("inf")}) is None
205 # 範囲外
206 assert converter.convert({"type": "initial_pose", "x": 99999.0, "y": 0.0}) is None
207 # yaw不正
208 assert converter.convert({"type": "initial_pose", "x": 0.0, "y": 0.0, "yaw": "abc"}) is None
209
210
212 # cleaning_zone用コンバーターにinitial_poseを直接渡しても扱えない想定だが、
213 # dispatcher経由では正しく振り分けられることを確認する。
214 dispatcher = DispatcherConverter(converters=[JsonPolygonConverter()])
215 payload = json.dumps({"type": "initial_pose", "x": 1.0, "y": 1.0}).encode("utf-8")
216 assert dispatcher.dispatch(payload) is None
217
218
219# --- 幾何ヘルパー ---
220
222 assert abs(_signed_area([(0.0, 0.0), (2.0, 0.0), (2.0, 1.0), (0.0, 1.0)]) - 2.0) < 1e-9
223
224
226 square = [(0.0, 0.0), (2.0, 0.0), (2.0, 1.0), (0.0, 1.0)]
227 assert not _has_self_intersection(square)
228 bowtie = [(0.0, 0.0), (2.0, 2.0), (0.0, 2.0), (2.0, 0.0)]
229 assert _has_self_intersection(bowtie)
230 concave = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (1.0, 1.0), (0.0, 2.0)]
231 assert not _has_self_intersection(concave)
type フィールドで適切な :class:IPayloadConverter へ振り分ける。
initial_pose JSONを検証し、PoseWithCovarianceStampedへ変換する。
cleaning_zone JSONを検証し、PolygonStampedへ変換する。
BLEペイロードを検証し、ROS 2メッセージへ変換する。
_dispatch(dict payload, **kwargs)
_square_zone(**overrides)
test_converter_rejects_closed_ring_with_interior_duplicate()
test_initial_pose_type_mismatch_rejected_by_dispatcher()