Merge pull request #108 from ValerioMa/terminate_cleanly

Clean rosnode termination.
PepperlFuchs · Feb 23, 2023 · 0b5870d · 0b5870d
2 parents 54361bb + 4aa5b46
commit 0b5870d
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 6 deletions.
diff --git a/src/pf_driver/include/pf_driver/pf/pf_writer.h b/src/pf_driver/include/pf_driver/pf/pf_writer.h
@@ -9,12 +9,14 @@ class PFWriter : public Writer<T>
 {
 public:
   PFWriter(std::unique_ptr<Transport>&& transport, std::shared_ptr<Parser<T>> parser)
-    : transport_(std::move(transport)), parser_(parser)
+    : transport_(std::move(transport)), parser_(parser), is_running_(false)
   {
   }
 
   virtual bool start()
   {
+    std::unique_lock<std::mutex> lck(mtx_);
+    is_running_ = true;
     if (transport_)
     {
       if (transport_->is_connected())
@@ -27,6 +29,8 @@ class PFWriter : public Writer<T>
 
   virtual bool stop()
   {
+    std::unique_lock<std::mutex> lck(mtx_);
+    is_running_ = false;
     if (transport_ && transport_->is_connected())
     {
       return transport_->disconnect();
@@ -36,6 +40,11 @@ class PFWriter : public Writer<T>
 
   virtual bool get(std::vector<std::unique_ptr<T>>& packets)
   {
+    std::unique_lock<std::mutex> lck(mtx_);
+    if (!is_running_)
+    {
+      return false;
+    }
     boost::array<uint8_t, 4096> buf;
     size_t read = 0;
     size_t used = 0;
@@ -60,4 +69,6 @@ class PFWriter : public Writer<T>
   std::unique_ptr<Transport> transport_;
   std::shared_ptr<Parser<T>> parser_;
   std::vector<uint8_t> persistent_buffer_;
+  bool is_running_;
+  std::mutex mtx_;
 };
diff --git a/src/pf_driver/src/ros/ros_main.cpp b/src/pf_driver/src/ros/ros_main.cpp
@@ -87,14 +87,21 @@ int main(int argc, char* argv[])
       return -1;
     }
     retrying = true;
-    // wait for condition variable
-    std::unique_lock<std::mutex> net_lock(*net_mtx_);
-    net_cv_->wait(net_lock, [&net_fail] { return net_fail; });
-    ROS_ERROR("Network failure");
+    {
+      // wait for condition variable
+      std::unique_lock<std::mutex> net_lock(*net_mtx_);
+      while (ros::ok() &&
+             !net_cv_->wait_for(net_lock, std::chrono::milliseconds(1000), [&net_fail] { return net_fail; }))
+      {
+      };
+      if (ros::ok())
+      {
+        ROS_ERROR("Network failure");
+      }
+    }
     pf_interface.terminate();
   }
 
-  ros::waitForShutdown();
   pf_interface.stop_transmission();
   return 0;
 }