簡介這份源碼面向工業(yè)自動化與機器視覺方向的開發(fā)者提供基于MVS SDK與ROS框架的海康機器人工業(yè)相機驅(qū)動程序用于解決相機在ROS系統(tǒng)中的集成、參數(shù)配置與圖像數(shù)據(jù)采集問題適合具備一定C與ROS基礎(chǔ)的中高級開發(fā)者參考使用。資源包共25個文件約64KB包含7個YAML配置文件與7個launch啟動文件分別用于相機參數(shù)標(biāo)定與節(jié)點啟動配置2個C源文件與頭文件實現(xiàn)底層通信邏輯另有cfg動態(tài)參數(shù)、rviz可視化配置、package.xml及許可證等輔助文件目錄結(jié)構(gòu)清晰便于按模塊查閱。目前已有444人學(xué)習(xí)下載。通過這份源碼讀者可以快速將?？倒I(yè)相機接入ROS系統(tǒng)理解曝光、分辨率、幀率等參數(shù)配置方式并參考室內(nèi)外多場景啟動配置與標(biāo)定文件完成圖像采集與傳輸功能的定制開發(fā)。1. 從一臺 HIKROBOT 相機接進 ROS 開始說起手里有一臺 HIKROBOT 工業(yè)相機想讓它出現(xiàn)在 ROS 的 topic 列表里這件事聽起來簡單實際動手會發(fā)現(xiàn)中間隔著一整套 SDK 和一層驅(qū)動封裝。MVS SDK 是?？禉C器人官方提供的相機訪問接口負(fù)責(zé)枚舉設(shè)備、取流、解析圖像ROS 負(fù)責(zé)把這些圖像變成標(biāo)準(zhǔn)話題讓下游的標(biāo)定、檢測、建圖節(jié)點能直接訂閱。兩者之間的橋就是我們要寫的驅(qū)動程序。這篇筆記面向的是已經(jīng)裝好 ROS、手頭有 HIKROBOT 相機、想把圖像接進自己機器人系統(tǒng)的工程師。不管你是做 ROS 小車自主導(dǎo)航仿真還是做機械臂視覺抓取只要相機是?？禉C器人系列這套驅(qū)動思路都能復(fù)用。我會從 SDK 選型講到節(jié)點實現(xiàn)再到參數(shù)配置和踩坑記錄目標(biāo)是讓你看完能自己寫出一份可用的驅(qū)動源碼而不是只會調(diào)別人的包。2. MVS SDK 與 ROS 驅(qū)動架構(gòu)先想清楚橋怎么搭2.1 為什么不能直接在 ROS 節(jié)點里調(diào) SDKMVS SDK 提供的是 C 接口和 C 接口底層走的是 USB3.0 或千兆網(wǎng)口取流是阻塞式的。ROS 節(jié)點的回調(diào)機制是事件驅(qū)動的如果直接在回調(diào)里調(diào) SDK 的取流函數(shù)整個節(jié)點會被卡死話題發(fā)布頻率直接掉到零。常見做法是開一個獨立的采集線程SDK 在這個線程里循環(huán)取幀取到之后放進一個線程安全的隊列ROS 的定時器或另一個線程從隊列里取圖并發(fā)布。這樣采集和發(fā)布解耦即使下游節(jié)點處理慢也不會把相機取流拖垮。另一個原因是生命周期管理。SDK 需要顯式地枚舉設(shè)備、創(chuàng)建句柄、開始取流、停止取流、銷毀句柄。ROS 節(jié)點的啟動和關(guān)閉是異步的如果不在析構(gòu)函數(shù)里正確釋放 SDK 資源下次啟動相機時會出現(xiàn)設(shè)備被占用的情況。我一般會把 SDK 的初始化和反初始化封裝在一個 RAII 風(fēng)格的類里構(gòu)造時枚舉并打開設(shè)備析構(gòu)時自動停止取流并銷毀句柄。2.2 驅(qū)動節(jié)點的最小功能集一個能用的 HIKROBOT ROS 驅(qū)動至少要做到四件事枚舉并打開指定序列號的相機、配置曝光和增益、持續(xù)取流并轉(zhuǎn)成 sensor_msgs/Image、支持動態(tài)參數(shù)調(diào)整。枚舉和打開是基礎(chǔ)曝光和增益決定了圖像質(zhì)量取流轉(zhuǎn) Image 是核心動態(tài)參數(shù)讓使用者不用重啟節(jié)點就能調(diào)亮度。圖像格式方面MVS SDK 輸出的原始數(shù)據(jù)可能是 Mono8、BayerRG8、RGB8 等。ROS 的 sensor_msgs/Image 需要指定 encoding如果 encoding 寫錯下游的 cv_bridge 轉(zhuǎn)換會直接報錯或者出綠屏。我一般會在驅(qū)動里根據(jù) SDK 返回的像素格式做一次映射Mono8 直接對應(yīng) mono8BayerRG8 對應(yīng) bayer_rggb8RGB8 對應(yīng) rgb8。如果相機輸出的是 Bayer 格式可以在驅(qū)動里做一次 debayer也可以直接發(fā) Bayer 讓下游處理前者省下游算力后者保留原始數(shù)據(jù)。2.3 采集線程與 ROS 發(fā)布線程的同步采集線程和發(fā)布線程之間需要一個緩沖區(qū)。最簡單的做法是用一個固定大小的環(huán)形隊列采集線程往隊尾寫發(fā)布線程從隊頭讀。隊列滿的時候采集線程丟棄最舊的一幀保證永遠取到的是最新圖像。這樣做的好處是延遲可控不會因為下游卡頓導(dǎo)致圖像越積越多。同步機制用 std::mutex 加 std::condition_variable 就夠了。采集線程取到幀后加鎖寫入隊列然后 notify 發(fā)布線程。發(fā)布線程等待條件變量被喚醒后加鎖取幀解鎖后再發(fā)布話題。注意發(fā)布話題的時候不要持鎖否則下游回調(diào)如果耗時會把采集線程也堵住。// 環(huán)形隊列的簡化實現(xiàn) class FrameBuffer { public: explicit FrameBuffer(size_t capacity) : capacity_(capacity) {} void push(const cv::Mat frame) { std::unique_lockstd::mutex lock(mutex_); if (queue_.size() capacity_) { queue_.pop(); // 丟棄最舊幀 } queue_.push(frame.clone()); cond_.notify_one(); } bool pop(cv::Mat frame, int timeout_ms) { std::unique_lockstd::mutex lock(mutex_); if (!cond_.wait_for(lock, std::chrono::milliseconds(timeout_ms), [this] { return !queue_.empty(); })) { return false; } frame queue_.front(); queue_.pop(); return true; } private: size_t capacity_; std::queuecv::Mat queue_; std::mutex mutex_; std::condition_variable cond_; };這段代碼的關(guān)鍵參數(shù)是 capacity_我一般設(shè)成 3 到 5。設(shè)太小會導(dǎo)致發(fā)布線程偶爾取不到幀設(shè)太大則延遲增加。timeout_ms 設(shè)成 100 毫秒左右超時后發(fā)布線程可以發(fā)一幀空圖或者直接跳過避免節(jié)點卡死。frame.clone() 是必須的因為 SDK 的緩沖區(qū)會被復(fù)用不 clone 的話下一幀會覆蓋當(dāng)前數(shù)據(jù)。3. 從零寫一個 HIKROBOT ROS 驅(qū)動節(jié)點3.1 工程結(jié)構(gòu)與依賴配置我一般把驅(qū)動做成一個標(biāo)準(zhǔn)的 ROS package目錄結(jié)構(gòu)如下include 放頭文件src 放源文件launch 放啟動文件config 放參數(shù) yaml。CMakeLists.txt 里需要 find_package 找 catkin、roscpp、sensor_msgs、cv_bridge、image_transport還要手動指定 MVS SDK 的頭文件和庫路徑。MVS SDK 在 Linux 下的默認(rèn)安裝路徑通常是 /opt/MVS頭文件在 /opt/MVS/include庫在 /opt/MVS/lib/64。CMakeLists.txt 里用 include_directories 把 SDK 頭文件目錄加進去用 link_directories 加庫目錄target_link_libraries 里鏈接 MvCameraControl。注意 SDK 的庫名在不同版本里可能略有差異常見的是 libMvCameraControl.so。如果編譯時報 undefined reference先用 ldconfig -p | grep MvCamera 確認(rèn)庫是否在系統(tǒng)路徑里。package.xml 里聲明依賴roscpp、sensor_msgs、cv_bridge、image_transport、camera_info_manager。如果要做相機標(biāo)定還要加 camera_calibration_parsers。這些依賴用 rosdep 裝就行不用手動編譯。3.2 相機枚舉與打開的代碼實現(xiàn)枚舉設(shè)備用 MV_CC_EnumDevices 函數(shù)傳入 MV_GIGE_DEVICE | MV_USB_DEVICE 可以同時枚舉網(wǎng)口和 USB 相機。枚舉結(jié)果是一個 MV_CC_DEVICE_INFO_LIST 結(jié)構(gòu)里面包含設(shè)備數(shù)量和信息數(shù)組。遍歷數(shù)組用 MV_CC_GetDeviceInfo 或者直接讀結(jié)構(gòu)體里的序列號找到目標(biāo)相機。打開相機分兩步先 MV_CC_CreateHandle 創(chuàng)建句柄再 MV_CC_OpenDevice 打開設(shè)備。網(wǎng)口相機還需要設(shè)置心跳時間和最優(yōu)包大小USB 相機不用。打開之后設(shè)置觸發(fā)模式為連續(xù)采集用 MV_CC_SetEnumValue 設(shè)置 TriggerMode 為 0。然后 MV_CC_StartGrabbing 開始取流。// 枚舉并打開指定序列號的相機 bool openCameraBySerial(const std::string target_serial) { MV_CC_DEVICE_INFO_LIST device_list; int ret MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, device_list); if (ret ! MV_OK || device_list.nDeviceNum 0) { ROS_ERROR(No HIKROBOT camera found, ret0x%x, ret); return false; } for (unsigned int i 0; i device_list.nDeviceNum; i) { MV_CC_DEVICE_INFO* info device_list.pDeviceInfo[i]; std::string serial; if (info-nTLayerType MV_GIGE_DEVICE) { serial reinterpret_castchar*(info-SpecialInfo.stGigEInfo.chSerialNumber); } else if (info-nTLayerType MV_USB_DEVICE) { serial reinterpret_castchar*(info-SpecialInfo.stUsb3VInfo.chSerialNumber); } if (serial target_serial) { handle_ nullptr; ret MV_CC_CreateHandle(handle_, info); if (ret ! MV_OK) { return false; } ret MV_CC_OpenDevice(handle_); if (ret ! MV_OK) { return false; } // 設(shè)置連續(xù)采集模式 MV_CC_SetEnumValue(handle_, TriggerMode, 0); MV_CC_StartGrabbing(handle_); return true; } } return false; }這段代碼里 target_serial 從 ROS 參數(shù)服務(wù)器讀取默認(rèn)值可以留空留空時打開第一個枚舉到的相機。MV_CC_EnumDevices 的第二個參數(shù)是設(shè)備類型掩碼如果只接 USB 相機可以只傳 MV_USB_DEVICE減少枚舉時間。打開網(wǎng)口相機時如果網(wǎng)絡(luò)環(huán)境復(fù)雜建議先設(shè)置 MV_CC_SetIntValue 調(diào)整包大小否則可能出現(xiàn)丟包導(dǎo)致圖像花屏。3.3 取流線程與圖像格式轉(zhuǎn)換取流線程是一個 while 循環(huán)每輪調(diào)用 MV_CC_GetImageBuffer 獲取一幀拿到之后根據(jù)像素格式轉(zhuǎn)成 cv::Mat推入 FrameBuffer然后調(diào)用 MV_CC_FreeImageBuffer 釋放 SDK 緩沖區(qū)。循環(huán)的退出條件是一個 atomic 標(biāo)志析構(gòu)時置為 false 并 join 線程。像素格式轉(zhuǎn)換是容易翻車的地方。MVS SDK 返回的 MV_FRAME_OUT_INFO_EX 結(jié)構(gòu)里有 enPixelType 字段常見值有 PixelType_Gvsp_Mono8、PixelType_Gvsp_BayerRG8、PixelType_Gvsp_RGB8_Packed。Mono8 直接構(gòu)造 cv::Mat 就行BayerRG8 需要用 cv::cvtColor 做 debayerRGB8 直接拷貝。如果格式不認(rèn)識打警告并跳過這一幀不要硬轉(zhuǎn)。void grabLoop() { MV_FRAME_OUT_INFO_EX frame_info; unsigned char* data new unsigned char[width_ * height_ * 3]; while (running_) { int ret MV_CC_GetImageBuffer(handle_, frame_info, 1000); if (ret ! MV_OK) { ROS_WARN_THROTTLE(1.0, GetImageBuffer failed, ret0x%x, ret); continue; } cv::Mat image; if (frame_info.enPixelType PixelType_Gvsp_Mono8) { image cv::Mat(frame_info.nHeight, frame_info.nWidth, CV_8UC1, frame_info.pBufAddr).clone(); } else if (frame_info.enPixelType PixelType_Gvsp_BayerRG8) { cv::Mat bayer(frame_info.nHeight, frame_info.nWidth, CV_8UC1, frame_info.pBufAddr); cv::cvtColor(bayer, image, cv::COLOR_BayerRG2BGR); } else if (frame_info.enPixelType PixelType_Gvsp_RGB8_Packed) { cv::Mat rgb(frame_info.nHeight, frame_info.nWidth, CV_8UC3, frame_info.pBufAddr); cv::cvtColor(rgb, image, cv::COLOR_RGB2BGR); } else { MV_CC_FreeImageBuffer(handle_, frame_info); continue; } buffer_.push(image); MV_CC_FreeImageBuffer(handle_, frame_info); } delete[] data; }MV_CC_GetImageBuffer 的超時參數(shù)設(shè)成 1000 毫秒超時后返回非 MV_OK打一條節(jié)流警告繼續(xù)循環(huán)。注意 MV_CC_FreeImageBuffer 必須在 clone 之后調(diào)用否則 SDK 緩沖區(qū)不會被釋放取幾幀之后就會卡住。Bayer 轉(zhuǎn) BGR 用 COLOR_BayerRG2BGR如果相機實際是 BayerGB 或 BayerBG轉(zhuǎn)出來的顏色會偏需要根據(jù)相機型號調(diào)整。3.4 曝光增益的動態(tài)參數(shù)配置曝光和增益通過 dynamic_reconfigure 暴露給用戶。在 cfg 文件里定義 exposure_time 和 gain 兩個參數(shù)范圍根據(jù)相機型號設(shè)定一般曝光時間 100 到 100000 微秒增益 0 到 20 分貝?；卣{(diào)函數(shù)里用 MV_CC_SetFloatValue 設(shè)置 ExposureTime用 MV_CC_SetFloatValue 設(shè)置 Gain。注意設(shè)置曝光時間之前要先把 ExposureAuto 設(shè)為 Off否則手動值會被自動曝光覆蓋。增益同理GainAuto 設(shè)為 Off。設(shè)置完之后可以讀回來確認(rèn)MV_CC_GetFloatValue 獲取當(dāng)前值如果和設(shè)定值差太多說明相機不支持這個范圍需要調(diào)整 cfg 里的邊界。#!/usr/bin/env python # dynamic_reconfigure 的 cfg 文件示例 PACKAGE hikrobot_camera from dynamic_reconfigure.parameter_generator_catkin import * gen ParameterGenerator() gen.add(exposure_time, double_t, 0, Exposure time in microseconds, 5000.0, 100.0, 100000.0) gen.add(gain, double_t, 0, Gain in dB, 0.0, 0.0, 20.0) exit(gen.generate(PACKAGE, hikrobot_camera, Camera))這個 cfg 文件生成之后ROS 會自動創(chuàng)建參數(shù)服務(wù)器接口和 rqt_reconfigure 界面?；卣{(diào)里先關(guān)自動模式再設(shè)值順序不能反。如果相機正在取流設(shè)置曝光時間會立即生效不需要停止取流。但有些型號的相機在取流過程中修改增益會導(dǎo)致一兩幀圖像異常這是正?，F(xiàn)象不用緊張。4. 避坑與排查那些讓我熬夜的相機驅(qū)動問題4.1 現(xiàn)象節(jié)點啟動后話題有發(fā)布但圖像全黑原因通常是曝光時間設(shè)得太短或者鏡頭光圈沒打開。MVS SDK 默認(rèn)曝光可能是自動模式但驅(qū)動里如果手動設(shè)了一個很小的值比如 100 微秒在室內(nèi)光線下就是全黑。另一個可能是像素格式映射錯了比如把 Mono8 當(dāng)成了 BayerRG8 轉(zhuǎn)轉(zhuǎn)出來就是全黑或者全綠。解決方法是先用 MVS 客戶端連上相機確認(rèn)能正常出圖記下當(dāng)時的曝光時間和像素格式。然后在驅(qū)動里把曝光時間設(shè)成同一個值像素格式映射也改成一致的。如果 MVS 客戶端也黑那就是硬件問題檢查鏡頭蓋和光圈。4.2 現(xiàn)象取流幾十秒后卡死話題停止發(fā)布這是 SDK 緩沖區(qū)沒有釋放的典型癥狀。MV_CC_GetImageBuffer 拿到的幀必須用 MV_CC_FreeImageBuffer 釋放如果中間某條分支 return 之前忘了釋放緩沖區(qū)很快耗盡取流就卡住了。我見過有人在格式轉(zhuǎn)換失敗時直接 continue沒有調(diào) FreeImageBuffer跑十幾秒就掛。解決方法是把 FreeImageBuffer 放在所有分支都會走到的地方或者用 RAII 包裝一下。最簡單的做法是在 continue 之前也調(diào)一次 FreeImageBuffer確保每一幀都被釋放。另外檢查 clone 是否真的做了如果直接把 SDK 緩沖區(qū)指針包成 cv::Mat 而不 clone下一幀會覆蓋當(dāng)前數(shù)據(jù)下游看到的就是花屏。4.3 現(xiàn)象網(wǎng)口相機丟包圖像下半部分花屏千兆網(wǎng)口相機在帶寬不足或者網(wǎng)卡沒開巨幀的時候容易丟包。圖像上半部分正常下半部分花屏或者綠屏基本就是丟包。MVS SDK 有重傳機制但驅(qū)動里如果包大小設(shè)得太大重傳也救不回來。解決辦法是在打開相機之后設(shè)置最優(yōu)包大小。用 MV_CC_GetOptimalPacketSize 獲取推薦值然后用 MV_CC_SetIntValue 設(shè)置 GevSCPSPacketSize。同時確認(rèn)網(wǎng)卡 MTU 設(shè)成了 9000 以上交換機也支持巨幀。如果還是丟把相機的幀率降下來或者換 USB 相機。4.4 現(xiàn)象編譯時報找不到 MvCameraControl.hMVS SDK 安裝之后頭文件在 /opt/MVS/include但 CMakeLists.txt 里沒有把這個路徑加進去。或者 SDK 裝在了非默認(rèn)路徑比如 /home/user/MVS那就需要手動指定。解決方法是先在終端確認(rèn)頭文件位置find / -name MvCameraControl.h 2/dev/null。然后在 CMakeLists.txt 里用 include_directories 加上對應(yīng)目錄link_directories 加上庫目錄。如果用的是 catkin 的 package記得把 SDK 路徑寫成絕對路徑不要用相對路徑否則編譯目錄變了就找不到。4.5 現(xiàn)象動態(tài)參數(shù)調(diào)了沒反應(yīng)dynamic_reconfigure 的回調(diào)函數(shù)里設(shè)置了曝光時間但圖像亮度沒變化。原因可能是 ExposureAuto 沒有關(guān)掉自動曝光把手動值覆蓋了?；蛘呋卣{(diào)函數(shù)里用的句柄是空的因為相機還沒打開。解決方法是先在回調(diào)里檢查句柄是否有效無效就直接返回。然后在設(shè)置曝光之前先調(diào) MV_CC_SetEnumValue 把 ExposureAuto 設(shè)為 Off再設(shè) ExposureTime。設(shè)置完之后用 MV_CC_GetFloatValue 讀回來如果讀回來的值和設(shè)定值差很多說明相機不支持這個范圍需要調(diào)整 cfg 文件的邊界。5. 進階技巧讓驅(qū)動更穩(wěn)、更好用5.1 用 camera_info_manager 管理標(biāo)定參數(shù)驅(qū)動發(fā)布圖像的同時最好也發(fā)布 camera_info。camera_info_manager 可以從 yaml 文件加載標(biāo)定參數(shù)也可以從參數(shù)服務(wù)器讀。我一般會在 launch 文件里指定 camera_info_url指向一個標(biāo)定好的 yaml。這樣下游的 image_proc 節(jié)點可以直接用不用再手動傳參數(shù)。標(biāo)定用 camera_calibration 包就行跑一次生成 yaml然后放到 config 目錄。驅(qū)動啟動時用 camera_info_manager 加載發(fā)布到 camera_info 話題。注意圖像話題和 camera_info 話題的命名要對應(yīng)比如 /camera/image_raw 對應(yīng) /camera/camera_info。5.2 多相機同步的簡單做法如果系統(tǒng)里有兩臺以上的 HIKROBOT 相機可以用 MV_CC_SetEnumValue 設(shè)置 TriggerMode 為 1觸發(fā)源設(shè)為軟觸發(fā)或硬觸發(fā)。軟觸發(fā)用 MV_CC_SetCommandValue 發(fā)觸發(fā)命令硬觸發(fā)需要接線。ROS 這邊可以用 message_filters 做時間同步把多路圖像按時間戳對齊。我一般會先讓每臺相機獨立跑起來確認(rèn)單臺穩(wěn)定之后再上同步。同步的時候注意時間戳來源最好都用 ROS 的 ros::Time::now()不要用相機內(nèi)部時間戳否則不同相機的時間基準(zhǔn)不一致同步會失敗。5.3 性能調(diào)優(yōu)的幾個參數(shù)取流線程的優(yōu)先級可以調(diào)高用 pthread_setschedparam 設(shè)成 SCHED_FIFO避免被其他線程搶占。FrameBuffer 的容量設(shè)成 3 左右太大延遲高太小丟幀。圖像發(fā)布用 image_transport 的 raw 模式就行如果需要壓縮再開 compressed 插件。如果 CPU 占用太高可以在驅(qū)動里做一次降采樣或者把 debayer 放到 GPU 上做。我一般會先看 top 里節(jié)點的 CPU 占用如果超過單核 80%就要考慮優(yōu)化。最簡單的優(yōu)化是減少 clone 次數(shù)能移動就不要拷貝。5.4 一個我常用的啟動文件模板launch node namehikrobot_camera pkghikrobot_camera typehikrobot_camera_node outputscreen param nameserial_number valueDA1234567 / param namepixel_format valueBayerRG8 / param nameframe_rate value30 / param namecamera_info_url valuefile://$(find hikrobot_camera)/config/camera.yaml / /node /launch這個模板里 serial_number 留空就打開第一個相機pixel_format 根據(jù)實際相機填frame_rate 設(shè)成相機支持的值。camera_info_url 用 file:// 前綴指向 yaml 文件。啟動之后用 rostopic hz 看發(fā)布頻率用 rqt_image_view 看圖像是否正常。寫驅(qū)動這件事最深的教訓(xùn)就是不要相信“默認(rèn)值”。曝光、增益、包大小、像素格式每一個默認(rèn)值都可能讓你調(diào)半天。我現(xiàn)在的習(xí)慣是拿到一臺新相機先用 MVS 客戶端把所有參數(shù)看一遍記下來然后在驅(qū)動里顯式設(shè)置不依賴 SDK 的默認(rèn)行為。這樣即使換一臺同型號的相機驅(qū)動也能直接跑起來。希望幫到你。本文還有配套的精品資源點擊獲取