diff --git a/safety_master_plugin/scripts/ros_master_wrapper.py b/safety_master_plugin/scripts/ros_master_wrapper.py index 9fa80be..0a51d8a 100644 --- a/safety_master_plugin/scripts/ros_master_wrapper.py +++ b/safety_master_plugin/scripts/ros_master_wrapper.py @@ -28,14 +28,14 @@ class ROSWrapper: def setup_subscribers_and_publishers(self): - self.master_stop_publisher = rospy.Publisher('/RosAria/master_stop',Bool,queue_size = 1) - self.restrictions_publisher = rospy.Publisher('/RosAria/restrictions',RestrictionsMsg,queue_size = 1) + self.master_stop_publisher = rospy.Publisher('/PIONIER/master_stop',Bool,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) def add_robot_subscriber(self,robot_id): 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) self.robot_info_subscribers[robot_id] = new_sub else: @@ -116,7 +116,7 @@ class ROSWrapper: pass else: robot_number = topic.split('/') - robot_number = robot_number[2] + robot_number = robot_number[1] robot_number = robot_number[7:] if len(robot_number) > 0: robot_number = int(robot_number) diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index 8e08f33..5178d02 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -41,12 +41,12 @@ class ROSWrapper: def setup_subscribers_and_publishers(self,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.master_stop_subscriber = rospy.Subscriber('/RosAria/master_stop', Bool, self.handle_master_stop_update) - self.restrictions_subscriber = rospy.Subscriber('/RosAria/restrictions', RestrictionsMsg, self.handle_restrictions_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('/PIONIER/master_stop', Bool, self.handle_master_stop_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.clutch_publisher = rospy.Publisher('/RosAria/{0}/clutch'.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('/{0}/RosAria/clutch'.format(robot_name),Bool,queue_size = 1) if self.periodic_timer != None: self.shutdown_timer(self.periodic_timer) @@ -74,7 +74,7 @@ class ROSWrapper: def publish_periodic_update(self,event): stop_state = self.get_user_stop_state() - clutch_state = self.get_clutch_state() + # clutch_state = self.get_clutch_state() if stop_state == SS.RUNNING: self.user_started() @@ -83,12 +83,12 @@ class ROSWrapper: 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') + # 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 handle_robot_connection_lost(self,event): self.shutdown_timer(self.connection_timer) @@ -155,7 +155,7 @@ class ROSWrapper: pass else: robot_number = topic.split('/') - robot_number = robot_number[2] + robot_number = robot_number[1] robot_number = robot_number[7:] if len(robot_number) > 0: robot_number = int(robot_number) diff --git a/safety_user_plugin/scripts/user_info.py b/safety_user_plugin/scripts/user_info.py index cb81450..6fcc49d 100644 --- a/safety_user_plugin/scripts/user_info.py +++ b/safety_user_plugin/scripts/user_info.py @@ -27,6 +27,7 @@ class UserInfo: self.selected_robot.angular_velocity = new_robot_info.angular_velocity self.selected_robot.state = new_robot_info.state self.selected_robot.clutch = new_robot_info.clutch + self.clutch_state = new_robot_info.clutch def get_user_stop_state(self): return self.user_stop_state