in camera_pkg/src/camera_node.cpp [79:109]
void scanCameraIndex(const std::vector<int> cameraIdxList) {
for (auto idx : cameraIdxList){
RCLCPP_INFO(this->get_logger(), "Scanning Camera Index %d ", idx);
auto cap = cv::VideoCapture(idx, cv::CAP_V4L);
cv::Mat test_frame;
cap >> test_frame;
if(test_frame.empty() || !cap.isOpened()){
RCLCPP_ERROR(this->get_logger(), "Unable to create video capture stream on index: %d", idx);
continue;
}
// Add to valid video capture list
videoCaptureList_.push_back(cap);
videoCaptureList_.back().set(cv::CAP_PROP_FOURCC,
cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
// Add to valid video index list
videoIndexList_.push_back(idx);
}
switch(videoIndexList_.size()) {
case 0 :
RCLCPP_ERROR(this->get_logger(), "[Camera Package] No cameras detected.");
break;
case 1 :
RCLCPP_INFO(this->get_logger(), "[Camera Package] Single Camera Detected at index: %d", videoIndexList_[0]);
break;
case 2 :
RCLCPP_INFO(this->get_logger(), "[Camera Package] Stereo Cameras Detected at indexes: %d, %d", videoIndexList_[0], videoIndexList_[1]);
break;
default :
RCLCPP_ERROR(this->get_logger(), "[Camera Package] Error while detecting cameras.");
}
}