updated

launch/v4l2_camera.launch.py

@@ -71,6 +71,8 @@ def load_composable_node_param(param_path):
                     "camera_info_url": LaunchConfiguration("camera_info_url"),
                     "use_sensor_data_qos": LaunchConfiguration("use_sensor_data_qos"),
                     "publish_rate": LaunchConfiguration("publish_rate"),
+                    "use_v4l2_buffer_timestamps": LaunchConfiguration("use_v4l2_buffer_timestamps"),
+                    "use_image_transport": LaunchConfiguration("use_image_transport"),
                 },
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -126,6 +128,12 @@ def add_launch_arg(name: str, default_value=None, description=None):
                    'otherwise be RELIABLE')
     add_launch_arg('publish_rate', "-1.0",
                    description='publish frame number per second. value <= 0 means no limitation on publish rate')
+    add_launch_arg('use_v4l2_buffer_timestamps', 'true',
+                   description='flag to use v4l2 buffer timestamps. '
+                   'If true, the image timestamps will be applied from the v4l2 buffer, '
+                   'otherwise, will be the system time when the buffer is read')
+    add_launch_arg('use_image_transport', 'true',
+                   description='flag to launch image_transport node')
 
     return LaunchDescription(
         [

src/v4l2_camera.cpp

@@ -148,8 +148,8 @@ V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
         if (use_image_transport_) {
           camera_transport_pub_.publish(*img, *ci);
         } else {
-          image_pub_->publish(*img);
-          info_pub_->publish(*ci);
+          image_pub_->publish(std::move(img));
+          info_pub_->publish(std::move(ci));
         }
       }
     }

src/v4l2_camera_device.cpp

@@ -268,6 +268,14 @@ Image::UniquePtr V4l2CameraDevice::capture()
   }
   buf_stamp = buf_stamp + timestamp_offset_;
 
+  // Create image object
+  auto img = std::make_unique<Image>();
+
+  // Copy over buffer data
+  auto const & buffer = buffers_[buf.index];
+  img->data.resize(cur_data_format_.imageByteSize);
+  std::copy(buffer.start, buffer.start + img->data.size(), img->data.begin());
+
   // Requeue buffer to be reused for new captures
   if (-1 == ioctl(fd_, VIDIOC_QBUF, &buf)) {
     RCLCPP_ERROR(
@@ -277,8 +285,7 @@ Image::UniquePtr V4l2CameraDevice::capture()
     return nullptr;
   }
 
-  // Create image object
-  auto img = std::make_unique<Image>();
+  // Fill in remaining image information
   img->header.stamp = buf_stamp;
   img->width = cur_data_format_.width;
   img->height = cur_data_format_.height;
@@ -292,10 +299,7 @@ Image::UniquePtr V4l2CameraDevice::capture()
   } else {
     RCLCPP_WARN(rclcpp::get_logger("v4l2_camera"), "Current pixel format is not supported yet");
   }
-  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());
   return img;
 }