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

fixed is publisher none

Browse Source 
Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>
This commit is contained in:
Jakub Delicat 2023-09-11 15:12:05 +02:00
parent 17b1596633
commit 9f6cfc2e9e
1 changed files with 5 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
safety_user_plugin/ros_wrapper.py
View File

@ -43,7 +43,6 @@ class ROSWrapper(Node):
self.user_stop_state_to_publish = True self.user_stop_state_to_publish = True
self.clutch_publisher = None self.clutch_publisher = 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
@ -90,19 +89,11 @@ class ROSWrapper(Node):
Bool, f"/pioneer{robot_id}/clutch", 10 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.shutdown_timer(self.connection_timer)
self.connection_timer = self.create_timer( self.connection_timer = self.create_timer(
5.0, self.handle_robot_connection_lost 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( self.master_connection_timer = self.create_timer(
1.5, self.handle_master_connection_lost() 1.5, self.handle_master_connection_lost()
@ -131,28 +122,6 @@ class ROSWrapper(Node):
self.destroy_timer(timer) self.destroy_timer(timer)
timer = None 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): def publish_robot_stop(self):
if self.user_stop_publisher is not None: if self.user_stop_publisher is not None:
msg = Bool() msg = Bool()
@ -201,7 +170,6 @@ class ROSWrapper(Node):
self.release_robot() 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")
def select_robot(self, robot_id): def select_robot(self, robot_id):
self.setup_subscribers_and_publishers(robot_id) self.setup_subscribers_and_publishers(robot_id)
@ -229,8 +197,9 @@ class ROSWrapper(Node):
except NodeNameNonExistentError: except NodeNameNonExistentError:
pass pass
if len(published_topics_list): if len(published_topics_list) > 0:
robots_id_list.append(pioneer_id) robots_id_list.append(pioneer_id)
return robots_id_list return robots_id_list
def release_robot(self): def release_robot(self):
@ -308,7 +277,7 @@ class ROSWrapper(Node):
self.clutch_publisher.publish(msg) self.clutch_publisher.publish(msg)
def user_robot_enabled(self): 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 self.user_stop_state_to_publish = False
msg = Bool() msg = Bool()
msg.data = self.user_stop_state_to_publish msg.data = self.user_stop_state_to_publish
@ -316,7 +285,7 @@ class ROSWrapper(Node):
def user_robot_disabled(self): 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 self.user_stop_state_to_publish = True
msg = Bool() msg = Bool()
msg.data = self.user_stop_state_to_publish msg.data = self.user_stop_state_to_publish