diff --git a/config/params_2.yaml b/config/params_2.yaml index a4d2cca5..b3c6cb88 100644 --- a/config/params_2.yaml +++ b/config/params_2.yaml @@ -4,8 +4,8 @@ framerate: 15.0 io_method: "mmap" frame_id: "camera2" - pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats - av_device_format: "YUV422P" + pixel_format: "mjpeg2rgb" + av_device_format: "YUV422P" image_width: 640 image_height: 480 camera_name: "test_camera2" diff --git a/include/usb_cam/formats/pixel_format_base.hpp b/include/usb_cam/formats/pixel_format_base.hpp index 62d09b84..7524cadf 100644 --- a/include/usb_cam/formats/pixel_format_base.hpp +++ b/include/usb_cam/formats/pixel_format_base.hpp @@ -84,7 +84,7 @@ class pixel_format_base /// @brief String value of V4L2 capture pixel format /// @return std::string V4L2 capture pixel format - inline std::string v4l2_str() { return usb_cam::conversions::FCC2S(m_v4l2); } + inline std::string v4l2_str() {return usb_cam::conversions::FCC2S(m_v4l2);} /// @brief Name of output pixel (encoding) format to ROS /// @return diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index 3568b350..1c8ef91d 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -61,6 +61,40 @@ namespace usb_cam using usb_cam::utils::io_method_t; using usb_cam::formats::pixel_format_base; +/// @brief Add more formats here and to driver_supported_formats below as +/// they are added to this library +using usb_cam::formats::RGB8; +using usb_cam::formats::YUYV; +using usb_cam::formats::YUYV2RGB; +using usb_cam::formats::UYVY; +using usb_cam::formats::UYVY2RGB; +using usb_cam::formats::MONO8; +using usb_cam::formats::MONO16; +using usb_cam::formats::Y102MONO8; +using usb_cam::formats::RAW_MJPEG; +using usb_cam::formats::MJPEG2RGB; +using usb_cam::formats::M4202RGB; + + +/// @brief list all supported formats that this driver supports +std::vector> driver_supported_formats( + const formats::format_arguments_t & args = formats::format_arguments_t()) +{ + std::vector> fmts = { + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + }; + return fmts; +} typedef struct { @@ -273,6 +307,51 @@ class UsbCam return m_supported_formats; } + /// @brief Check if the given format is supported by this device + /// If it is supported, set the m_pixel_format variable to it. + /// @param format the format to check if it is supported + /// @return bool true if the given format is supported, false otherwise + inline bool set_pixel_format(const formats::format_arguments_t & args) + { + bool result = false; + + std::shared_ptr found_driver_format = nullptr; + + // First check if given format is supported by this driver + for (auto driver_fmt : driver_supported_formats(args)) { + if (driver_fmt->name() == args.name) { + found_driver_format = driver_fmt; + } + } + + if (found_driver_format == nullptr) { + // List the supported formats of this driver for the user before throwing + std::cerr << "This driver supports the following formats:" << std::endl; + for (auto driver_fmt : driver_supported_formats(args)) { + std::cerr << "\t" << driver_fmt->name() << std::endl; + } + throw std::invalid_argument( + "Specified format `" + args.name + "` is unsupported by this ROS driver" + ); + } + + std::cout << "This device supports the following formats:" << std::endl; + for (auto fmt : this->supported_formats()) { + // Always list the devices supported formats for the user + std::cout << "\t" << fmt.format.description << " "; + std::cout << fmt.v4l2_fmt.width << " x " << fmt.v4l2_fmt.height << " ("; + std::cout << fmt.v4l2_fmt.discrete.denominator / fmt.v4l2_fmt.discrete.numerator << " Hz)"; + std::cout << std::endl; + + if (fmt.v4l2_fmt.pixel_format == found_driver_format->v4l2()) { + result = true; + m_image.pixel_format = found_driver_format; + } + } + + return result; + } + /// @brief Set pixel format from parameter list. Required to have logic within UsbCam object /// in case pixel format class requires additional information for conversion function /// (e.g. number of pixels, width, height, etc.) @@ -280,50 +359,21 @@ class UsbCam /// @return pixel format structure corresponding to a given name inline std::shared_ptr set_pixel_format(const parameters_t & parameters) { - using usb_cam::formats::RGB8; - using usb_cam::formats::YUYV; - using usb_cam::formats::YUYV2RGB; - using usb_cam::formats::UYVY; - using usb_cam::formats::UYVY2RGB; - using usb_cam::formats::MONO8; - using usb_cam::formats::MONO16; - using usb_cam::formats::Y102MONO8; - using usb_cam::formats::RAW_MJPEG; - using usb_cam::formats::MJPEG2RGB; - using usb_cam::formats::M4202RGB; - - if (parameters.pixel_format_name == "rgb8") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "yuyv") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "yuyv2rgb") { - // number of pixels required for conversion method - m_image.pixel_format = std::make_shared(m_image.number_of_pixels); - } else if (parameters.pixel_format_name == "uyvy") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "uyvy2rgb") { - // number of pixels required for conversion method - m_image.pixel_format = std::make_shared(m_image.number_of_pixels); - } else if (parameters.pixel_format_name == "mjpeg") { - m_image.pixel_format = std::make_shared( - formats::get_av_pixel_format_from_string(parameters.av_device_format)); - } else if (parameters.pixel_format_name == "mjpeg2rgb") { - m_image.pixel_format = std::make_shared( - m_image.width, m_image.height, - formats::get_av_pixel_format_from_string(parameters.av_device_format)); - } else if (parameters.pixel_format_name == "m4202rgb") { - m_image.pixel_format = std::make_shared( - m_image.width, m_image.height); - } else if (parameters.pixel_format_name == "mono8") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "mono16") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "y102mono8") { - m_image.pixel_format = std::make_shared(m_image.number_of_pixels); - } else { + // create format arguments structure + formats::format_arguments_t format_args({ + parameters.pixel_format_name, + parameters.image_width, + parameters.image_height, + m_image.number_of_pixels, + parameters.av_device_format, + }); + + // Look for specified pixel format + if (!this->set_pixel_format(format_args)) { throw std::invalid_argument( - "Unsupported pixel format specified: " + - parameters.pixel_format_name); + "Specified format `" + parameters.pixel_format_name + "` is unsupported by the " + + "selected device `" + parameters.device_name + "`" + ); } return m_image.pixel_format; diff --git a/src/ros2/usb_cam_node.cpp b/src/ros2/usb_cam_node.cpp index 0133ba31..1a1cca97 100644 --- a/src/ros2/usb_cam_node.cpp +++ b/src/ros2/usb_cam_node.cpp @@ -189,20 +189,10 @@ void UsbCamNode::init() rclcpp::shutdown(); return; } + // configure the camera m_camera->configure(m_parameters, io_method); - RCLCPP_INFO(this->get_logger(), "This devices supproted formats:"); - for (auto fmt : m_camera->supported_formats()) { - RCLCPP_INFO( - this->get_logger(), - "\t%s: %d x %d (%d Hz)", - fmt.format.description, - fmt.v4l2_fmt.width, - fmt.v4l2_fmt.height, - fmt.v4l2_fmt.discrete.denominator / fmt.v4l2_fmt.discrete.numerator); - } - set_v4l2_params(); // start the camera