プログラムの概要
ソケット通信を用いてUSBカメラの映像をサーバーに送信します．
※ROS2ノードにしていますが，私情でそうしているだけで，ROS2のフレームワークを使用していません．
手順
送信側（ロボット）
1. ワークスペース作成
ワークスペースの作成を行います．
今回は「cam_ws」をワークスペース名にしています．
$ cd
$ mkdir -p ~/cam_ws/src/
2. パッケージ作成
今回，ノード名を「my_cam_node」，
パッケージ名を「my_cam」とします．
$ cd ~/cam_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_cam_node my_cam
3. プログラミング
1. setup.py ファイルの編集
setup.pyファイルを編集します．
$ cd ~/cam_ws/src/my_cam
$ nano setup.py
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要がある．今回パッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっています．
entry_point={
'console_scripts': [
'my_cam_node = my_cam.my_cam_node:main'
],
},
今回は複数のプログラムファイルを扱うため，以下のように変更します．
entry_point={
'console_scripts': [
'my_cam_node = my_cam.my_cam_node:main',
'usbcam = my_cam.usbcam:main'
],
},
１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を必ず変更する必要があります．
2. ソースコード作成
パッケージ作成時に自動的に作られているmy_cam_node.pyを編集します．
$ cd ~/cam_ws/src/my_cam/my_cam
$ nano my_cam_node.py
下記のように編集します．このとき，13行目の'受信側のIPアドレス'の部分を，画像を受信するコンピュータのIPアドレスに変更してください．
import rclpy
from rclpy.node import Node
import cv2
from my_cam.usbcam import usbcam
class MainNode(Node):
def __init__(self):
super().__init__('my_cam_node')
self.usbcam = usbcam(5000,'受信側のIPアドレス')
self.usbcam.settings(15,640,480)
while True:
self.usbcam.publish_camImg()
def main():
rclpy.init()
node = MainNode()
rclpy.shutdown()
また，usb_cam_node.pyと同じディレクトリにusbcam.pyを新規作成します．
$ nano usbcam.py
import cv2
import socket
import numpy as np
import struct
# send
class usbcam:
def __init__(self,PORT,hostIP):
self.JPEG_QUALITY = 90
self.cam = cv2.VideoCapture(0)
# socket settings
self.s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
self.s.connect((hostIP,PORT))
self.ret = None
print('Connected to',hostIP)
# camera settings
def settings(self,fps,width,height):
self.cam.set(cv2.CAP_PROP_FRAME_WIDTH,width)
self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT,height)
self.cam.set(cv2.CAP_PROP_FPS,fps)
def publish_camImg(self):
self.ret,frame = self.cam.read()
# フレームをJPEG形式にエンコード
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), self.JPEG_QUALITY]
result, frame_data = cv2.imencode('.jpg', frame, encode_param)
# フレームのサイズを送信
size = len(frame_data)
self.s.send(struct.pack('!I', size))
# フレームデータを送信
self.s.sendall(frame_data)
return True
# exit
def exit_cam(self):
self.s.close()
self.cam.release()
# receive
class usbcam_rcv:
def __init__ (self,PORT):
# IP Adress
my_ip = socket.gethostbyname(socket.gethostname())
# my_ip = ''
self.s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
self.s.bind((my_ip,PORT))
self.s.listen(1)
print('Waiting for connection...')
print('IP Adress:',my_ip)
self.conn,addr = self.s.accept()
print('Connected by',addr)
def rcv_Image(self):
# フレームデータのサイズを受信
data = self.conn.recv(4)
data_error = False
try:
size = struct.unpack('!I', data)[0]
except struct.error:
data_error = True
# フレームデータを受信
data = b''
if data_error == False:
while len(data) < size:
packet = self.conn.recv(size - len(data))
if not packet:
break
data += packet
# 受信したデータをデコード
frame_data = np.frombuffer(data, dtype=np.uint8)
# データを画像に変換
frame = cv2.imdecode(frame_data, 1)
else:
# dummy
frame = np.zeros(640,480,3)
return frame
def exit(self):
self.s.close()
if __name__ == "__main__":
usbcam_receive = usbcam_rcv(5000)
while True:
img = usbcam_receive.rcv_Image()
cv2.imshow('frame',img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# publish cam image sample
"""
if __name__ == "__main__":
usbcam = usbcam(5000,'Your IP Adress')
while usbcam.ret:
usbcam.publish_camimg()
if cv2.waitKey(1) == 27:
break
usbcam.exit_cam()
"""
5. ビルド
ビルドを行います．
$ cd ~/cam_Ws
$ colcon build
ワークスペース名直下のディレクトリ以外はビルドできません
また，初めてビルドしたとき build, install, log ディレクトリが作成されます
6. 設定ファイルの反映
アンダーレイ設定ファイル（ROS2システムの設定）は，.bashrc に記述済みとします．
※下記が.bashrcに既に書かれているという意味
source /opt/ros/foxy/setup.bash
$ nano ~/.bashrc
以下を追記する．
source ~/cam_ws/install/setup.bash
受信側（コンピュータ）
続いて，受信側の設定をしていきます．
1. ワークスペース作成
ワークスペースの作成を行います．
今回は，「view_ws」をワークスペース名にしています．
$ cd
$ mkdir -p ~/view_ws/src/
2. パッケージ作成
今回，ノード名を「my_view_node」
パッケージ名を「my_view」とします．
$ cd ~/view_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_view_node my_view
3. プログラミング
1. setup.pyファイルの編集
setup.pyファイルを編集します．
$ cd ~/view_ws/src/my_view
$ nano setup.py
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要がある．今回パッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっています．
entry_point={
'console_scripts': [
'my_view_node = my_view.my_view_node:main',
],
},
今回は複数のプログラムファイルを扱うため，以下のように変更します．
entry_point={
'console_scripts': [
'my_view_node = my_view.my_view_node:main',
'usbcam = my_view.usbcam:main'
],
},
2. ソースコード作成
パッケージ作成時に自動的に作られているmy_view_node.pyを編集します．
$ cd ~/view_ws/src/my_view/my_view
$ nano my_view_node.py
下記のように変更します．
import cv2
import rclpy
from rclpy.node import Node
from my_view.usbcam import usbcam_rcv
class MainNode(Node):
def __init__(self):
super().__init__('my_view_node')
#self.timer = self.create_timer(0.001,self.timer_callback)
self.usbcam_receive = usbcam_rcv(5000)
while True:
img = self.usbcam_receive.rcv_Image()
cv2.imshow('frame',img)
cv2.waitKey(1)
def main():
rclpy.init()
node = MainNode()
rclpy.shutdown()
また，もう1つはusbcam.pyという名前で新規に作成します．
$ nano usbcam.py
下記のように記述して保存します．
import cv2
import socket
import numpy as np
import struct
# send
class usbcam:
def __init__(self,PORT,hostIP):
self.JPEG_QUALITY = 90
self.cam = cv2.VideoCapture(0)
# socket settings
self.s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
self.s.connect((hostIP,PORT))
self.ret = None
print('Connected to',hostIP)
# camera settings
def settings(self,fps,width,height):
self.cam.set(cv2.CAP_PROP_FRAME_WIDTH,width)
self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT,height)
self.cam.set(cv2.CAP_PROP_FPS,fps)
def publish_camImg(self):
self.ret,frame = self.cam.read()
# フレームをJPEG形式にエンコード
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), self.JPEG_QUALITY]
result, frame_data = cv2.imencode('.jpg', frame, encode_param)
# フレームのサイズを送信
size = len(frame_data)
self.s.send(struct.pack('!I', size))
# フレームデータを送信
self.s.sendall(frame_data)
return True
# exit
def exit_cam(self):
self.s.close()
self.cam.release()
# receive
class usbcam_rcv:
def __init__ (self,PORT):
# IP Adress
my_ip = socket.gethostbyname(socket.gethostname())
# my_ip = ''
self.s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
self.s.bind((my_ip,PORT))
self.s.listen(1)
print('Waiting for connection...')
print('IP Adress:',my_ip)
self.conn,addr = self.s.accept()
print('Connected by',addr)
def rcv_Image(self):
# フレームデータのサイズを受信
data = self.conn.recv(4)
data_error = False
try:
size = struct.unpack('!I', data)[0]
except struct.error:
data_error = True
# フレームデータを受信
data = b''
if data_error == False:
while len(data) < size:
packet = self.conn.recv(size - len(data))
if not packet:
break
data += packet
# 受信したデータをデコード
frame_data = np.frombuffer(data, dtype=np.uint8)
# データを画像に変換
frame = cv2.imdecode(frame_data, 1)
else:
# dummy
frame = np.zeros(640,480,3)
return frame
def exit(self):
self.s.close()
if __name__ == "__main__":
usbcam_receive = usbcam_rcv(5000)
while True:
img = usbcam_receive.rcv_Image()
cv2.imshow('frame',img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# publish cam image sample
"""
if __name__ == "__main__":
usbcam = usbcam(5000,'Your IP Adress')
while usbcam.ret:
usbcam.publish_camimg()
if cv2.waitKey(1) == 27:
break
usbcam.exit_cam()
"""
5. ビルド
ビルドを行います．
$ cd ~/view_ws
$ colcon build
6. 設定ファイルの反映
オーバーレイ設定ファイル（自作ワークスペースの設定）を.bashrcに記載
$ nano ~/.bashrc
下記のコマンドを追記します．
$ source ~/view_ws/install/setup.bash
を追加してから反映させます．
ノードの実行
実行する際には，必ず受信側のプログラムの起動を確認してから送信側のプログラムを実行するようにしてください．
1. 受信側のプログラムを起動する
$ ros2 run my_view my_view_node
起動が完了すると，Waiting for connection...と表示されるはずです．
起動が完了したら手順2に進んでください．
2. 送信側のプログラムを起動する
$ ros2 run my_cam my_cam_node
受信先と送信元の端末が正しく接続されると，connected by(to) '受信（送信）先のIPアドレス'という表示がされ，ロボット側のカメラ映像が送信されてくるはずです．
トラブルシューティング
ポートが開かない
他のアプリケーション等で使用しようとしているポートが埋まってしまっている場合，プログラム上でポートが開けず，エラーになってしまうことがあります．
その場合，開くポートを変更する必要があります．
解決方法
my_cam_node.py（送信側のプログラム）をmy_view_node.py（受信側のプログラム）を以下のようにそれぞれ変更し，使用するポートを変更してください．
1. my_cam_node.pyの変更
my_cam_node.pyの13行目を以下のように変更します．
self.usbcam = usbcam(適当なポート番号,'受信側のIPアドレス')
'適当なポート番号'の部分には，適当なポート番号（例：5001,5002等）を入力してください．
2. my_view_node.pyの変更
my_view_node.pyの14行目を以下のように変更します．
self.usbcam_receive = usbcam_rcv(先ほど設定したポート番号)
ポート番号は受信側と送信側で合わせる必要があります．
Connected by(to)の表示は出るが，エラーで終了してしまう．
受信側のIPアドレスが正しく取得できていない可能性があります．
解決方法
受信側のusbcam.pyの51行目を変更します．
# IP Adress
my_ip = socket.gethostbyname(socket.gethostname())
# my_ip = ''
手順通りに進めていれば，このような記載がされているはずです．
51行目のコメントアウトを外し，''の中に受信側のIPアドレスを入力してください．
参考