接下來的幾篇，我想要介紹怎麼利用無人機身上的感測器（主要是相機），讓無人機可以根據從環境收集到的數據來調整自身的位置與姿態。

首先，第一步是寫一個可以取得鏡頭畫面的節點。

Opencv 取得影像

OpenCV（Open Source Computer Vision Library）是一個專為電腦視覺的應用而設計的開源函式庫，它可以進行即時的影像處理（Image Processing）、電腦視覺（Computer Vision）以及圖型識別（Pattern Recognization）等工作，

雖然在系列文開頭假設各位熟悉 OpenCV 的各種函式、方法，這邊還是快速帶過常用的幾種功能。

讀取和儲存圖片

OpenCV 中的 imread() 方法可以用來開啟影像，imshow() 將儲存影像顯示出來，而 imwrite() 方法則是寫入並儲存圖片。

格式：

img = cv2.imread('file_name')

cv2.imshow('window_name', img)
cv2.waitKey(0) # 設定 0 表示不要主動關閉視窗

cv2.imwrite('file_name', img)
cv2.destroyAllWindows() # 關閉視窗

讀取和儲存影片

對於影片的處理，OpenCV 提供 VideoCapture() 方法來取得影片檔案，用 VideoWriter() 將擷取到的影像組成串流儲存成新影片。

fourcc() 是四字元代碼（Four-Character Code）的縮寫，用於指定視訊編碼格式。

對 VideoCapture 物件使用 read() 的方法讀取影片的每一幀，對 VideoWriter 物件使用 write() 儲存每一幀，然後透過 imshow() 將該幀的畫面顯示出來。最後再使用 release() 方法來釋放資源。

格式：

cap = cv2.VideoCapture(cam_param)
height, width, channel = cap.shape
fourcc = cv2.VideoWriter_fourcc(*'code')
out = cv2.VideoWriter('file_name', fourcc, fps, (width,  height))

if not cap.isOpened():
    print("Cannot open camera")
    exit()

while True:
    retval, frame = cap.read() # 讀取影片的每一幀
    out.write(frame)
    cv2.imshow('selfie', frame)
    if cv2.waitKey(1) == ord('q'):
        break

cap.release() # 所有作業都完成後，釋放資源
out.release() # 所有作業都完成後，釋放資源
cv2.destroyAllWindows() # 關閉視窗

使用 CvBridge 取得 ROS 影像訊息

要取得無人機的影像，就需要 ROS 提供的 CvBridge 這個工具，它可以將 ROS 的影像訊息格式（sensor_msgs/Image）轉換成 OpenCV 的影像格式（Numpy 陣列），以方便在 ROS 節點中進行影像處理。

同樣也是前往套件的 src 目錄裡建立取得鏡頭畫面的節點，可以參考我寫的 get_video.py 。

#! /usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from cv_bridge import CvBridge, CvBridgeError
import cv2
import os
import time

from sensor_msgs.msg import Image

class VideoCapture:
    def __init__(self):
        rospy.init_node('video_capture')
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber('/ardrone/down_camera/image_raw', Image, self.image_callback)

        # OpenCV 視訊編碼器與設定
        self.video_writer = None
        self.fourcc = cv2.VideoWriter_fourcc('X', 'V', 'I', 'D')
        timestamp = time.strftime("%H%M%S")
        self.dir_path = '~/catkin_ws/src/my_siimulation/videos/' # 要改成套件的完整路徑
        self.file_name = os.path.join(self.dir_path, f'output_video_{timestamp}.avi')
        self.frame_size = None  # 將在接收到第一幀時初始化

        rospy.on_shutdown(self.clean_up)

    # 處理影像的回呼函數
    def image_callback(self, msg):
        try:
            # 取得 ROS 訊息並轉換成 OpenCV 影像
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

            # 第一次錄影時建立 videos 目錄
            if not os.path.exists(self.dir_path):
                os.makdirs(self.dir_path)

            # 初始化 video_writer
            if self.video_writer is None:
                height, width, channel = cv_image.shape
                self.frame_size = (width, height)
                self.video_writer = cv2.VideoWriter(self.file_name, self.fourcc, 30.0, self.frame_size)
            
                if not self.video_writer.isOpened():
                    rospy.logerr("Fail to open video writer")
                    rospy.signal_shutdown("Fail to open video writer")

            self.video_writer.write(cv_image)

            cv2.imshow("ARDrone Down Camera", cv_image)
            key = cv2.waitKey(1)

            # 在影像視窗按下 q 鍵時關閉 ROS 節點
            if key == ord('q'):
                rospy.signal_shutdown("User requested shutdown")

        except Exception as e:
            rospy.logerr("Exception: {0}".format(e))
    
    def clean_up(self):
        # 釋放影片資源
        if self.video_writer is not None:
            self.video_writer.release()
            rospy.loginfo(f"Video saved as {self.file_name}")
        
        # 釋放視窗資源
        cv2.destroyAllWindows()
        rospy.loginfo("Shutting down video capture node")

if __name__ == "__main__":
    try:
        video_capture = VideoCapture()
        rospy.spin()

    except rospy.ROSInterruptException:
        pass

VideoCapture 類別的 __init__()

• CvBridge 轉換影像格式：

  轉換格式需要先建立 CvBridge 物件，並且在後面接收到訊息後，將訊息轉換成 OpenCV 的影像格式。

  格式：

  bridge = CvBridge()
  cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
  

• ROS 訂閱者節點：

  self.image_sub = rospy.Subscriber('/ardrone/down_camera/image_raw', Image, self.image_callback) 定義接收到 /ardrone/down_camera/image_raw 的訊息後，會直接呼叫 image_callback，讓回呼函式執行預先定義的動作。

• ROS 節點關閉時的清理：

  rospy.on_shutdown(self.clean_up) 這行設定在 ROS 節點被關閉（如 Ctrl+C 結束程式）時，自動執行 clean_up() 以釋放資源。

image_callback() 和 clean_up() 的程式碼說明

• image_callback() 取得影像後的動作：

  一開始先將 ROS 訊息轉換成 OpenCV 的影像格式，接著使用 VideoWriter() 將影像儲存起來，同時也對一些例外或錯誤狀況設定日誌訊息輸出。

• clean_up() 清理系統資源：

  這個函式會在訂閱者節點關閉後立後馬上被呼叫，清除視訊資源和關閉視窗。

將 python 檔轉成可執行檔並執行

同樣也取得影像的程式轉換成可執行檔，以便之後能直接使用 rosrun 執行。

$ chmod +x get_video.py

現在，我們可以再次啟動前面寫過的啟動檔，執行錄影程式和控制程式，接著就獲得無人機視角的影片啦。

現在我們可以知道無人機看到的世界長什麼樣子了，但是在這樣空蕩蕩、到處都長差不多的人工草皮上，要讓無人機單憑這樣的線索就知道自己在哪裡，還是太為難它了。

下一篇就會來介紹一個在機器人領域經常使用的標記工具，讓無人機在調整自己的位置和姿態可以有所依據。