L1.5/l1.5-safety-system
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Small adjustments in ros topics names and fix for clutch handling in user plugin

Browse Source
This commit is contained in:
Olek Bojda 2018-09-28 17:59:45 +02:00
parent d2b0a3ad1a
commit bdf9d0f04c
3 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
safety_master_plugin/scripts/ros_master_wrapper.py
View File

@ -28,14 +28,14 @@ class ROSWrapper:
def setup_subscribers_and_publishers(self): def setup_subscribers_and_publishers(self):
self.master_stop_publisher = rospy.Publisher('/RosAria/master_stop',Bool,queue_size = 1) self.master_stop_publisher = rospy.Publisher('/PIONIER/master_stop',Bool,queue_size = 1)
self.restrictions_publisher = rospy.Publisher('/RosAria/restrictions',RestrictionsMsg,queue_size = 1) self.restrictions_publisher = rospy.Publisher('/PIONIER/restrictions',RestrictionsMsg,queue_size = 1)
self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update) self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update)
def add_robot_subscriber(self,robot_id): def add_robot_subscriber(self,robot_id):
if robot_id not in self.robot_info_subscribers: if robot_id not in self.robot_info_subscribers:
topic_name = '/RosAria/PIONIER{0}/robot_info'.format(robot_id) topic_name = '/PIONIER{0}/RosAria/robot_info'.format(robot_id)
new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update) new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update)
self.robot_info_subscribers[robot_id] = new_sub self.robot_info_subscribers[robot_id] = new_sub
else: else:
@ -116,7 +116,7 @@ class ROSWrapper:
pass pass
else: else:
robot_number = topic.split('/') robot_number = topic.split('/')
robot_number = robot_number[2] robot_number = robot_number[1]
robot_number = robot_number[7:] robot_number = robot_number[7:]
if len(robot_number) > 0: if len(robot_number) > 0:
robot_number = int(robot_number) robot_number = int(robot_number)

26
safety_user_plugin/scripts/ros_wrapper.py
View File

@ -41,12 +41,12 @@ class ROSWrapper:
def setup_subscribers_and_publishers(self,robot_id): def setup_subscribers_and_publishers(self,robot_id):
robot_name = 'PIONIER' + str(robot_id) robot_name = 'PIONIER' + str(robot_id)
self.robot_info_subscriber = rospy.Subscriber('/RosAria/{0}/robot_info'.format(robot_name), RobotInfoMsg, self.handle_selected_robot_info_update) self.robot_info_subscriber = rospy.Subscriber('/{0}/RosAria/robot_info'.format(robot_name), RobotInfoMsg, self.handle_selected_robot_info_update)
self.master_stop_subscriber = rospy.Subscriber('/RosAria/master_stop', Bool, self.handle_master_stop_update) self.master_stop_subscriber = rospy.Subscriber('/PIONIER/master_stop', Bool, self.handle_master_stop_update)
self.restrictions_subscriber = rospy.Subscriber('/RosAria/restrictions', RestrictionsMsg, self.handle_restrictions_update) self.restrictions_subscriber = rospy.Subscriber('/PIONIER/restrictions', RestrictionsMsg, self.handle_restrictions_update)
self.user_stop_publisher = rospy.Publisher('/RosAria/{0}/user_stop'.format(robot_name),Bool,queue_size = 1) self.user_stop_publisher = rospy.Publisher('/{0}/RosAria/user_stop'.format(robot_name),Bool,queue_size = 1)
self.clutch_publisher = rospy.Publisher('/RosAria/{0}/clutch'.format(robot_name),Bool,queue_size = 1) self.clutch_publisher = rospy.Publisher('/{0}/RosAria/clutch'.format(robot_name),Bool,queue_size = 1)
if self.periodic_timer != None: if self.periodic_timer != None:
self.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.periodic_timer)
@ -74,7 +74,7 @@ class ROSWrapper:
def publish_periodic_update(self,event): def publish_periodic_update(self,event):
stop_state = self.get_user_stop_state() stop_state = self.get_user_stop_state()
clutch_state = self.get_clutch_state() # clutch_state = self.get_clutch_state()
if stop_state == SS.RUNNING: if stop_state == SS.RUNNING:
self.user_started() self.user_started()
@ -83,12 +83,12 @@ class ROSWrapper:
else: else:
raise ValueError('stop_state UNKNOWN when attempting to publish periodic update') raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
if clutch_state == CS.ENGAGED: # if clutch_state == CS.ENGAGED:
self.engage_clutch() # self.engage_clutch()
elif clutch_state == CS.DISENGAGED: # elif clutch_state == CS.DISENGAGED:
self.disengage_clutch() # self.disengage_clutch()
else: # else:
raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update') # raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
def handle_robot_connection_lost(self,event): def handle_robot_connection_lost(self,event):
self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.connection_timer)
@ -155,7 +155,7 @@ class ROSWrapper:
pass pass
else: else:
robot_number = topic.split('/') robot_number = topic.split('/')
robot_number = robot_number[2] robot_number = robot_number[1]
robot_number = robot_number[7:] robot_number = robot_number[7:]
if len(robot_number) > 0: if len(robot_number) > 0:
robot_number = int(robot_number) robot_number = int(robot_number)

1
safety_user_plugin/scripts/user_info.py
View File

@ -27,6 +27,7 @@ class UserInfo:
self.selected_robot.angular_velocity = new_robot_info.angular_velocity self.selected_robot.angular_velocity = new_robot_info.angular_velocity
self.selected_robot.state = new_robot_info.state self.selected_robot.state = new_robot_info.state
self.selected_robot.clutch = new_robot_info.clutch self.selected_robot.clutch = new_robot_info.clutch
self.clutch_state = new_robot_info.clutch
def get_user_stop_state(self): def get_user_stop_state(self):
return self.user_stop_state return self.user_stop_state