fix disconnect crash

Author: ChuckEllison

doc/custom-subscriber-plugin.md

@@ -90,6 +90,7 @@ This tutorial will guide you through the steps to create a simple custom subscri
     TestSubscriber::~TestSubscriber()
     {
       std::scoped_lock lock(subscriber_mutex_);
+      inactive_ = true;
       
       RCLCPP_INFO(logger_, "TestSubscriber destroyed!");
     }
@@ -142,6 +143,8 @@ This tutorial will guide you through the steps to create a simple custom subscri
     {
       std::scoped_lock lock(subscriber_mutex_);
 
+      if(inactive_) return;      
+
       RCLCPP_INFO_STREAM(logger_,  "New TestSubscriber msg: " << input_msg->data);
       
       // Convert input msg to image
@@ -153,7 +156,7 @@ This tutorial will guide you through the steps to create a simple custom subscri
       sensor_msgs::msg::Image output_msg;
       bridge_image.toImageMsg(output_msg);
       sensor_msgs::msg::Image::ConstSharedPtr output_ptr = std::make_shared<sensor_msgs::msg::Image>(output_msg);
-     callback_(output_ptr);
+      try_forward_image(output_ptr);
     }
 
     std::shared_ptr<web_video_server::SubscriberInterface> TestSubscriberFactory::create_subscriber(

include/web_video_server/streamer.hpp

@@ -89,6 +89,8 @@ class StreamerBase : public StreamerInterface
     std::map<std::string, std::shared_ptr<SubscriberFactoryInterface>> & subscriber_factories,
     rclcpp::Node::WeakPtr node,
     std::string logger_name = "streamer");
+  
+    std::mutex send_mutex_;
 
   bool is_inactive() override
   {

include/web_video_server/streamers/image_streamer.hpp

@@ -31,7 +31,6 @@
 
 #include <chrono>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <vector>
 
@@ -80,7 +79,6 @@ class ImageStreamerBase : public StreamerBase
 
   std::chrono::steady_clock::time_point last_frame_;
   cv::Mat output_size_image_;
-  std::mutex send_mutex_;
 
 private:
   bool initialized_;

include/web_video_server/streamers/libav_streamer.hpp

@@ -74,7 +74,6 @@ class LibavStreamerBase : public ImageStreamerBase
     const std::string & format_name,
     const std::string & codec_name,
     const std::string & content_type);
-
   ~LibavStreamerBase();
 
 protected:

include/web_video_server/subscriber.hpp

@@ -81,11 +81,21 @@ class SubscriberBase : public SubscriberInterface
                  const std::string & topic,
                  const ImageCallback & callback);
 
-protected:
+  void try_forward_image(const sensor_msgs::msg::Image::ConstSharedPtr &input_msg)
+  {
+    try {
+      callback_(input_msg);
+    } catch (...) {
+      RCLCPP_ERROR(logger_, "The subscriber plugin failed send image for some reason.");
+    }
+  }                 
+
+ protected:
   void subscriberCallback(const sensor_msgs::msg::Image::ConstSharedPtr &input_msg);                         
 
   rclcpp::Node::SharedPtr node_;
   rclcpp::Logger logger_;
+  bool inactive_;
 
   ImageCallback callback_;
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;

src/subscriber.cpp

@@ -49,6 +49,7 @@ SubscriberBase::SubscriberBase(
   std::string logger_name)
 : node_(node)
 , logger_(node->get_logger().get_child(logger_name))
+, inactive_(false)
 {
 }
 
@@ -87,7 +88,8 @@ void SubscriberBase::subscribe(
 void SubscriberBase::subscriberCallback(const sensor_msgs::msg::Image::ConstSharedPtr &input_msg)
 {
   std::scoped_lock lock(subscriber_mutex_);
-  callback_(input_msg);
+
+  try_forward_image(input_msg);
 }
 
 std::vector<std::string> SubscriberFactoryInterface::get_available_topics(

src/subscribers/image_transport_subscriber.cpp

@@ -49,7 +49,8 @@ ImageTransportSubscriber::ImageTransportSubscriber(rclcpp::Node::SharedPtr _node
 
 ImageTransportSubscriber::~ImageTransportSubscriber()
 {
-    std::scoped_lock lock(subscriber_mutex_);
+  std::scoped_lock lock(subscriber_mutex_);
+  inactive_ = true;
 }
 
 void ImageTransportSubscriber::subscribe(const async_web_server_cpp::HttpRequest &request,
@@ -88,8 +89,10 @@ void ImageTransportSubscriber::subscribe(const async_web_server_cpp::HttpRequest
 void ImageTransportSubscriber::subscriberCallback(const sensor_msgs::msg::Image::ConstSharedPtr &input_msg)
 {
   std::scoped_lock lock(subscriber_mutex_);
+
+  if(inactive_) return;
 
-  callback_(input_msg);
+  try_forward_image(input_msg);
 }
 
 std::shared_ptr<SubscriberInterface> ImageTransportSubscriberFactory::create_subscriber(

src/subscribers/pointcloud2_subscriber.cpp

@@ -61,7 +61,8 @@ PointCloud2Subscriber::PointCloud2Subscriber(rclcpp::Node::SharedPtr node)
 
 PointCloud2Subscriber::~PointCloud2Subscriber()
 {
-    std::scoped_lock lock(subscriber_mutex_);
+  std::scoped_lock lock(subscriber_mutex_);
+  inactive_ = true;
 }
 
 void PointCloud2Subscriber::subscribe(const async_web_server_cpp::HttpRequest &request,
@@ -125,6 +126,8 @@ void PointCloud2Subscriber::subscriberCallback(const sensor_msgs::msg::PointClou
 {
   std::scoped_lock lock(subscriber_mutex_);
 
+  if(inactive_) return;
+
   if (input_msg->data.size() == 0)
   {
     RCLCPP_WARN_STREAM_THROTTLE(logger_, *node_->get_clock(), 1000, "No data in pointcloud!");
@@ -300,7 +303,7 @@ void PointCloud2Subscriber::subscriberCallback(const sensor_msgs::msg::PointClou
   sensor_msgs::msg::Image output_msg;
   colorImage.toImageMsg(output_msg);
   sensor_msgs::msg::Image::ConstSharedPtr output_ptr = std::make_shared<sensor_msgs::msg::Image>(output_msg);
-  callback_(output_ptr);
+  try_forward_image(output_ptr);
 
   return;
 }

src/subscribers/test_subscriber.cpp

@@ -17,6 +17,7 @@
     TestSubscriber::~TestSubscriber()
     {
       std::scoped_lock lock(subscriber_mutex_);
+      inactive_ = true;
 
       RCLCPP_INFO(logger_, "TestSubscriber destroyed!");
     }
@@ -69,6 +70,8 @@
     {
       std::scoped_lock lock(subscriber_mutex_);
 
+      if(inactive_) return;      
+
       RCLCPP_INFO_STREAM(logger_,  "New TestSubscriber msg: " << input_msg->data);
 
       // Convert input msg to image
@@ -80,7 +83,7 @@
       sensor_msgs::msg::Image output_msg;
       bridge_image.toImageMsg(output_msg);
       sensor_msgs::msg::Image::ConstSharedPtr output_ptr = std::make_shared<sensor_msgs::msg::Image>(output_msg);
-     callback_(output_ptr);
+        try_forward_image(output_ptr);
     }
 
     std::shared_ptr<web_video_server::SubscriberInterface> TestSubscriberFactory::create_subscriber(