┌────────┐
┌───│カメラドライバ │
│出力 └────────┘
│ （処理）
┌──│───────────────────┐
│ ↓ ┌────────┐ │
│┌───┐ │ │ ┌───┐│
││ＩＮ │→│ アルゴリズ │→│ＯＵＴ││
│└───┘ │ │ └───┘│
└────────┘ │ │
└───────＜カメラストア＞────│──┘
Ｐｕｂ│
│
┌───────────────────↓──┐
│ 視覚ＮＮストア│
│ 通信ネットワーク │
└──────────────────────┘
アルゴリズム：
0.1秒周期で，画像を撮影してそれを publish
START
│
カメラの configuration
カメラ起動
│
┌─────→│
│ ↓
│ カメラで静止画撮影
│ ↓
│ 画像をパブリッシュ
│ │
└──────┘
- アルゴリズムの周期
0.1秒 (10 Hz)
- ROS2 ノード
ノード名： camera
publisher：
camera_publisher (→ 視覚NNストア)
トピック：/camera/data
データ型：my_robot_msgs/CameraData
（中身は sensor_msgs/Image と同じ)
- IN・OUT
IN：
カメラ画像
← カメラドライバ
OUT：
カメラ画像
─(publish)→ 視覚NN ストア
- OUT データの型
publish の型 CameraData であり，
nt16 width : 画像の幅 (＝320)
int16 height : 画像の高さ (＝240)
int8[320×240] data : 画素列
- プログラム
<![CDATA[
#####################################
# Python の ROS2ライブラリ
import rclpy
# ＜ユーザ定義のデータ型を使う＞用
from rclpy.node import Node
# データ型をimport
# subscriber のデータ型
# 中身は sensor_msgs/Image と同じ
from my_robot_msgs.msg import CameraData
# カメラ を使うためのライブラリ
# (使う カメラ に応じて変更)
# ここでは PiCamera2 を使う
from picamera2 import Picamera2
#####################################
# ストアをノード Camera に設定
class Camera(Node):
############################
# コントラクタ
# (クラスがもつ属性の定義と初期化を行う）
def __init__(self):
super().__init__('camera')
##################
# カメラの設定
# カメラオブジェクト作成
self.picam2 = Picamera2()
config =
self.picam2.create_preview_configuration(
main={"size": (320, 240)}
)
self.picam2.configure(config)
# カメラを起動
self.picam2.start()
##################
# publisher
# publish するデータは，CameraData
# publisher の設定
self.publisher =
# 形式 : (データ型, topic, バッファ)
self.create_publisher(
CameraData,
'/camera/data',
10
)
##################
# タイマーの設定
# timer は 「一定周期で関数を呼び出す仕組み」。
# データ取得間隔を 10Hz (0.1秒周期) とする
# そこで ｢0.1秒｣ を publish の周期とする
self.timer =
self.create_timer(
0.1,
self.publish_camera_data
)
###############################
# サブルーチン
##########################
# 撮影から publish まで
def publish_camera_data(self):
# 撮影・画像取得
frame = self.get_image()
# CameraData作成
msg = CameraData()
msg.height = frame.shape[0]
msg.width = frame.shape[1]
# RGBデータを1次元化
msg.data = frame.flatten().tolist()
# publish
self.publisher.publish(msg)
#####################################
def main(args=None):
rclpy.init(args=args)
# object 化
node = Camera()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
]]>
|