Small adjustments in ros topics names and fix for clutch handling in user plugin
This commit is contained in:
parent
d2b0a3ad1a
commit
bdf9d0f04c
@ -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)
|
||||
|
@ -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)
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user