Merge pull request #205 from amelhassan/foxy-devel

Fixed node not shutting down, if sensor stream breaks (ROS2)

Author: amelhassan

src/k4a_ros_device.cpp

@@ -874,12 +874,15 @@ void K4AROSDevice::framePublisherThread()
   calibration_data_.getRgbCameraInfo(depth_rect_camera_info);
   calibration_data_.getDepthCameraInfo(ir_raw_camera_info);
 
+  const std::chrono::milliseconds firstFrameWaitTime = std::chrono::milliseconds(4 * 1000);
+  const std::chrono::milliseconds regularFrameWaitTime = std::chrono::milliseconds(1000 * 5 / params_.fps);
+  std::chrono::milliseconds waitTime = firstFrameWaitTime;
+
   while (running_ && rclcpp::ok())
   {
     if (k4a_device_)
     {
-      // TODO: consider appropriate capture timeout based on camera framerate
-      if (!k4a_device_.get_capture(&capture, std::chrono::milliseconds(K4A_WAIT_INFINITE)))
+      if (!k4a_device_.get_capture(&capture, waitTime))
       {
         RCLCPP_FATAL(this->get_logger(),"Failed to poll cameras: node cannot continue.");
         rclcpp::shutdown();
@@ -900,6 +903,7 @@ void K4AROSDevice::framePublisherThread()
                                 capture.get_color_image().get_system_timestamp());
         }
       }
+      waitTime = regularFrameWaitTime;
     }
     else if (k4a_playback_handle_)
     {