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.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)