Add stream handler, increase robustness (#79)

* Add stream handler, increase robustness

* Use clock as shared_ptr, fix indent

CMakeLists.txt

@@ -29,7 +29,8 @@ if(DEFINED ENV{BUILD_GSTREAMER})
 endif()
 
 project(depthai_ctrl LANGUAGES CXX C)
-
+find_package(backward_ros REQUIRED)
+# add_compile_options(-g)
 # # pkg-config configurations path.
 # if(NOT DEFINED ENV{PKG_CONFIG_PATH})
 #   set(ENV{PKG_CONFIG_PATH} "/usr/lib/x86_64-linux-gnu/pkgconfig/")

Dockerfile

@@ -41,6 +41,7 @@ RUN apt update \
         yaml-cpp-vendor \
         cv-bridge \
         vision-msgs \
+        backward-ros \
         camera-info-manager \
         camera-calibration-parsers \
         xacro \
@@ -60,6 +61,7 @@ COPY --from=builder /tmp/yolo-v4-tiny-tf_openvino_2021.4_6shave.blob /depthai_co
 
 VOLUME /depthai_configs
 ENV DEPTHAI_PARAM_FILE /depthai_configs/parameters.yaml
+LABEL org.opencontainers.image.licenses="Apache-2.0"
 
 ENTRYPOINT [ "/entrypoint.sh" ]
 

entrypoint.sh

@@ -35,6 +35,10 @@ if [ "${USE_NEURAL_NETWORK}" = "1" ]; then
     ROS_FLAGS="${ROS_FLAGS} use_neural_network:=true"
 fi
 
+if [ "${EXIT_IF_CAMERA_START_FAILS}" = "1" ]; then
+    ROS_FLAGS="${ROS_FLAGS} exit_if_camera_start_fails:=true"
+fi
+
 if [ "${USE_USB_THREE}" = "1" ]; then
     ROS_FLAGS="${ROS_FLAGS} use_usb_three:=true"
 else

include/depthai_ctrl/DisparityConverter.hpp

@@ -29,6 +29,7 @@ class DisparityConverter
     const std::string frameName, float focalLength, float baseline = 7.5,
     float minDepth = 80, float maxDepth = 1100);
 
+  ~DisparityConverter();
   void toRosMsg(
     std::shared_ptr<dai::ImgFrame> inData,
     std::deque<DisparityMsgs::DisparityImage> & outImageMsg);

include/depthai_ctrl/ImageConverter.hpp

@@ -41,6 +41,7 @@ class ImageConverter
 public:
   // ImageConverter() = default;
   ImageConverter(const std::string frameName, bool interleaved);
