Read images from buffer at normal speed, adjust publish rate via param

include/usb_cam/usb_cam.hpp

@@ -238,6 +238,11 @@ class UsbCam
     return m_image.height;
   }
 
+  inline size_t get_frame_rate()
+  {
+    return m_framerate;
+  }
+
   inline size_t get_image_size_in_bytes()
   {
     return m_image.size_in_bytes;
@@ -401,6 +406,50 @@ class UsbCam
     return m_image.pixel_format;
   }
 
+  inline size_t set_frame_rate(const parameters_t & parameters)
+  {
+    std::shared_ptr<pixel_format_base> found_driver_format = nullptr;
+
+    formats::format_arguments_t args({
+        parameters.pixel_format_name,
+        parameters.image_width,
+        parameters.image_height,
+        m_image.number_of_pixels,
+        parameters.av_device_format,
+      });
+    // 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"
+      );
+    }
+
+    for (auto fmt : this->supported_formats()) {
+      if (fmt.v4l2_fmt.width == static_cast<size_t>(parameters.image_width) &&
+        fmt.v4l2_fmt.height == static_cast<size_t>(parameters.image_height) &&
+        fmt.v4l2_fmt.pixel_format == found_driver_format->v4l2()) {
+        return fmt.v4l2_fmt.discrete.denominator / fmt.v4l2_fmt.discrete.numerator;
+      }
+    }
+
+    throw std::invalid_argument(
+      "Specified resolution `" + std::to_string(parameters.image_width) +
+      "x" + std::to_string(parameters.image_height) +
+      "` is unsupported by `" + parameters.device_name + "`"
+    );
+  }
+
 private:
   void init_read();
   void init_mmap();

include/usb_cam/usb_cam_node.hpp

@@ -66,6 +66,7 @@ class UsbCamNode : public rclcpp::Node
   void assign_params(const std::vector<rclcpp::Parameter> & parameters);
   void set_v4l2_params();
   void update();
+  void publish();
   bool take_and_send_image();
   bool take_and_send_image_mjpeg();
 
@@ -91,6 +92,7 @@ class UsbCamNode : public rclcpp::Node
   std::shared_ptr<camera_info_manager::CameraInfoManager> m_camera_info;
 
   rclcpp::TimerBase::SharedPtr m_timer;
+  rclcpp::TimerBase::SharedPtr m_publish_timer;
 
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr m_service_capture;
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr m_parameters_callback_handle;

src/usb_cam.cpp

@@ -524,7 +524,7 @@ void UsbCam::configure(
   m_image.pixel_format = set_pixel_format(parameters);
   m_image.set_bytes_per_line();
   m_image.set_size_in_bytes();
-  m_framerate = parameters.framerate;
+  m_framerate = set_frame_rate(parameters);
 
   init_device();
 }

src/usb_cam_node.cpp

@@ -100,6 +100,7 @@ UsbCamNode::~UsbCamNode()
   m_camera_info_msg.reset();
   m_camera_info.reset();
   m_timer.reset();
+  m_publish_timer.reset();
   m_service_capture.reset();
   m_parameters_callback_handle.reset();
 
@@ -214,13 +215,26 @@ void UsbCamNode::init()
   // start the camera
   m_camera->start();
 
+  auto frame_rate = m_camera->get_frame_rate();
+  if (static_cast<size_t>(m_parameters.framerate) > frame_rate) {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(),
+      "Desired framerate " << m_parameters.framerate << " is higher than the camera's capability " <<
+        frame_rate << " fps");
+    m_parameters.framerate = frame_rate;
+  }
+
   // TODO(lucasw) should this check a little faster than expected frame rate?
   // TODO(lucasw) how to do small than ms, or fractional ms- std::chrono::nanoseconds?
-  const int period_ms = 1000.0 / m_parameters.framerate;
+  const int period_ms = 1000.0 / frame_rate;
   m_timer = this->create_wall_timer(
     std::chrono::milliseconds(static_cast<int64_t>(period_ms)),
     std::bind(&UsbCamNode::update, this));
   RCLCPP_INFO_STREAM(this->get_logger(), "Timer triggering every " << period_ms << " ms");
+  const int publish_period_ms = 1000.0 / m_parameters.framerate;
+  m_publish_timer = this->create_wall_timer(
+    std::chrono::milliseconds(static_cast<int64_t>(publish_period_ms)),
+    std::bind(&UsbCamNode::publish, this));
 }
 
 void UsbCamNode::get_params()
@@ -384,7 +398,6 @@ bool UsbCamNode::take_and_send_image()
 
   *m_camera_info_msg = m_camera_info->getCameraInfo();
   m_camera_info_msg->header = m_image_msg->header;
-  m_image_publisher->publish(*m_image_msg, *m_camera_info_msg);
   return true;
 }
 
@@ -406,8 +419,6 @@ bool UsbCamNode::take_and_send_image_mjpeg()
   *m_camera_info_msg = m_camera_info->getCameraInfo();
   m_camera_info_msg->header = m_compressed_img_msg->header;
 
-  m_compressed_image_publisher->publish(*m_compressed_img_msg);
-  m_compressed_cam_info_publisher->publish(*m_camera_info_msg);
   return true;
 }
 
@@ -438,6 +449,16 @@ void UsbCamNode::update()
     }
   }
 }
+
+void UsbCamNode::publish()
+{
+  if (m_parameters.pixel_format_name == "mjpeg") {
+    m_compressed_image_publisher->publish(*m_compressed_img_msg);
+    m_compressed_cam_info_publisher->publish(*m_camera_info_msg);
+  } else {
+    m_image_publisher->publish(*m_image_msg, *m_camera_info_msg);
+  }
+}
 }  // namespace usb_cam