# 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()