+  ~ImageConverter();
   ImageConverter(bool interleaved);
   void toRosMsgFromBitStream(
     std::shared_ptr<dai::ImgFrame> inData,

include/depthai_ctrl/ImgDetectionConverter.hpp

@@ -21,7 +21,7 @@ class ImgDetectionConverter
 public:
   // DetectionConverter() = default;
   ImgDetectionConverter(std::string frameName, int width, int height, bool normalized = false);
-
+  ~ImgDetectionConverter();
   void toRosMsg(
     std::shared_ptr<dai::ImgDetections> inNetData,
     std::deque<VisionMsgs::Detection2DArray> & opDetectionMsgs);

include/depthai_ctrl/ImuConverter.hpp

@@ -22,7 +22,7 @@ class ImuConverter
 {
 public:
   ImuConverter(const std::string & frameName);
-
+  ~ImuConverter();
   void toRosMsg(std::shared_ptr<dai::IMUData> inData, ImuMsgs::Imu & outImuMsg);
   ImuPtr toRosMsgPtr(const std::shared_ptr<dai::IMUData> inData);
 

include/depthai_ctrl/depthai_camera.hpp

@@ -21,6 +21,7 @@
 #define FOG_SW_DEPTHAI_CAMERA_H
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
+#include <mutex>
 #include <depthai/device/Device.hpp>
 #include <depthai/depthai.hpp>
 #include <depthai/pipeline/datatype/ImgFrame.hpp>
@@ -66,42 +67,15 @@ class DepthAICamera : public rclcpp::Node
   using CameraInfoMsg = sensor_msgs::msg::CameraInfo;
   using CompressedImageMsg = sensor_msgs::msg::CompressedImage;
   using Ptr = std::shared_ptr<depthai_ctrl::DepthAICamera>;
+
+  DepthAICamera([[maybe_unused]] const rclcpp::NodeOptions & options)
+  : DepthAICamera("camera_node") {}
+
   DepthAICamera()
-  : Node("camera_node"),
-    _videoWidth(1280),
-    _videoHeight(720),
-    _videoFps(25.),
-    _videoBitrate(3000000),
-    _videoLensPosition(110),
-    _videoH265(false),
-    _useMonoCams(false),
-    _useRawColorCam(false),
-    _useDepth(false),
-    _useVideoFromColorCam(true),
-    _useAutoFocus(false),
-    _useUSB3(false),
-    _firstFrameReceived(false),
-    _useNeuralNetwork(false),
-    _syncNN(true),
-    _thread_running(false),
-    _left_camera_frame("left_camera_frame"),
-    _right_camera_frame("right_camera_frame"),
-    _color_camera_frame("color_camera_frame"),
-    _nn_directory("tiny-yolo-v4_openvino_2021.2_6shave.blob"),
-    _cameraName("oak"),
-    _leftCamCallback(0),
-    _rightCamCallback(0),
-    _colorCamCallback(0),
-    _depthCallback(0),
-    _videoEncoderCallback(0),
-    _passthroughCallback(0)
-  {
-    Initialize();
-    TryRestarting();
-  }
+  : DepthAICamera("camera_node") {}
 
-  DepthAICamera(const rclcpp::NodeOptions & options)
-  : Node("camera_node", options),
+  DepthAICamera(const std::string & name)
+  : Node(name),
     _videoWidth(1280),
     _videoHeight(720),
     _videoFps(25.),
@@ -117,6 +91,7 @@ class DepthAICamera : public rclcpp::Node
     _firstFrameReceived(false),
     _useNeuralNetwork(false),
     _syncNN(true),
+    _exit_if_camera_start_fails(false),
     _thread_running(false),
     _left_camera_frame("left_camera_frame"),
     _right_camera_frame("right_camera_frame"),
@@ -130,32 +105,78 @@ class DepthAICamera : public rclcpp::Node
     _videoEncoderCallback(0),
     _passthroughCallback(0)
   {
-
     Initialize();
-    TryRestarting();
+    if (!TryRestarting()) {
+      RCLCPP_ERROR(get_logger(), "Failed to start camera");
+      if (_exit_if_camera_start_fails) {
+        rclcpp::shutdown();
+      }
+    }
   }
 
+
   ~DepthAICamera()
   {
     Stop();
+    Cleanup();
+  }
+
+  void Cleanup()
+  {
+    _steady_clock.reset();
+    _pipeline.reset();
+    _device.reset();
+    _depth_disparity_converter.reset();
+    _color_camera_converter.reset();
+    _left_camera_converter.reset();
+    _right_camera_converter.reset();
+    _passthrough_converter.reset();
+    _neural_network_converter.reset();
+    _videoQueue.reset();
+    _leftQueue.reset();
+    _rightQueue.reset();
+    _colorQueue.reset();
+    _depthQueue.reset();
+    _passthroughQueue.reset();
+    _colorCamInputQueue.reset();
+    _neuralNetworkOutputQueue.reset();
+    if (_auto_focus_timer) {
+      _auto_focus_timer->cancel();
+      _auto_focus_timer.reset();
+    }
+    if (_handle_camera_status_timer) {
+      _handle_camera_status_timer->cancel();
+      _handle_camera_status_timer.reset();
+    }
   }
 
   bool IsNodeRunning() {return bool(_device) && !_device->isClosed() && _thread_running;}
 
   void Stop()
   {
+    RCLCPP_INFO(get_logger(), "Stopping DepthAI Camera");
     // TODO, maybe remove callbacks?
     if (_thread_running && bool(_device)) {
+      _firstFrameReceived = false;
+      _lastFrameTime = _steady_clock->now();
       _thread_running = false;
+      _pipeline.reset();
       _device.reset();
+      if (_handle_camera_status_timer) {
+        _handle_camera_status_timer->cancel();
+        _handle_camera_status_timer.reset();
+      }
     }
+    RCLCPP_INFO(get_logger(), "Stopped DepthAI Camera");
   }
 
-  void TryRestarting();
+  bool TryRestarting();
 
 private:
   void ProcessingThread();
+  void HandleStreamStatus();
   void AutoFocusTimer();
+  void stopHandleStreamTimer();
   void changeLensPosition(int lens_position);
   void changeFocusMode(bool use_auto_focus);
   void onLeftCamCallback(const std::shared_ptr<dai::ADatatype> data);
@@ -195,11 +216,14 @@ class DepthAICamera : public rclcpp::Node
   bool _useVideoFromColorCam;
   bool _useAutoFocus;
   bool _useUSB3;
-  bool _firstFrameReceived;
+  std::atomic<bool> _firstFrameReceived;
   bool _useNeuralNetwork;
   bool _syncNN;
-  rclcpp::Time _lastFrameTime;
+  bool _exit_if_camera_start_fails;
 
+  std::mutex _callback_mutex;
+  rclcpp::Time _lastFrameTime;
+  rclcpp::Clock::SharedPtr _steady_clock;
   std::shared_ptr<rclcpp::Publisher<ImageMsg>> _left_publisher;
   std::shared_ptr<rclcpp::Publisher<ImageMsg>> _right_publisher;
   std::shared_ptr<rclcpp::Publisher<ImageMsg>> _color_publisher;
@@ -214,6 +238,10 @@ class DepthAICamera : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr _auto_focus_timer;
   std::shared_ptr<rclcpp::Publisher<vision_msgs::msg::Detection2DArray>> _detection_roi_publisher;
 
+  rclcpp::TimerBase::SharedPtr _handle_camera_status_timer;
+
+  rclcpp::CallbackGroup::SharedPtr _callback_group_timer;
+
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _stream_command_subscriber;
 
   std::atomic<bool> _thread_running;

launch/depthai_launch.py

@@ -23,6 +23,7 @@ def generate_launch_description():
 
     xacro_path = os.path.join(pkg_path, 'urdf', 'depthai_descr.urdf.xacro')
 
+    exit_if_camera_start_fails = LaunchConfiguration('exit_if_camera_start_fails', default = 'false')
     camera_model = LaunchConfiguration('camera_model',  default = 'OAK-D')
     camera_name  = LaunchConfiguration('camera_name',   default = 'oak')
     base_frame   = LaunchConfiguration('base_frame',    default = 'oak-d_frame')
@@ -44,6 +45,11 @@ def generate_launch_description():
     use_state_publisher = LaunchConfiguration('use_state_publisher')
     remappings = []
 
+    declare_exit_if_camera_start_fails_cmd = DeclareLaunchArgument(
+        'exit_if_camera_start_fails',
+        default_value=exit_if_camera_start_fails,
+        description='Whether to exit if the camera fails to start.')
+
     declare_camera_model_cmd = DeclareLaunchArgument(
         'camera_model',
         default_value=camera_model,
@@ -158,6 +164,7 @@ def generate_launch_description():
                             'use_usb_three': use_usb_three,
                             'use_neural_network': use_neural_network,
                             'use_passthrough_preview': use_passthrough_preview,
+                            'exit_if_camera_start_fails': exit_if_camera_start_fails
                         }],
     )
 
