使用カメラ:ELP 720P デュアルレンズ USBカメラモジュール(推奨というわけでなくあくまで購入当時ステレオカメラで検索した時たまたまこれが出てきただけ。USBカメラを2台使ってもよいと思うが同期などは試してはいない。)
ROS2側:ROSがらUnityにカメラデータ通信の使用するカメラを二つにしただけ。
ただしIDはつないだ時によって異なるので、実用的には設定ファイルなどで変更出るようにする必要がある。
メッセージファイル
uint8[] img_data_l
uint8[] img_data_r
以下pythoncコード
# Copyright 2016 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# Copyright 2022 robot-creation-diary.com
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# 整数とテキストを送信するチュートリアルの内容から、ステレオカメラデータを取得し
# 変換・送信するように変更してある。
import rclpy
from rclpy.node import Node
from cpp_pubsub.msg import CameraMsg
import cv2
class MinimalPublisher(Node):
def __init__(self):
super().__init__('minimal_publisher')
self.publisher_ = self.create_publisher(CameraMsg, 'topic', 10)
timer_period = 0.033 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
# カメラ関連
# 0はカメラを指定する番号。状況に応じて変更のこと
self.video_capture_l = cv2.VideoCapture(0)
self.video_capture_l.set(cv2.CAP_PROP_FRAME_WIDTH, 320) # 解像度指定
self.video_capture_l.set(cv2.CAP_PROP_FRAME_HEIGHT, 240) # 解像度指定
# 1はカメラを指定する番号。状況に応じて変更のこと
self.video_capture_r = cv2.VideoCapture(1)
self.video_capture_r.set(cv2.CAP_PROP_FRAME_WIDTH, 320) # 解像度指定
self.video_capture_r.set(cv2.CAP_PROP_FRAME_HEIGHT, 240) # 解像度指定
def __del__(self):
pass
def timer_callback(self):
# カメラ関連
ret, frame_data_l = self.video_capture_l.read()
ret, frame_data_r = self.video_capture_r.read()
ret, encoded_data_l = cv2.imencode(".jpg",
frame_data_l,
(cv2.IMWRITE_JPEG_QUALITY, 50))
ret, encoded_data_r = cv2.imencode(".jpg",
frame_data_r,
(cv2.IMWRITE_JPEG_QUALITY, 50))
# データ送信関連
msg = CameraMsg()
# 送信用データの代入単純に=で代入しようとすると型が違うと怒られる
msg.img_data_l.frombytes(encoded_data_l)
msg.img_data_r.frombytes(encoded_data_r)
self.publisher_.publish(msg)
# コンソールに何も表示されないと不安なのでデータの内容を表示
self.get_logger().info('Publishing: %d %d' % (msg.img_data_l[0],
msg.img_data_r[0]))
def main(args=None):
rclpy.init(args=args)
minimal_publisher = MinimalPublisher()
rclpy.spin(minimal_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()