From 9f6cfc2e9e053d4a1af93a46ecbc494bc96bea52 Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@pwr.edu.pl>
Date: Mon, 11 Sep 2023 15:12:05 +0200
Subject: [PATCH] fixed is publisher none

Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>
---
 safety_user_plugin/ros_wrapper.py | 41 ++++---------------------------
 1 file changed, 5 insertions(+), 36 deletions(-)

diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py
index 3880ff5..b11e400 100644
--- a/safety_user_plugin/ros_wrapper.py
+++ b/safety_user_plugin/ros_wrapper.py
@@ -43,7 +43,6 @@ class ROSWrapper(Node):
         self.user_stop_state_to_publish = True
         self.clutch_publisher = None
 
-        # self.periodic_timer = None
         self.robots_list_timer = None
         self.connection_timer = None
         self.master_connection_timer = None
@@ -90,20 +89,12 @@ class ROSWrapper(Node):
             Bool, f"/pioneer{robot_id}/clutch", 10
         )
 
-
-        # if self.periodic_timer != None:
-        #     self.shutdown_timer(self.periodic_timer)
-        # self.periodic_timer = rospy.Timer(
-        #     rospy.Duration(0.1), self.publish_periodic_update
-        # )
-        # print("NEW")
         self.shutdown_timer(self.connection_timer)
         self.connection_timer = self.create_timer(
             5.0, self.handle_robot_connection_lost
         )
 
-        if self.master_connection_timer != None:
-            self.shutdown_timer(self.master_connection_timer)
+        self.shutdown_timer(self.master_connection_timer)
         self.master_connection_timer = self.create_timer(
             1.5, self.handle_master_connection_lost()
         )
@@ -131,28 +122,6 @@ class ROSWrapper(Node):
             self.destroy_timer(timer)
             timer = None
 
-    def publish_periodic_update(self):
-        stop_state = self.get_user_stop_state()
-        clutch_state = self.get_clutch_state()
-
-        if stop_state == SS.RUNNING:
-            self.user_robot_enabled()
-        elif stop_state == SS.STOPPED:
-            self.user_robot_disabled()
-        else:
-            raise ValueError(
-                "stop_state UNKNOWN when attempting to publish periodic update"
-            )
-
-        if clutch_state == CS.ENGAGED:
-            self.engage_clutch()
-        elif clutch_state == CS.DISENGAGED:
-            self.disengage_clutch()
-        else:
-            raise ValueError(
-                "clutch_state UNKNOWN when attempting to publish periodic update"
-            )
-
     def publish_robot_stop(self):
         if self.user_stop_publisher is not None:
             msg = Bool()
@@ -201,7 +170,6 @@ class ROSWrapper(Node):
         self.release_robot()
         self.cancel_subscribers_and_publishers()
         print("Unregister node.")
-        # rospy.signal_shutdown("Closing safety user plugin")
 
     def select_robot(self, robot_id):
         self.setup_subscribers_and_publishers(robot_id)
@@ -229,8 +197,9 @@ class ROSWrapper(Node):
             except NodeNameNonExistentError:
                 pass
 
-            if len(published_topics_list):
+            if len(published_topics_list) > 0:
                 robots_id_list.append(pioneer_id)
+
         return robots_id_list
 
     def release_robot(self):
@@ -308,7 +277,7 @@ class ROSWrapper(Node):
             self.clutch_publisher.publish(msg)
 
     def user_robot_enabled(self):
-        if self.user_stop_publisher.publish is not None:
+        if self.user_stop_publisher is not None:
             self.user_stop_state_to_publish = False
             msg = Bool()
             msg.data = self.user_stop_state_to_publish
@@ -316,7 +285,7 @@ class ROSWrapper(Node):
 
 
     def user_robot_disabled(self):
-        if self.user_stop_publisher.publish is not None:
+        if self.user_stop_publisher is not None:
             self.user_stop_state_to_publish = True
             msg = Bool()
             msg.data = self.user_stop_state_to_publish