ble_server
読み取り中…
検索中…
一致する文字列を見つけられません
ble_receiver_node.py
[詳解]
1#!/usr/bin/env python3
2"""BLEメッセージを受信し、型に応じたROS 2トピックへ配信する。
3
4設計方針
5--------
6* ノードはメッセージの解釈を一切知らない。変換・トピック名の決定は
7 :class:`~ble_server.converter.DispatcherConverter` に委ねる。
8* 新しいメッセージ型を追加するには ``converter.py`` にコンバーターを
9 追加して ``build_default_dispatcher`` に登録するだけでよく、
10 このノードは変更不要。
11"""
12
13import asyncio
14import sys
15import threading
16from typing import Dict, Optional
17
18import rclpy
19from rclpy.node import Node
20from rclpy.publisher import Publisher
21
22from ble_server.ble_interface import IBleGattServer
23from ble_server.bluez_server import RESPONSE_ACK, RESPONSE_NACK, BluezBleGattServer
24from ble_server.chunk_assembler import AssemblerOverflowError, ChunkAssembler
25from ble_server.converter import DispatcherConverter, build_default_dispatcher
26
27
28class BleReceiverNode(Node):
29 """BLEフレームを受信し、 :class:`DispatcherConverter` 経由でpublishするROS 2ノード。
30
31 配信先トピックとメッセージ型はコンバーターが返す :class:`ConvertResult`
32 から動的に決定する。未知のトピックが現れた際は自動でpublisherを生成する。
33 """
34
36 self,
37 ble_server: IBleGattServer,
38 dispatcher: Optional[DispatcherConverter] = None,
39 ) -> None:
40 """受信ノードを初期化する。
41
42 Args:
43 ble_server: BLE受信と応答を担当するGATTサーバー。
44 dispatcher: ペイロードをディスパッチするコンバーター。
45 省略時は :func:`~ble_server.converter.build_default_dispatcher`
46 をROSパラメータで構成して使う。
47 """
48 super().__init__('ble_receiver_node')
49
50 self._ble_server = ble_server
51
52 self.declare_parameter('default_frame_id', 'map')
53 self.declare_parameter('max_coord_limit_m', 1000.0)
54 # 各メッセージ型の配信先トピック名。既定値はコンバーターの定数と合わせる。
55 self.declare_parameter('cleaning_zone_topic', '/cleaning_zone')
56 self.declare_parameter('initial_pose_topic', '/initialpose')
57
58 default_frame_id = self.get_parameter('default_frame_id').get_parameter_value().string_value
59 max_coord = self.get_parameter('max_coord_limit_m').get_parameter_value().double_value
60 cleaning_zone_topic = self.get_parameter('cleaning_zone_topic').get_parameter_value().string_value
61 initial_pose_topic = self.get_parameter('initial_pose_topic').get_parameter_value().string_value
62
63 self._dispatcher = dispatcher or build_default_dispatcher(
64 default_frame_id=default_frame_id,
65 max_coord_abs_val=max_coord,
66 cleaning_zone_topic=cleaning_zone_topic,
67 initial_pose_topic=initial_pose_topic,
68 )
70
71 # トピック名 → Publisher のキャッシュ。
72 # ConvertResult が返すトピックが増えるたびに自動で追加する。
73 # 注意: rclpy.Node が内部で self._publishers (list) を使うため、
74 # 名前が衝突しないよう _pub_cache と命名する。
75 self._pub_cache: Dict[str, Publisher] = {}
76
77 self.get_logger().info(
78 f"🚀 BLE Receiver Node 起動 "
79 f"(Default Frame: '{default_frame_id}', "
80 f"登録済みtype: {self._dispatcher.registered_types}, "
81 f"cleaning_zone→'{cleaning_zone_topic}', "
82 f"initial_pose→'{initial_pose_topic}')"
83 )
84
85 self._ble_server.set_on_data_received_callback(self._on_ble_data_received)
86
87 # ------------------------------------------------------------------
88 # BLE受信コールバック
89 # ------------------------------------------------------------------
90
91 def _on_ble_data_received(self, raw_bytes: bytes) -> None:
92 """WriteValueチャンクを再構成し、完成フレームの結果を応答値へ反映する。"""
93 try:
94 frames = self._assembler.feed(raw_bytes)
95 except AssemblerOverflowError as e:
96 self.get_logger().error(f'受信バッファ溢れのため破棄します: {e}')
97 self._ble_server.set_response(RESPONSE_NACK)
98 return
99 if not frames:
100 return
101 ok = True
102 for frame in frames:
103 if not self._handle_frame(frame):
104 ok = False
105 self._ble_server.set_response(RESPONSE_ACK if ok else RESPONSE_NACK)
106
107 def _handle_frame(self, frame: bytes) -> bool:
108 """完成フレームをディスパッチ・配信し、成功時はTrueを返す。"""
109 self.get_logger().info(f'📥 フレーム受信: {len(frame)} bytes')
110
111 current_time = self.get_clock().now().to_msg()
112 result = self._dispatcher.dispatch(frame, stamp=current_time)
113
114 if result is None:
115 self.get_logger().error(
116 "❌ 受信データのパース・検証に失敗したため、トピックへの配信を中断しました。"
117 )
118 return False
119
120 pub = self._get_or_create_publisher(result.topic, type(result.message))
121 pub.publish(result.message)
122 self.get_logger().info(
123 f"📢 配信完了: topic='{result.topic}', type='{type(result.message).__name__}'"
124 )
125 return True
126
127 # ------------------------------------------------------------------
128 # Publisher キャッシュ
129 # ------------------------------------------------------------------
130
131 def _get_or_create_publisher(self, topic: str, msg_type) -> Publisher:
132 """トピック名に対応するpublisherを返す。未登録なら生成してキャッシュする。"""
133 if topic not in self._pub_cache:
134 self._pub_cache[topic] = self.create_publisher(msg_type, topic, 10)
135 self.get_logger().info(f"📌 Publisher を生成しました: '{topic}' ({msg_type.__name__})")
136 return self._pub_cache[topic]
137
138
139# ---------------------------------------------------------------------------
140# BLEサーバースレッド管理(変更なし)
141# ---------------------------------------------------------------------------
142
144 """BLEサーバー用asyncioループを専用スレッドで開始・停止する。"""
145
146 def __init__(self, ble_server: IBleGattServer, logger=None) -> None:
147 """BLEサーバーと任意のROS loggerをイベントループ管理へ結び付ける。"""
148 self._server = ble_server
149 self._loop: Optional[asyncio.AbstractEventLoop] = None
150 self._thread: Optional[threading.Thread] = None
151 self._logger = logger
152
153 def start(self) -> None:
154 """イベントループ用スレッドを開始する。"""
155 self._loop = asyncio.new_event_loop()
156 self._thread = threading.Thread(
157 target=self._run_loop,
158 args=(self._loop,),
159 daemon=True,
160 name="BleServerThread"
161 )
162 self._thread.start()
163
164 def _run_loop(self, loop: asyncio.AbstractEventLoop) -> None:
165 """BLEサーバーを起動し、停止要求までイベントを処理する。"""
166 asyncio.set_event_loop(loop)
167 try:
168 loop.run_until_complete(self._server.start())
169 loop.run_forever()
170 except Exception as e:
171 msg = f"BLE イベントループで例外が発生しました: {e}"
172 if self._logger:
173 self._logger.error(msg)
174 else:
175 print(f"[ERROR] {msg}", file=sys.stderr)
176 finally:
177 try:
178 loop.run_until_complete(self._server.stop())
179 except Exception:
180 pass
181 loop.close()
182
183 def stop(self) -> None:
184 """イベントループへ停止を通知し、スレッド終了を最大3.0s待つ。"""
185 if self._loop and self._loop.is_running():
186 self._loop.call_soon_threadsafe(self._loop.stop)
187 if self._thread and self._thread.is_alive():
188 self._thread.join(timeout=3.0)
189
190
191# ---------------------------------------------------------------------------
192# エントリーポイント
193# ---------------------------------------------------------------------------
194
195def main(args=None) -> None:
196 """BLE受信ノードとBlueZサーバーを起動し、ROS 2 spinを実行する。"""
197 rclpy.init(args=args)
198
199 ble_server: IBleGattServer = BluezBleGattServer()
200
201 node = BleReceiverNode(ble_server=ble_server)
202
203 manager = BleServerManager(ble_server=ble_server, logger=node.get_logger())
204 manager.start()
205
206 try:
207 rclpy.spin(node)
208 except KeyboardInterrupt:
209 node.get_logger().info("シャットダウン要求を受信しました。")
210 finally:
211 node.get_logger().info("ノードおよび BLE サーバーを終了中...")
212 manager.stop()
213 node.destroy_node()
214 rclpy.shutdown()
215
216
217if __name__ == '__main__':
218 main()
BLEフレームを受信し、 :class:DispatcherConverter 経由でpublishするROS 2ノード。
None __init__(self, IBleGattServer ble_server, Optional[DispatcherConverter] dispatcher=None)
受信ノードを初期化する。
Publisher _get_or_create_publisher(self, str topic, msg_type)
トピック名に対応するpublisherを返す。未登録なら生成してキャッシュする。
bool _handle_frame(self, bytes frame)
完成フレームをディスパッチ・配信し、成功時はTrueを返す。
BLEサーバー用asyncioループを専用スレッドで開始・停止する。
None __init__(self, IBleGattServer ble_server, logger=None)
BLEサーバーと任意のROS loggerをイベントループ管理へ結び付ける。
None start(self)
イベントループ用スレッドを開始する。
None _run_loop(self, asyncio.AbstractEventLoop loop)
BLEサーバーを起動し、停止要求までイベントを処理する。
None stop(self)
イベントループへ停止を通知し、スレッド終了を最大3.0s待つ。
BlueZ D-Bus APIを使うLinux向けBLE GATTサーバー実装。
区切り文字で分割された入力を、完全なフレーム列へ再構成する。
BLE GATTサーバー実装が満たす共通インターフェースを定義する。
None main(args=None)
BLE受信ノードとBlueZサーバーを起動し、ROS 2 spinを実行する。
BlueZ D-Busを使ってBLE GATTサービスとLEアドバタイズを提供する。
BLE WriteValueで分割受信した改行区切りフレームを再構成する。
BLEペイロードを検証し、ROS 2メッセージへ変換する。