Publish raw mjpeg stream directly via compressed image topic

CMakeLists.txt

@@ -30,6 +30,7 @@ endif()
 find_package(OpenCV REQUIRED)
 
 find_package(PkgConfig REQUIRED)
+pkg_check_modules(avcodec REQUIRED libavcodec)
 pkg_check_modules(avutil REQUIRED libavutil)
 pkg_check_modules(swscale REQUIRED libswscale)
 

include/usb_cam/formats/av_pixel_format_helper.hpp

@@ -38,6 +38,8 @@ extern "C" {
 #include "libavutil/pixfmt.h"
 }
 
+#include "usb_cam/constants.hpp"
+
 #define stringify(name) #name
 
 
@@ -958,6 +960,152 @@ inline AVPixelFormat get_av_pixel_format_from_string(const std::string & str)
   return STR_2_AVPIXFMT.find(fullFmtStr)->second;
 }
 
+/// @brief Get ROS PixelFormat from AVPixelFormat.
+/// @param avPixelFormat AVPixelFormat
+/// @return String specifying the ROS pixel format.
+inline std::string get_ros_pixel_format_from_av_format(const AVPixelFormat & avPixelFormat)
+{
+  std::string ros_format = "";
+
+  switch (avPixelFormat) {
+    default:
+      {
+        ros_format = usb_cam::constants::UNKNOWN;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_RGB24:
+      {
+        ros_format = usb_cam::constants::RGB8;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_RGBA:
+      {
+        ros_format = usb_cam::constants::RGBA8;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_BGR24:
+      {
+        ros_format = usb_cam::constants::BGR8;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_BGRA:
+      {
+        ros_format = usb_cam::constants::BGRA8;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_GRAY8:
+      {
+        ros_format = usb_cam::constants::MONO8;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_GRAY16BE:
+      {
+        ros_format = usb_cam::constants::MONO16;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_YUV422P:
+      {
+        ros_format = usb_cam::constants::YUV422;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_YUV420P:
+      {
+        ros_format = usb_cam::constants::NV21;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_YUV444P:
+      {
+        ros_format = usb_cam::constants::NV24;
+      }
+      break;
+  }
+
+  return ros_format;
+}
+
+/// @brief Get the number of channels from AVPixelFormat.
+/// @param avPixelFormat AVPixelFormat
+/// @return Number of channels as uint8
+inline uint8_t get_channels_from_av_format(const AVPixelFormat & avPixelFormat)
+{
+  uint8_t channels = 1;
+
+  switch (avPixelFormat) {
+    default:
+    case AVPixelFormat::AV_PIX_FMT_GRAY8:
+    case AVPixelFormat::AV_PIX_FMT_GRAY16BE:
+      {
+        channels = 1;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_YUV422P:
+    case AVPixelFormat::AV_PIX_FMT_YUV420P:
+    case AVPixelFormat::AV_PIX_FMT_YUV444P:
+      {
+        channels = 2;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_RGB24:
+    case AVPixelFormat::AV_PIX_FMT_BGR24:
+      {
+        channels = 3;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_RGBA:
+    case AVPixelFormat::AV_PIX_FMT_BGRA:
+      {
+        channels = 4;
+      }
+      break;
+  }
+
+  return channels;
+}
+
+/// @brief Get the pixel bit depth from AVPixelFormat.
+/// @param avPixelFormat AVPixelFormat
+/// @return Bit depth as uint8
+inline uint8_t get_bit_depth_from_av_format(const AVPixelFormat & avPixelFormat)
+{
+  uint8_t bit_depth = 8;
+
+  switch (avPixelFormat) {
+    default:
+    case AVPixelFormat::AV_PIX_FMT_GRAY8:
+    case AVPixelFormat::AV_PIX_FMT_RGB24:
+    case AVPixelFormat::AV_PIX_FMT_BGR24:
+    case AVPixelFormat::AV_PIX_FMT_RGBA:
+    case AVPixelFormat::AV_PIX_FMT_BGRA:
+    case AVPixelFormat::AV_PIX_FMT_YUV422P:
+    case AVPixelFormat::AV_PIX_FMT_YUV420P:
+    case AVPixelFormat::AV_PIX_FMT_YUV444P:
+      {
+        bit_depth = 8;
+      }
+      break;
+
+    case AVPixelFormat::AV_PIX_FMT_GRAY16BE:
+      {
+        bit_depth = 16;
+      }
+      break;
+  }
+
+  return bit_depth;
+}
+
 }  // namespace formats
 }  // namespace usb_cam
 

include/usb_cam/formats/mjpeg.hpp

@@ -52,13 +52,29 @@ extern "C" {
 #include "usb_cam/usb_cam.hpp"
 #include "usb_cam/formats/pixel_format_base.hpp"
 #include "usb_cam/formats/utils.hpp"
+#include "usb_cam/formats/av_pixel_format_helper.hpp"
 
 
 namespace usb_cam
 {
 namespace formats
 {
 
+class RAW_MJPEG : public pixel_format_base
+{
+public:
+  explicit RAW_MJPEG(const AVPixelFormat & avDeviceFormat)
+  : pixel_format_base(
+      "raw_mjpeg",
+      V4L2_PIX_FMT_MJPEG,
+      get_ros_pixel_format_from_av_format(avDeviceFormat),
+      get_channels_from_av_format(avDeviceFormat),
+      get_bit_depth_from_av_format(avDeviceFormat),
+      false)
+  {
+  }
+};
+
 class MJPEG2RGB : public pixel_format_base
 {
 public:

include/usb_cam/usb_cam.hpp

@@ -288,6 +288,7 @@ class UsbCam
     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;
 
@@ -303,6 +304,9 @@ class UsbCam
     } else if (parameters.pixel_format_name == "uyvy2rgb") {
       // number of pixels required for conversion method
       m_image.pixel_format = std::make_shared<UYVY2RGB>(m_image.number_of_pixels);
+    } else if (parameters.pixel_format_name == "mjpeg") {
+      m_image.pixel_format = std::make_shared<RAW_MJPEG>(
+        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<MJPEG2RGB>(
         m_image.width, m_image.height,

include/usb_cam/usb_cam_node.hpp

@@ -39,6 +39,7 @@
 #include "image_transport/image_transport.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/compressed_image.hpp"
 #include "std_srvs/srv/set_bool.hpp"
 
 #include "usb_cam/usb_cam.hpp"
@@ -66,6 +67,7 @@ class UsbCamNode : public rclcpp::Node
   void set_v4l2_params();
   void update();
   bool take_and_send_image();
+  bool take_and_send_image_mjpeg();
 
   rcl_interfaces::msg::SetParametersResult parameters_callback(
     const std::vector<rclcpp::Parameter> & parameters);
@@ -77,8 +79,11 @@ class UsbCamNode : public rclcpp::Node
 
   UsbCam * m_camera;
 
-  sensor_msgs::msg::Image::SharedPtr m_image_msg;
-  image_transport::CameraPublisher m_image_publisher;
+  sensor_msgs::msg::Image::UniquePtr m_image_msg;
+  sensor_msgs::msg::CompressedImage::UniquePtr m_compressed_img_msg;
+  std::shared_ptr<image_transport::CameraPublisher> m_image_publisher;
+  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr m_compressed_image_publisher;
+  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr m_compressed_cam_info_publisher;
 
   parameters_t m_parameters;
 

include/usb_cam/utils.hpp

@@ -35,6 +35,7 @@ extern "C" {
 
 #include <sys/ioctl.h>
 #include <sys/time.h>
+#include <unistd.h>  // for access
 #include <cmath>
 #include <ctime>
 #include <cstring>
@@ -47,6 +48,7 @@ extern "C" {
 #include "usb_cam/constants.hpp"
 #include "usb_cam/formats/pixel_format_base.hpp"
 
+
 namespace usb_cam
 {
 namespace utils

src/ros2/usb_cam_node.cpp

@@ -34,6 +34,7 @@
 #include "usb_cam/usb_cam_node.hpp"
 #include "usb_cam/utils.hpp"
 
+const char BASE_TOPIC_NAME[] = "image_raw";
 
 namespace usb_cam
 {
@@ -42,6 +43,12 @@ UsbCamNode::UsbCamNode(const rclcpp::NodeOptions & node_options)
 : Node("usb_cam", node_options),
   m_camera(new usb_cam::UsbCam()),
   m_image_msg(new sensor_msgs::msg::Image()),
+  m_compressed_img_msg(nullptr),
+  m_image_publisher(std::make_shared<image_transport::CameraPublisher>(
+      image_transport::create_camera_publisher(this, BASE_TOPIC_NAME,
+      rclcpp::QoS {100}.get_rmw_qos_profile()))),
+  m_compressed_image_publisher(nullptr),
+  m_compressed_cam_info_publisher(nullptr),
   m_parameters(),
   m_camera_info_msg(new sensor_msgs::msg::CameraInfo()),
   m_service_capture(
@@ -54,10 +61,6 @@ UsbCamNode::UsbCamNode(const rclcpp::NodeOptions & node_options)
         std::placeholders::_2,
         std::placeholders::_3)))
 {
-  m_image_publisher = image_transport::create_camera_publisher(
-    this, "image_raw", rclcpp::QoS {100}.get_rmw_qos_profile()
-  );
-
   // declare params
   this->declare_parameter("camera_name", "default_cam");
   this->declare_parameter("camera_info_url", "");
@@ -93,6 +96,7 @@ UsbCamNode::~UsbCamNode()
 {
   RCLCPP_WARN(this->get_logger(), "Shutting down");
   m_image_msg.reset();
+  m_compressed_img_msg.reset();
   m_camera_info_msg.reset();
   m_camera_info.reset();
   m_timer.reset();
@@ -144,8 +148,8 @@ void UsbCamNode::init()
   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 << "`"
+      "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) {
@@ -156,6 +160,19 @@ void UsbCamNode::init()
     return;
   }
 
+  // if pixel format is equal to 'mjpeg', i.e. raw mjpeg stream, initialize compressed image message
+  // and publisher
+  if (m_parameters.pixel_format_name == "mjpeg") {
+    m_compressed_img_msg.reset(new sensor_msgs::msg::CompressedImage());
+    m_compressed_img_msg->header.frame_id = m_parameters.frame_id;
+    m_compressed_image_publisher =
+      this->create_publisher<sensor_msgs::msg::CompressedImage>(
+      std::string(BASE_TOPIC_NAME) + "/compressed", rclcpp::QoS(100));
+    m_compressed_cam_info_publisher =
+      this->create_publisher<sensor_msgs::msg::CameraInfo>(
+      "camera_info", rclcpp::QoS(100));
+  }
+
   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",
@@ -361,7 +378,30 @@ 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);
+  m_image_publisher->publish(*m_image_msg, *m_camera_info_msg);
+  return true;
+}
+
+bool UsbCamNode::take_and_send_image_mjpeg()
+{
+  // Only resize if required
+  if (sizeof(m_compressed_img_msg->data) != m_camera->get_image_size_in_bytes()) {
+    m_compressed_img_msg->format = "jpeg";
+    m_compressed_img_msg->data.resize(m_camera->get_image_size_in_bytes());
+  }
+
+  // grab the image, pass image msg buffer to fill
+  m_camera->get_image(reinterpret_cast<char *>(&m_compressed_img_msg->data[0]));
+
+  auto stamp = m_camera->get_image_timestamp();
+  m_compressed_img_msg->header.stamp.sec = stamp.tv_sec;
+  m_compressed_img_msg->header.stamp.nanosec = stamp.tv_nsec;
+
+  *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;
 }
 
@@ -382,8 +422,12 @@ void UsbCamNode::update()
 {
   if (m_camera->is_capturing()) {
     // If the camera exposure longer higher than the framerate period
-    // then that caps the framerate
-    if (!take_and_send_image()) {
+    // then that caps the framerate.
+    // auto t0 = now();
+    bool isSuccessful = (m_parameters.pixel_format_name == "mjpeg") ?
+      take_and_send_image_mjpeg() :
+      take_and_send_image();
+    if (!isSuccessful) {
       RCLCPP_WARN_ONCE(this->get_logger(), "USB camera did not respond in time.");
     }
   }