Merge pull request #286 from ros-drivers/285-handle-unavailable-device

285 handle unavailable device, list available v4l2 devices

.github/workflows/build_test.yml

@@ -1,11 +1,11 @@
 name: ROS2 CI
 
 on:
-  workflow_dispatch:
   pull_request:
     branches:
       - 'ros2/*'
       - 'ros2'
+  workflow_dispatch:
 
 jobs:
   test_environment:

include/usb_cam/utils.hpp

@@ -29,13 +29,18 @@
 #ifndef USB_CAM__UTILS_HPP_
 #define USB_CAM__UTILS_HPP_
 
+extern "C" {
+#include <fcntl.h>  // for open()
+}
+
 #include <sys/ioctl.h>
 #include <sys/time.h>
 #include <cmath>
 #include <ctime>
 #include <cstring>
 #include <sstream>
 #include <string>
+#include <map>
 
 #include "linux/videodev2.h"
 
@@ -132,6 +137,47 @@ inline io_method_t io_method_from_string(const std::string & str)
   }
 }
 
+/// \brief List currently available valid V4L2 devices
+///   Can be used to check if a device string is valid before
+///   starting up.
+///
+///   Inspired by: http://stackoverflow.com/questions/4290834/how-to-get-a-list-of-video-capture-devices-web-cameras-on-linux-ubuntu-c
+inline std::map<std::string, v4l2_capability> available_devices()
+{
+  // Initialize vector of device strings to fill in
+  std::map<std::string, v4l2_capability> v4l2_devices;
+
+  // V4L2 spec says there can only be a maximum of 64 devices
+  const uint8_t MAX_DEVICES = 64;
+
+  for (uint8_t i = 0; i < MAX_DEVICES; i++) {
+    std::string device_str = "/dev/video" + std::to_string(i);
+    // See if device exists
+    if (access(device_str.c_str(), F_OK) != -1) {
+      int fd;
+      // Try and open device to test access
+      if ((fd = open(device_str.c_str(), O_RDONLY)) == -1) {
+        std::cerr << "Cannot open device: `" << device_str << "`, ";
+        std::cerr << "double-check read / write permissions for device" << std::endl;
+      } else {
+        struct v4l2_capability device_capabilities = {};
+
+        if (xioctl(fd, VIDIOC_QUERYCAP, &device_capabilities) == -1) {
+          std::cerr << "Could not retrieve device capabilities: `" << device_str;
+          std::cerr << "`" << std::endl;
+        } else {
+          v4l2_devices[device_str] = device_capabilities;
+        }
+      }
+    } else {
+      // device doesn't exist, break
+      break;
+    }
+  }
+
+  return v4l2_devices;
+}
+
 }  // namespace utils
 }  // namespace usb_cam
 

src/usb_cam_node.cpp

@@ -138,6 +138,23 @@ void UsbCamNode::init()
     m_camera_info->setCameraInfo(*m_camera_info_msg);
   }
 
+  // Check if given device name is an available v4l2 device
+  auto available_devices = usb_cam::utils::available_devices();
+  if (available_devices.find(m_parameters.device_name) == available_devices.end()) {
+    RCLCPP_ERROR_STREAM(
+      this->get_logger(),
+      "Device specified is not available or is not a vaild V4L2 device: `"
+        << m_parameters.device_name << "`"
+    );
+    RCLCPP_INFO(this->get_logger(), "Available V4L2 devices are:");
+    for (const auto & device : available_devices) {
+      RCLCPP_INFO_STREAM(this->get_logger(), "    " << device.first);
+      RCLCPP_INFO_STREAM(this->get_logger(), "        " << device.second.card);
+    }
+    rclcpp::shutdown();
+    return;
+  }
+
   m_image_msg->header.frame_id = m_parameters.frame_id;
   RCLCPP_INFO(
     this->get_logger(), "Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS",