From 89b4ff79fa5f96f43448a46f2171d45b2fe0b27c Mon Sep 17 00:00:00 2001
From: lookas <i@18kas.com>
Date: Tue, 11 Jun 2024 04:18:54 +0000
Subject: [PATCH] Fix raw_mjpeg function and shrink the compressed image msg
 size

---
 include/usb_cam/usb_cam.hpp | 5 +++++
 src/ros2/usb_cam_node.cpp   | 6 ++++--
 src/usb_cam.cpp             | 3 ++-
 3 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp
index 1663e0f1..60f00e64 100644
--- a/include/usb_cam/usb_cam.hpp
+++ b/include/usb_cam/usb_cam.hpp
@@ -221,6 +221,11 @@ class UsbCam
     return m_image.size_in_bytes;
   }
 
+  inline void reset_image_size_in_bytes()
+  {
+    m_image.set_size_in_bytes();
+  }
+
   inline size_t get_image_size_in_pixels()
   {
     return m_image.number_of_pixels;
diff --git a/src/ros2/usb_cam_node.cpp b/src/ros2/usb_cam_node.cpp
index f8e8deda..657cd41b 100644
--- a/src/ros2/usb_cam_node.cpp
+++ b/src/ros2/usb_cam_node.cpp
@@ -171,7 +171,7 @@ void UsbCamNode::init()
 
   // 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") {
+  if (m_parameters.pixel_format_name == "raw_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 =
@@ -391,6 +391,8 @@ bool UsbCamNode::take_and_send_image_mjpeg()
 
   // grab the image, pass image msg buffer to fill
   m_camera->get_image(reinterpret_cast<char *>(&m_compressed_img_msg->data[0]));
+  m_compressed_img_msg->data.resize(m_camera->get_image_size_in_bytes());
+  m_camera->reset_image_size_in_bytes(); // reset for next grab
 
   auto stamp = m_camera->get_image_timestamp();
   m_compressed_img_msg->header.stamp.sec = stamp.tv_sec;
@@ -423,7 +425,7 @@ void UsbCamNode::update()
     // If the camera exposure longer higher than the framerate period
     // then that caps the framerate.
     // auto t0 = now();
-    bool isSuccessful = (m_parameters.pixel_format_name == "mjpeg") ?
+    bool isSuccessful = (m_parameters.pixel_format_name == "raw_mjpeg") ?
       take_and_send_image_mjpeg() :
       take_and_send_image();
     if (!isSuccessful) {
diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp
index b82cdf67..987c0e7c 100644
--- a/src/usb_cam.cpp
+++ b/src/usb_cam.cpp
@@ -82,7 +82,8 @@ void UsbCam::process_image(const char * src, char * & dest, const int & bytes_us
   // TODO(flynneva): could we skip the copy here somehow?
   // If no conversion required, just copy the image from V4L2 buffer
   if (m_image.pixel_format->requires_conversion() == false) {
-    memcpy(dest, src, m_image.size_in_bytes);
+    m_image.size_in_bytes = bytes_used;
+    memcpy(dest, src, bytes_used);
   } else {
     m_image.pixel_format->convert(src, dest, bytes_used);
   }