<![CDATA[
#####################################
# Python の ROS2ライブラリ
import rclpy
# ＜ユーザ定義のデータ型を使う＞用
from rclpy.node import Node
# データ型をimport
# publisher のデータ型
from my_robot_msgs.msg import PositionStatus
# ＜チャイルドプロセスで
# NN に入力し，そして出力を受ける＞用
import subprocess
import json
###############
# I2C 用
# I2C ライブラリ
import smbus
# get_clock 用
import time
# Python の struct ライブラリ
import struct
#####################################
# ストアをノード Position, position に設定
class Position(Node):
############################
# コントラクタ
# (クラスがもつ属性の定義・初期化）
def __init__(self):
super().__init__('position')
##################
# NN に対する＜入力する・出力を受ける＞を
# child process として実行
# NNプロセス起動
self.nn = subprocess.Popen(
['/usr/bin/position_nn'],
stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
text=True,
bufsize=1
)
##################
# publisher
# publish するデータは，PositionStatus
# publisher の設定
self.publisher =
# 形式 : (データ型, topic, バッファ)
self.create_publisher(
PositionStatus,
'/position/status',
10
)
# self.publisher をこのように設定すると，
# self.publisher.publish(msg) で
# msg のpublish となる
# (こういう仕組みになっている）
##################
# I2C
# MCU の I2C 設定が既にされている
# その内容を書く
# 回路 I2C3 を使用
self.bus = smbus.SMBus(3)
# slave の imu_store_buffer は
# アドレスが 0x30
self.slave_address = 0x30
##################
# タイマー
#
# self.publish_position_status
# (NN への入力から publish までの処理)
# に timer を付ける
# NN の処理速度 : 約100Hz (0.01秒周期)
# そこで ｢0.01秒｣ を publish の周期とする
self.timer =
self.create_timer(
0.01,
self.publish_position_status
)
###############################
# サブルーチン
###############################
############################
# NN呼び出し
# NN に入力し，出力を受ける（プロセス通信ラッパ）
# publish_position_status(self) から呼び出される
def position_nn(self, position_input):
# 入力を JSON にして送る
input_json = json.dumps(
position_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
############################
# (1) IMUデータを取得
# (2) NN の入力を生成
# (3) NN 呼び出し → 入力→ 出力
# (4) publish
############################
def publish_position_status(self):
##################
# (1) imu_store_buffer を読み出す
data = bus.read_i2c_block_data(
0x30,
0,
16
)
##########
# floatへ復元
# Python bytes へ
raw = bytes(data)
# f = float が４つ
# → ax, ay, wx, wy が復元される
ax, ay, wx, wy = struct.unpack(
"<ffff",
raw
)
##############
# (2) 移動覚NN の入力を生成
position_input = {
"ax": ax,
"ay": ay,
"wx": wx,
"wy": wy
}
##############
# (3) NN に入力して出力を得る
nn_output = self.position_nn(
position_input
)
##############
# (4) PositionStatus メッセージを生成して，publish
# 出力は (dx, dy, dθ）
# これをそのままパブリッシュ
msg = PositionStatus()
msg.dx = nn_output[0]
msg.dy = nn_output[1]
msg.dtheta = nn_output[2]
# publish
self.publisher.publish(
msg
)
#####################################
def main(args=None):
rclpy.init(args=args)
# object 化
node = Position()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
]]>
|