def __init__(self, node_name):
self._node_name = node_name
self._master = Master("NodeInfo")
try:
self._uri = self._master.lookupNode(self._node_name)
except MasterError:
raise LifecycleTestException("Node %s not found" % self._node_name)
import threading
self.lock = threading.Lock()
self._mproxy = MasterProxy(self._uri)
def __main_logic(self):
self.master = Master('/rostopic')
check_cnt = 0
self.logger.info('begin start __main_logic')
while not rospy.is_shutdown():
check_cnt += 1
if check_cnt == 5:
check_cnt = 0
self.check_ui_ros_topic_is_sub()
if time.time() - self.received_time < 1:
self.link_mcu = True
self.link_ros = True
else:
self.link_mcu = False
self.link_ros = False
if self.send_link_status != self.link_ros:
self.logger.info('send link ros : ' + str(self.link_ros))
self.send_link_status = self.link_ros
self.send_msg_link_status(self.link_ros)
time.sleep(1)
self.send_msg_dispatcher(self.msg_id.mode_start_status, {
'index': setting.start_xiaoyuan,
'status': self.link_ros
})
time.sleep(1)
self.logger.info('end start __main_logic')
return
def get_rosmaster_domain(self):
'''
Workaround for rosgraph.Master.getUri() that does NOT return
a domain name with ".local".
'''
master = Master('/hironxo_command_widget')
#masteruri_http = master.getUri() # This does not obtain a hostname with ".local",
# regardless ROS_MASTER_URI actually contains it.
masteruri_http = os.environ['ROS_MASTER_URI']
urlparsed = urlparse(masteruri_http)
return urlparsed
def is_topic_subscribed(self, topicname):
return topicname in dict(
Master("test_multi_subscriber").getSystemState()[1])
class NodeInfo(object):
def __init__(self, node_name):
self._node_name = node_name
self._master = Master("NodeInfo")
try:
self._uri = self._master.lookupNode(self._node_name)
except MasterError:
raise LifecycleTestException("Node %s not found" % self._node_name)
import threading
self.lock = threading.Lock()
self._mproxy = MasterProxy(self._uri)
def get_publisher_list(self):
'''
Returns the list of publisher names that are registered by the node
:return _pub: list of publishers
:return type: [string]
'''
self._result = self._mproxy.getPublications()
if (self._result != []):
_, _, self._pub = self._result
else:
self._pub = []
return self._pub
def get_publishers_hz(self, publishers):
'''
Finds the publishing rate of the given publishers
:param publishers: List of publisher names
:param type: [string]
:return _pub_hz: list of rate of each publisher
:return type: [float]
'''
self._node_pub = self.get_publisher_list()
self._pub_hz = []
for pub in publishers:
self._temp = [x for x in self._node_pub if (x[0] == pub)]
if self._temp == []:
hz = -1 #report error as the required topic is not published by the node
else:
hz = self._get_publisher_hz(
self._temp[0]
) #we expect to find only one topic with a particular name
self._pub_hz.append(hz)
return self._pub_hz
def get_subscriber_list(self):
'''
Returns the list of subscriber names that are registered by the node
:return _sub: list of subscribers
:return type: [string]
'''
self._result = self._mproxy.getSubscriptions()
if (self._result != []):
_, _, self._sub = self._result
else:
self._sub = []
return self._sub
def get_services_list(self):
'''
Returns the list of service names that are registered by the node
:return _srv: list of services
:return type: [string]
'''
self._srv = get_service_list('/' + self._node_name)
return self._srv
def _get_publisher_hz(self, publisher):
'''
Finds the publishing rate of the given publisher by subscribing to it for 3 seconds
:param publishers: publisher name
:param type: string
:return _pub_hz: rate of the publisher
:return type: float
'''
#TODO:Need to implement a different logic to compute the rate, maybe use logic similar to class ROSTopicHz from rostopic
self._msg_count = 0
self._wait_time = 3
self._temp_sub = rospy.Subscriber(publisher[0], rospy.AnyMsg,
self._hz_calc_cb)
rospy.sleep(self._wait_time)
self._rate = self._msg_count / self._wait_time
self._msg_count = 0
return self._rate
def _hz_calc_cb(self, msg):
'''
Callback function for the subscribed topic
'''
with self.lock:
self._msg_count = self._msg_count + 1
return True