L1.5/safety_user_plugin
L1.5/safety_user_plugin
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

added safety exit

Browse Source 
Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>
This commit is contained in:
Jakub Delicat 2023-09-07 16:24:20 +02:00
parent c648d2838c
commit 17b1596633
1 changed files with 60 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
safety_user_plugin/ros_wrapper.py
View File

@ -40,13 +40,15 @@ class ROSWrapper(Node):
self.restrictions_subscriber = None self.restrictions_subscriber = None
self.user_stop_publisher = None self.user_stop_publisher = None
self.user_stop_state_to_publish = True
self.clutch_publisher = None self.clutch_publisher = None
self.periodic_timer = None # self.periodic_timer = None
self.robots_list_timer = None self.robots_list_timer = None
self.connection_timer = None self.connection_timer = None
self.master_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.robots_hz_subscribers_dict = {}
self.rostopics_hz_dict = {} self.rostopics_hz_dict = {}
@ -88,6 +90,7 @@ class ROSWrapper(Node):
Bool, f"/pioneer{robot_id}/clutch", 10 Bool, f"/pioneer{robot_id}/clutch", 10
) )
# if self.periodic_timer != None: # if self.periodic_timer != None:
# self.shutdown_timer(self.periodic_timer) # self.shutdown_timer(self.periodic_timer)
# self.periodic_timer = rospy.Timer( # self.periodic_timer = rospy.Timer(
@ -99,12 +102,16 @@ class ROSWrapper(Node):
5.0, self.handle_robot_connection_lost 5.0, self.handle_robot_connection_lost
) )
# if self.master_connection_timer != None: if self.master_connection_timer != None:
# self.shutdown_timer(self.master_connection_timer) self.shutdown_timer(self.master_connection_timer)
# self.master_connection_timer = rospy.Timer( self.master_connection_timer = self.create_timer(
# rospy.Duration(1.5), self.handle_master_connection_lost 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: # if self.hz_update_timer != None:
# self.shutdown_timer(self.hz_update_timer) # self.shutdown_timer(self.hz_update_timer)
# self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values) # 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" "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") print("update_hz_values")
# for key in self.rostopics_hz_dict: # for key in self.rostopics_hz_dict:
# self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz( # self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz(
# "/PIONEER{0}/RosAria/user_stop".format(key) # "/PIONEER{0}/RosAria/user_stop".format(key)
@ -161,14 +177,6 @@ class ROSWrapper(Node):
self.shutdown_timer(self.master_connection_timer) self.shutdown_timer(self.master_connection_timer)
self.master_connection_lost_callback() 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): def restart_connection_timer(self):
self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.connection_timer)
self.connection_timer = self.create_timer( self.connection_timer = self.create_timer(
@ -181,7 +189,16 @@ class ROSWrapper(Node):
1.5, self.handle_master_connection_lost 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): def unregister_node(self):
self.release_robot()
self.cancel_subscribers_and_publishers() self.cancel_subscribers_and_publishers()
print("Unregister node.") print("Unregister node.")
# rospy.signal_shutdown("Closing safety user plugin") # rospy.signal_shutdown("Closing safety user plugin")
@ -217,15 +234,20 @@ class ROSWrapper(Node):
return robots_id_list return robots_id_list
def release_robot(self): 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.disengage_clutch()
self.user_robot_disabled() self.user_robot_disabled()
self.unsubscribe(self.robot_info_subscriber) self.unsubscribe(self.robot_info_subscriber)
self.unsubscribe(self.restrictions_subscriber) self.unsubscribe(self.restrictions_subscriber)
self.unsubscribe(self.master_stop_subscriber) self.unsubscribe(self.master_stop_subscriber)
self.shutdown_timer(self.periodic_timer) self.unregister_publisher(self.user_stop_publisher)
self.shutdown_timer(self.connection_timer) self.unregister_publisher(self.clutch_publisher)
self.shutdown_timer(self.master_connection_timer)
def handle_selected_robot_info_update(self, msg): def handle_selected_robot_info_update(self, msg):
# Restarting connection timer to avoid raising robot_connection_lost_callback # Restarting connection timer to avoid raising robot_connection_lost_callback
@ -274,24 +296,31 @@ class ROSWrapper(Node):
self.master_connection_lost_callback = callback_function self.master_connection_lost_callback = callback_function
def engage_clutch(self): def engage_clutch(self):
msg = Bool() if self.clutch_publisher is not None:
msg.data = True msg = Bool()
self.clutch_publisher.publish(msg) msg.data = True
self.clutch_publisher.publish(msg)
def disengage_clutch(self): def disengage_clutch(self):
msg = Bool() if self.clutch_publisher is not None:
msg.data = False msg = Bool()
self.clutch_publisher.publish(msg) msg.data = False
self.clutch_publisher.publish(msg)
def user_robot_enabled(self): def user_robot_enabled(self):
msg = Bool() if self.user_stop_publisher.publish is not None:
msg.data = False self.user_stop_state_to_publish = False
self.user_stop_publisher.publish(msg) msg = Bool()
msg.data = self.user_stop_state_to_publish
self.user_stop_publisher.publish(msg)
def user_robot_disabled(self): def user_robot_disabled(self):
msg = Bool() if self.user_stop_publisher.publish is not None:
msg.data = True self.user_stop_state_to_publish = True
self.user_stop_publisher.publish(msg) msg = Bool()
msg.data = self.user_stop_state_to_publish
self.user_stop_publisher.publish(msg)
def qt_timer_callback(self): def qt_timer_callback(self):
rclpy.spin_once(self, timeout_sec=0.001) rclpy.spin_once(self, timeout_sec=0.001)