ソケット通信を使ってWebカメラの映像をやりとりする

# プログラムの概要
ソケット通信を用いてUSBカメラの映像をサーバーに送信します．

:::note warn
※ROS2ノードにしていますが，私情でそうしているだけで，ROS2のフレームワークを使用していません．
:::

# 手順
## 送信側（ロボット）
### 1. ワークスペース作成
ワークスペースの作成を行います．
今回は「cam_ws」をワークスペース名にしています．

```ワークスペースの作成.bash
$ cd
$ mkdir -p ~/cam_ws/src/
```

### 2. パッケージ作成
今回，ノード名を「my_cam_node」，
パッケージ名を「my_cam」とします．

```パッケージの作成.bash
$ cd ~/cam_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_cam_node my_cam
```

### 3. プログラミング
#### 1. ``setup.py`` ファイルの編集

``setup.py``ファイルを編集します．

```setup.pyファイルの編集
$ cd ~/cam_ws/src/my_cam
$ nano setup.py
```
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要がある．今回パッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっています．

```setup.py
entry_point={
    'console_scripts': [
        'my_cam_node = my_cam.my_cam_node:main' 
    ],
},
```

今回は複数のプログラムファイルを扱うため，以下のように変更します．
```setup.py
entry_point={
    'console_scripts': [
        'my_cam_node = my_cam.my_cam_node:main',
        'usbcam = my_cam.usbcam:main'
    ],
},
```
::: note info
１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を必ず変更する必要があります．
:::

#### 2. ソースコード作成
パッケージ作成時に自動的に作られている``my_cam_node.py``を編集します．
``` my_cam_node.py へ移動.bash
$ cd ~/cam_ws/src/my_cam/my_cam
$ nano my_cam_node.py
```

下記のように編集します．このとき，**13行目の``'受信側のIPアドレス'``の部分を，画像を受信するコンピュータのIPアドレスに変更**してください．
``` my_cam_node.py
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``を新規作成します．

```ファイルの新規作成.bash
$ nano usbcam.py
```

```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. ビルド
ビルドを行います．

```ビルド.bash
$ cd ~/cam_Ws
$ colcon build
```

:::note warn
ワークスペース名直下のディレクトリ以外はビルドできません
また，初めてビルドしたとき build, install, log ディレクトリが作成されます
:::

## 6. 設定ファイルの反映
アンダーレイ設定ファイル（ROS2システムの設定）は，.bashrc に記述済みとします．
※下記が.bashrcに既に書かれているという意味
source /opt/ros/foxy/setup.bash


```.bashrcの編集.bash
$ nano ~/.bashrc
```

以下を追記する．
```~/.bashrc
source ~/cam_ws/install/setup.bash
```

## 受信側（コンピュータ）
続いて，受信側の設定をしていきます．

### 1. ワークスペース作成

ワークスペースの作成を行います．
今回は，「view_ws」をワークスペース名にしています．
```ワークスペースの作成.bash
$ cd
$ mkdir -p ~/view_ws/src/
```

#### 2. パッケージ作成
今回，ノード名を「my_view_node」
パッケージ名を「my_view」とします．
```パッケージの作成.bash
$ cd ~/view_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_view_node my_view
```

### 3. プログラミング
####  1. ``setup.py``ファイルの編集
``setup.py``ファイルを編集します．

```setup.pyファイルの編集
$ cd ~/view_ws/src/my_view
$ nano setup.py
```
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要がある．今回パッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっています．

```setup.py
entry_point={
    'console_scripts': [
        'my_view_node = my_view.my_view_node:main',
    ],
},
```

今回は複数のプログラムファイルを扱うため，以下のように変更します．
```setup.py
entry_point={
    'console_scripts': [
        'my_view_node = my_view.my_view_node:main',
        'usbcam = my_view.usbcam:main'
    ],
},
```

### 2. ソースコード作成
　パッケージ作成時に自動的に作られている``my_view_node.py``を編集します．

```my_view_node.pyへ移動
$ cd ~/view_ws/src/my_view/my_view
$ nano my_view_node.py
```

下記のように変更します．
```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``という名前で新規に作成します．
```shell
$ nano usbcam.py
```
下記のように記述して保存します．
```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. ビルド
ビルドを行います．

```ビルド.bash
$ cd ~/view_ws
$ colcon build
```

### 6. 設定ファイルの反映
オーバーレイ設定ファイル（自作ワークスペースの設定）を``.bashrc``に記載

```.bashrcの編集.bash
$ nano ~/.bashrc
```

下記のコマンドを追記します．
```
$ source ~/view_ws/install/setup.bash
```
を追加してから反映させます．

# ノードの実行
:::note warn
実行する際には，必ず**受信側のプログラムの起動を確認してから送信側のプログラムを実行する**ようにしてください．
:::
## 1. 受信側のプログラムを起動する
```受信プログラム起動.bash
$ ros2 run my_view my_view_node
```
起動が完了すると，``Waiting for connection...``と表示されるはずです．
起動が完了したら手順2に進んでください．
## 2. 送信側のプログラムを起動する
```送信プログラム起動.bash
$ 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行目を以下のように変更します．
```my_cam_node.py
        self.usbcam = usbcam(適当なポート番号,'受信側のIPアドレス')
```
``'適当なポート番号'``の部分には，適当なポート番号（例：5001,5002等）を入力してください．

#### 2. ``my_view_node.py``の変更
``my_view_node.py``の14行目を以下のように変更します．

```my_view_node.py
self.usbcam_receive = usbcam_rcv(先ほど設定したポート番号)
```
:::note warn
ポート番号は受信側と送信側で合わせる必要があります．
:::

## ``Connected by(to)``の表示は出るが，エラーで終了してしまう．
受信側のIPアドレスが正しく取得できていない可能性があります．

### 解決方法
受信側の``usbcam.py``の51行目を変更します．
```usbcam.py
        # IP Adress
        my_ip  = socket.gethostbyname(socket.gethostname())
        # my_ip = ''
```
手順通りに進めていれば，このような記載がされているはずです．
51行目のコメントアウトを外し，``''``の中に**受信側のIPアドレス**を入力してください．


# 参考
https://zenn.dev/pinto0309/scraps/101f111b7f4deb