Publish raw mjpeg stream directly via compressed image topic

config/params_1.yaml

@@ -5,6 +5,7 @@
       io_method: "mmap"
       frame_id: "camera"
       pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
+      av_device_format: "YUV422P"
       image_width: 640
       image_height: 480
       camera_name: "test_camera"

config/params_2.yaml

@@ -5,6 +5,7 @@
       io_method: "mmap"
       frame_id: "camera2"
       pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
+            av_device_format: "YUV422P"
       image_width: 640
       image_height: 480
       camera_name: "test_camera2"

include/usb_cam/formats/mjpeg.hpp

@@ -59,10 +59,96 @@ namespace usb_cam
 namespace formats
 {
 
+class RAW_MJPEG : public pixel_format_base
+{
+public:
+  RAW_MJPEG(const AVPixelFormat & avDeviceFormat)
+  : pixel_format_base(
+      "raw_mjpeg",
+      V4L2_PIX_FMT_MJPEG,
+      usb_cam::constants::YUV422,
+      2,
+      8,
+      false)
+  {
+    switch(avDeviceFormat)
+    {      
+      default:
+      {
+        m_ros = usb_cam::constants::UNKNOWN;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_RGB24:
+      {
+        m_ros = usb_cam::constants::RGB8;
+        m_channels = 3;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_RGBA:
+      {
+        m_ros = usb_cam::constants::RGBA8;
+        m_channels = 4;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_BGR24:
+      {
+        m_ros = usb_cam::constants::BGR8;
+        m_channels = 3;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_BGRA:
+      {
+        m_ros = usb_cam::constants::BGRA8;
+        m_channels = 4;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_GRAY8:
+      {
+        m_ros = usb_cam::constants::MONO8;
+        m_channels = 1;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_GRAY16BE:
+      {
+        m_ros = usb_cam::constants::MONO16;
+        m_channels = 1;
+        m_bit_depth = 16;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_YUV422P:
+      {
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_YUV420P:
+      {
+        m_ros = usb_cam::constants::NV21;
+        m_channels = 2;
+      }
+      break;
+
+      case AVPixelFormat::AV_PIX_FMT_YUV444P:
+      {
+        m_ros = usb_cam::constants::NV24;
+        m_channels = 2;
+      }
+      break;
+    }
+
+  }
+};
+
 class MJPEG2RGB : public pixel_format_base
 {
 public:
-  MJPEG2RGB(const int & width, const int & height)
+  MJPEG2RGB(const int & width, const int & height, const AVPixelFormat & avDeviceFormat)
   : pixel_format_base(
       "mjpeg2rgb",
       V4L2_PIX_FMT_MJPEG,
@@ -94,14 +180,15 @@ class MJPEG2RGB : public pixel_format_base
     m_avframe_device->width = width;
     m_avframe_device->height = height;
     m_avframe_device->format = AV_PIX_FMT_YUV422P;
+    m_avframe_device->format = avDeviceFormat;
 
     m_avframe_rgb->width = width;
     m_avframe_rgb->height = height;
     m_avframe_rgb->format = AV_PIX_FMT_RGB24;
 
     m_sws_context = sws_getContext(
       width, height, (AVPixelFormat)m_avframe_device->format,
-      width, height, AV_PIX_FMT_RGB24, SWS_FAST_BILINEAR,
+      width, height, (AVPixelFormat)m_avframe_rgb->format, SWS_FAST_BILINEAR,
       NULL, NULL, NULL);
 
     // Suppress warnings from ffmpeg libraries to avoid spamming the console
@@ -187,7 +274,9 @@ class MJPEG2RGB : public pixel_format_base
     // Pass src MJPEG image to decoder
     m_result = avcodec_send_packet(m_avcodec_context, m_avpacket);
 
+    #if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(58, 133, 100)
     av_packet_unref(m_avpacket);
+    #endif
     // If result is not 0, report what went wrong
     if (m_result != 0) {
       std::cerr << "Failed to send AVPacket to decode: ";

include/usb_cam/usb_cam.hpp

@@ -37,13 +37,15 @@ extern "C" {
 
 #include <chrono>
 #include <memory>
+#include <algorithm>
 #include <sstream>
 #include <iostream>
 #include <string>
 #include <vector>
 
 #include "usb_cam/utils.hpp"
 #include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/av_pixel_format_helper.hpp"
 
 #include "usb_cam/formats/mjpeg.hpp"
 #include "usb_cam/formats/mono.hpp"
@@ -79,6 +81,7 @@ typedef struct
   // to discover them,
   // or guvcview
   std::string pixel_format_name;
+  std::string av_device_format;
   int image_width;
   int image_height;
   int framerate;
@@ -270,12 +273,12 @@ class UsbCam
     return m_supported_formats;
   }
 
-  /// @brief Get pixel format from string. Required to have logic within UsbCam object
+  /// @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.)
-  /// @param str name of supported format (see `usb_cam/supported_formats.hpp`)
+  /// @param parameters list of parameters from which the pixel format is to be set
   /// @return pixel format structure corresponding to a given name
-  inline std::shared_ptr<pixel_format_base> set_pixel_format_from_string(const std::string & str)
+  inline std::shared_ptr<pixel_format_base> set_pixel_format(const parameters_t & parameters)
   {
     using usb_cam::formats::RGB8;
     using usb_cam::formats::YUYV;
@@ -285,35 +288,41 @@ 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;
 
-    if (str == "rgb8") {
+    if (parameters.pixel_format_name == "rgb8") {
       m_image.pixel_format = std::make_shared<RGB8>();
-    } else if (str == "yuyv") {
+    } else if (parameters.pixel_format_name == "yuyv") {
       m_image.pixel_format = std::make_shared<YUYV>();
-    } else if (str == "yuyv2rgb") {
+    } else if (parameters.pixel_format_name == "yuyv2rgb") {
       // number of pixels required for conversion method
       m_image.pixel_format = std::make_shared<YUYV2RGB>(m_image.number_of_pixels);
-    } else if (str == "uyvy") {
+    } else if (parameters.pixel_format_name == "uyvy") {
       m_image.pixel_format = std::make_shared<UYVY>();
-    } else if (str == "uyvy2rgb") {
+    } 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 (str == "mjpeg2rgb") {
+    } 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);
-    } else if (str == "m4202rgb") {
+        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<M4202RGB>(
         m_image.width, m_image.height);
-    } else if (str == "mono8") {
+    } else if (parameters.pixel_format_name == "mono8") {
       m_image.pixel_format = std::make_shared<MONO8>();
-    } else if (str == "mono16") {
+    } else if (parameters.pixel_format_name == "mono16") {
       m_image.pixel_format = std::make_shared<MONO16>();
-    } else if (str == "y102mono8") {
+    } else if (parameters.pixel_format_name == "y102mono8") {
       m_image.pixel_format = std::make_shared<Y102MONO8>(m_image.number_of_pixels);
     } else {
-      throw std::invalid_argument("Unsupported pixel format specified: " + str);
+      throw std::invalid_argument("Unsupported pixel format specified: " + 
+                                  parameters.pixel_format_name);
     }
 
     return m_image.pixel_format;

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;
 

src/usb_cam.cpp

@@ -521,7 +521,7 @@ void UsbCam::configure(
   m_image.set_number_of_pixels();
 
   // Do this before calling set_bytes_per_line and set_size_in_bytes
-  m_image.pixel_format = set_pixel_format_from_string(parameters.pixel_format_name);
+  m_image.pixel_format = set_pixel_format(parameters);
   m_image.set_bytes_per_line();
   m_image.set_size_in_bytes();
   m_framerate = parameters.framerate;