機器人與機器視覺：在ROS中使用OpenCV
ROS 教程：如何在計算機視覺的機器人拾取和放置任務中使用 OpenCV計算機視覺是機器人技術的重要組成部分。它幫助機器人從相機資料中提取資訊以了解其環境。應用範圍從提取物體及其位置到檢查制造零件是否存在生産錯誤，再到檢測自動駕駛應用中的行人。
在本文中，我将展示如何在機器人中使用計算機視覺來使機器人手臂執行有點智能的拾取和放置任務。為此，我們将使用流行的開源庫OpenCV。我們将學習如何安裝 OpenCV，将其連接配接到 ROS 和 Kinect 深度攝像頭，應用計算機視覺算法提取必要的資訊并将其發送給我們的機器人。這将是一篇更長的文章，我将在其中詳細解釋所有内容，以描述它從頭到尾是如何工作的。對于那些隻對特定主題感興趣的人，在我們深入研究之前，我整理了一份内容清單。
在本教程中，我們将讨論：
- 什麼是OpenCV以及如何安裝它？
- 如何設定拾取和放置任務并運作應用程式
- 演練 OpenCV 節點實作
- 使用 cv_bridge 包在 ROS 和 OpenCV 之間轉換相機圖像
- 将計算機視覺算法應用于圖像
- 從圖像中提取邊界及其中心
- 提取對象位置
- 将 2D 圖像像素轉換為世界幀中的 3D 位置
- 将結果作為服務調用發送
- 如何從拾取和放置節點調用 OpenCV 節點服務
讓我們開始吧！
什麼是OpenCV以及如何安裝它？
OpenCV最初是在英特爾開始的，後來也由Willow Garage開發（這就是ROS發明的地方）。它提供了一組優化的計算機視覺算法，這些算法可移植且易于使用。它是開源的，自2012年以來，非營利組織 OpenCV.org 接管了支援。
如果您的機器上正确安裝了 ROS，則 OpenCV 也應該已經安裝。您可以通過運作以下指令來檢查： $ pkg-config --modversion opencv 如果這沒有産生任何結果，您可以嘗試： $ dpkg -l | grep libopencv
如果您發現 OpenCV 尚未安裝，請按照以下連結中的說明進行操作。
我使用ROS Melodic，預設情況下使用OpenCV 3，是以這是本教程中使用的版本。也可以使用帶有Melodic的較新的OpenCV 4，但需要更多的努力才能使其運作。其中一個附加步驟涉及我們用于轉換圖像cv_bridge。要使用OpenCV 4，您需要從源代碼克隆和編譯它。這是您将遇到的問題之一，可能還有更多。如果您仍然想嘗試使用 OpenCV 4 進行設定，您可能希望從堆棧溢出的這個問題開始。
如何設定和運作“拾取和放置”任務
我們希望開發我們的計算機視覺解決方案，并将其應用于真實的機器人場景中。是以，我們選擇了在前面的教程中也使用的拾取和放置任務，其中我們使用 Moveit C++ 界面處理拾取和放置任務，以及如何使用 Moveit 和深度攝像頭進行防撞。
下圖顯示了設定。左邊的藍色框應該放在右邊的白闆上。在第一個教程中，我們使用 Moveit 的C++接口對任務進行程式設計，并對盒子（起始位置）和闆（目标位置）的位置進行寫死。我們還告訴Moveit中間綠色障礙物的确切位置和大小，以便它可以找到繞過它的路徑。在第二個教程中，我們添加了一個深度攝像頭，可以自動檢測中間的障礙物，而無需事先告訴 Moveit。現在我們想使用從相機獲得的圖像，并使用計算機視覺算法自動提取盒子和闆的位置！
我們将首先完成設定并運作它，以便您可以看到程式的實際功能。然後，我将解釋實作的所有細節，并引導您完成代碼。
該項目由三個主要包組成：
- 适用于 UR5 機器人、抓手和 Kinect 深度攝像頭的 Moveit 配置包。如果您執行了前面的教程，則應該已安裝此包。如果沒有，請轉到有關如何建立 Moveit 配置包的教程的快速入門部分，然後按照步驟操作。之後，請檢視避免碰撞教程中有關如何将 Kinect 3D 相機添加到環境的部分，并複制前兩個步驟。
- OpenCV 節點，通過應用計算機視覺算法從相機圖像中提取對象位置并将其發送到我們的應用程式。在本文的後面，我将詳細解釋确切的實作。現在請繼續将此存儲庫克隆到 catkin 工作區的 src 檔案夾中：
~/catkin_ws/src$ git clone https://github.com/dairal/opencv_services.git
3. 包含拾取和放置任務的實際實作的“拾取和放置”節點。這将是一個新的包，但實作将與我們在前兩個教程中使用的實作非常相似。要安裝軟體包，還請将以下存儲庫克隆到 catkin 工作區的源檔案夾中：
~/catkin_ws/src$ git clone https://github.com/dairal/ur5_pick_and_place_opencv.git
現在我們已經有了我們需要的所有軟體包，繼續建構您的 catkin 工作區：
~/catkin_ws/src$ catkin_make
為了運作任務，我們需要在 Linux 終端中執行三個指令。首先，我們需要啟動我們的環境來模拟 Rviz 和 Gazebo 中的機器人。為此，我們使用兩個附加參數運作ur5_gripper_moveit_config包的 demo_gazebo.launch 檔案：
$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch world_name:=$(rospack find ur5_pick_and_place_opencv)/world/simple_pick_and_place_collision robot_spawn_position_z_axis:=1.21
希望您的環境看起來與上圖相似，并且您會看到在 Rviz 中可視化的深度相機資料。其次，我們啟動OpenCV節點。我們要運作的節點稱為opencv_extract_object_positions。通過在新終端中執行以下指令來啟動它：
$ rosrun opencv_services opencv_extract_object_positions
除了節點的輸出之外，您還将看到一個自動顯示在計算機上的圖像（更準确地說，它是多個圖像不斷被新圖像替換）。在這張圖檔上，你已經可以看到我們的計算機視覺算法的輸出。您可能會看到在盒子和盤子的中心繪制的點。這些是我們從圖像中提取的特征。opencv 節點已準備好将提取的倉位發送到我們的拾取和放置節點。是以，最後，在我們的第三個指令中，我們啟動包含在ur5_pick_and_place_opencv包中的pick_and_place_opencv節點。是以，請執行：
$ rosrun ur5_pick_and_place_opencv pick_and_place_opencv
現在你應該能夠看到機器人在盒子上方移動，用抓手撿起它，把它移到盤子上方，然後放在那裡。手指按下，一切都對你有用。
如果你做過前面的教程，你可能會認為這很好，但我們之前已經可以這樣做了，而無需将一些花哨的計算機視覺算法應用于相機圖像。确實如此，但現在您可以嘗試修改設定中藍框和盤子的位置。但是，不要太瘋狂。隻需将盒子和闆從其初始起始位置移動一點點，然後再次啟動拾取和放置節點，稍後我們将讨論我們不能過多地改變位置的原因。 我承認它不是最強大的應用程式，有時可能會失敗。需要作出更多努力，使其更加健壯和可靠。但是，該實作主要用于教育目的，我試圖使其盡可能易于了解，是以讓我們繼續，我将引導您完成代碼的每個細節。 建立 OpenCV 節點 這個節點應該為我們提供拾取和放置任務的開始和目标位置。在我們的例子中，這是藍色盒子和盤子的位置。讓我向您概述實作這一目标所需的步驟：
- 我們需要擷取設定中 Kinect 相機釋出的相機資料。這需要一些轉換，因為OpenCV使用的圖像格式與相機釋出的圖像格式不同。
- 我們将計算機視覺算法應用于圖像。我将向您解釋所選算法，解釋我為什麼選擇它們以及如何使用它們。
- 之後，我們将能夠從圖像中提取邊界及其中心。
- 中間步驟（我将在後面解釋）是提取框和目标的對象位置所必需的。
- 此時，我們在從相機獲得的圖像中擁有2D起點和目标位置。但是，如果我們想向機器人發送位置指令，我們需要将 2D 圖像位置轉換為機器人坐标框中的 3D 位置。我們将使用 ROS tf2 包進行此轉換。
- 獲得最終結果後，我們希望将其發送到拾取和放置節點。我們在 OpenCV 節點中實作一個服務調用，可以從外部子產品調用，并傳回起點和目标位置。
現在您已經大緻了解了必要的步驟，讓我們開始檢視實作。您可以在opencv_extract_object_positions.cpp檔案中找到OpenCV節點的完整代碼，您可以通過此連結在Github上檢視該檔案。 與往常一樣，我們通過初始化節點并建立一個節點處理程式來開始我們的 main 函數： 1. 使用 cv_bridge 包在 ROS 和 OpenCV 之間轉換相機圖像 首先，我們需要擷取 Kinect 相機釋出的相機資料。對于映像訂閱和釋出，建議使用 image_transport 包。用法與訂閱原始sensor_msgs/圖像消息非常相似。我們包括以下套餐： 然後，我們定義一個名為的常量字元串，該字元串儲存釋出 Kinect 資料的主題的名稱：IMAGE_TOPIC
接下來，我們建立一個與節點處理程式等效image_transport的對象，并使用它來建立訂閱消息的訂閱者。使用第二個參數，我們定義是否隻想擷取圖像的子集（例如，10 意味着我們每獲得第 10 個已釋出的圖像）。我們想要每個樣本，是以我們定義.第三個參數是我們在收到新圖像時要調用的函數的名稱。ImageTransportIMAGE_TOPIC1image_cb
現在讓我們看一下函數。ROS對中定義的圖像使用自己的消息格式，而OpenCV使用不同的格式實作為。它們不相容，但幸運的是，有cv_bridge包可以為我們進行格式之間的轉換，是以我們很樂意在.cpp檔案中包含cv_bridge：image_cbsensor_msgs/Imagecv::Mat
在函數内部，我們執行個體化一個 cv 圖像指針 .然後我們調用函數，該函數接受傳入的圖像消息，并将消息的編碼類型轉換為輸入參數。轉換後，它會傳回一個 cv 圖像指針，我們将其配置設定給我們之前建立的。如果抛出異常，我們會捕獲它。image_cbcv_ptrtoCvCopycv_ptr
最後，我們從深度攝像頭接收的圖像如下所示：
2. 将計算機視覺算法應用于圖像
現在我們要從此圖像中提取資訊。有不同的方法可以做到這一點。您可以将計算機視覺算法視為一個工具箱，可用于擷取所需的資訊。您可以通過不同的方式組合這些算法，并且通常有多種方法可以實作您的目标。 在函數内部（第 66 行），我們要使用的第一種方法是灰階縮放。它使應用進一步的算法更容易。此外，我們不需要顔色資訊，盡管可以通過從圖像中提取某些顔色（分别為藍色和白色）來考慮擷取盒子和闆的位置。這就是使用OpenCV完成灰階縮放的方式：apply_cv_algorithms OpenCV的功能将圖像從一個色彩空間轉換為另一個色彩空間。使用第三個參數，我們選擇轉換為灰階縮放的圖像，然後将其存儲在對象中。cvtColorcv::COLOR_BGR2GRAYimg_gray
接下來，我們采用邊緣檢測算法來檢測物體的輪廓。我們使用 Canny 算法，根據強度梯度提取邊緣。這意味着它将檢測彼此相鄰的像素之間的顯着差異作為邊緣。從上圖中可以看出，盒子和闆具有不同的亮度水準，但它們在背景下都很突出，這使得該算法特别适合。我們像這樣應用算法： 該函數接受四個參數。前兩個是輸入和輸出圖像。第三個和第四個定義門檻值并影響要解釋為邊的梯度的顯著性水準。特别是第四個參數定義了這種敏感性。在我們的例子中，我們把這是一個相當高的值。對象和背景之間的強烈對比使我們能夠設定如此高的值，其優點是我們可以“過濾掉”中間的碰撞框。cv::Canny350 我不會詳細介紹Canny算法的工作原理，但如果您有興趣，可以在這裡找到更多資訊。這是我們得到的結果：
3. 從圖像中提取邊界及其中心
即使生成的圖檔中的明亮區域看起來像一條實線，到目前為止，對計算機來說，它隻是一堆具有陡峭強度梯度的像素。在extract_centroids函數（第 77 行）中，我們現在想要連接配接這些點以獲得對象的實際邊界。是以，我們使用OpenCV的函數，它隻是将所有具有相同強度的像素連接配接到曲線上：findContours
讓我們看一下函數采用的最後三個參數。我們選擇輪廓檢索模式。如果其他形狀中存在形狀，則此參數很重要。如果是這種情況，您可能想知道它們之間的關系，即哪種形狀在哪個形狀中。使用第四個參數，我們選擇要存儲多少有關這些關系的資訊。然後将結果存儲在第 81 行執行個體化的層次結構矩陣中。我們的對象是分開的，是以我們并不真正關心那部分，是以這隻是給你一些上下文。findContourscv::RETR_TREE 使用最後一個參數，我們可以定義是否使用一些近似值來存儲可以節省記憶體的邊界。有了設定，我們沒有。生成的等值線存儲在矢量中。cv::CHAIN_APPROX_NONEcontours
現在我們找到了邊界，但最終我們想知道物體的位置。我們如何做到這一點？通過計算等值線的中心： 該算法并不像看起來那麼複雜。在第 86 行中，我們周遊我們發現的所有輪廓，可以計算圖像時刻。圖像時刻是特定形狀中像素強度的權重平均值。從這些時刻M中，我們可以計算形狀的中心（質心），如下所示： 我們對第 90 到 94 行中的所有時刻進行此計算。這就是我們為了獲得對象的質心而需要做的一切。 通過以下幾條線，我們繪制輪廓和質心，并在單獨的視窗中顯示它們： 這是我們得到的結果：
質心被繪制成小圓圈，我們可以看到物體的中心被很好地檢測到。但是，在圖像的底部，我們可以看到還檢測到其他形狀中心。我們得到結果之前的最後一步是選擇正确的結果。
4. 提取對象位置
為了選擇正确的質心，我們使用一種簡單的方法。我們隻是假設我們的起始位置（藍色框）在圖像的右上角，我們的目标位置（闆）在左上角。 為此，我編寫了一個簡單的函數，該函數将質心的向量和使用者定義的矩形作為輸入。然後，它計算矩形區域内所有質心的平均值。為什麼我們需要計算平均值？多個輪廓，是以，可能會為同一對象檢測到多個質心。為了解決這個問題，我們隻需取平均值即可。在圖像中無法清楚地看到額外的質心，因為它們彼此非常接近，但它們顯示在矢量中。
此函數被調用兩次。一次是起始區域的搜尋區域，一次是目标位置的區域： 我們隻需選擇左上角和右上角作為我們的搜尋區域：
這種方法非常簡單，當然還有更複雜的解決方案。例如，我們可以嘗試根據物體的形狀或顔色來檢測物體。 5. 将 2D 像素轉換為世界幀中的 3D 位置 讓我們花點時間拍拍自己的肩膀。我們成功獲得了2D圖像中物體的x和y坐标！但是這些與機器人有什麼關系呢？最後，我們希望向機器人控制一個3D位置，它應該在其中拾取或放置物體。是以，在将位置發送到拾取和放置節點之前，我們需要将 2D 圖像中的像素位置轉換為機器人架構中的 3D 位置。這需要兩個步驟：
- 我們使用點雲資料從 3D 像素資訊中擷取相機幀中的 2D 位置。
- 我們使用 tf3 包将相機幀中的 2D 位置轉換為機器人幀。
為了擷取相機幀中的 3D 位置，我們需要使用 Kinect 深度相機的點雲資料。是以，我們在主函數中訂閱它： 定義為 。我們擷取資料的每個樣本，我們的回調函數被調用。讓我們立即看一下此函數的第一部分：POINT_CLOUD2_TOPIC"/camera1/depth/points"point_cloud_cb
當我們收到一個新的點雲時，我們調用該函數兩次，然後列印出結果。此函數将 2D 像素轉換為相機幀中的 3D 位置，如下所示：pixel_to_3d_point 我們需要在與我們的 2D 圖像像素相關的點雲資料中找到深度資訊。資料存儲在一個數組中，我們必須找到正确的元素。該參數為我們提供了一行中的位元組數，同時告訴我們一個點的位元組數。pCloud.row_steppCloud.point_step 為了計算我們在第 154 行中的數組位置，我們首先将變量 v 乘以像素位置的 y 坐标。這意味着在數組中，我們向前跳轉到包含像素的行開始的位置。
然後，我們将 u（像素的 x 坐标）乘以得到 .pCloud.row_steppCloud.point_steparrayPosition 每個 3D 點由三個坐标組成。這些坐标中的每一個都是存儲在我們的位元組數組中的浮點值。一個浮點數通常有 4 個位元組，是以 y 坐标的偏移量為 4，因為它存儲在 x 坐标之後。每個坐标的确切偏移量存儲在數組中，在第 157 到 159 行中，我們将該偏移值添加到每個坐标的 中。現在我們将浮點數中的位元組數從資料數組中的正确位置複制到坐标。然後我們把它們存儲在我們的變量中。pCloud.fieldsarrayPositiongeometry_msgs::Pointp
在函數的第二部分中，我們将 3D 位置從相機幀轉換為機器人（基礎）幀：point_cloud_cb 幸運的是，有 tf2 轉換庫，它使幀之間的轉換變得容易。在使用轉換功能之前，我們包含一些庫标頭： 然後我們建立一個對象：tf_buffer 在 main 函數中，我們建立一個将對象作為參數的轉換：listenertf_buffer 現在，我們已準備好使用轉換功能。
讓我們看一下函數中的實作：transform_between_frames 該函數采用 3D 點和我們要轉換的幀。變換函數需要類型（類似于點對象，但具有附加資訊）作為輸入。geometry_msgs::PoseStamped 我們隻是通過添加有關幀和時間的資訊來建構 3D 點。然後我們可以調用我們之前建立的對象的函數。它需要構造的，我們想要轉換為的幀和一些逾時值。結果我們得到轉換後的輸出姿勢，也是類型 ，我們隻傳回位置。input_pose_stampedtransformtf_bufferinput_pose_stampedgeometry_msgs::PoseStamped 很酷，我們終于有一個機器人可以了解的3D位置。
6. 将結果倉位作為服務呼叫發送 請記住，我們執行所有這些操作是為了自動檢測拾取和放置任務的起點和目标位置。是以，我們需要将結果傳達給我們的拾取和放置節點。到目前為止，我們主要使用主題在節點之間交換資料，這對于多對多和單向通信非常有用。但是，隻有拾取和放置節點需要在開始運動之前根據請求擷取提取的位置一次。為此，服務比主題更适合。 我們命名我們的服務，并在 OpenCV 包的 srv 檔案夾中建立以下 box_and_target_position.srv 檔案：opencv_services/box_and_target_position
我們還需要在 CMakeLists.txt 檔案中添加該服務，以確定建立 opencv_node/box_and_target_position.h 頭檔案： 将此标頭包含在我們的.cpp檔案中後，我們可以使用該服務。為了調用它，我們在函數中通告它：main 當另一個節點調用服務時，将調用該函數：get_box_and_target_position
在其中，我們隻需将存儲在全局和變量中的提取位置配置設定給我們發送回請求服務的節點的響應。box_position_base_frametarget_position_base_frame 呼，那可是很多工作啊！OpenCV 節點中需要完成大量步驟，但現在我們終于可以将其用于拾取和放置任務了。
如何從拾取和放置節點調用 OpenCV 節點服務 正如我在開頭提到的，我們将使用與上一教程中幾乎相同的拾取和放置實作，并且僅将寫死的位置值替換為我們從 OpenCV 節點接收的值。在本教程中，我将僅讨論與上一教程相比我們需要添加的行。如果您對拾取和放置任務的确切實作方式感興趣，可以學習最後兩個教程（使用 Moveit C++ 界面的拾取和放置任務和如何使用深度相機與 Moveit 避免碰撞）。
您還可以在 Github 存儲庫中找到新的拾取和放置節點。 pick_and_place_opencv.cpp檔案包含完整的實作，包括描述性注釋。 要使用該服務，我們需要将opencv_node添加為 CMakeList 中的依賴項.txt： 在package.xml： 生成的服務标頭需要包含在.cpp檔案中： 在拾取和放置節點的主函數中，我們為服務構造一個服務用戶端，并在第 79 行建立服務對象。現在我們調用服務，如果成功，則列印出存儲在 中的接收位置。box_and_target_position_srv_clientbox_and_target_positionsrv.response 最後，我們将值而不是寫死位置配置設定給我們的盒子姿勢： 并針對目标姿勢：
結論
就是這樣，這就是如何将OpenCV與ROS一起使用，将計算機視覺內建到機器人應用程式中的方式。您已經看到，從相機圖像中提取資訊并将其轉換為機器人任務的有用資料需要相當多的工作。本教程中使用的方法肯定不是最優雅或最健壯的方法，但它向您展示了一種從頭到尾如何做到這一點的方法，這就是目标。
OpenCV是一個強大的庫，有更多的功能可供發現。在本教程中，我有意使用了經典的計算機視覺算法，但越來越多的機器學習方法被用于現代計算機視覺。在接下來的教程中，我們将讨論如何使用OpenCV在機器人應用程式中采用機器學習方法。在那之前，感謝您一直堅持到最後，并確定檢視機器人教程概述中的其他教程。與往常一樣，我很高興在評論中收到您的回報！
參考：
ROS 教程：如何在計算機視覺的機器人拾取和放置任務中使用 OpenCV - 機器人休閑 (roboticscasual.com)