From 17b1596633764d4758f35dd0c4aea92eab805700 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 7 Sep 2023 16:24:20 +0200 Subject: [PATCH] added safety exit Signed-off-by: Jakub Delicat --- safety_user_plugin/ros_wrapper.py | 91 ++++++++++++++++++++----------- 1 file changed, 60 insertions(+), 31 deletions(-) diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py index 33ef8d7..3880ff5 100644 --- a/safety_user_plugin/ros_wrapper.py +++ b/safety_user_plugin/ros_wrapper.py @@ -40,13 +40,15 @@ class ROSWrapper(Node): self.restrictions_subscriber = None self.user_stop_publisher = None + self.user_stop_state_to_publish = True self.clutch_publisher = None - self.periodic_timer = None + # self.periodic_timer = None self.robots_list_timer = None self.connection_timer = None self.master_connection_timer = None - self.hz_update_timer = self.create_timer(0.2, self.update_hz_values) + self.robot_stop_timer = None + # self.hz_update_timer = self.create_timer(0.2, self.publish_user_stop_and_update_hz_values) self.robots_hz_subscribers_dict = {} self.rostopics_hz_dict = {} @@ -88,6 +90,7 @@ 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( @@ -99,12 +102,16 @@ class ROSWrapper(Node): 5.0, self.handle_robot_connection_lost ) - # if self.master_connection_timer != None: - # self.shutdown_timer(self.master_connection_timer) - # self.master_connection_timer = rospy.Timer( - # rospy.Duration(1.5), self.handle_master_connection_lost - # ) + if self.master_connection_timer != None: + self.shutdown_timer(self.master_connection_timer) + self.master_connection_timer = self.create_timer( + 1.5, self.handle_master_connection_lost() + ) + self.shutdown_timer(self.robot_stop_timer) + self.robot_stop_timer = self.create_timer( + 0.1, self.publish_robot_stop + ) # if self.hz_update_timer != None: # self.shutdown_timer(self.hz_update_timer) # self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values) @@ -146,8 +153,17 @@ class ROSWrapper(Node): "clutch_state UNKNOWN when attempting to publish periodic update" ) - def update_hz_values(self): + def publish_robot_stop(self): + if self.user_stop_publisher is not None: + msg = Bool() + msg.data = self.user_stop_state_to_publish + self.user_stop_publisher.publish(msg) + + def publish_user_stop_and_update_hz_values(self): print("update_hz_values") + + + # for key in self.rostopics_hz_dict: # self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz( # "/PIONEER{0}/RosAria/user_stop".format(key) @@ -161,14 +177,6 @@ class ROSWrapper(Node): self.shutdown_timer(self.master_connection_timer) self.master_connection_lost_callback() - def cancel_subscribers_and_publishers(self): - self.shutdown_timer(self.periodic_timer) - self.unsubscribe(self.robot_info_subscriber) - self.unsubscribe(self.master_stop_subscriber) - self.unsubscribe(self.restrictions_subscriber) - self.unregister_publisher(self.user_stop_publisher) - self.unregister_publisher(self.clutch_publisher) - def restart_connection_timer(self): self.shutdown_timer(self.connection_timer) self.connection_timer = self.create_timer( @@ -181,7 +189,16 @@ class ROSWrapper(Node): 1.5, self.handle_master_connection_lost ) + def cancel_subscribers_and_publishers(self): + # self.shutdown_timer(self.periodic_timer) + self.unsubscribe(self.robot_info_subscriber) + self.unsubscribe(self.master_stop_subscriber) + self.unsubscribe(self.restrictions_subscriber) + self.unregister_publisher(self.user_stop_publisher) + self.unregister_publisher(self.clutch_publisher) + def unregister_node(self): + self.release_robot() self.cancel_subscribers_and_publishers() print("Unregister node.") # rospy.signal_shutdown("Closing safety user plugin") @@ -217,15 +234,20 @@ class ROSWrapper(Node): return robots_id_list def release_robot(self): + # self.shutdown_timer(self.periodic_timer) + self.shutdown_timer(self.robot_stop_timer) + self.shutdown_timer(self.connection_timer) + self.shutdown_timer(self.master_connection_timer) + self.disengage_clutch() self.user_robot_disabled() self.unsubscribe(self.robot_info_subscriber) self.unsubscribe(self.restrictions_subscriber) self.unsubscribe(self.master_stop_subscriber) - self.shutdown_timer(self.periodic_timer) - self.shutdown_timer(self.connection_timer) - self.shutdown_timer(self.master_connection_timer) + self.unregister_publisher(self.user_stop_publisher) + self.unregister_publisher(self.clutch_publisher) + def handle_selected_robot_info_update(self, msg): # Restarting connection timer to avoid raising robot_connection_lost_callback @@ -274,24 +296,31 @@ class ROSWrapper(Node): self.master_connection_lost_callback = callback_function def engage_clutch(self): - msg = Bool() - msg.data = True - self.clutch_publisher.publish(msg) + if self.clutch_publisher is not None: + msg = Bool() + msg.data = True + self.clutch_publisher.publish(msg) def disengage_clutch(self): - msg = Bool() - msg.data = False - self.clutch_publisher.publish(msg) + if self.clutch_publisher is not None: + msg = Bool() + msg.data = False + self.clutch_publisher.publish(msg) def user_robot_enabled(self): - msg = Bool() - msg.data = False - self.user_stop_publisher.publish(msg) + if self.user_stop_publisher.publish is not None: + self.user_stop_state_to_publish = False + msg = Bool() + msg.data = self.user_stop_state_to_publish + self.user_stop_publisher.publish(msg) + def user_robot_disabled(self): - msg = Bool() - msg.data = True - self.user_stop_publisher.publish(msg) + if self.user_stop_publisher.publish is not None: + self.user_stop_state_to_publish = True + msg = Bool() + msg.data = self.user_stop_state_to_publish + self.user_stop_publisher.publish(msg) def qt_timer_callback(self): rclpy.spin_once(self, timeout_sec=0.001)