1"""BLEペイロードを検証し、ROS 2メッセージへ変換する。
5* ``IPayloadConverter`` はtype文字列ひとつを担当する単一責任コンバーター。
6 ``handled_type`` プロパティで担当を宣言し、``convert`` で変換を行う。
7* ``DispatcherConverter`` が受信JSONの ``type`` を見て適切なコンバーターへ
8 ディスパッチする。新しいメッセージ型は ``DispatcherConverter`` に登録する
9 だけでよく、既存コンバーターは無変更で追加できる(開放/閉鎖原則)。
10* ``BleReceiverNode`` は ``DispatcherConverter`` だけを知り、配信先トピック
11 の選択はコンバーター側が返す ``ConvertResult`` を介して行う。
14from __future__
import annotations
16from abc
import ABC, abstractmethod
20from dataclasses
import dataclass
21from typing
import Any, Dict, List, Optional
23from geometry_msgs.msg
import PolygonStamped, Point32, PoseWithCovarianceStamped, Pose, Point, Quaternion
24from std_msgs.msg
import Header
25from builtin_interfaces.msg
import Time
27logger = logging.getLogger(__name__)
37 ``topic`` と ``message`` のペアを格納する。
38 変換失敗時は ``None`` の代わりにこのクラスを返さず、呼び出し元が
39 戻り値 ``None`` で失敗を判断する。
45 """配信するROSメッセージオブジェクト。"""
53 """BLE受信バイト列を *ひとつの* メッセージ型へ変換する契約。
55 担当する ``type`` 文字列を :attr:`handled_type` で宣言し、
56 :meth:`convert` で変換を実装する。
62 """このコンバーターが担当するJSONの ``type`` 値。"""
65 def convert(self, data: dict, stamp: Optional[Time] =
None) -> Optional[ConvertResult]:
66 """パース済みdictを検証・変換し、成功時は :class:`ConvertResult` を返す。
69 data: JSON デシリアライズ済み dict(``type`` キーを含む)。
70 stamp: headerへ設定するROS時刻。省略時は既定値。
73 変換成功時は :class:`ConvertResult`、検証エラー時は ``None``。
82 """``type`` フィールドで適切な :class:`IPayloadConverter` へ振り分ける。
84 新しいメッセージ型は :meth:`register` で登録するだけでよく、
85 既存コンバーターや受信ノードを変更する必要はない。
88 converters: 起動時に登録するコンバーターのリスト。
91 def __init__(self, converters: Optional[List[IPayloadConverter]] =
None) ->
None:
93 for c
in (converters
or []):
96 def register(self, converter: IPayloadConverter) ->
None:
97 """コンバーターを登録する。同一 ``handled_type`` は上書きされる。"""
98 key = converter.handled_type
100 logger.warning(f
"DispatcherConverter: '{key}' を上書き登録します。")
102 logger.debug(f
"DispatcherConverter: '{key}' を登録しました。")
109 def dispatch(self, raw_data: bytes, stamp: Optional[Time] =
None) -> Optional[ConvertResult]:
110 """バイト列をデコード・パースし、担当コンバーターへ委譲する。
113 raw_data: UTF-8で符号化されたBLEペイロード。
117 変換成功時は :class:`ConvertResult`、失敗時は ``None``。
120 logger.warning(
"空のデータを受信しました。")
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}")
129 except json.JSONDecodeError
as e:
130 logger.error(f
"JSON パース失敗: {e} (生データ: {raw_data[:100]!r})")
133 if not isinstance(data, dict):
134 logger.error(f
"JSON ルートがオブジェクトではありません: {type(data)}")
137 msg_type = data.get(
"type",
"")
139 logger.error(
"'type' フィールドが存在しないか空です。")
143 if converter
is None:
145 f
"未知の type='{msg_type}' です。登録済み: {self.registered_types}"
149 return converter.convert(data, stamp=stamp)
157 """``cleaning_zone`` JSONを検証し、PolygonStampedへ変換する。
159 入力は ``type`` に ``cleaning_zone``、任意の ``frame_id``、
160 ``polygon`` にx/y座標の配列を持つ。頂点数、有限性、座標範囲、
161 重複点、面積、自己交差を検証する。閉路表現(末尾が始点と一致)は
165 MIN_POLYGON_VERTICES = 3
167 _DEFAULT_TOPIC =
"/cleaning_zone"
171 default_frame_id: str =
"map",
172 max_coord_abs_val: float = 1000.0,
173 topic: str = _DEFAULT_TOPIC,
181 return "cleaning_zone"
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}")
192 f
"頂点数が不足しています (最低 {self.MIN_POLYGON_VERTICES} 頂点必要, 受信数: {len(raw_polygon)})"
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}")
204 z = float(pt.get(
"z", 0.0))
205 except (ValueError, TypeError)
as e:
206 logger.error(f
"頂点 [{idx}] の数値変換に失敗しました: {e} ({pt})")
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}")
214 f
"頂点 [{idx}] の座標値が許容範囲 (±{self._max_coord_abs_val}m) を超過: x={x}, y={y}"
224 xy = [(p.x, p.y)
for p
in points]
227 if len(xy) >= 2
and xy[0] == xy[-1]:
231 logger.error(
"重複頂点を含むため拒否します")
234 logger.error(f
"面積が微小({self.MIN_AREA}未満)のため拒否します")
237 logger.error(
"自己交差を含むため拒否します")
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
249 logger.info(f
"✅ PolygonStamped 生成成功 (頂点数: {len(points)}, frame_id: '{frame_id}')")
258 """``initial_pose`` JSONを検証し、PoseWithCovarianceStampedへ変換する。
263 "type": "initial_pose",
264 "frame_id": "map", // 省略時は default_frame_id
267 "yaw": 0.0 // ラジアン。省略時は 0.0
270 yawからクォータニオンへの変換は z 軸回転(2D平面上の向き)のみを行う。
273 _DEFAULT_TOPIC =
"/initialpose"
277 default_frame_id: str =
"map",
278 max_coord_abs_val: float = 1000.0,
279 topic: str = _DEFAULT_TOPIC,
287 return "initial_pose"
289 def convert(self, data: dict, stamp: Optional[Time] =
None) -> Optional[ConvertResult]:
290 """dictを検証し、PoseWithCovarianceStampedを返す。"""
294 except (KeyError, ValueError, TypeError)
as e:
295 logger.error(f
"initial_pose の x/y フィールドが不正です: {e}")
298 yaw = data.get(
"yaw", 0.0)
301 except (ValueError, TypeError)
as e:
302 logger.error(f
"initial_pose の yaw フィールドが不正です: {e}")
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}")
312 f
"initial_pose の座標が範囲外 (±{self._max_coord_abs_val}m): x={x}, y={y}"
318 qz = math.sin(yaw / 2.0)
319 qw = math.cos(yaw / 2.0)
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)
330 msg.pose.covariance = [0.0] * 36
333 f
"✅ PoseWithCovarianceStamped 生成成功 (x={x}, y={y}, yaw={yaw:.4f}, frame_id='{frame_id}')"
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` を組み立てて返す。
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`` に合わせるのが通常の運用。
357 将来新しいメッセージ型を追加する際はここに ``register`` を追記する。
361 default_frame_id=default_frame_id,
362 max_coord_abs_val=max_coord_abs_val,
363 topic=cleaning_zone_topic,
366 default_frame_id=default_frame_id,
367 max_coord_abs_val=max_coord_abs_val,
368 topic=initial_pose_topic,
378 """平面ポリゴンの符号付き面積を頂点座標から求める [m^2]。"""
383 x1, y1 = points[(i + 1) % n]
384 area += x0 * y1 - x1 * y0
389 """座標が完全一致する重複頂点があるか判定する。"""
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])
404 """線分abとcdが真に交差するか判定する。端点接触は交差に含めない。"""
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)
415 """閉じたポリゴンの隣接しない辺に真性交差があるか判定する。"""
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:
422 if i == 0
and j == n - 1:
424 c, d = points[j], points[(j + 1) % n]
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 を組み立てて返す。