@@ -184,6 +191,7 @@ def generate_launch_description():
         )
 
     ld = LaunchDescription()
+    ld.add_action(declare_exit_if_camera_start_fails_cmd)
     ld.add_action(declare_camera_name_cmd)
     ld.add_action(declare_camera_model_cmd)
     ld.add_action(declare_base_frame_cmd)

package.xml

@@ -20,7 +20,7 @@
   <depend>rclcpp_components</depend>
   <depend>cv_bridge</depend>
   <depend>libopencv-dev</depend>
-
+  <depend>backward_ros</depend>
   <depend>camera_info_manager</depend>
   <depend>image_transport</depend>
   <exec_depend>xacro</exec_depend>

src/DisparityConverter.cpp

@@ -19,6 +19,8 @@ DisparityConverter::DisparityConverter(
   _rosBaseTime = rclcpp::Clock().now();
 }
 
+DisparityConverter::~DisparityConverter() = default;
+
 void DisparityConverter::toRosMsg(
   std::shared_ptr<dai::ImgFrame> inData,
   std::deque<DisparityMsgs::DisparityImage> & outDispImageMsgs)

src/ImageConverter.cpp

@@ -40,6 +40,9 @@ ImageConverter::ImageConverter(const std::string frameName, bool interleaved)
 {
   _rosBaseTime = rclcpp::Clock().now();
 }
+
+ImageConverter::~ImageConverter() = default;
+
 void ImageConverter::toRosMsgFromBitStream(
   std::shared_ptr<dai::ImgFrame> inData,
   std::deque<ImageMsgs::Image> & outImageMsgs,

src/ImgDetectionConverter.cpp

@@ -14,6 +14,7 @@ ImgDetectionConverter::ImgDetectionConverter(
 {
   _rosBaseTime = rclcpp::Clock().now();
 }
+ImgDetectionConverter::~ImgDetectionConverter() = default;
 
 void ImgDetectionConverter::toRosMsg(
   std::shared_ptr<dai::ImgDetections> inNetData,

src/ImuConverter.cpp

@@ -8,7 +8,7 @@ namespace ros
 
 ImuConverter::ImuConverter(const std::string & frameName)
 : _frameName(frameName), _sequenceNum(0) {}
-
+ImuConverter::~ImuConverter() = default;
 void ImuConverter::toRosMsg(std::shared_ptr<dai::IMUData> inData, ImuMsgs::Imu & outImuMsg)
 {
 // setting the header

src/camera_node.cpp

@@ -16,6 +16,7 @@
 *******************************************************************************/
 
 /* Authors(Unikie Oy): Mehmet Killioglu, Manuel Segarra-Abad, Sergey */
+#include <memory>
 
 #include "depthai_ctrl/depthai_camera.hpp"
 #include <rclcpp/rclcpp.hpp>
@@ -27,7 +28,6 @@ int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  std::cout << "DepthAI Camera Node." << std::endl;
   rclcpp::executors::MultiThreadedExecutor exec;
   rclcpp::NodeOptions options;
   auto cameraNode = std::make_shared<DepthAICamera>();