┌────────┐
┌───│ 視覚ＮＮ │←──┐
│出力 └────────┘ 入力│
│ （処理） │
┌──│────────────────│──┐
│ ↓ ┌────────┐ │ │
│┌───┐ │ │ ┌───┐│
││ＩＮ │→│ アルゴリズ │→│ＯＵＴ││
│└───┘ │ │ └───┘│
↑ └────────┘ │ │
└──│───＜視覚ＮＮストア＞────│──┘
│Ｓｕｂ Ｐｕｂ│
│ （データ流通ループ） │
┌──│────────────────↓──┐
│カメラストア 歩行脳ＮＮストア│
│ 通信ネットワーク │
└──────────────────────┘
アルゴリズム：
START
│
┌─────→│←──────┐
│ ↓ │No
│ ＩＮにサブスクライブが │
│ できているか？ ───┘
│ ↓Yes
│ ＮＮの入力をつくる (→ OUT)
│ ＮＮに入力
│ │
│ ＮＮの出力を受ける (→ IN)
│ OUT用に成形 (→ OUT)
│ ↓
│ パブリッシュ
│ │
└──────┘
- アルゴリズムの周期
0.01秒 (100 Hz)
- ROS2 ノード
ノード名： visual
publisher：
visual_publisher (→ 歩行脳NNストア)
トピック：/visual/status
データ型：my_robot_msgs/VisualStatus
subscriber：
camera_subscriber (← カメラストア)
- IN・OUT
IN：
カメラ画像
←(subscribe)─ カメラストア
OUT：
カメラ画像の特徴量
─(publish)→ 歩行脳NN ストア
- OUT データの型
publish の型 VisualStatus であり，
float32[128] feature : 画像の特徴量ベクトル
- プログラム
<![CDATA[
#####################################
# Python の ROS2ライブラリ
import rclpy
# ＜ユーザ定義のデータ型を使う＞用
from rclpy.node import Node
# データ型をimport
# subscriber のデータ型
from my_robot_msgs.msg import CameraData
# publisher のデータ型
from my_robot_msgs.msg import VisualStatus
# ＜チャイルドプロセスで
# NN に入力し，そして出力を受ける＞用
import subprocess
import json
#####################################
# ストアをノード Visual, visual に設定
class Visual(Node):
############################
# コントラクタ
# (クラスがもつ属性の定義・初期化）
def __init__(self):
super().__init__('visual')
##################
# NN に対する＜入力する・出力を受ける＞を
# child process として実行
# NNプロセス起動
self.nn = subprocess.Popen(
['/usr/bin/visual_nn'],
stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
text=True,
bufsize=1
)
##################
# subscriber
# subscribe するデータは，CameraData
# CameraData を保持する変数
# None に初期設定
self.camera = None
# subscriber の設定
self.create_subscriber(
CameraData,
'/camera/data',
self.camera_callback,
10
)
##################
# publisher
# publish するデータは，VisualStatus
# publisher の設定
self.publisher =
# 形式 : (データ型, topic, バッファ)
self.create_publisher(
VisualStatus,
'/visual/status',
10
)
# self.publisher をこのように設定すると，
# self.publisher.publish(msg) で
# msg の publish となる
# (こういう仕組みになっている）
##################
# タイマー
#
# publish_visual_status
# (NN への入力から publish までの処理)
# に timer を付ける
# NN の処理速度 : 約10Hz (0.1秒周期)
# そこで ｢0.1秒｣ を publish の周期とする
self.timer =
self.create_timer(
0.1,
self.publish_visual_status
)
###############################
# サブルーチン
##########################
# callback
#
# publisher の call に対し，callback する
# → 新しい画像が届く
def camera_callback(self, msg):
self.camera = msg
############################
# NN呼び出し
# NN に入力し，出力を受ける（プロセス通信ラッパ）
# publish_visual_status(self) から呼び出される
def visual_nn(self, visual_input):
# 入力を JSON にして送る
input_json = json.dumps(visual_input)
self.nn.stdin.write(
input_json + "\n"
)
self.nn.stdin.flush()
# NN の出力を受け取る
output_json = \
self.nn.stdout.readline()
nn_output = json.loads(output_json)
return nn_output
############################
# NN 呼び出し → 入力→ 出力→ publish
def publish_visual_status(self):
############################
# NN の入力を生成して，入力
# IN庫が空 (CameraData 無し) なら，スキップ
if None in (
self.camera
):
return
# 視覚NN の入力は，
visual_input = {
"height": self.camera.height,
"width": self.camera.width,
"image": self.camera.data
}
# NN に入力して出力を得る
# （出力は画像特徴量ベクトル）
nn_output = self.visual_nn(
visual_input
)
############################
# VisualStatus メッセージを生成して，publish
msg = VisualStatus()
# VisualStatus のデータ型は，
# float32[128] featur
msg.feature = nn_output
# publish
self.publisher.publish(msg)
#####################################
def main(args=None):
rclpy.init(args=args)
# object 化
node = Visual()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
]]>
|