Use unique_ptr for images in preparation for intra process comms

ZhenshengLee · Jun 15, 2019 · ca908c3 · ca908c3
1 parent 95fd294
commit ca908c3
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 29 deletions.
diff --git a/include/v4l2_camera/v4l2_camera.hpp b/include/v4l2_camera/v4l2_camera.hpp
@@ -52,7 +52,7 @@ class V4L2Camera : public rclcpp::Node
 
   bool requestImageSize(std::vector<int64_t> const & size);
 
-  sensor_msgs::msg::Image convert(sensor_msgs::msg::Image const & img) const;
+  sensor_msgs::msg::Image::UniquePtr convert(sensor_msgs::msg::Image const & img) const;
 
   bool checkCameraInfo(
     sensor_msgs::msg::Image const & img,

diff --git a/include/v4l2_camera/v4l2_camera_device.hpp b/include/v4l2_camera/v4l2_camera_device.hpp
@@ -45,7 +45,7 @@ class V4l2CameraDevice
 
   std::string getCameraName();
 
-  sensor_msgs::msg::Image capture();
+  sensor_msgs::msg::Image::UniquePtr capture();
 
 private:
   /// Image buffer

diff --git a/src/v4l2_camera.cpp b/src/v4l2_camera.cpp
@@ -55,21 +55,21 @@ V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
         RCLCPP_DEBUG(get_logger(), "Capture...");
         auto img = camera_->capture();
         auto stamp = now();
-        if (img.encoding != output_encoding_) {
-          img = convert(img);
+        if (img->encoding != output_encoding_) {
+          img = convert(*img);
         }
-        img.header.stamp = stamp;
+        img->header.stamp = stamp;
 
         auto ci = cinfo_->getCameraInfo();
-        if (!checkCameraInfo(img, ci)) {
+        if (!checkCameraInfo(*img, ci)) {
           ci = sensor_msgs::msg::CameraInfo{};
-          ci.height = img.height;
-          ci.width = img.width;
+          ci.height = img->height;
+          ci.width = img->width;
         }
 
         ci.header.stamp = stamp;
 
-        camera_transport_pub_.publish(img, ci);
+        camera_transport_pub_.publish(*img, ci);
       }
     }
   };
@@ -279,7 +279,7 @@ static void yuyv2rgb(unsigned char const * YUV, unsigned char * RGB, int NumPixe
   }
 }
 
-sensor_msgs::msg::Image V4L2Camera::convert(sensor_msgs::msg::Image const & img) const
+sensor_msgs::msg::Image::UniquePtr V4L2Camera::convert(sensor_msgs::msg::Image const & img) const
 {
   RCLCPP_DEBUG(get_logger(),
     std::string{"Coverting: "} + img.encoding + " -> " + output_encoding_);
@@ -288,20 +288,20 @@ sensor_msgs::msg::Image V4L2Camera::convert(sensor_msgs::msg::Image const & img)
   if (img.encoding == sensor_msgs::image_encodings::YUV422 &&
     output_encoding_ == sensor_msgs::image_encodings::RGB8)
   {
-    auto outImg = sensor_msgs::msg::Image{};
-    outImg.width = img.width;
-    outImg.height = img.height;
-    outImg.step = img.width * 3;
-    outImg.encoding = output_encoding_;
-    outImg.data.resize(outImg.height * outImg.step);
-    for (auto i = 0u; i < outImg.height; ++i) {
-      yuyv2rgb(img.data.data() + i * img.step, outImg.data.data() + i * outImg.step, outImg.width);
+    auto outImg = std::make_unique<sensor_msgs::msg::Image>();
+    outImg->width = img.width;
+    outImg->height = img.height;
+    outImg->step = img.width * 3;
+    outImg->encoding = output_encoding_;
+    outImg->data.resize(outImg->height * outImg->step);
+    for (auto i = 0u; i < outImg->height; ++i) {
+      yuyv2rgb(img.data.data() + i * img.step, outImg->data.data() + i * outImg->step, outImg->width);
     }
     return outImg;
   } else {
     RCLCPP_WARN_ONCE(get_logger(),
       std::string{"Conversion not supported yet: "} + img.encoding + " -> " + output_encoding_);
-    return img;
+    return nullptr;
   }
 }
 

diff --git a/src/v4l2_camera_device.cpp b/src/v4l2_camera_device.cpp
@@ -162,7 +162,7 @@ std::string V4l2CameraDevice::getCameraName()
   return name;
 }
 
-Image V4l2CameraDevice::capture()
+Image::UniquePtr V4l2CameraDevice::capture()
 {
   auto buf = v4l2_buffer{};
 
@@ -174,31 +174,31 @@ Image V4l2CameraDevice::capture()
     RCLCPP_ERROR(rclcpp::get_logger("v4l2_camera"),
       std::string{"Error dequeueing buffer: "} +
       strerror(errno) + " (" + std::to_string(errno) + ")");
-    return Image{};
+    return nullptr;
   }
 
   // Requeue buffer to be reused for new captures
   if (-1 == ioctl(fd_, VIDIOC_QBUF, &buf)) {
     RCLCPP_ERROR(rclcpp::get_logger("v4l2_camera"),
       std::string{"Error re-queueing buffer: "} +
       strerror(errno) + " (" + std::to_string(errno) + ")");
-    return Image{};
+    return nullptr;
   }
 
   // Create image object
-  auto img = Image{};
-  img.width = cur_data_format_.width;
-  img.height = cur_data_format_.height;
-  img.step = cur_data_format_.bytesPerLine;
+  auto img = std::make_unique<Image>();
+  img->width = cur_data_format_.width;
+  img->height = cur_data_format_.height;
+  img->step = cur_data_format_.bytesPerLine;
   if (cur_data_format_.pixelFormat == V4L2_PIX_FMT_YUYV) {
-    img.encoding = sensor_msgs::image_encodings::YUV422;
+    img->encoding = sensor_msgs::image_encodings::YUV422;
   } else {
     RCLCPP_WARN(rclcpp::get_logger("v4l2_camera"), "Current pixel format is not supported yet");
   }
-  img.data.resize(cur_data_format_.imageByteSize);
+  img->data.resize(cur_data_format_.imageByteSize);
 
   auto const & buffer = buffers_[buf.index];
-  std::copy(buffer.start, buffer.start + img.data.size(), img.data.begin());
+  std::copy(buffer.start, buffer.start + img->data.size(), img->data.begin());
   return img;
 }