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",