ble_server
読み取り中…
検索中…
一致する文字列を見つけられません
converter.py
[詳解]
1"""BLEペイロードを検証し、ROS 2メッセージへ変換する。
2
3設計方針
4--------
5* ``IPayloadConverter`` はtype文字列ひとつを担当する単一責任コンバーター。
6 ``handled_type`` プロパティで担当を宣言し、``convert`` で変換を行う。
7* ``DispatcherConverter`` が受信JSONの ``type`` を見て適切なコンバーターへ
8 ディスパッチする。新しいメッセージ型は ``DispatcherConverter`` に登録する
9 だけでよく、既存コンバーターは無変更で追加できる(開放/閉鎖原則)。
10* ``BleReceiverNode`` は ``DispatcherConverter`` だけを知り、配信先トピック
11 の選択はコンバーター側が返す ``ConvertResult`` を介して行う。
12"""
13
14from __future__ import annotations
15
16from abc import ABC, abstractmethod
17import json
18import logging
19import math
20from dataclasses import dataclass
21from typing import Any, Dict, List, Optional
22
23from geometry_msgs.msg import PolygonStamped, Point32, PoseWithCovarianceStamped, Pose, Point, Quaternion
24from std_msgs.msg import Header
25from builtin_interfaces.msg import Time
26
27logger = logging.getLogger(__name__)
28
29# ---------------------------------------------------------------------------
30# 変換結果コンテナ
31# ---------------------------------------------------------------------------
32
33@dataclass
35 """コンバーター1回の変換結果。
36
37 ``topic`` と ``message`` のペアを格納する。
38 変換失敗時は ``None`` の代わりにこのクラスを返さず、呼び出し元が
39 戻り値 ``None`` で失敗を判断する。
40 """
41
42 topic: str
43 """配信先トピック名。"""
44 message: Any
45 """配信するROSメッセージオブジェクト。"""
46
47
48# ---------------------------------------------------------------------------
49# コンバーターインターフェース
50# ---------------------------------------------------------------------------
51
53 """BLE受信バイト列を *ひとつの* メッセージ型へ変換する契約。
54
55 担当する ``type`` 文字列を :attr:`handled_type` で宣言し、
56 :meth:`convert` で変換を実装する。
57 """
58
59 @property
60 @abstractmethod
61 def handled_type(self) -> str:
62 """このコンバーターが担当するJSONの ``type`` 値。"""
63
64 @abstractmethod
65 def convert(self, data: dict, stamp: Optional[Time] = None) -> Optional[ConvertResult]:
66 """パース済みdictを検証・変換し、成功時は :class:`ConvertResult` を返す。
67
68 Args:
69 data: JSON デシリアライズ済み dict(``type`` キーを含む)。
70 stamp: headerへ設定するROS時刻。省略時は既定値。
71
72 Returns:
73 変換成功時は :class:`ConvertResult`、検証エラー時は ``None``。
74 """
75
76
77# ---------------------------------------------------------------------------
78# ディスパッチャー
79# ---------------------------------------------------------------------------
80
82 """``type`` フィールドで適切な :class:`IPayloadConverter` へ振り分ける。
83
84 新しいメッセージ型は :meth:`register` で登録するだけでよく、
85 既存コンバーターや受信ノードを変更する必要はない。
86
87 Args:
88 converters: 起動時に登録するコンバーターのリスト。
89 """
90
91 def __init__(self, converters: Optional[List[IPayloadConverter]] = None) -> None:
92 self._registry: Dict[str, IPayloadConverter] = {}
93 for c in (converters or []):
94 self.register(c)
95
96 def register(self, converter: IPayloadConverter) -> None:
97 """コンバーターを登録する。同一 ``handled_type`` は上書きされる。"""
98 key = converter.handled_type
99 if key in self._registry:
100 logger.warning(f"DispatcherConverter: '{key}' を上書き登録します。")
101 self._registry[key] = converter
102 logger.debug(f"DispatcherConverter: '{key}' を登録しました。")
103
104 @property
105 def registered_types(self) -> List[str]:
106 """登録済みtypeキーの一覧。"""
107 return list(self._registry.keys())
108
109 def dispatch(self, raw_data: bytes, stamp: Optional[Time] = None) -> Optional[ConvertResult]:
110 """バイト列をデコード・パースし、担当コンバーターへ委譲する。
111
112 Args:
113 raw_data: UTF-8で符号化されたBLEペイロード。
114 stamp: ROSタイムスタンプ。
115
116 Returns:
117 変換成功時は :class:`ConvertResult`、失敗時は ``None``。
118 """
119 if not raw_data:
120 logger.warning("空のデータを受信しました。")
121 return None
122
123 try:
124 text = raw_data.decode("utf-8")
125 data: Any = json.loads(text)
126 except UnicodeDecodeError as e:
127 logger.error(f"UTF-8 デコード失敗: {e}")
128 return None
129 except json.JSONDecodeError as e:
130 logger.error(f"JSON パース失敗: {e} (生データ: {raw_data[:100]!r})")
131 return None
132
133 if not isinstance(data, dict):
134 logger.error(f"JSON ルートがオブジェクトではありません: {type(data)}")
135 return None
136
137 msg_type = data.get("type", "")
138 if not msg_type:
139 logger.error("'type' フィールドが存在しないか空です。")
140 return None
141
142 converter = self._registry.get(msg_type)
143 if converter is None:
144 logger.error(
145 f"未知の type='{msg_type}' です。登録済み: {self.registered_types}"
146 )
147 return None
148
149 return converter.convert(data, stamp=stamp)
150
151
152# ---------------------------------------------------------------------------
153# 清掃ゾーン コンバーター
154# ---------------------------------------------------------------------------
155
157 """``cleaning_zone`` JSONを検証し、PolygonStampedへ変換する。
158
159 入力は ``type`` に ``cleaning_zone``、任意の ``frame_id``、
160 ``polygon`` にx/y座標の配列を持つ。頂点数、有限性、座標範囲、
161 重複点、面積、自己交差を検証する。閉路表現(末尾が始点と一致)は
162 許容し、末尾を除去して開路に正規化する。
163 """
164
165 MIN_POLYGON_VERTICES = 3
166 MIN_AREA = 0.0001
167 _DEFAULT_TOPIC = "/cleaning_zone"
168
170 self,
171 default_frame_id: str = "map",
172 max_coord_abs_val: float = 1000.0,
173 topic: str = _DEFAULT_TOPIC,
174 ) -> None:
175 self._default_frame_id = default_frame_id
176 self._max_coord_abs_val = max_coord_abs_val
177 self._topic = topic
178
179 @property
180 def handled_type(self) -> str: # type: ignore[override]
181 return "cleaning_zone"
182
183 def convert(self, data: dict, stamp: Optional[Time] = None) -> Optional[ConvertResult]:
184 """dictを検証し、失敗時は ``None`` を返す。"""
185 raw_polygon = data.get("polygon")
186 if not isinstance(raw_polygon, list):
187 logger.error(f"'polygon' フィールドが配列ではありません: {raw_polygon}")
188 return None
189
190 if len(raw_polygon) < self.MIN_POLYGON_VERTICES:
191 logger.error(
192 f"頂点数が不足しています (最低 {self.MIN_POLYGON_VERTICES} 頂点必要, 受信数: {len(raw_polygon)})"
193 )
194 return None
195
196 points: List[Point32] = []
197 for idx, pt in enumerate(raw_polygon):
198 if not isinstance(pt, dict) or "x" not in pt or "y" not in pt:
199 logger.error(f"頂点 [{idx}] のキーが不正です (x, y が必要): {pt}")
200 return None
201 try:
202 x = float(pt["x"])
203 y = float(pt["y"])
204 z = float(pt.get("z", 0.0))
205 except (ValueError, TypeError) as e:
206 logger.error(f"頂点 [{idx}] の数値変換に失敗しました: {e} ({pt})")
207 return None
208
209 if any(math.isnan(v) or math.isinf(v) for v in (x, y, z)):
210 logger.error(f"頂点 [{idx}] に NaN または Inf が含まれています: x={x}, y={y}, z={z}")
211 return None
212 if any(abs(v) > self._max_coord_abs_val for v in (x, y, z)):
213 logger.error(
214 f"頂点 [{idx}] の座標値が許容範囲 (±{self._max_coord_abs_val}m) を超過: x={x}, y={y}"
215 )
216 return None
217
218 p = Point32()
219 p.x = x
220 p.y = y
221 p.z = z
222 points.append(p)
223
224 xy = [(p.x, p.y) for p in points]
225 # 閉路表現(末尾が始点と一致)は許容し、末尾を除去して正規化する。
226 # 閉形化は下流のcoverage_bridgeが担うため、topic上の形式は開路に統一する。
227 if len(xy) >= 2 and xy[0] == xy[-1]:
228 points = points[:-1]
229 xy = xy[:-1]
231 logger.error("重複頂点を含むため拒否します")
232 return None
233 if abs(_signed_area(xy)) < self.MIN_AREA:
234 logger.error(f"面積が微小({self.MIN_AREA}未満)のため拒否します")
235 return None
237 logger.error("自己交差を含むため拒否します")
238 return None
239
240 frame_id = str(data.get("frame_id", self._default_frame_id)).strip() or self._default_frame_id
241
242 msg = PolygonStamped()
243 msg.header = Header()
244 if stamp is not None:
245 msg.header.stamp = stamp
246 msg.header.frame_id = frame_id
247 msg.polygon.points = points
248
249 logger.info(f"✅ PolygonStamped 生成成功 (頂点数: {len(points)}, frame_id: '{frame_id}')")
250 return ConvertResult(topic=self._topic, message=msg)
251
252
253# ---------------------------------------------------------------------------
254# 初期姿勢 コンバーター
255# ---------------------------------------------------------------------------
256
258 """``initial_pose`` JSONを検証し、PoseWithCovarianceStampedへ変換する。
259
260 入力フォーマット::
261
262 {
263 "type": "initial_pose",
264 "frame_id": "map", // 省略時は default_frame_id
265 "x": 1.0,
266 "y": 2.0,
267 "yaw": 0.0 // ラジアン。省略時は 0.0
268 }
269
270 yawからクォータニオンへの変換は z 軸回転(2D平面上の向き)のみを行う。
271 """
272
273 _DEFAULT_TOPIC = "/initialpose"
274
276 self,
277 default_frame_id: str = "map",
278 max_coord_abs_val: float = 1000.0,
279 topic: str = _DEFAULT_TOPIC,
280 ) -> None:
281 self._default_frame_id = default_frame_id
282 self._max_coord_abs_val = max_coord_abs_val
283 self._topic = topic
284
285 @property
286 def handled_type(self) -> str:
287 return "initial_pose"
288
289 def convert(self, data: dict, stamp: Optional[Time] = None) -> Optional[ConvertResult]:
290 """dictを検証し、PoseWithCovarianceStampedを返す。"""
291 try:
292 x = float(data["x"])
293 y = float(data["y"])
294 except (KeyError, ValueError, TypeError) as e:
295 logger.error(f"initial_pose の x/y フィールドが不正です: {e}")
296 return None
297
298 yaw = data.get("yaw", 0.0)
299 try:
300 yaw = float(yaw)
301 except (ValueError, TypeError) as e:
302 logger.error(f"initial_pose の yaw フィールドが不正です: {e}")
303 return None
304
305 for name, val in (("x", x), ("y", y), ("yaw", yaw)):
306 if math.isnan(val) or math.isinf(val):
307 logger.error(f"initial_pose の '{name}' に NaN/Inf が含まれています: {val}")
308 return None
309
310 if abs(x) > self._max_coord_abs_val or abs(y) > self._max_coord_abs_val:
311 logger.error(
312 f"initial_pose の座標が範囲外 (±{self._max_coord_abs_val}m): x={x}, y={y}"
313 )
314 return None
315
316 frame_id = str(data.get("frame_id", self._default_frame_id)).strip() or self._default_frame_id
317
318 qz = math.sin(yaw / 2.0)
319 qw = math.cos(yaw / 2.0)
320
321 msg = PoseWithCovarianceStamped()
322 msg.header = Header()
323 if stamp is not None:
324 msg.header.stamp = stamp
325 msg.header.frame_id = frame_id
326 msg.pose.pose = Pose()
327 msg.pose.pose.position = Point(x=x, y=y, z=0.0)
328 msg.pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=qz, w=qw)
329 # 共分散はゼロ初期化(Nav2のinitialpose topicはゼロ共分散を受け入れる)。
330 msg.pose.covariance = [0.0] * 36
331
332 logger.info(
333 f"✅ PoseWithCovarianceStamped 生成成功 (x={x}, y={y}, yaw={yaw:.4f}, frame_id='{frame_id}')"
334 )
335 return ConvertResult(topic=self._topic, message=msg)
336
337
338# ---------------------------------------------------------------------------
339# ファクトリー
340# ---------------------------------------------------------------------------
341
343 default_frame_id: str = "map",
344 max_coord_abs_val: float = 1000.0,
345 cleaning_zone_topic: str = "/cleaning_zone",
346 initial_pose_topic: str = "/initialpose",
347) -> DispatcherConverter:
348 """デフォルト設定の :class:`DispatcherConverter` を組み立てて返す。
349
350 Args:
351 default_frame_id: JSONに frame_id がない場合の座標フレーム。
352 max_coord_abs_val: 座標の絶対値上限 [m]。
353 cleaning_zone_topic: cleaning_zone メッセージの配信先トピック名。
354 initial_pose_topic: initial_pose メッセージの配信先トピック名。
355 AMCLが購読する ``/initialpose`` に合わせるのが通常の運用。
356
357 将来新しいメッセージ型を追加する際はここに ``register`` を追記する。
358 """
359 dispatcher = DispatcherConverter()
360 dispatcher.register(JsonPolygonConverter(
361 default_frame_id=default_frame_id,
362 max_coord_abs_val=max_coord_abs_val,
363 topic=cleaning_zone_topic,
364 ))
365 dispatcher.register(JsonInitialPoseConverter(
366 default_frame_id=default_frame_id,
367 max_coord_abs_val=max_coord_abs_val,
368 topic=initial_pose_topic,
369 ))
370 return dispatcher
371
372
373# ---------------------------------------------------------------------------
374# ポリゴン幾何ヘルパー(モジュール内公開・テストから直接利用)
375# ---------------------------------------------------------------------------
376
377def _signed_area(points: list) -> float:
378 """平面ポリゴンの符号付き面積を頂点座標から求める [m^2]。"""
379 area = 0.0
380 n = len(points)
381 for i in range(n):
382 x0, y0 = points[i]
383 x1, y1 = points[(i + 1) % n]
384 area += x0 * y1 - x1 * y0
385 return area / 2.0
386
387
388def _has_duplicate_points(points: list) -> bool:
389 """座標が完全一致する重複頂点があるか判定する。"""
390 seen = set()
391 for p in points:
392 if p in seen:
393 return True
394 seen.add(p)
395 return False
396
397
398def _cross(o: tuple, a: tuple, b: tuple) -> float:
399 """点oからa、bへのベクトルの2次元外積を返す。"""
400 return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
401
402
403def _segments_cross(a: tuple, b: tuple, c: tuple, d: tuple) -> bool:
404 """線分abとcdが真に交差するか判定する。端点接触は交差に含めない。"""
405 ab_c = _cross(a, b, c)
406 ab_d = _cross(a, b, d)
407 cd_a = _cross(c, d, a)
408 cd_b = _cross(c, d, b)
409 return ((ab_c > 0 and ab_d < 0) or (ab_c < 0 and ab_d > 0)) and (
410 (cd_a > 0 and cd_b < 0) or (cd_a < 0 and cd_b > 0)
411 )
412
413
414def _has_self_intersection(points: list) -> bool:
415 """閉じたポリゴンの隣接しない辺に真性交差があるか判定する。"""
416 n = len(points)
417 for i in range(n):
418 a, b = points[i], points[(i + 1) % n]
419 for j in range(i + 1, n):
420 if j == i or j == (i + 1) % n:
421 continue
422 if i == 0 and j == n - 1:
423 continue
424 c, d = points[j], points[(j + 1) % n]
425 if _segments_cross(a, b, c, d):
426 return True
427 return False
コンバーター1回の変換結果。
type フィールドで適切な :class:IPayloadConverter へ振り分ける。
List[str] registered_types(self)
登録済みtypeキーの一覧。
None __init__(self, Optional[List[IPayloadConverter]] converters=None)
None register(self, IPayloadConverter converter)
コンバーターを登録する。同一 handled_type は上書きされる。
Optional[ConvertResult] dispatch(self, bytes raw_data, Optional[Time] stamp=None)
バイト列をデコード・パースし、担当コンバーターへ委譲する。
BLE受信バイト列を ひとつの メッセージ型へ変換する契約。
str handled_type(self)
このコンバーターが担当するJSONの type 値。
Optional[ConvertResult] convert(self, dict data, Optional[Time] stamp=None)
パース済みdictを検証・変換し、成功時は :class:ConvertResult を返す。
initial_pose JSONを検証し、PoseWithCovarianceStampedへ変換する。
str handled_type(self)
このコンバーターが担当するJSONの type 値。
None __init__(self, str default_frame_id="map", float max_coord_abs_val=1000.0, str topic=_DEFAULT_TOPIC)
Optional[ConvertResult] convert(self, dict data, Optional[Time] stamp=None)
dictを検証し、PoseWithCovarianceStampedを返す。
cleaning_zone JSONを検証し、PolygonStampedへ変換する。
None __init__(self, str default_frame_id="map", float max_coord_abs_val=1000.0, str topic=_DEFAULT_TOPIC)
str handled_type(self)
このコンバーターが担当するJSONの type 値。
Optional[ConvertResult] convert(self, dict data, Optional[Time] stamp=None)
dictを検証し、失敗時は None を返す。
float _cross(tuple o, tuple a, tuple b)
点oからa、bへのベクトルの2次元外積を返す。
float _signed_area(list points)
平面ポリゴンの符号付き面積を頂点座標から求める [m^2]。
bool _has_self_intersection(list points)
閉じたポリゴンの隣接しない辺に真性交差があるか判定する。
bool _has_duplicate_points(list points)
座標が完全一致する重複頂点があるか判定する。
bool _segments_cross(tuple a, tuple b, tuple c, tuple d)
線分abとcdが真に交差するか判定する。端点接触は交差に含めない。
DispatcherConverter build_default_dispatcher(str default_frame_id="map", float max_coord_abs_val=1000.0, str cleaning_zone_topic="/cleaning_zone", str initial_pose_topic="/initialpose")
デフォルト設定の :class:DispatcherConverter を組み立てて返す。