ble_server
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
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
13
import
asyncio
14
import
sys
15
import
threading
16
from
typing
import
Dict, Optional
17
18
import
rclpy
19
from
rclpy.node
import
Node
20
from
rclpy.publisher
import
Publisher
21
22
from
ble_server.ble_interface
import
IBleGattServer
23
from
ble_server.bluez_server
import
RESPONSE_ACK, RESPONSE_NACK, BluezBleGattServer
24
from
ble_server.chunk_assembler
import
AssemblerOverflowError, ChunkAssembler
25
from
ble_server.converter
import
DispatcherConverter, build_default_dispatcher
26
27
28
class
BleReceiverNode
(Node):
29
"""BLEフレームを受信し、 :class:`DispatcherConverter` 経由でpublishするROS 2ノード。
30
31
配信先トピックとメッセージ型はコンバーターが返す :class:`ConvertResult`
32
から動的に決定する。未知のトピックが現れた際は自動でpublisherを生成する。
33
"""
34
35
def
__init__
(
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
)
69
self.
_assembler
=
ChunkAssembler
()
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
143
class
BleServerManager
:
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
195
def
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
217
if
__name__ ==
'__main__'
:
218
main
()
ble_server.ble_receiver_node.BleReceiverNode
BLEフレームを受信し、 :class:DispatcherConverter 経由でpublishするROS 2ノード。
定義
ble_receiver_node.py:28
ble_server.ble_receiver_node.BleReceiverNode._ble_server
_ble_server
定義
ble_receiver_node.py:50
ble_server.ble_receiver_node.BleReceiverNode.__init__
None __init__(self, IBleGattServer ble_server, Optional[DispatcherConverter] dispatcher=None)
受信ノードを初期化する。
定義
ble_receiver_node.py:39
ble_server.ble_receiver_node.BleReceiverNode._get_or_create_publisher
Publisher _get_or_create_publisher(self, str topic, msg_type)
トピック名に対応するpublisherを返す。未登録なら生成してキャッシュする。
定義
ble_receiver_node.py:131
ble_server.ble_receiver_node.BleReceiverNode._on_ble_data_received
_on_ble_data_received
定義
ble_receiver_node.py:85
ble_server.ble_receiver_node.BleReceiverNode._pub_cache
dict _pub_cache
定義
ble_receiver_node.py:75
ble_server.ble_receiver_node.BleReceiverNode._handle_frame
bool _handle_frame(self, bytes frame)
完成フレームをディスパッチ・配信し、成功時はTrueを返す。
定義
ble_receiver_node.py:107
ble_server.ble_receiver_node.BleReceiverNode._assembler
_assembler
定義
ble_receiver_node.py:69
ble_server.ble_receiver_node.BleReceiverNode._dispatcher
_dispatcher
定義
ble_receiver_node.py:63
ble_server.ble_receiver_node.BleServerManager
BLEサーバー用asyncioループを専用スレッドで開始・停止する。
定義
ble_receiver_node.py:143
ble_server.ble_receiver_node.BleServerManager.__init__
None __init__(self, IBleGattServer ble_server, logger=None)
BLEサーバーと任意のROS loggerをイベントループ管理へ結び付ける。
定義
ble_receiver_node.py:146
ble_server.ble_receiver_node.BleServerManager.start
None start(self)
イベントループ用スレッドを開始する。
定義
ble_receiver_node.py:153
ble_server.ble_receiver_node.BleServerManager._server
_server
定義
ble_receiver_node.py:148
ble_server.ble_receiver_node.BleServerManager._loop
Optional[asyncio.AbstractEventLoop] _loop
定義
ble_receiver_node.py:149
ble_server.ble_receiver_node.BleServerManager._run_loop
None _run_loop(self, asyncio.AbstractEventLoop loop)
BLEサーバーを起動し、停止要求までイベントを処理する。
定義
ble_receiver_node.py:164
ble_server.ble_receiver_node.BleServerManager._thread
Optional[threading.Thread] _thread
定義
ble_receiver_node.py:150
ble_server.ble_receiver_node.BleServerManager._logger
_logger
定義
ble_receiver_node.py:151
ble_server.ble_receiver_node.BleServerManager.stop
None stop(self)
イベントループへ停止を通知し、スレッド終了を最大3.0s待つ。
定義
ble_receiver_node.py:183
ble_server.bluez_server.BluezBleGattServer
BlueZ D-Bus APIを使うLinux向けBLE GATTサーバー実装。
定義
bluez_server.py:168
ble_server.chunk_assembler.ChunkAssembler
区切り文字で分割された入力を、完全なフレーム列へ再構成する。
定義
chunk_assembler.py:10
ble_server.ble_interface
BLE GATTサーバー実装が満たす共通インターフェースを定義する。
定義
ble_interface.py:1
ble_server.ble_receiver_node.main
None main(args=None)
BLE受信ノードとBlueZサーバーを起動し、ROS 2 spinを実行する。
定義
ble_receiver_node.py:195
ble_server.bluez_server
BlueZ D-Busを使ってBLE GATTサービスとLEアドバタイズを提供する。
定義
bluez_server.py:1
ble_server.chunk_assembler
BLE WriteValueで分割受信した改行区切りフレームを再構成する。
定義
chunk_assembler.py:1
ble_server.converter
BLEペイロードを検証し、ROS 2メッセージへ変換する。
定義
converter.py:1
ble_server
ble_receiver_node.py
構築:
1.18.0