<![CDATA[
#####################################
# Python の ROS2ライブラリ
import rclpy
# ＜ユーザ定義のデータ型を使う＞用
from rclpy.node import Node
# データ型をimport
# publisher のデータ型
from my_robot_msgs.msg import BrainCmd
# subscriber のデータ型
# from my_robot_msgs.msg import BrainCmd
from my_robot_msgs.msg import PositionStatus
from my_robot_msgs.msg import EarthStatus
from my_robot_msgs.msg import VisualStatus
# ＜チャイルドプロセスで
# NN に入力し，そして出力を受ける＞用
import subprocess
import json
# ＜電流値の確率分布から max値を取り出す＞用
import numpy as np
###############
# I2C
import smbus
# get_clock 用
import time
#####################################
# ストアをノード Brain, brain に設定
class Brain(Node):
###############################
# コントラクタ
# (クラスがもつ属性の定義・初期化）
def __init__(self):
# ノート名 : brain
super().__init__('brain')
##################
# NN に対する＜入力する・出力を受ける＞を
# child process として実行
# NNプロセス起動
self.nn =
subprocess.Popen(
['/usr/bin/brain_nn'],
stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
text=True,
bufsize=1
)
##################
# subscriber
# subscriber データを保持する変数
# None に初期設定
self.brain = None
self.position = None
self.earth = None
self.visual = None
# subscriber の設定
self.create_subscriber(
BrainCmd,
'/brain/cmd',
self.brain_callback,
10
)
self.create_subscriber(
PositionStatus,
'/position/status',
self.position_callback,
10
)
self.create_subscriber(
EarthStatus,
'/earth/status',
self.earth_callback,
10
)
self.create_subscriber(
VisualStatus,
'/vision/status',
self.visual_callback,
10
)
##################
# publisher の設定
# publish するデータは，BrainCmd
# publisher の設定
self.publisher =
# 形式 : (データ型, topic, バッファ)
self.create_publisher(
BrainCmd,
'/brain/cmd',
10
)
# self.publisher をこのように設定すると，
# self.publisher.publish(msg) で
# msg のpublish となる
# (こういう仕組みになっている）
##################
# I2C
# MCU の I2C 設定が既にされている
# その内容を書く
# 回路 I2C1 を使用
self.bus = smbus.SMBus(1)
# slave の motor_store_buffer は
# アドレスが 0x10
self.slave_address = 0x10
##################
# タイマー
#
# brain_command
# (NN の入力の生成から publish までの処理)
# に timer を付ける
# タイマーは ｢新製品出たよ｣ コール用
# NN の処理速度は，約100Hz (0.01秒周期)
# そこで ｢0.01秒｣ を publish の周期とする
self.timer =
self.create_timer(
0.01,
self.brain_command
)
####################################
# サブルーチン
#
# callback
# brain_callback
# position_callback
# earth_callback
# visual_callback
# brain_nn
# current_to_i2c_data
# brain_command
####################################
##########################
# callback
# publisher の call に対し，callback する
# → 新製品が届く
##########################
def brain_callback(self, msg):
self.brain = msg
def position_callback(self, msg):
self.position = msg
def earth_callback(self, msg):
self.earth = msg
def visual_callback(self, msg):
self.visual = msg
##########################
# brain_nn
# NN に入力し，出力を受ける（プロセス通信ラッパ）
# motor_command(self) から呼び出される
############################
def brain_nn(self, brain_input):
# 入力を JSON にして送る
input_json = json.dumps(brain_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
##########################
# current_to_i2c_data
# fload32[12] の current を
# 2byte が 12個の配列 data に変換
############################
def current_to_i2c_data(current):
# A → mA
current_mA = (
current * 1000
).astype(np.int16)
data = []
for value in current_mA:
high = (value >> 8) & 0xFF
low = value & 0xFF
data.append(high)
data.append(low)
return data
############################
# brain_command
# (1) NN 入力生成
# (2) NN 呼び出し → 入力→ 出力
# (3) 出力を OUT 用に成形
# (4) publish
# (5) モータストアへ I2C 送信
############################
def brain_command(self):
##################
# (1) 歩行脳NN の入力を生成
##################
# IN庫の入荷棚が全部埋まっていなければスキップ
if None in (
self.brain,
self.position,
self.earth,
self.visual
):
return
# 歩行脳NN の入力は，
# ４つの subscriber のデータをつないたもの
brain_input = {
"brain": self.brain.__dict__
"position": self.position.__dict__,
"visual": self.visual.__dict__,
"earth": self.earth.__dict__,
}
##################
# (2) NN 呼び出し → 入力→ 出力
##################
nn_output = self.brain_nn(brain_input)
##################
# (3) 出力を OUT 用に成形
##################
# 出力はモータ12個の電流値の確率分布
# それぞれに対し最大確率の電流値を択る
current =
np.argmax(
nn_output,
axis=1
)
.astype(np.float32)
##################
# (4) publish
##################
# BrainCmd メッセージを生成
msg = BrainCmd()
# BrainCmd のデータ型は，電流値 だけ
# この変数を current とする。
# ROS2 は Python list を要求する
msg.current = current.tolist()
# publish
self.publisher.publish(msg)
##################
# (5) モータストアへ I2C 送信
##################
# byte列へ変換
i2c_data = current_to_i2c_data( current.)
# motor_store_buffer に書き込む
self.bus.write_i2c_block_data(
self.slave_address,
0x00,
i2c_data
)
#####################################
#####################################
##################
# main
##################
# main は，つぎの定型にするのが，Python 流
# (｢＊＊＊クラスの物を spin させる｣)
def main(args=None):
rclpy.init(args=args)
# オブジェクト化
node = Brain()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
##################
# プログラムの起動
##################
if __name__ == '__main__':
main()
]]>
|