feat: omit rclcpp::ok and improve termination

include/v4l2_camera/v4l2_camera_device.hpp

@@ -39,6 +39,7 @@ class V4l2CameraDevice
   explicit V4l2CameraDevice(std::string device, bool use_v4l2_buffer_timestamps, rclcpp::Duration timestamp_offset_duration);
 
   bool open();
+  bool close();
   bool start();
   bool stop();
 

src/v4l2_camera.cpp

@@ -115,7 +115,7 @@ V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
   // Start capture thread
   capture_thread_ = std::thread{
     [this]() -> void {
-      while (rclcpp::ok() && !canceled_.load()) {
+      while (!canceled_.load()) {
         RCLCPP_DEBUG(get_logger(), "Capture...");
         auto img = camera_->capture();
         if (img == nullptr) {
@@ -170,6 +170,7 @@ V4L2Camera::~V4L2Camera()
   if (capture_thread_.joinable()) {
     capture_thread_.join();
   }
+  camera_->close();
 }
 
 void V4L2Camera::createParameters()

src/v4l2_camera_device.cpp

@@ -18,6 +18,7 @@
 #include <sensor_msgs/image_encodings.hpp>
 
 #include <fcntl.h>
+#include <unistd.h>
 #include <sys/ioctl.h>
 #include <sys/mman.h>
 
@@ -144,6 +145,18 @@ bool V4l2CameraDevice::open()
   return true;
 }
 
+bool V4l2CameraDevice::close()
+{
+  stop();
+  if (::close(fd_) < 0) {
+    auto msg = std::ostringstream{};
+    msg << "Failed closing device " << device_ << ": " << strerror(errno) << " (" << errno << ")";
+    RCLCPP_ERROR(rclcpp::get_logger("v4l2_camera"), "%s", msg.str().c_str());
+    return false;
+  }
+  return true;
+}
+
 bool V4l2CameraDevice::start()
 {
   RCLCPP_INFO(rclcpp::get_logger("v4l2_camera"), "Starting camera");
@@ -191,13 +204,6 @@ bool V4l2CameraDevice::stop()
     return false;
   }
 
-  // De-initialize buffers
-  for (auto const & buffer : buffers_) {
-    munmap(buffer.start, buffer.length);
-  }
-
-  buffers_.clear();
-
   auto req = v4l2_requestbuffers{};
 
   // Free all buffers
@@ -206,6 +212,13 @@ bool V4l2CameraDevice::stop()
   req.memory = V4L2_MEMORY_MMAP;
   ioctl(fd_, VIDIOC_REQBUFS, &req);
 
+  // De-initialize buffers
+  for (auto const & buffer : buffers_) {
+    munmap(buffer.start, buffer.length);
+  }
+
+  buffers_.clear();
+
   return true;
 }