Fix raw_mjpeg function and shrink the compressed image msg size

Author: hilookas

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;

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) {

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);
   }