Fix shutdown issues

Call shutdown_internal in destructors
ros-industrial · May 6, 2021 · 1acd493 · 1acd493
1 parent 8fa6157
commit 1acd493
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 26 deletions.
diff --git a/canopen_master/test/test_node.cpp b/canopen_master/test/test_node.cpp
@@ -56,7 +56,6 @@ TEST(TestNode, testInitandShutdown){
         ASSERT_TRUE(status.bounded<canopen::LayerStatus::Ok>());
     }
     EXPECT_TRUE(replay.done());
-    driver->shutdown();
 }
 
 // Run all the tests that were declared with TEST()

diff --git a/socketcan_bridge/test/to_topic_test.cpp b/socketcan_bridge/test/to_topic_test.cpp
@@ -112,8 +112,6 @@ TEST(SocketCANToTopicTest, checkCorrectData)
   EXPECT_EQ(received.is_rtr, f.is_rtr);
   EXPECT_EQ(received.is_error, f.is_error);
   EXPECT_EQ(received.data, f.data);
-
-  dummy->shutdown();
 }
 
 TEST(SocketCANToTopicTest, checkInvalidFrameHandling)
@@ -164,8 +162,6 @@ TEST(SocketCANToTopicTest, checkInvalidFrameHandling)
   ros::WallDuration(1.0).sleep();
   ros::spinOnce();
   EXPECT_EQ(message_collector_.messages.size(), 1);
-
-  dummy->shutdown();
 }
 
 TEST(SocketCANToTopicTest, checkCorrectCanIdFilter)
@@ -226,8 +222,6 @@ TEST(SocketCANToTopicTest, checkCorrectCanIdFilter)
   EXPECT_EQ(received.is_rtr, f.is_rtr);
   EXPECT_EQ(received.is_error, f.is_error);
   EXPECT_EQ(received.data, f.data);
-
-  dummy->shutdown();
 }
 
 TEST(SocketCANToTopicTest, checkInvalidCanIdFilter)

diff --git a/socketcan_interface/include/socketcan_interface/asio_base.h b/socketcan_interface/include/socketcan_interface/asio_base.h
@@ -21,6 +21,14 @@ template<typename Socket> class AsioDriver : public DriverInterface{
     boost::mutex state_mutex_;
     boost::mutex socket_mutex_;
 
+    void shutdown_internal(){
+        if(socket_.is_open()){
+            socket_.cancel();
+            socket_.close();
+        }
+        io_service_.stop();
+    }
+
 protected:
     boost::asio::io_service io_service_;
 #if BOOST_ASIO_VERSION >= 101200 // Boost 1.66+
@@ -78,7 +86,7 @@ template<typename Socket> class AsioDriver : public DriverInterface{
     {}
 
 public:
-    virtual ~AsioDriver() { shutdown(); }
+    virtual ~AsioDriver() { shutdown_internal(); }
 
     State getState(){
         boost::mutex::scoped_lock lock(state_mutex_);
@@ -109,11 +117,7 @@ template<typename Socket> class AsioDriver : public DriverInterface{
     }
 
     virtual void shutdown(){
-        if(socket_.is_open()){
-            socket_.cancel();
-            socket_.close();
-        }
-        io_service_.stop();
+        shutdown_internal();
     }
 
     virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){

diff --git a/socketcan_interface/include/socketcan_interface/dummy.h b/socketcan_interface/include/socketcan_interface/dummy.h
@@ -78,10 +78,15 @@ class DummyInterface : public DriverInterface{
         cond_lock.unlock();
         cond_.notify_all();
     }    
+
+    void shutdown_internal(){
+        setDriverState(State::closed);
+        bus_.reset();
+    };
 public:
     DummyInterface() : loopback_(false), trace_(false) {}
     DummyInterface(bool loopback) : loopback_(loopback), trace_(false) {}
-    virtual ~DummyInterface() { shutdown(); }
+    virtual ~DummyInterface() { shutdown_internal(); }
 
 
     virtual bool send(const Frame & msg){
@@ -112,8 +117,7 @@ class DummyInterface : public DriverInterface{
 
     virtual void shutdown(){
         flush();
-        setDriverState(State::closed);
-        bus_.reset();
+        shutdown_internal();
     };
 
     virtual bool translateError(unsigned int internal_error, std::string & str){
@@ -132,7 +136,7 @@ class DummyInterface : public DriverInterface{
         while (true) {
             {
                 boost::mutex::scoped_lock cond_lock(mutex_);
-                if (in_.empty()) {
+                if (in_.empty() || state_.driver_state == State::closed) {
                     return;
                 }            
             }
@@ -147,7 +151,7 @@ class DummyInterface : public DriverInterface{
             state_.driver_state = State::ready;
             state_dispatcher_.dispatch(state_);
 
-            cond_.wait(cond_lock);
+            cond_.wait_for(cond_lock, boost::chrono::seconds(1));
             while(!in_.empty()){
                 const can::Frame msg = in_.front();
                 in_.pop_front();

diff --git a/socketcan_interface/include/socketcan_interface/threading.h b/socketcan_interface/include/socketcan_interface/threading.h
@@ -41,6 +41,13 @@ template<typename WrappedInterface> class ThreadedInterface : public WrappedInte
     void run_thread(){
         WrappedInterface::run();
     }
+    void shutdown_internal(){
+        if(thread_){
+            thread_->interrupt();
+            thread_->join();
+            thread_.reset();
+        }
+    }
 public:
     [[deprecated("provide settings explicitly")]] virtual bool init(const std::string &device, bool loopback) override {
         #pragma GCC diagnostic push
@@ -61,23 +68,18 @@ template<typename WrappedInterface> class ThreadedInterface : public WrappedInte
         }
         return WrappedInterface::getState().isReady();
     }
+
     virtual void shutdown(){
         WrappedInterface::shutdown();
-        if(thread_){
-            thread_->interrupt();
-            thread_->join();
-            thread_.reset();
-        }
+        shutdown_internal();
     }
     void join(){
         if(thread_){
             thread_->join();
         }
     }
     virtual ~ThreadedInterface() {
-        if(thread_){
-            shutdown();
-        }
+        shutdown_internal();
     }
     ThreadedInterface(): WrappedInterface() {}
     template<typename T1> ThreadedInterface(const T1 &t1): WrappedInterface(t1) {}