From afe40ede0f184a436cfa42a28c0cb935ad07bc04 Mon Sep 17 00:00:00 2001 From: Prasanna Kannappan Date: Fri, 5 May 2023 10:31:17 -0400 Subject: [PATCH 1/3] Enable sustitute args while loading yaml from file --- tools/rostopic/src/rostopic/__init__.py | 505 ++++++++++++++---------- 1 file changed, 295 insertions(+), 210 deletions(-) diff --git a/tools/rostopic/src/rostopic/__init__.py b/tools/rostopic/src/rostopic/__init__.py index 4de59017aa..526cff5712 100644 --- a/tools/rostopic/src/rostopic/__init__.py +++ b/tools/rostopic/src/rostopic/__init__.py @@ -35,34 +35,34 @@ # make sure we aren't using floor division from __future__ import division, print_function +import rospy +import rosgraph +import roslib.message +import genpy +from operator import itemgetter +import yaml +import traceback +import time +import socket +import math +import sys +import os +import argparse -NAME='rostopic' +NAME = 'rostopic' -import argparse -import os -import sys -import math -import socket -import time -import traceback -import yaml try: from xmlrpc.client import Fault except ImportError: from xmlrpclib import Fault -from operator import itemgetter try: from urllib.parse import urlparse except ImportError: from urlparse import urlparse -import genpy -import roslib.message -import rosgraph -#TODO: lazy-import rospy or move rospy-dependent routines to separate location -import rospy +# TODO: lazy-import rospy or move rospy-dependent routines to separate location try: long @@ -75,12 +75,15 @@ class ROSTopicException(Exception): Base exception class of rostopic-related errors """ pass + + class ROSTopicIOException(ROSTopicException): """ rostopic errors related to network I/O failures """ pass + def _check_master(): """ Make sure that master is available @@ -90,20 +93,23 @@ def _check_master(): rosgraph.Master('/rostopic').getPid() except socket.error: raise ROSTopicIOException("Unable to communicate with master!") - + + def _master_get_topic_types(master): try: val = master.getTopicTypes() except Fault: - #TODO: remove, this is for 1.1 + # TODO: remove, this is for 1.1 sys.stderr.write("WARNING: rostopic is being used against an older version of ROS/roscore\n") val = master.getPublishedTopics('/') return val + class ROSTopicHz(object): """ ROSTopicHz receives messages for a topic and computes frequency stats """ + def __init__(self, window_size, filter_expr=None, use_wtime=False): import threading from collections import defaultdict @@ -118,7 +124,7 @@ def __init__(self, window_size, filter_expr=None, use_wtime=False): self._times = defaultdict(list) self.filter_expr = filter_expr self.use_wtime = use_wtime - + # can't have infinite window size due to memory restrictions if window_size < 0: window_size = 50000 @@ -175,7 +181,7 @@ def callback_hz(self, m, topic=None): return with self.lock: curr_rostime = rospy.get_rostime() if not self.use_wtime else \ - rospy.Time.from_sec(time.time()) + rospy.Time.from_sec(time.time()) # time reset if curr_rostime.is_zero(): @@ -183,9 +189,9 @@ def callback_hz(self, m, topic=None): print("time has reset, resetting counters") self.set_times([], topic=topic) return - + curr = curr_rostime.to_sec() if not self.use_wtime else \ - rospy.Time.from_sec(time.time()).to_sec() + rospy.Time.from_sec(time.time()).to_sec() if self.get_msg_t0(topic=topic) < 0 or self.get_msg_t0(topic=topic) > curr: self.set_msg_t0(curr, topic=topic) self.set_msg_tn(curr, topic=topic) @@ -194,7 +200,7 @@ def callback_hz(self, m, topic=None): self.get_times(topic=topic).append(curr - self.get_msg_tn(topic=topic)) self.set_msg_tn(curr, topic=topic) - #only keep statistics for the last 10000 messages so as not to run out of memory + # only keep statistics for the last 10000 messages so as not to run out of memory if len(self.get_times(topic=topic)) > self.window_size - 1: self.get_times(topic=topic).pop(0) @@ -211,22 +217,22 @@ def get_hz(self, topic=None): elif self.get_msg_tn(topic=topic) == self.get_last_printed_tn(topic=topic): return with self.lock: - #frequency - + # frequency + # kwc: In the past, the rate decayed when a publisher # dies. Now, we use the last received message to perform # the calculation. This change was made because we now # report a count and keep track of last_printed_tn. This # makes it easier for users to see when a publisher dies, # so the decay is no longer necessary. - + n = len(self.get_times(topic=topic)) #rate = (n - 1) / (rospy.get_time() - self.msg_t0) mean = sum(self.get_times(topic=topic)) / n rate = 1./mean if mean > 0. else 0 - #std dev - std_dev = math.sqrt(sum((x - mean)**2 for x in self.get_times(topic=topic)) /n) + # std dev + std_dev = math.sqrt(sum((x - mean)**2 for x in self.get_times(topic=topic)) / n) # min and max max_delta = max(self.get_times(topic=topic)) @@ -246,7 +252,8 @@ def print_hz(self, topics=(None,)): print("no new messages") return rate, min_delta, max_delta, std_dev, window = ret - print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, window)) + print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s" % + (rate, min_delta, max_delta, std_dev, window)) return # monitoring multiple topics' hz @@ -269,6 +276,7 @@ def print_hz(self, topics=(None,)): return print(_get_ascii_table(header, stats)) + def _get_ascii_table(header, cols): # compose table with left alignment header_aligned = [] @@ -287,9 +295,11 @@ def _get_ascii_table(header, cols): header=' '.join(header_aligned), hline='=' * table_width, body=body) return table + def _sleep(duration): rospy.rostime.wallsleep(duration) + def _rostopic_hz(topics, window_size=-1, filter_expr=None, use_wtime=False, tcp_nodelay=False): """ Periodically print the publishing rate of a topic to console until @@ -305,7 +315,7 @@ def _rostopic_hz(topics, window_size=-1, filter_expr=None, use_wtime=False, tcp_ rospy.init_node(NAME, anonymous=True) rt = ROSTopicHz(window_size, filter_expr=filter_expr, use_wtime=use_wtime) for topic in topics: - msg_class, real_topic, _ = get_topic_class(topic, blocking=True) # pause hz until topic is published + msg_class, real_topic, _ = get_topic_class(topic, blocking=True) # pause hz until topic is published # we use a large buffer size as we don't know what sort of messages we're dealing with. # may parameterize this in the future if filter_expr is not None: @@ -316,12 +326,13 @@ def _rostopic_hz(topics, window_size=-1, filter_expr=None, use_wtime=False, tcp_ print("subscribed to [%s]" % real_topic) if rospy.get_param('use_sim_time', False): - print("WARNING: may be using simulated time",file=sys.stderr) + print("WARNING: may be using simulated time", file=sys.stderr) while not rospy.is_shutdown(): _sleep(1.0) rt.print_hz(topics) + class ROSTopicDelay(object): def __init__(self, window_size): @@ -394,7 +405,8 @@ def print_delay(self): print("no new messages") return delay, min_delta, max_delta, std_dev, window = ret - print("average delay: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(delay, min_delta, max_delta, std_dev, window)) + print("average delay: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s" % + (delay, min_delta, max_delta, std_dev, window)) def _rostopic_delay(topic, window_size=-1, tcp_nodelay=False): @@ -415,7 +427,7 @@ def _rostopic_delay(topic, window_size=-1, tcp_nodelay=False): print("subscribed to [%s]" % real_topic) if rospy.get_param('use_sim_time', False): - print("WARNING: may be using simulated time",file=sys.stderr) + print("WARNING: may be using simulated time", file=sys.stderr) while not rospy.is_shutdown(): _sleep(1.0) @@ -427,18 +439,18 @@ def __init__(self, window_size=100): import threading self.lock = threading.Lock() self.last_printed_tn = 0 - self.sizes =[] - self.times =[] + self.sizes = [] + self.times = [] self.window_size = window_size or 100 - + def callback(self, data): """ros sub callback""" with self.lock: try: t = time.time() self.times.append(t) - self.sizes.append(len(data._buff)) #AnyMsg instance - assert(len(self.times) == len(self.sizes)) + self.sizes.append(len(data._buff)) # AnyMsg instance + assert (len(self.times) == len(self.sizes)) if len(self.times) > self.window_size: self.times.pop(0) @@ -454,7 +466,7 @@ def print_bw(self): n = len(self.times) tn = time.time() t0 = self.times[0] - + total = sum(self.sizes) bytes_per_s = total / (tn - t0) mean = total / n @@ -465,15 +477,16 @@ def print_bw(self): max_s = max(self.sizes) min_s = min(self.sizes) - #min/max and even mean are likely to be much smaller, but for now I prefer unit consistency + # min/max and even mean are likely to be much smaller, but for now I prefer unit consistency if bytes_per_s < 1000: - bw, mean, min_s, max_s = ["%.2fB"%v for v in [bytes_per_s, mean, min_s, max_s]] + bw, mean, min_s, max_s = ["%.2fB" % v for v in [bytes_per_s, mean, min_s, max_s]] elif bytes_per_s < 1000000: - bw, mean, min_s, max_s = ["%.2fKB"%(v/1000) for v in [bytes_per_s, mean, min_s, max_s]] + bw, mean, min_s, max_s = ["%.2fKB" % (v/1000) for v in [bytes_per_s, mean, min_s, max_s]] else: - bw, mean, min_s, max_s = ["%.2fMB"%(v/1000000) for v in [bytes_per_s, mean, min_s, max_s]] - - print("average: %s/s\n\tmean: %s min: %s max: %s window: %s"%(bw, mean, min_s, max_s, n)) + bw, mean, min_s, max_s = ["%.2fMB" % (v/1000000) for v in [bytes_per_s, mean, min_s, max_s]] + + print("average: %s/s\n\tmean: %s min: %s max: %s window: %s" % (bw, mean, min_s, max_s, n)) + def _isstring_type(t): valid_types = [str] @@ -483,13 +496,14 @@ def _isstring_type(t): pass return t in valid_types + def _rostopic_bw(topic, window_size=-1): """ periodically print the received bandwidth of a topic to console until shutdown """ _check_master() - _, real_topic, _ = get_topic_type(topic, blocking=True) #pause hz until topic is published + _, real_topic, _ = get_topic_type(topic, blocking=True) # pause hz until topic is published if rospy.is_shutdown(): return # #3543 disable all auto-subscriptions to /clock @@ -498,12 +512,14 @@ def _rostopic_bw(topic, window_size=-1): # we use a large buffer size as we don't know what sort of messages we're dealing with. # may parameterize this in the future sub = rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback) - print("subscribed to [%s]"%real_topic) + print("subscribed to [%s]" % real_topic) while not rospy.is_shutdown(): _sleep(1.0) rt.print_bw() # code adapted from rqt_plot + + def msgevalgen(pattern): """ Generates a function that returns the relevant field(s) (aka 'subtopic(s)') of a Message object @@ -531,13 +547,13 @@ def msgevalgen(pattern): def msgeval(msg, evals): for i, (field_name, slice_object) in enumerate(evals): - try: # access field first + try: # access field first msg = getattr(msg, field_name) except AttributeError: print("no field named %s in %s" % (field_name, pattern), file=sys.stderr) return None - if slice_object is not None: # access slice + if slice_object is not None: # access slice try: msg = msg.__getitem__(slice_object) except IndexError as e: @@ -577,20 +593,22 @@ def _get_array_index_or_slice_object(index_string): except ValueError: assert False, "non-integer slice stop '%s'" % index_string_parts[1] if len(index_string_parts) > 2 and index_string_parts[2] != '': - try: - slice_args[2] = int(index_string_parts[2]) - except ValueError: - assert False, "non-integer slice step '%s'" % index_string_parts[2] + try: + slice_args[2] = int(index_string_parts[2]) + except ValueError: + assert False, "non-integer slice step '%s'" % index_string_parts[2] if len(index_string_parts) > 3: assert False, 'too many slice arguments' return slice(*slice_args) + def _get_nested_attribute(msg, nested_attributes): value = msg for attr in nested_attributes.split('/'): value = getattr(value, attr) return value + def _get_topic_type(topic): """ subroutine for getting the topic type @@ -639,14 +657,15 @@ def _get_topic_type(topic): return None, None, None # NOTE: this is used externally by rxplot - + + def get_topic_type(topic, blocking=False): """ Get the topic type. :param topic: topic name, ``str`` :param blocking: (default False) block until topic becomes available, ``bool`` - + :returns: topic type, real topic name and fn to evaluate the message instance if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)`` :raises: :exc:`ROSTopicException` If master cannot be contacted @@ -655,7 +674,7 @@ def get_topic_type(topic, blocking=False): if topic_type: return topic_type, real_topic, msg_eval elif blocking: - sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic) + sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n" % topic) while not rospy.is_shutdown(): topic_type, real_topic, msg_eval = _get_topic_type(topic) if topic_type: @@ -664,6 +683,7 @@ def get_topic_type(topic, blocking=False): _sleep(0.1) return None, None, None + def get_topic_class(topic, blocking=False): """ Get the topic message class @@ -680,6 +700,7 @@ def get_topic_class(topic, blocking=False): raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % topic_type) return msg_class, real_topic, msg_eval + def _str_plot_fields(val, f, field_filter): """ get CSV representation of fields used by _str_plot @@ -691,12 +712,13 @@ def _str_plot_fields(val, f, field_filter): else: return 'time,' + def _sub_str_plot_fields(val, f, field_filter): """recursive helper function for _str_plot_fields""" # CSV type_ = type(val) if type_ in (bool, int, long, float) or \ - isinstance(val, genpy.TVal): + isinstance(val, genpy.TVal): return f # duck-type check for messages elif hasattr(val, "_slot_types"): @@ -704,7 +726,8 @@ def _sub_str_plot_fields(val, f, field_filter): fields = list(field_filter(val)) else: fields = val.__slots__ - sub = (_sub_str_plot_fields(_convert_getattr(val, a, t), f+"."+a, field_filter) for a,t in zip(val.__slots__, val._slot_types) if a in fields) + sub = (_sub_str_plot_fields(_convert_getattr(val, a, t), f+"."+a, field_filter) + for a, t in zip(val.__slots__, val._slot_types) if a in fields) sub = [s for s in sub if s is not None] if sub: return ','.join([s for s in sub]) @@ -717,14 +740,14 @@ def _sub_str_plot_fields(val, f, field_filter): type0 = type(val0) # no arrays of arrays if type0 in (bool, int, long, float) or \ - isinstance(val0, genpy.TVal): - return ','.join(["%s%s"%(f,x) for x in range(0,len(val))]) + isinstance(val0, genpy.TVal): + return ','.join(["%s%s" % (f, x) for x in range(0, len(val))]) elif _isstring_type(type0): - - return ','.join(["%s%s"%(f,x) for x in range(0,len(val))]) + + return ','.join(["%s%s" % (f, x) for x in range(0, len(val))]) elif hasattr(val0, "_slot_types"): - labels = ["%s%s"%(f,x) for x in range(0,len(val))] - sub = [s for s in [_sub_str_plot_fields(v, sf, field_filter) for v,sf in zip(val, labels)] if s] + labels = ["%s%s" % (f, x) for x in range(0, len(val))] + sub = [s for s in [_sub_str_plot_fields(v, sf, field_filter) for v, sf in zip(val, labels)] if s] if sub: return ','.join([s for s in sub]) return None @@ -741,7 +764,7 @@ def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_ :param value_transform: Not used but for same API as CallbackEcho.custom_strify_message :returns: comma-separated list of field values in val, ``str`` """ - + s = _sub_str_plot(val, time_offset, field_filter) if s is None: s = '' @@ -749,35 +772,37 @@ def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_ if time_offset is not None: time_offset = time_offset.to_nsec() else: - time_offset = 0 - + time_offset = 0 + if current_time is not None: - return "%s,%s"%(current_time.to_nsec()-time_offset, s) + return "%s,%s" % (current_time.to_nsec()-time_offset, s) elif getattr(val, "_has_header", False): - return "%s,%s"%(val.header.stamp.to_nsec()-time_offset, s) + return "%s,%s" % (val.header.stamp.to_nsec()-time_offset, s) else: - return "%s,%s"%(rospy.get_rostime().to_nsec()-time_offset, s) - + return "%s,%s" % (rospy.get_rostime().to_nsec()-time_offset, s) + + def _sub_str_plot(val, time_offset, field_filter): """Helper routine for _str_plot.""" # CSV type_ = type(val) - + if type_ == bool: return '1' if val else '0' elif type_ in (int, long, float) or \ - isinstance(val, genpy.TVal): + isinstance(val, genpy.TVal): if time_offset is not None and isinstance(val, genpy.Time): return str(val-time_offset) else: - return str(val) + return str(val) elif hasattr(val, "_slot_types"): if field_filter is not None: fields = list(field_filter(val)) else: - fields = val.__slots__ + fields = val.__slots__ - sub = (_sub_str_plot(_convert_getattr(val, f, t), time_offset, field_filter) for f,t in zip(val.__slots__, val._slot_types) if f in fields) + sub = (_sub_str_plot(_convert_getattr(val, f, t), time_offset, field_filter) + for f, t in zip(val.__slots__, val._slot_types) if f in fields) sub = [s for s in sub if s is not None] if sub: return ','.join(sub) @@ -792,17 +817,19 @@ def _sub_str_plot(val, time_offset, field_filter): if type0 == bool: return ','.join([('1' if v else '0') for v in val]) elif type0 in (int, long, float) or \ - isinstance(val0, genpy.TVal): + isinstance(val0, genpy.TVal): return ','.join([str(v) for v in val]) elif _isstring_type(type0): - return ','.join([v for v in val]) + return ','.join([v for v in val]) elif hasattr(val0, "_slot_types"): sub = [s for s in [_sub_str_plot(v, time_offset, field_filter) for v in val] if s is not None] if sub: return ','.join([s for s in sub]) return None - + # copied from roslib.message + + def _convert_getattr(val, f, t): """ Convert atttribute types on the fly, if necessary. This is mainly @@ -814,6 +841,7 @@ def _convert_getattr(val, f, t): else: return attr + class CallbackEcho(object): """ Callback instance that can print callback data in a variety of @@ -844,8 +872,8 @@ def __init__(self, topic, msg_eval, plot=False, filter_fn=None, self.fixed_numeric_width = fixed_numeric_width self.prefix = '' - self.suffix = '\n---' if not plot else ''# same as YAML document separator, bug #3291 - + self.suffix = '\n---' if not plot else '' # same as YAML document separator, bug #3291 + self.echo_all_topics = echo_all_topics self.offset_time = offset_time @@ -856,18 +884,18 @@ def __init__(self, topic, msg_eval, plot=False, filter_fn=None, # determine which strifying function to use if plot: - #TODOXXX: need to pass in filter function + # TODOXXX: need to pass in filter function self.str_fn = _str_plot self.sep = '' else: - #TODOXXX: need to pass in filter function + # TODOXXX: need to pass in filter function self.str_fn = self.custom_strify_message if echo_clear: self.prefix = '\033[2J\033[;H' - self.field_filter=field_filter_fn - self.value_transform=value_transform_fn - + self.field_filter = field_filter_fn + self.value_transform = value_transform_fn + # first tracks whether or not we've printed anything yet. Need this for printing plot fields. self.first = True @@ -901,7 +929,7 @@ def callback(self, data, callback_args, current_time=None): if self.max_count is not None and self.count >= self.max_count: self.done = True return - + try: msg_eval = self.msg_eval if topic == self.topic: @@ -920,31 +948,31 @@ def callback(self, data, callback_args, current_time=None): if msg_eval is not None: data = msg_eval(data) - + # data can be None if msg_eval returns None if data is not None: # NOTE: we do all prints using direct writes to sys.stdout, which works better with piping - + self.count += 1 - + # print fields header for plot if self.plot and self.first: sys.stdout.write("%"+_str_plot_fields(data, 'field', self.field_filter)+'\n') self.first = False if self.offset_time: - sys.stdout.write(self.prefix+\ + sys.stdout.write(self.prefix + self.str_fn(data, time_offset=rospy.get_rostime(), current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width, - value_transform=self.value_transform) + \ + value_transform=self.value_transform) + self.suffix + '\n') else: - sys.stdout.write(self.prefix+\ + sys.stdout.write(self.prefix + self.str_fn(data, current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width, - value_transform=self.value_transform) + \ + value_transform=self.value_transform) + self.suffix + '\n') # we have to flush in order before piping to work @@ -959,7 +987,8 @@ def callback(self, data, callback_args, current_time=None): # set done flag so we exit self.done = True traceback.print_exc() - + + def _rostopic_type(topic): """ Print ROS message type of topic to screen @@ -967,7 +996,7 @@ def _rostopic_type(topic): """ topic_type, topic_real_name, _ = get_topic_type(topic, blocking=False) if topic_type is None: - sys.stderr.write('unknown topic type [%s]\n'%topic) + sys.stderr.write('unknown topic type [%s]\n' % topic) sys.exit(1) elif topic == topic_real_name: print(topic_type) @@ -977,7 +1006,8 @@ def _rostopic_type(topic): for current_field in field.split('/'): msg_class = roslib.message.get_message_class(field_type) field_type = msg_class._slot_types[msg_class.__slots__.index(current_field)] - print('%s %s %s'%(topic_type, field, field_type)) + print('%s %s %s' % (topic_type, field, field_type)) + def _rostopic_echo_bag(callback_echo, bag_file): """ @@ -985,13 +1015,13 @@ def _rostopic_echo_bag(callback_echo, bag_file): :param bag_file: name of bag file to echo messages from or ``None``, ``str`` """ if not os.path.exists(bag_file): - raise ROSTopicException("bag file [%s] does not exist"%bag_file) + raise ROSTopicException("bag file [%s] does not exist" % bag_file) first = True - + import rosbag with rosbag.Bag(bag_file) as b: for t, msg, timestamp in b.read_messages(): - # bag files can have relative paths in them, this respects any + # bag files can have relative paths in them, this respects any # dynamic renaming if t[0] != '/': t = rosgraph.names.script_resolve_name('rostopic', t) @@ -1000,10 +1030,11 @@ def _rostopic_echo_bag(callback_echo, bag_file): if callback_echo.done: break + def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): """ Print new messages on topic to screen. - + :param topic: topic name, ``str`` :param bag_file: name of bag file to echo messages from or ``None``, ``str`` """ @@ -1011,7 +1042,7 @@ def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): if bag_file: # initialize rospy time due to potential timestamp printing - rospy.rostime.set_rostime_initialized(True) + rospy.rostime.set_rostime_initialized(True) _rostopic_echo_bag(callback_echo, bag_file) else: _check_master() @@ -1038,10 +1069,12 @@ def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): if fields: submsg_class = roslib.message.get_message_class(type_information.split('[', 1)[0]) if not submsg_class: - raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % type_information) + raise ROSTopicException( + "Cannot load message class for [%s]. Are your messages built?" % type_information) use_sim_time = rospy.get_param('/use_sim_time', False) - sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, {'topic': topic, 'type_information': type_information}) + sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, { + 'topic': topic, 'type_information': type_information}) if use_sim_time: # #2950: print warning if nothing received for two seconds @@ -1056,12 +1089,16 @@ def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): if callback_echo.count == 0 and \ not rospy.is_shutdown() and \ not callback_echo.done: - sys.stderr.write("WARNING: no messages received and simulated time is active.\nIs /clock being published?\n") + sys.stderr.write( + "WARNING: no messages received and simulated time is active.\nIs /clock being published?\n") while not rospy.is_shutdown() and not callback_echo.done: _sleep(0.1) + _caller_apis = {} + + def get_api(master, caller_id): """ Get XML-RPC API of node @@ -1078,10 +1115,11 @@ def get_api(master, caller_id): except socket.error: raise ROSTopicIOException("Unable to communicate with master!") except rosgraph.MasterError: - caller_api = 'unknown address %s'%caller_id + caller_api = 'unknown address %s' % caller_id return caller_api + def _rostopic_list_bag(bag_file, topic=None): """ Prints topics in bag file to screen @@ -1090,7 +1128,7 @@ def _rostopic_list_bag(bag_file, topic=None): """ import rosbag if not os.path.exists(bag_file): - raise ROSTopicException("bag file [%s] does not exist"%bag_file) + raise ROSTopicException("bag file [%s] does not exist" % bag_file) with rosbag.Bag(bag_file) as b: if topic: @@ -1109,8 +1147,9 @@ def _rostopic_list_bag(bag_file, topic=None): if rospy.is_shutdown(): break import time - earliest, latest = [time.strftime("%d %b %Y %H:%M:%S", time.localtime(t.to_time())) for t in (earliest, latest)] - print("%s message(s) from %s to %s"%(count, earliest, latest)) + earliest, latest = [time.strftime("%d %b %Y %H:%M:%S", time.localtime(t.to_time())) + for t in (earliest, latest)] + print("%s message(s) from %s to %s" % (count, earliest, latest)) else: topics = set() for top, msg, _ in b.read_messages(raw=True): @@ -1120,26 +1159,27 @@ def _rostopic_list_bag(bag_file, topic=None): if rospy.is_shutdown(): break + def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, verbose, indent=''): if verbose: topic_types = _master_get_topic_types(master) if not subscribers_only: - print("\n%sPublished topics:"%indent) + print("\n%sPublished topics:" % indent) for t, ttype, tlist in pubs: if len(tlist) > 1: - print(indent+" * %s [%s] %s publishers"%(t, ttype, len(tlist))) + print(indent+" * %s [%s] %s publishers" % (t, ttype, len(tlist))) else: - print(indent+" * %s [%s] 1 publisher"%(t, ttype)) + print(indent+" * %s [%s] 1 publisher" % (t, ttype)) if not publishers_only: print(indent) print(indent+"Subscribed topics:") for t, ttype, tlist in subs: if len(tlist) > 1: - print(indent+" * %s [%s] %s subscribers"%(t, ttype, len(tlist))) + print(indent+" * %s [%s] %s subscribers" % (t, ttype, len(tlist))) else: - print(indent+" * %s [%s] 1 subscriber"%(t, ttype)) + print(indent+" * %s [%s] 1 subscriber" % (t, ttype)) print('') else: if publishers_only: @@ -1147,13 +1187,15 @@ def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, ve elif subscribers_only: topics = [t for t, _, _ in subs] else: - topics = list(set([t for t, _, _ in pubs] + [t for t, _, _ in subs])) + topics = list(set([t for t, _, _ in pubs] + [t for t, _, _ in subs])) topics.sort() - print('\n'.join(["%s%s"%(indent, t) for t in topics])) + print('\n'.join(["%s%s" % (indent, t) for t in topics])) + def get_topic_list(master=None): if not master: master = rosgraph.Master('/rostopic') + def topic_type(t, topic_types): matches = [t_type for t_name, t_type in topic_types if t_name == t] if matches: @@ -1163,7 +1205,7 @@ def topic_type(t, topic_types): # Return an array of tuples; (, , ) state = master.getSystemState() topic_types = _master_get_topic_types(master) - + pubs, subs, _ = state pubs_out = [] for topic, nodes in pubs: @@ -1176,6 +1218,8 @@ def topic_type(t, topic_types): return (pubs_out, subs_out) # #3145 + + def _rostopic_list_group_by_host(master, pubs, subs): """ Build up maps for hostname to topic list per hostname @@ -1186,7 +1230,7 @@ def build_map(master, state, uricache): for topic, ttype, tnodes in state: for p in tnodes: if not p in uricache: - uricache[p] = master.lookupNode(p) + uricache[p] = master.lookupNode(p) uri = uricache[p] puri = urlparse(uri) if not puri.hostname in tmap: @@ -1198,18 +1242,19 @@ def build_map(master, state, uricache): else: tmap[puri.hostname].append((topic, ttype, [p])) return tmap - + uricache = {} host_pub_topics = build_map(master, pubs, uricache) host_sub_topics = build_map(master, subs, uricache) return host_pub_topics, host_sub_topics + def _rostopic_list(topic, verbose=False, subscribers_only=False, publishers_only=False, group_by_host=False): """ Print topics to screen - + :param topic: topic name to list information or None to match all topics, ``str`` :param verbose: print additional debugging information, ``bool`` :param subscribers_only: print information about subscriptions only, ``bool`` @@ -1219,7 +1264,7 @@ def _rostopic_list(topic, verbose=False, # #1563 if subscribers_only and publishers_only: raise ROSTopicException("cannot specify both subscribers- and publishers-only") - + master = rosgraph.Master('/rostopic') try: pubs, subs = get_topic_list(master=master) @@ -1232,9 +1277,9 @@ def _rostopic_list(topic, verbose=False, if group_by_host: # #3145 - host_pub_topics, host_sub_topics = _rostopic_list_group_by_host(master, pubs, subs) - for hostname in set(list(host_pub_topics.keys()) + list(host_sub_topics.keys())): #py3k - pubs, subs = host_pub_topics.get(hostname,[]), host_sub_topics.get(hostname, []), + host_pub_topics, host_sub_topics = _rostopic_list_group_by_host(master, pubs, subs) + for hostname in set(list(host_pub_topics.keys()) + list(host_sub_topics.keys())): # py3k + pubs, subs = host_pub_topics.get(hostname, []), host_sub_topics.get(hostname, []), if (pubs and not subscribers_only) or (subs and not publishers_only): print("Host [%s]:" % hostname) _sub_rostopic_list(master, pubs, subs, @@ -1245,10 +1290,11 @@ def _rostopic_list(topic, verbose=False, publishers_only, subscribers_only, verbose) + def get_info_text(topic): """ Get human-readable topic description - + :param topic: topic name, ``str`` """ try: @@ -1257,6 +1303,7 @@ def get_info_text(topic): from io import StringIO import itertools buff = StringIO() + def topic_type(t, topic_types): matches = [t_type for t_name, t_type in topic_types if t_name == t] if matches: @@ -1271,19 +1318,19 @@ def topic_type(t, topic_types): pubs = [x for x in pubs if x[0] == topic] topic_types = _master_get_topic_types(master) - + except socket.error: raise ROSTopicIOException("Unable to communicate with master!") if not pubs and not subs: - raise ROSTopicException("Unknown topic %s"%topic) + raise ROSTopicException("Unknown topic %s" % topic) - buff.write("Type: %s\n\n"%topic_type(topic, topic_types)) + buff.write("Type: %s\n\n" % topic_type(topic, topic_types)) if pubs: buff.write("Publishers: \n") for p in itertools.chain(*[nodes for topic, ttype, nodes in pubs]): - buff.write(" * %s (%s)\n"%(p, get_api(master, p))) + buff.write(" * %s (%s)\n" % (p, get_api(master, p))) else: buff.write("Publishers: None\n") buff.write('\n') @@ -1291,23 +1338,25 @@ def topic_type(t, topic_types): if subs: buff.write("Subscribers: \n") for p in itertools.chain(*[nodes for topic, ttype, nodes in subs]): - buff.write(" * %s (%s)\n"%(p, get_api(master, p))) + buff.write(" * %s (%s)\n" % (p, get_api(master, p))) else: buff.write("Subscribers: None\n") buff.write('\n') return buff.getvalue() - + + def _rostopic_info(topic): """ Print topic information to screen. - + :param topic: topic name, ``str`` """ print(get_info_text(topic)) - + ########################################################################################## # COMMAND PROCESSING ##################################################################### - + + def _rostopic_cmd_echo(argv): def expr_eval(expr): def eval_fn(m): @@ -1320,18 +1369,18 @@ def eval_fn(m): parser.add_option("-b", "--bag", dest="bag", default=None, help="echo messages from .bag file", metavar="BAGFILE") - parser.add_option("-p", + parser.add_option("-p", dest="plot", default=False, action="store_true", help="echo in a plotting friendly format") parser.add_option("-w", dest="fixed_numeric_width", default=None, metavar="NUM_WIDTH", help="fixed width for numeric values") - parser.add_option("--filter", + parser.add_option("--filter", dest="filter_expr", default=None, metavar="FILTER-EXPRESSION", help="Python expression to filter messages that are printed. Expression can use Python builtins as well as m (the message) and topic (the topic name).") - parser.add_option("--nostr", + parser.add_option("--nostr", dest="nostr", default=False, action="store_true", help="exclude string fields") @@ -1347,7 +1396,7 @@ def eval_fn(m): dest="all_topics", default=False, action="store_true", help="display all message in bag, only valid with -b option") - parser.add_option("-n", + parser.add_option("-n", dest="msg_count", default=None, metavar="COUNT", help="number of messages to echo") parser.add_option("--offset", @@ -1366,12 +1415,12 @@ def eval_fn(m): topic = '' else: if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") topic = rosgraph.names.script_resolve_name('rostopic', args[0]) # suppressing output to keep it clean - #if not options.plot: + # if not options.plot: # print "rostopic: topic is [%s]"%topic - + filter_fn = None if options.filter_expr: filter_fn = expr_eval(options.filter_expr) @@ -1407,6 +1456,7 @@ def eval_fn(m): except socket.error: sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") + def create_value_transform(echo_nostr, echo_noarr): def value_transform(val, type_information=None): def transform_field_value(value, value_type, echo_nostr, echo_noarr): @@ -1455,6 +1505,7 @@ class TransformedMessage(genpy.Message): return val_trans return value_transform + def create_field_filter(echo_nostr, echo_noarr): def field_filter(val): fields = val.__slots__ @@ -1467,23 +1518,26 @@ def field_filter(val): yield f return field_filter + def _optparse_topic_only(cmd, argv): args = argv[2:] from optparse import OptionParser - parser = OptionParser(usage="usage: %%prog %s /topic"%cmd, prog=NAME) + parser = OptionParser(usage="usage: %%prog %s /topic" % cmd, prog=NAME) (options, args) = parser.parse_args(args) if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") if len(args) > 1: parser.error("you may only specify one input topic") return rosgraph.names.script_resolve_name('rostopic', args[0]) + def _rostopic_cmd_type(argv): parser = argparse.ArgumentParser(prog='%s type' % NAME) parser.add_argument('topic_or_field', help='Topic or field name') args = parser.parse_args(argv[2:]) _rostopic_type(rosgraph.names.script_resolve_name('rostopic', args.topic_or_field)) + def _rostopic_cmd_hz(argv): args = argv[2:] from optparse import OptionParser @@ -1503,7 +1557,7 @@ def _rostopic_cmd_hz(argv): (options, args) = parser.parse_args(args) if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") try: window_size = int(options.window_size) except: @@ -1552,7 +1606,7 @@ def _rostopic_cmd_bw(argv=sys.argv): help="window size, in # of messages, for calculating rate", metavar="WINDOW") options, args = parser.parse_args(args) if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") if len(args) > 1: parser.error("you may only specify one input topic") try: @@ -1562,6 +1616,7 @@ def _rostopic_cmd_bw(argv=sys.argv): topic = rosgraph.names.script_resolve_name('rostopic', args[0]) _rostopic_bw(topic, window_size=window_size) + def find_by_type(topic_type): """ Lookup topics by topic_type @@ -1574,7 +1629,8 @@ def find_by_type(topic_type): except socket.error: raise ROSTopicIOException("Unable to communicate with master!") return [t_name for t_name, t_type in t_list if t_type == topic_type] - + + def _rostopic_cmd_find(argv=sys.argv): """ Implements 'rostopic type' @@ -1589,19 +1645,20 @@ def _rostopic_cmd_find(argv=sys.argv): if len(args) > 1: parser.error("you may only specify one message type") print('\n'.join(find_by_type(args[0]))) - + def _resource_name_package(name): """ pkg/typeName -> pkg, typeName -> None - + :param name: package resource name, e.g. 'std_msgs/String', ``str`` :returns: package name of resource, ``str`` - """ + """ if not '/' in name: return None return name[:name.find('/')] + def create_publisher(topic_name, topic_type, latch, disable_rostime=True): """ Create rospy.Publisher instance from the string topic name and @@ -1619,19 +1676,21 @@ def create_publisher(topic_name, topic_type, latch, disable_rostime=True): try: msg_class = roslib.message.get_message_class(topic_type) except: - raise ROSTopicException("invalid topic type: %s"%topic_type) + raise ROSTopicException("invalid topic type: %s" % topic_type) if msg_class is None: pkg = _resource_name_package(topic_type) - raise ROSTopicException("invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'"%(topic_type, pkg)) + raise ROSTopicException( + "invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'" % (topic_type, pkg)) # disable /rosout and /rostime as this causes blips in the pubsub network due to rostopic pub often exiting quickly rospy.init_node('rostopic', anonymous=True, disable_rosout=True, disable_rostime=disable_rostime) pub = rospy.Publisher(topic_name, msg_class, latch=latch, queue_size=100) return pub, msg_class + def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, pub_args=None): """ Publish message at specified rate. Subroutine of L{publish_message()}. - + :param pub: :class:rospy.Publisher` instance for topic :param msg: message instance to publish :param rate: publishing rate (hz) or None for just once, ``int`` @@ -1645,15 +1704,18 @@ def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, p if substitute_keywords: _fillMessageArgs(msg, pub_args) if verbose: - print("publishing %s"%msg) + print("publishing %s" % msg) pub.publish(msg) r.sleep() + _ONCE_DELAY = 3. + + def _publish_latched(pub, msg, once=False, verbose=False): """ Publish and latch message. Subroutine of L{publish_message()}. - + :param pub: :class:`rospy.Publisher` instance for topic :param msg: message instance to publish :param once: if ``True``, publish message once and then exit after sleep interval, ``bool`` @@ -1665,13 +1727,14 @@ def _publish_latched(pub, msg, once=False, verbose=False): raise ROSTopicException(str(e)) if not once: - rospy.spin() + rospy.spin() + def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=False, substitute_keywords=False): """ Create new instance of msg_class, populate with pub_args, and publish. This may print output to screen. - + :param pub: :class:`rospy.Publisher` instance for topic :param msg_class: Message type, ``Class`` :param pub_args: Arguments to initialize message that is published, ``[val]`` @@ -1684,24 +1747,26 @@ def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=Fal _fillMessageArgs(msg, pub_args) try: - + if rate is None: - s = "publishing and latching [%s]"%(msg) if verbose else "publishing and latching message" + s = "publishing and latching [%s]" % (msg) if verbose else "publishing and latching message" if once: - s = s + " for %s seconds"%_ONCE_DELAY + s = s + " for %s seconds" % _ONCE_DELAY else: s = s + ". Press ctrl-C to terminate" print(s) _publish_latched(pub, msg, once, verbose) else: - _publish_at_rate(pub, msg, rate, verbose=verbose, substitute_keywords=substitute_keywords, pub_args=pub_args) - + _publish_at_rate(pub, msg, rate, verbose=verbose, + substitute_keywords=substitute_keywords, pub_args=pub_args) + except rospy.ROSSerializationException as e: import rosmsg # we could just print the message definition, but rosmsg is more readable - raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n"+\ - " %s\n\nmsg file:\n%s"%(e, rosmsg.get_msg_text(msg_class._type))) + raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n" + + " %s\n\nmsg file:\n%s" % (e, rosmsg.get_msg_text(msg_class._type))) + def _fillMessageArgs(msg, pub_args): try: @@ -1718,11 +1783,12 @@ def _fillMessageArgs(msg, pub_args): # allow the use of the 'now' string with timestamps and 'auto' with header now = rospy.get_rostime() import std_msgs.msg - keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) } + keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)} genpy.message.fill_message_args(msg, pub_args, keys=keys) except genpy.MessageException as e: - raise ROSTopicException(str(e)+"\n\nArgs are: [%s]"%genpy.message.get_printable_message_args(msg)) - + raise ROSTopicException(str(e)+"\n\nArgs are: [%s]" % genpy.message.get_printable_message_args(msg)) + + def _rostopic_cmd_pub(argv): """ Parse 'pub' command arguments and run command. Will cause a system @@ -1749,9 +1815,9 @@ def _rostopic_cmd_pub(argv): help="When publishing with a rate, performs keyword ('now' or 'auto') substitution for each message") parser.add_option('--use-rostime', dest="use_rostime", default=False, action="store_true", help="use rostime for time stamps, else walltime is used") - #parser.add_option("-p", '--param', dest="parameter", metavar='/PARAM', default=None, + # parser.add_option("-p", '--param', dest="parameter", metavar='/PARAM', default=None, # help="read args from ROS parameter (Bagy format)") - + (options, args) = parser.parse_args(args) if options.rate is not None: if options.once: @@ -1765,7 +1831,7 @@ def _rostopic_cmd_pub(argv): else: # we will default this to 10 for file/stdin later rate = None - + # validate args len if len(args) == 0: parser.error("/topic must be specified") @@ -1773,9 +1839,9 @@ def _rostopic_cmd_pub(argv): parser.error("topic type must be specified") if 0: if len(args) > 2 and options.parameter: - parser.error("args conflict with -p setting") + parser.error("args conflict with -p setting") if len(args) > 2 and options.file: - parser.error("args conflict with -f setting") + parser.error("args conflict with -f setting") topic_name, topic_type = args[0], args[1] # type-case using YAML @@ -1800,17 +1866,19 @@ def _rostopic_cmd_pub(argv): param_publish_once(pub, msg_class, param_name, rate, options.verbose) else: param_publish(pub, msg_class, param_name, rate, options.verbose) - + elif not pub_args and len(msg_class.__slots__): if not options.file and sys.stdin.isatty(): parser.error("Please specify message values") # stdin/file input has a rate by default if rate is None and not options.latch and not options.once: rate = 10. - stdin_publish(pub, msg_class, rate, options.once, options.file, options.verbose) + stdin_publish(pub, msg_class, rate, options.once, options.file, + options.verbose, substitute_keywords=options.substitute_keywords) else: - argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, substitute_keywords=options.substitute_keywords) - + argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, + substitute_keywords=options.substitute_keywords) + def file_yaml_arg(filename): """ @@ -1819,8 +1887,9 @@ def file_yaml_arg(filename): :raises: :exc:`ROSTopicException` If filename is invalid """ if not os.path.isfile(filename): - raise ROSTopicException("file does not exist: %s"%(filename)) + raise ROSTopicException("file does not exist: %s" % (filename)) import yaml + def bagy_iter(): try: with open(filename, 'r') as f: @@ -1829,9 +1898,10 @@ def bagy_iter(): for d in data: yield [d] except yaml.YAMLError as e: - raise ROSTopicException("invalid YAML in file: %s"%(str(e))) + raise ROSTopicException("invalid YAML in file: %s" % (str(e))) return bagy_iter - + + def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywords=False): publish_message(pub, msg_class, pub_args, rate, once, verbose=verbose, substitute_keywords=substitute_keywords) @@ -1841,17 +1911,21 @@ def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywo while not rospy.is_shutdown() and time.time() < timeout_t: rospy.sleep(0.2) + SUBSCRIBER_TIMEOUT = 5. + + def wait_for_subscriber(pub, timeout): timeout_t = time.time() + timeout while pub.get_num_connections() == 0 and timeout_t > time.time(): _sleep(0.01) + def param_publish_once(pub, msg_class, param_name, verbose): if not rospy.has_param(param_name): - raise ROSTopicException("parameter does not exist: %s"%(param_name)) + raise ROSTopicException("parameter does not exist: %s" % (param_name)) pub_args = rospy.get_param(param_name) - argv_publish(pub, msg_class, pub_args, None, True, verbose) + argv_publish(pub, msg_class, pub_args, None, True, verbose) class _ParamNotifier(object): @@ -1881,7 +1955,8 @@ def __call__(self, key, value): self.updates.append(self.value) self.lock.notify_all() - + + def param_publish(pub, msg_class, param_name, rate, verbose): """ :param param_name: ROS parameter name, ``str`` @@ -1891,9 +1966,9 @@ def param_publish(pub, msg_class, param_name, rate, verbose): import rospy import rospy.impl.paramserver import rosgraph - + if not rospy.has_param(param_name): - raise ROSTopicException("parameter does not exist: %s"%(param_name)) + raise ROSTopicException("parameter does not exist: %s" % (param_name)) # reach deep into subscription APIs here. Very unstable stuff # here, don't copy elsewhere! @@ -1907,7 +1982,7 @@ def param_publish(pub, msg_class, param_name, rate, verbose): if type(pub_args) == dict: pub_args = [pub_args] elif type(pub_args) != list: - raise ROSTopicException("Parameter [%s] in not a valid type"%(param_name)) + raise ROSTopicException("Parameter [%s] in not a valid type" % (param_name)) r = rospy.Rate(rate) if rate is not None else None publish = True @@ -1916,7 +1991,7 @@ def param_publish(pub, msg_class, param_name, rate, verbose): if publish: publish_message(pub, msg_class, pub_args, None, True, verbose=verbose) except ValueError as e: - sys.stderr.write("%s\n"%str(e)) + sys.stderr.write("%s\n" % str(e)) break if r is not None: r.sleep() @@ -1935,11 +2010,12 @@ def param_publish(pub, msg_class, param_name, rate, verbose): pub_args = notifier.updates.pop(0) if type(pub_args) == dict: pub_args = [pub_args] - + if rospy.is_shutdown(): break -def stdin_publish(pub, msg_class, rate, once, filename, verbose): + +def stdin_publish(pub, msg_class, rate, once, filename, verbose, substitute_keywords): """ :param filename: name of file to read from instead of stdin, or ``None``, ``str`` """ @@ -1974,7 +2050,8 @@ def stdin_publish(pub, msg_class, rate, once, filename, verbose): # None, repeatedly publish it if exactly_one_message and rate is not None: print("Got one message and a rate, publishing repeatedly") - publish_message(pub, msg_class, pub_args, rate=rate, once=once, verbose=verbose) + publish_message(pub, msg_class, pub_args, rate=rate, once=once, + verbose=verbose, substitute_keywords=substitute_keywords) # we use 'bool(r) or once' for the once value, which # controls whether or not publish_message blocks and # latches until exit. We want to block if the user @@ -1983,15 +2060,17 @@ def stdin_publish(pub, msg_class, rate, once, filename, verbose): # but, for now, this is the best re-use of the # underlying methods. else: - publish_message(pub, msg_class, pub_args, None, bool(r) or once, verbose=verbose) + publish_message(pub, msg_class, pub_args, None, bool(r) or once, + verbose=verbose, substitute_keywords=substitute_keywords) except ValueError as e: - sys.stderr.write("%s\n"%str(e)) + sys.stderr.write("%s\n" % str(e)) break if r is not None: r.sleep() if rospy.is_shutdown() or once: break + def stdin_yaml_arg(): """ Iterate over YAML documents in stdin @@ -2011,27 +2090,28 @@ def stdin_yaml_arg(): if not val: continue # sys.stdin.readline() returns empty string on EOF - arg = sys.stdin.readline() + arg = sys.stdin.readline() if arg != '' and arg.strip() != '---': buff = buff + arg - if arg.strip() == '---': # End of document + if arg.strip() == '---': # End of document try: loaded = yaml.safe_load(buff.rstrip()) except Exception as e: - sys.stderr.write("Invalid YAML: %s\n"%str(e)) + sys.stderr.write("Invalid YAML: %s\n" % str(e)) if loaded is not None: yield loaded - elif arg == '': #EOF + elif arg == '': # EOF # we don't yield the remaining buffer in this case # because we don't want to publish partial inputs return - - arg = 'x' # reset + + arg = 'x' # reset except select_error: - return # most likely ctrl-c interrupt - + return # most likely ctrl-c interrupt + + def _rostopic_cmd_list(argv): """ Command-line parsing for 'rostopic list' command. @@ -2043,13 +2123,13 @@ def _rostopic_cmd_list(argv): dest="bag", default=None, help="list topics in .bag file", metavar="BAGFILE") parser.add_option("-v", "--verbose", - dest="verbose", default=False,action="store_true", + dest="verbose", default=False, action="store_true", help="list full details about each topic") parser.add_option("-p", - dest="publishers", default=False,action="store_true", + dest="publishers", default=False, action="store_true", help="list only publishers") parser.add_option("-s", - dest="subscribers", default=False,action="store_true", + dest="subscribers", default=False, action="store_true", help="list only subscribers") parser.add_option("--host", dest="hostname", default=False, action="store_true", help="group by host name") @@ -2062,7 +2142,7 @@ def _rostopic_cmd_list(argv): elif len(args) > 1: parser.error("you may only specify one input topic") if options.bag: - if options.subscribers: + if options.subscribers: parser.error("-s option is not valid with bags") elif options.publishers: parser.error("-p option is not valid with bags") @@ -2073,10 +2153,12 @@ def _rostopic_cmd_list(argv): if options.subscribers and options.publishers: parser.error("you may only specify one of -p, -s") - exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, publishers_only=options.publishers, group_by_host=options.hostname) or 0 + exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, + publishers_only=options.publishers, group_by_host=options.hostname) or 0 if exitval != 0: sys.exit(exitval) + def _rostopic_cmd_info(argv): """ Command-line parsing for 'rostopic info' command. @@ -2090,12 +2172,13 @@ def _rostopic_cmd_info(argv): parser.error("you must specify a topic name") elif len(args) > 1: parser.error("you may only specify one topic name") - + topic = rosgraph.names.script_resolve_name('rostopic', args[0]) exitval = _rostopic_info(topic) or 0 if exitval != 0: sys.exit(exitval) - + + def _fullusage(): print("""rostopic is a command-line tool for printing information about ROS Topics. @@ -2114,13 +2197,14 @@ def _fullusage(): """) sys.exit(getattr(os, 'EX_USAGE', 1)) + def rostopicmain(argv=None): import rosbag if argv is None: - argv=sys.argv + argv = sys.argv # filter out remapping arguments in case we are being invoked via roslaunch argv = rospy.myargv(argv) - + # process argv if len(argv) == 1: _fullusage() @@ -2150,13 +2234,14 @@ def rostopicmain(argv=None): sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") sys.exit(1) except rosbag.ROSBagException as e: - sys.stderr.write("ERROR: unable to use bag file: %s\n"%str(e)) + sys.stderr.write("ERROR: unable to use bag file: %s\n" % str(e)) sys.exit(1) except rosgraph.MasterException as e: # mainly for invalid master URI/rosgraph.masterapi - sys.stderr.write("ERROR: %s\n"%str(e)) + sys.stderr.write("ERROR: %s\n" % str(e)) sys.exit(1) except ROSTopicException as e: - sys.stderr.write("ERROR: %s\n"%str(e)) + sys.stderr.write("ERROR: %s\n" % str(e)) sys.exit(1) - except KeyboardInterrupt: pass + except KeyboardInterrupt: + pass From 679f93d2c0456d9fb04d3640eedfd614561c84f5 Mon Sep 17 00:00:00 2001 From: Prasanna Kannappan Date: Fri, 5 May 2023 11:13:52 -0400 Subject: [PATCH 2/3] Undo formatting changes --- tools/rostopic/src/rostopic/__init__.py | 507 ++++++++++-------------- 1 file changed, 212 insertions(+), 295 deletions(-) diff --git a/tools/rostopic/src/rostopic/__init__.py b/tools/rostopic/src/rostopic/__init__.py index 526cff5712..4b4a1d7a34 100644 --- a/tools/rostopic/src/rostopic/__init__.py +++ b/tools/rostopic/src/rostopic/__init__.py @@ -35,34 +35,34 @@ # make sure we aren't using floor division from __future__ import division, print_function -import rospy -import rosgraph -import roslib.message -import genpy -from operator import itemgetter -import yaml -import traceback -import time -import socket -import math -import sys -import os -import argparse -NAME = 'rostopic' +NAME='rostopic' +import argparse +import os +import sys +import math +import socket +import time +import traceback +import yaml try: from xmlrpc.client import Fault except ImportError: from xmlrpclib import Fault +from operator import itemgetter try: from urllib.parse import urlparse except ImportError: from urlparse import urlparse +import genpy -# TODO: lazy-import rospy or move rospy-dependent routines to separate location +import roslib.message +import rosgraph +#TODO: lazy-import rospy or move rospy-dependent routines to separate location +import rospy try: long @@ -75,15 +75,12 @@ class ROSTopicException(Exception): Base exception class of rostopic-related errors """ pass - - class ROSTopicIOException(ROSTopicException): """ rostopic errors related to network I/O failures """ pass - def _check_master(): """ Make sure that master is available @@ -93,23 +90,20 @@ def _check_master(): rosgraph.Master('/rostopic').getPid() except socket.error: raise ROSTopicIOException("Unable to communicate with master!") - - + def _master_get_topic_types(master): try: val = master.getTopicTypes() except Fault: - # TODO: remove, this is for 1.1 + #TODO: remove, this is for 1.1 sys.stderr.write("WARNING: rostopic is being used against an older version of ROS/roscore\n") val = master.getPublishedTopics('/') return val - class ROSTopicHz(object): """ ROSTopicHz receives messages for a topic and computes frequency stats """ - def __init__(self, window_size, filter_expr=None, use_wtime=False): import threading from collections import defaultdict @@ -124,7 +118,7 @@ def __init__(self, window_size, filter_expr=None, use_wtime=False): self._times = defaultdict(list) self.filter_expr = filter_expr self.use_wtime = use_wtime - + # can't have infinite window size due to memory restrictions if window_size < 0: window_size = 50000 @@ -181,7 +175,7 @@ def callback_hz(self, m, topic=None): return with self.lock: curr_rostime = rospy.get_rostime() if not self.use_wtime else \ - rospy.Time.from_sec(time.time()) + rospy.Time.from_sec(time.time()) # time reset if curr_rostime.is_zero(): @@ -189,9 +183,9 @@ def callback_hz(self, m, topic=None): print("time has reset, resetting counters") self.set_times([], topic=topic) return - + curr = curr_rostime.to_sec() if not self.use_wtime else \ - rospy.Time.from_sec(time.time()).to_sec() + rospy.Time.from_sec(time.time()).to_sec() if self.get_msg_t0(topic=topic) < 0 or self.get_msg_t0(topic=topic) > curr: self.set_msg_t0(curr, topic=topic) self.set_msg_tn(curr, topic=topic) @@ -200,7 +194,7 @@ def callback_hz(self, m, topic=None): self.get_times(topic=topic).append(curr - self.get_msg_tn(topic=topic)) self.set_msg_tn(curr, topic=topic) - # only keep statistics for the last 10000 messages so as not to run out of memory + #only keep statistics for the last 10000 messages so as not to run out of memory if len(self.get_times(topic=topic)) > self.window_size - 1: self.get_times(topic=topic).pop(0) @@ -217,22 +211,22 @@ def get_hz(self, topic=None): elif self.get_msg_tn(topic=topic) == self.get_last_printed_tn(topic=topic): return with self.lock: - # frequency - + #frequency + # kwc: In the past, the rate decayed when a publisher # dies. Now, we use the last received message to perform # the calculation. This change was made because we now # report a count and keep track of last_printed_tn. This # makes it easier for users to see when a publisher dies, # so the decay is no longer necessary. - + n = len(self.get_times(topic=topic)) #rate = (n - 1) / (rospy.get_time() - self.msg_t0) mean = sum(self.get_times(topic=topic)) / n rate = 1./mean if mean > 0. else 0 - # std dev - std_dev = math.sqrt(sum((x - mean)**2 for x in self.get_times(topic=topic)) / n) + #std dev + std_dev = math.sqrt(sum((x - mean)**2 for x in self.get_times(topic=topic)) /n) # min and max max_delta = max(self.get_times(topic=topic)) @@ -252,8 +246,7 @@ def print_hz(self, topics=(None,)): print("no new messages") return rate, min_delta, max_delta, std_dev, window = ret - print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s" % - (rate, min_delta, max_delta, std_dev, window)) + print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, window)) return # monitoring multiple topics' hz @@ -276,7 +269,6 @@ def print_hz(self, topics=(None,)): return print(_get_ascii_table(header, stats)) - def _get_ascii_table(header, cols): # compose table with left alignment header_aligned = [] @@ -290,16 +282,14 @@ def _get_ascii_table(header, cols): # sum of col and each 3 spaces width table_width = sum(col_widths) + 3 * (len(header) - 1) n_rows = len(cols[header[0]]) - body = '\n'.join(' '.join(cols[h][i] for h in header) for i in xrange(n_rows)) + body = '\n'.join(' '.join(cols[h][i] for h in header) for i in range(n_rows)) table = '{header}\n{hline}\n{body}\n'.format( header=' '.join(header_aligned), hline='=' * table_width, body=body) return table - def _sleep(duration): rospy.rostime.wallsleep(duration) - def _rostopic_hz(topics, window_size=-1, filter_expr=None, use_wtime=False, tcp_nodelay=False): """ Periodically print the publishing rate of a topic to console until @@ -315,7 +305,7 @@ def _rostopic_hz(topics, window_size=-1, filter_expr=None, use_wtime=False, tcp_ rospy.init_node(NAME, anonymous=True) rt = ROSTopicHz(window_size, filter_expr=filter_expr, use_wtime=use_wtime) for topic in topics: - msg_class, real_topic, _ = get_topic_class(topic, blocking=True) # pause hz until topic is published + msg_class, real_topic, _ = get_topic_class(topic, blocking=True) # pause hz until topic is published # we use a large buffer size as we don't know what sort of messages we're dealing with. # may parameterize this in the future if filter_expr is not None: @@ -326,13 +316,12 @@ def _rostopic_hz(topics, window_size=-1, filter_expr=None, use_wtime=False, tcp_ print("subscribed to [%s]" % real_topic) if rospy.get_param('use_sim_time', False): - print("WARNING: may be using simulated time", file=sys.stderr) + print("WARNING: may be using simulated time",file=sys.stderr) while not rospy.is_shutdown(): _sleep(1.0) rt.print_hz(topics) - class ROSTopicDelay(object): def __init__(self, window_size): @@ -405,8 +394,7 @@ def print_delay(self): print("no new messages") return delay, min_delta, max_delta, std_dev, window = ret - print("average delay: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s" % - (delay, min_delta, max_delta, std_dev, window)) + print("average delay: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(delay, min_delta, max_delta, std_dev, window)) def _rostopic_delay(topic, window_size=-1, tcp_nodelay=False): @@ -427,7 +415,7 @@ def _rostopic_delay(topic, window_size=-1, tcp_nodelay=False): print("subscribed to [%s]" % real_topic) if rospy.get_param('use_sim_time', False): - print("WARNING: may be using simulated time", file=sys.stderr) + print("WARNING: may be using simulated time",file=sys.stderr) while not rospy.is_shutdown(): _sleep(1.0) @@ -439,18 +427,18 @@ def __init__(self, window_size=100): import threading self.lock = threading.Lock() self.last_printed_tn = 0 - self.sizes = [] - self.times = [] + self.sizes =[] + self.times =[] self.window_size = window_size or 100 - + def callback(self, data): """ros sub callback""" with self.lock: try: t = time.time() self.times.append(t) - self.sizes.append(len(data._buff)) # AnyMsg instance - assert (len(self.times) == len(self.sizes)) + self.sizes.append(len(data._buff)) #AnyMsg instance + assert(len(self.times) == len(self.sizes)) if len(self.times) > self.window_size: self.times.pop(0) @@ -466,7 +454,7 @@ def print_bw(self): n = len(self.times) tn = time.time() t0 = self.times[0] - + total = sum(self.sizes) bytes_per_s = total / (tn - t0) mean = total / n @@ -477,16 +465,15 @@ def print_bw(self): max_s = max(self.sizes) min_s = min(self.sizes) - # min/max and even mean are likely to be much smaller, but for now I prefer unit consistency + #min/max and even mean are likely to be much smaller, but for now I prefer unit consistency if bytes_per_s < 1000: - bw, mean, min_s, max_s = ["%.2fB" % v for v in [bytes_per_s, mean, min_s, max_s]] + bw, mean, min_s, max_s = ["%.2fB"%v for v in [bytes_per_s, mean, min_s, max_s]] elif bytes_per_s < 1000000: - bw, mean, min_s, max_s = ["%.2fKB" % (v/1000) for v in [bytes_per_s, mean, min_s, max_s]] + bw, mean, min_s, max_s = ["%.2fKB"%(v/1000) for v in [bytes_per_s, mean, min_s, max_s]] else: - bw, mean, min_s, max_s = ["%.2fMB" % (v/1000000) for v in [bytes_per_s, mean, min_s, max_s]] - - print("average: %s/s\n\tmean: %s min: %s max: %s window: %s" % (bw, mean, min_s, max_s, n)) - + bw, mean, min_s, max_s = ["%.2fMB"%(v/1000000) for v in [bytes_per_s, mean, min_s, max_s]] + + print("average: %s/s\n\tmean: %s min: %s max: %s window: %s"%(bw, mean, min_s, max_s, n)) def _isstring_type(t): valid_types = [str] @@ -496,14 +483,13 @@ def _isstring_type(t): pass return t in valid_types - def _rostopic_bw(topic, window_size=-1): """ periodically print the received bandwidth of a topic to console until shutdown """ _check_master() - _, real_topic, _ = get_topic_type(topic, blocking=True) # pause hz until topic is published + _, real_topic, _ = get_topic_type(topic, blocking=True) #pause hz until topic is published if rospy.is_shutdown(): return # #3543 disable all auto-subscriptions to /clock @@ -512,14 +498,12 @@ def _rostopic_bw(topic, window_size=-1): # we use a large buffer size as we don't know what sort of messages we're dealing with. # may parameterize this in the future sub = rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback) - print("subscribed to [%s]" % real_topic) + print("subscribed to [%s]"%real_topic) while not rospy.is_shutdown(): _sleep(1.0) rt.print_bw() # code adapted from rqt_plot - - def msgevalgen(pattern): """ Generates a function that returns the relevant field(s) (aka 'subtopic(s)') of a Message object @@ -547,13 +531,13 @@ def msgevalgen(pattern): def msgeval(msg, evals): for i, (field_name, slice_object) in enumerate(evals): - try: # access field first + try: # access field first msg = getattr(msg, field_name) except AttributeError: print("no field named %s in %s" % (field_name, pattern), file=sys.stderr) return None - if slice_object is not None: # access slice + if slice_object is not None: # access slice try: msg = msg.__getitem__(slice_object) except IndexError as e: @@ -593,22 +577,20 @@ def _get_array_index_or_slice_object(index_string): except ValueError: assert False, "non-integer slice stop '%s'" % index_string_parts[1] if len(index_string_parts) > 2 and index_string_parts[2] != '': - try: - slice_args[2] = int(index_string_parts[2]) - except ValueError: - assert False, "non-integer slice step '%s'" % index_string_parts[2] + try: + slice_args[2] = int(index_string_parts[2]) + except ValueError: + assert False, "non-integer slice step '%s'" % index_string_parts[2] if len(index_string_parts) > 3: assert False, 'too many slice arguments' return slice(*slice_args) - def _get_nested_attribute(msg, nested_attributes): value = msg for attr in nested_attributes.split('/'): value = getattr(value, attr) return value - def _get_topic_type(topic): """ subroutine for getting the topic type @@ -657,15 +639,14 @@ def _get_topic_type(topic): return None, None, None # NOTE: this is used externally by rxplot - - + def get_topic_type(topic, blocking=False): """ Get the topic type. :param topic: topic name, ``str`` :param blocking: (default False) block until topic becomes available, ``bool`` - + :returns: topic type, real topic name and fn to evaluate the message instance if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)`` :raises: :exc:`ROSTopicException` If master cannot be contacted @@ -674,7 +655,7 @@ def get_topic_type(topic, blocking=False): if topic_type: return topic_type, real_topic, msg_eval elif blocking: - sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n" % topic) + sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic) while not rospy.is_shutdown(): topic_type, real_topic, msg_eval = _get_topic_type(topic) if topic_type: @@ -683,7 +664,6 @@ def get_topic_type(topic, blocking=False): _sleep(0.1) return None, None, None - def get_topic_class(topic, blocking=False): """ Get the topic message class @@ -700,7 +680,6 @@ def get_topic_class(topic, blocking=False): raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % topic_type) return msg_class, real_topic, msg_eval - def _str_plot_fields(val, f, field_filter): """ get CSV representation of fields used by _str_plot @@ -712,13 +691,12 @@ def _str_plot_fields(val, f, field_filter): else: return 'time,' - def _sub_str_plot_fields(val, f, field_filter): """recursive helper function for _str_plot_fields""" # CSV type_ = type(val) if type_ in (bool, int, long, float) or \ - isinstance(val, genpy.TVal): + isinstance(val, genpy.TVal): return f # duck-type check for messages elif hasattr(val, "_slot_types"): @@ -726,8 +704,7 @@ def _sub_str_plot_fields(val, f, field_filter): fields = list(field_filter(val)) else: fields = val.__slots__ - sub = (_sub_str_plot_fields(_convert_getattr(val, a, t), f+"."+a, field_filter) - for a, t in zip(val.__slots__, val._slot_types) if a in fields) + sub = (_sub_str_plot_fields(_convert_getattr(val, a, t), f+"."+a, field_filter) for a,t in zip(val.__slots__, val._slot_types) if a in fields) sub = [s for s in sub if s is not None] if sub: return ','.join([s for s in sub]) @@ -740,14 +717,14 @@ def _sub_str_plot_fields(val, f, field_filter): type0 = type(val0) # no arrays of arrays if type0 in (bool, int, long, float) or \ - isinstance(val0, genpy.TVal): - return ','.join(["%s%s" % (f, x) for x in range(0, len(val))]) + isinstance(val0, genpy.TVal): + return ','.join(["%s%s"%(f,x) for x in range(0,len(val))]) elif _isstring_type(type0): - - return ','.join(["%s%s" % (f, x) for x in range(0, len(val))]) + + return ','.join(["%s%s"%(f,x) for x in range(0,len(val))]) elif hasattr(val0, "_slot_types"): - labels = ["%s%s" % (f, x) for x in range(0, len(val))] - sub = [s for s in [_sub_str_plot_fields(v, sf, field_filter) for v, sf in zip(val, labels)] if s] + labels = ["%s%s"%(f,x) for x in range(0,len(val))] + sub = [s for s in [_sub_str_plot_fields(v, sf, field_filter) for v,sf in zip(val, labels)] if s] if sub: return ','.join([s for s in sub]) return None @@ -764,7 +741,7 @@ def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_ :param value_transform: Not used but for same API as CallbackEcho.custom_strify_message :returns: comma-separated list of field values in val, ``str`` """ - + s = _sub_str_plot(val, time_offset, field_filter) if s is None: s = '' @@ -772,37 +749,35 @@ def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_ if time_offset is not None: time_offset = time_offset.to_nsec() else: - time_offset = 0 - + time_offset = 0 + if current_time is not None: - return "%s,%s" % (current_time.to_nsec()-time_offset, s) + return "%s,%s"%(current_time.to_nsec()-time_offset, s) elif getattr(val, "_has_header", False): - return "%s,%s" % (val.header.stamp.to_nsec()-time_offset, s) + return "%s,%s"%(val.header.stamp.to_nsec()-time_offset, s) else: - return "%s,%s" % (rospy.get_rostime().to_nsec()-time_offset, s) - - + return "%s,%s"%(rospy.get_rostime().to_nsec()-time_offset, s) + def _sub_str_plot(val, time_offset, field_filter): """Helper routine for _str_plot.""" # CSV type_ = type(val) - + if type_ == bool: return '1' if val else '0' elif type_ in (int, long, float) or \ - isinstance(val, genpy.TVal): + isinstance(val, genpy.TVal): if time_offset is not None and isinstance(val, genpy.Time): return str(val-time_offset) else: - return str(val) + return str(val) elif hasattr(val, "_slot_types"): if field_filter is not None: fields = list(field_filter(val)) else: - fields = val.__slots__ + fields = val.__slots__ - sub = (_sub_str_plot(_convert_getattr(val, f, t), time_offset, field_filter) - for f, t in zip(val.__slots__, val._slot_types) if f in fields) + sub = (_sub_str_plot(_convert_getattr(val, f, t), time_offset, field_filter) for f,t in zip(val.__slots__, val._slot_types) if f in fields) sub = [s for s in sub if s is not None] if sub: return ','.join(sub) @@ -817,19 +792,17 @@ def _sub_str_plot(val, time_offset, field_filter): if type0 == bool: return ','.join([('1' if v else '0') for v in val]) elif type0 in (int, long, float) or \ - isinstance(val0, genpy.TVal): + isinstance(val0, genpy.TVal): return ','.join([str(v) for v in val]) elif _isstring_type(type0): - return ','.join([v for v in val]) + return ','.join([v for v in val]) elif hasattr(val0, "_slot_types"): sub = [s for s in [_sub_str_plot(v, time_offset, field_filter) for v in val] if s is not None] if sub: return ','.join([s for s in sub]) return None - + # copied from roslib.message - - def _convert_getattr(val, f, t): """ Convert atttribute types on the fly, if necessary. This is mainly @@ -841,7 +814,6 @@ def _convert_getattr(val, f, t): else: return attr - class CallbackEcho(object): """ Callback instance that can print callback data in a variety of @@ -872,8 +844,8 @@ def __init__(self, topic, msg_eval, plot=False, filter_fn=None, self.fixed_numeric_width = fixed_numeric_width self.prefix = '' - self.suffix = '\n---' if not plot else '' # same as YAML document separator, bug #3291 - + self.suffix = '\n---' if not plot else ''# same as YAML document separator, bug #3291 + self.echo_all_topics = echo_all_topics self.offset_time = offset_time @@ -884,18 +856,18 @@ def __init__(self, topic, msg_eval, plot=False, filter_fn=None, # determine which strifying function to use if plot: - # TODOXXX: need to pass in filter function + #TODOXXX: need to pass in filter function self.str_fn = _str_plot self.sep = '' else: - # TODOXXX: need to pass in filter function + #TODOXXX: need to pass in filter function self.str_fn = self.custom_strify_message if echo_clear: self.prefix = '\033[2J\033[;H' - self.field_filter = field_filter_fn - self.value_transform = value_transform_fn - + self.field_filter=field_filter_fn + self.value_transform=value_transform_fn + # first tracks whether or not we've printed anything yet. Need this for printing plot fields. self.first = True @@ -929,7 +901,7 @@ def callback(self, data, callback_args, current_time=None): if self.max_count is not None and self.count >= self.max_count: self.done = True return - + try: msg_eval = self.msg_eval if topic == self.topic: @@ -948,31 +920,31 @@ def callback(self, data, callback_args, current_time=None): if msg_eval is not None: data = msg_eval(data) - + # data can be None if msg_eval returns None if data is not None: # NOTE: we do all prints using direct writes to sys.stdout, which works better with piping - + self.count += 1 - + # print fields header for plot if self.plot and self.first: sys.stdout.write("%"+_str_plot_fields(data, 'field', self.field_filter)+'\n') self.first = False if self.offset_time: - sys.stdout.write(self.prefix + + sys.stdout.write(self.prefix+\ self.str_fn(data, time_offset=rospy.get_rostime(), current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width, - value_transform=self.value_transform) + + value_transform=self.value_transform) + \ self.suffix + '\n') else: - sys.stdout.write(self.prefix + + sys.stdout.write(self.prefix+\ self.str_fn(data, current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width, - value_transform=self.value_transform) + + value_transform=self.value_transform) + \ self.suffix + '\n') # we have to flush in order before piping to work @@ -987,8 +959,7 @@ def callback(self, data, callback_args, current_time=None): # set done flag so we exit self.done = True traceback.print_exc() - - + def _rostopic_type(topic): """ Print ROS message type of topic to screen @@ -996,7 +967,7 @@ def _rostopic_type(topic): """ topic_type, topic_real_name, _ = get_topic_type(topic, blocking=False) if topic_type is None: - sys.stderr.write('unknown topic type [%s]\n' % topic) + sys.stderr.write('unknown topic type [%s]\n'%topic) sys.exit(1) elif topic == topic_real_name: print(topic_type) @@ -1006,8 +977,7 @@ def _rostopic_type(topic): for current_field in field.split('/'): msg_class = roslib.message.get_message_class(field_type) field_type = msg_class._slot_types[msg_class.__slots__.index(current_field)] - print('%s %s %s' % (topic_type, field, field_type)) - + print('%s %s %s'%(topic_type, field, field_type)) def _rostopic_echo_bag(callback_echo, bag_file): """ @@ -1015,13 +985,13 @@ def _rostopic_echo_bag(callback_echo, bag_file): :param bag_file: name of bag file to echo messages from or ``None``, ``str`` """ if not os.path.exists(bag_file): - raise ROSTopicException("bag file [%s] does not exist" % bag_file) + raise ROSTopicException("bag file [%s] does not exist"%bag_file) first = True - + import rosbag with rosbag.Bag(bag_file) as b: for t, msg, timestamp in b.read_messages(): - # bag files can have relative paths in them, this respects any + # bag files can have relative paths in them, this respects any # dynamic renaming if t[0] != '/': t = rosgraph.names.script_resolve_name('rostopic', t) @@ -1030,11 +1000,10 @@ def _rostopic_echo_bag(callback_echo, bag_file): if callback_echo.done: break - def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): """ Print new messages on topic to screen. - + :param topic: topic name, ``str`` :param bag_file: name of bag file to echo messages from or ``None``, ``str`` """ @@ -1042,7 +1011,7 @@ def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): if bag_file: # initialize rospy time due to potential timestamp printing - rospy.rostime.set_rostime_initialized(True) + rospy.rostime.set_rostime_initialized(True) _rostopic_echo_bag(callback_echo, bag_file) else: _check_master() @@ -1069,12 +1038,10 @@ def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): if fields: submsg_class = roslib.message.get_message_class(type_information.split('[', 1)[0]) if not submsg_class: - raise ROSTopicException( - "Cannot load message class for [%s]. Are your messages built?" % type_information) + raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % type_information) use_sim_time = rospy.get_param('/use_sim_time', False) - sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, { - 'topic': topic, 'type_information': type_information}) + sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, {'topic': topic, 'type_information': type_information}) if use_sim_time: # #2950: print warning if nothing received for two seconds @@ -1089,16 +1056,12 @@ def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False): if callback_echo.count == 0 and \ not rospy.is_shutdown() and \ not callback_echo.done: - sys.stderr.write( - "WARNING: no messages received and simulated time is active.\nIs /clock being published?\n") + sys.stderr.write("WARNING: no messages received and simulated time is active.\nIs /clock being published?\n") while not rospy.is_shutdown() and not callback_echo.done: _sleep(0.1) - _caller_apis = {} - - def get_api(master, caller_id): """ Get XML-RPC API of node @@ -1115,11 +1078,10 @@ def get_api(master, caller_id): except socket.error: raise ROSTopicIOException("Unable to communicate with master!") except rosgraph.MasterError: - caller_api = 'unknown address %s' % caller_id + caller_api = 'unknown address %s'%caller_id return caller_api - def _rostopic_list_bag(bag_file, topic=None): """ Prints topics in bag file to screen @@ -1128,7 +1090,7 @@ def _rostopic_list_bag(bag_file, topic=None): """ import rosbag if not os.path.exists(bag_file): - raise ROSTopicException("bag file [%s] does not exist" % bag_file) + raise ROSTopicException("bag file [%s] does not exist"%bag_file) with rosbag.Bag(bag_file) as b: if topic: @@ -1147,9 +1109,8 @@ def _rostopic_list_bag(bag_file, topic=None): if rospy.is_shutdown(): break import time - earliest, latest = [time.strftime("%d %b %Y %H:%M:%S", time.localtime(t.to_time())) - for t in (earliest, latest)] - print("%s message(s) from %s to %s" % (count, earliest, latest)) + earliest, latest = [time.strftime("%d %b %Y %H:%M:%S", time.localtime(t.to_time())) for t in (earliest, latest)] + print("%s message(s) from %s to %s"%(count, earliest, latest)) else: topics = set() for top, msg, _ in b.read_messages(raw=True): @@ -1159,27 +1120,26 @@ def _rostopic_list_bag(bag_file, topic=None): if rospy.is_shutdown(): break - def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, verbose, indent=''): if verbose: topic_types = _master_get_topic_types(master) if not subscribers_only: - print("\n%sPublished topics:" % indent) + print("\n%sPublished topics:"%indent) for t, ttype, tlist in pubs: if len(tlist) > 1: - print(indent+" * %s [%s] %s publishers" % (t, ttype, len(tlist))) + print(indent+" * %s [%s] %s publishers"%(t, ttype, len(tlist))) else: - print(indent+" * %s [%s] 1 publisher" % (t, ttype)) + print(indent+" * %s [%s] 1 publisher"%(t, ttype)) if not publishers_only: print(indent) print(indent+"Subscribed topics:") for t, ttype, tlist in subs: if len(tlist) > 1: - print(indent+" * %s [%s] %s subscribers" % (t, ttype, len(tlist))) + print(indent+" * %s [%s] %s subscribers"%(t, ttype, len(tlist))) else: - print(indent+" * %s [%s] 1 subscriber" % (t, ttype)) + print(indent+" * %s [%s] 1 subscriber"%(t, ttype)) print('') else: if publishers_only: @@ -1187,15 +1147,13 @@ def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, ve elif subscribers_only: topics = [t for t, _, _ in subs] else: - topics = list(set([t for t, _, _ in pubs] + [t for t, _, _ in subs])) + topics = list(set([t for t, _, _ in pubs] + [t for t, _, _ in subs])) topics.sort() - print('\n'.join(["%s%s" % (indent, t) for t in topics])) - + print('\n'.join(["%s%s"%(indent, t) for t in topics])) def get_topic_list(master=None): if not master: master = rosgraph.Master('/rostopic') - def topic_type(t, topic_types): matches = [t_type for t_name, t_type in topic_types if t_name == t] if matches: @@ -1205,7 +1163,7 @@ def topic_type(t, topic_types): # Return an array of tuples; (, , ) state = master.getSystemState() topic_types = _master_get_topic_types(master) - + pubs, subs, _ = state pubs_out = [] for topic, nodes in pubs: @@ -1218,8 +1176,6 @@ def topic_type(t, topic_types): return (pubs_out, subs_out) # #3145 - - def _rostopic_list_group_by_host(master, pubs, subs): """ Build up maps for hostname to topic list per hostname @@ -1230,7 +1186,7 @@ def build_map(master, state, uricache): for topic, ttype, tnodes in state: for p in tnodes: if not p in uricache: - uricache[p] = master.lookupNode(p) + uricache[p] = master.lookupNode(p) uri = uricache[p] puri = urlparse(uri) if not puri.hostname in tmap: @@ -1242,19 +1198,18 @@ def build_map(master, state, uricache): else: tmap[puri.hostname].append((topic, ttype, [p])) return tmap - + uricache = {} host_pub_topics = build_map(master, pubs, uricache) host_sub_topics = build_map(master, subs, uricache) return host_pub_topics, host_sub_topics - def _rostopic_list(topic, verbose=False, subscribers_only=False, publishers_only=False, group_by_host=False): """ Print topics to screen - + :param topic: topic name to list information or None to match all topics, ``str`` :param verbose: print additional debugging information, ``bool`` :param subscribers_only: print information about subscriptions only, ``bool`` @@ -1264,7 +1219,7 @@ def _rostopic_list(topic, verbose=False, # #1563 if subscribers_only and publishers_only: raise ROSTopicException("cannot specify both subscribers- and publishers-only") - + master = rosgraph.Master('/rostopic') try: pubs, subs = get_topic_list(master=master) @@ -1277,9 +1232,9 @@ def _rostopic_list(topic, verbose=False, if group_by_host: # #3145 - host_pub_topics, host_sub_topics = _rostopic_list_group_by_host(master, pubs, subs) - for hostname in set(list(host_pub_topics.keys()) + list(host_sub_topics.keys())): # py3k - pubs, subs = host_pub_topics.get(hostname, []), host_sub_topics.get(hostname, []), + host_pub_topics, host_sub_topics = _rostopic_list_group_by_host(master, pubs, subs) + for hostname in set(list(host_pub_topics.keys()) + list(host_sub_topics.keys())): #py3k + pubs, subs = host_pub_topics.get(hostname,[]), host_sub_topics.get(hostname, []), if (pubs and not subscribers_only) or (subs and not publishers_only): print("Host [%s]:" % hostname) _sub_rostopic_list(master, pubs, subs, @@ -1290,11 +1245,10 @@ def _rostopic_list(topic, verbose=False, publishers_only, subscribers_only, verbose) - def get_info_text(topic): """ Get human-readable topic description - + :param topic: topic name, ``str`` """ try: @@ -1303,7 +1257,6 @@ def get_info_text(topic): from io import StringIO import itertools buff = StringIO() - def topic_type(t, topic_types): matches = [t_type for t_name, t_type in topic_types if t_name == t] if matches: @@ -1318,19 +1271,19 @@ def topic_type(t, topic_types): pubs = [x for x in pubs if x[0] == topic] topic_types = _master_get_topic_types(master) - + except socket.error: raise ROSTopicIOException("Unable to communicate with master!") if not pubs and not subs: - raise ROSTopicException("Unknown topic %s" % topic) + raise ROSTopicException("Unknown topic %s"%topic) - buff.write("Type: %s\n\n" % topic_type(topic, topic_types)) + buff.write("Type: %s\n\n"%topic_type(topic, topic_types)) if pubs: buff.write("Publishers: \n") for p in itertools.chain(*[nodes for topic, ttype, nodes in pubs]): - buff.write(" * %s (%s)\n" % (p, get_api(master, p))) + buff.write(" * %s (%s)\n"%(p, get_api(master, p))) else: buff.write("Publishers: None\n") buff.write('\n') @@ -1338,25 +1291,23 @@ def topic_type(t, topic_types): if subs: buff.write("Subscribers: \n") for p in itertools.chain(*[nodes for topic, ttype, nodes in subs]): - buff.write(" * %s (%s)\n" % (p, get_api(master, p))) + buff.write(" * %s (%s)\n"%(p, get_api(master, p))) else: buff.write("Subscribers: None\n") buff.write('\n') return buff.getvalue() - - + def _rostopic_info(topic): """ Print topic information to screen. - + :param topic: topic name, ``str`` """ print(get_info_text(topic)) - + ########################################################################################## # COMMAND PROCESSING ##################################################################### - - + def _rostopic_cmd_echo(argv): def expr_eval(expr): def eval_fn(m): @@ -1369,18 +1320,18 @@ def eval_fn(m): parser.add_option("-b", "--bag", dest="bag", default=None, help="echo messages from .bag file", metavar="BAGFILE") - parser.add_option("-p", + parser.add_option("-p", dest="plot", default=False, action="store_true", help="echo in a plotting friendly format") parser.add_option("-w", dest="fixed_numeric_width", default=None, metavar="NUM_WIDTH", help="fixed width for numeric values") - parser.add_option("--filter", + parser.add_option("--filter", dest="filter_expr", default=None, metavar="FILTER-EXPRESSION", help="Python expression to filter messages that are printed. Expression can use Python builtins as well as m (the message) and topic (the topic name).") - parser.add_option("--nostr", + parser.add_option("--nostr", dest="nostr", default=False, action="store_true", help="exclude string fields") @@ -1396,7 +1347,7 @@ def eval_fn(m): dest="all_topics", default=False, action="store_true", help="display all message in bag, only valid with -b option") - parser.add_option("-n", + parser.add_option("-n", dest="msg_count", default=None, metavar="COUNT", help="number of messages to echo") parser.add_option("--offset", @@ -1415,12 +1366,12 @@ def eval_fn(m): topic = '' else: if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") topic = rosgraph.names.script_resolve_name('rostopic', args[0]) # suppressing output to keep it clean - # if not options.plot: + #if not options.plot: # print "rostopic: topic is [%s]"%topic - + filter_fn = None if options.filter_expr: filter_fn = expr_eval(options.filter_expr) @@ -1456,7 +1407,6 @@ def eval_fn(m): except socket.error: sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") - def create_value_transform(echo_nostr, echo_noarr): def value_transform(val, type_information=None): def transform_field_value(value, value_type, echo_nostr, echo_noarr): @@ -1505,7 +1455,6 @@ class TransformedMessage(genpy.Message): return val_trans return value_transform - def create_field_filter(echo_nostr, echo_noarr): def field_filter(val): fields = val.__slots__ @@ -1518,26 +1467,23 @@ def field_filter(val): yield f return field_filter - def _optparse_topic_only(cmd, argv): args = argv[2:] from optparse import OptionParser - parser = OptionParser(usage="usage: %%prog %s /topic" % cmd, prog=NAME) + parser = OptionParser(usage="usage: %%prog %s /topic"%cmd, prog=NAME) (options, args) = parser.parse_args(args) if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") if len(args) > 1: parser.error("you may only specify one input topic") return rosgraph.names.script_resolve_name('rostopic', args[0]) - def _rostopic_cmd_type(argv): parser = argparse.ArgumentParser(prog='%s type' % NAME) parser.add_argument('topic_or_field', help='Topic or field name') args = parser.parse_args(argv[2:]) _rostopic_type(rosgraph.names.script_resolve_name('rostopic', args.topic_or_field)) - def _rostopic_cmd_hz(argv): args = argv[2:] from optparse import OptionParser @@ -1557,7 +1503,7 @@ def _rostopic_cmd_hz(argv): (options, args) = parser.parse_args(args) if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") try: window_size = int(options.window_size) except: @@ -1606,7 +1552,7 @@ def _rostopic_cmd_bw(argv=sys.argv): help="window size, in # of messages, for calculating rate", metavar="WINDOW") options, args = parser.parse_args(args) if len(args) == 0: - parser.error("topic must be specified") + parser.error("topic must be specified") if len(args) > 1: parser.error("you may only specify one input topic") try: @@ -1616,7 +1562,6 @@ def _rostopic_cmd_bw(argv=sys.argv): topic = rosgraph.names.script_resolve_name('rostopic', args[0]) _rostopic_bw(topic, window_size=window_size) - def find_by_type(topic_type): """ Lookup topics by topic_type @@ -1629,8 +1574,7 @@ def find_by_type(topic_type): except socket.error: raise ROSTopicIOException("Unable to communicate with master!") return [t_name for t_name, t_type in t_list if t_type == topic_type] - - + def _rostopic_cmd_find(argv=sys.argv): """ Implements 'rostopic type' @@ -1645,20 +1589,19 @@ def _rostopic_cmd_find(argv=sys.argv): if len(args) > 1: parser.error("you may only specify one message type") print('\n'.join(find_by_type(args[0]))) - + def _resource_name_package(name): """ pkg/typeName -> pkg, typeName -> None - + :param name: package resource name, e.g. 'std_msgs/String', ``str`` :returns: package name of resource, ``str`` - """ + """ if not '/' in name: return None return name[:name.find('/')] - def create_publisher(topic_name, topic_type, latch, disable_rostime=True): """ Create rospy.Publisher instance from the string topic name and @@ -1676,21 +1619,19 @@ def create_publisher(topic_name, topic_type, latch, disable_rostime=True): try: msg_class = roslib.message.get_message_class(topic_type) except: - raise ROSTopicException("invalid topic type: %s" % topic_type) + raise ROSTopicException("invalid topic type: %s"%topic_type) if msg_class is None: pkg = _resource_name_package(topic_type) - raise ROSTopicException( - "invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'" % (topic_type, pkg)) + raise ROSTopicException("invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'"%(topic_type, pkg)) # disable /rosout and /rostime as this causes blips in the pubsub network due to rostopic pub often exiting quickly rospy.init_node('rostopic', anonymous=True, disable_rosout=True, disable_rostime=disable_rostime) pub = rospy.Publisher(topic_name, msg_class, latch=latch, queue_size=100) return pub, msg_class - def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, pub_args=None): """ Publish message at specified rate. Subroutine of L{publish_message()}. - + :param pub: :class:rospy.Publisher` instance for topic :param msg: message instance to publish :param rate: publishing rate (hz) or None for just once, ``int`` @@ -1704,18 +1645,15 @@ def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, p if substitute_keywords: _fillMessageArgs(msg, pub_args) if verbose: - print("publishing %s" % msg) + print("publishing %s"%msg) pub.publish(msg) r.sleep() - _ONCE_DELAY = 3. - - def _publish_latched(pub, msg, once=False, verbose=False): """ Publish and latch message. Subroutine of L{publish_message()}. - + :param pub: :class:`rospy.Publisher` instance for topic :param msg: message instance to publish :param once: if ``True``, publish message once and then exit after sleep interval, ``bool`` @@ -1727,14 +1665,13 @@ def _publish_latched(pub, msg, once=False, verbose=False): raise ROSTopicException(str(e)) if not once: - rospy.spin() - + rospy.spin() def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=False, substitute_keywords=False): """ Create new instance of msg_class, populate with pub_args, and publish. This may print output to screen. - + :param pub: :class:`rospy.Publisher` instance for topic :param msg_class: Message type, ``Class`` :param pub_args: Arguments to initialize message that is published, ``[val]`` @@ -1747,26 +1684,24 @@ def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=Fal _fillMessageArgs(msg, pub_args) try: - + if rate is None: - s = "publishing and latching [%s]" % (msg) if verbose else "publishing and latching message" + s = "publishing and latching [%s]"%(msg) if verbose else "publishing and latching message" if once: - s = s + " for %s seconds" % _ONCE_DELAY + s = s + " for %s seconds"%_ONCE_DELAY else: s = s + ". Press ctrl-C to terminate" print(s) _publish_latched(pub, msg, once, verbose) else: - _publish_at_rate(pub, msg, rate, verbose=verbose, - substitute_keywords=substitute_keywords, pub_args=pub_args) - + _publish_at_rate(pub, msg, rate, verbose=verbose, substitute_keywords=substitute_keywords, pub_args=pub_args) + except rospy.ROSSerializationException as e: import rosmsg # we could just print the message definition, but rosmsg is more readable - raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n" + - " %s\n\nmsg file:\n%s" % (e, rosmsg.get_msg_text(msg_class._type))) - + raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n"+\ + " %s\n\nmsg file:\n%s"%(e, rosmsg.get_msg_text(msg_class._type))) def _fillMessageArgs(msg, pub_args): try: @@ -1783,12 +1718,11 @@ def _fillMessageArgs(msg, pub_args): # allow the use of the 'now' string with timestamps and 'auto' with header now = rospy.get_rostime() import std_msgs.msg - keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)} + keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) } genpy.message.fill_message_args(msg, pub_args, keys=keys) except genpy.MessageException as e: - raise ROSTopicException(str(e)+"\n\nArgs are: [%s]" % genpy.message.get_printable_message_args(msg)) - - + raise ROSTopicException(str(e)+"\n\nArgs are: [%s]"%genpy.message.get_printable_message_args(msg)) + def _rostopic_cmd_pub(argv): """ Parse 'pub' command arguments and run command. Will cause a system @@ -1815,9 +1749,9 @@ def _rostopic_cmd_pub(argv): help="When publishing with a rate, performs keyword ('now' or 'auto') substitution for each message") parser.add_option('--use-rostime', dest="use_rostime", default=False, action="store_true", help="use rostime for time stamps, else walltime is used") - # parser.add_option("-p", '--param', dest="parameter", metavar='/PARAM', default=None, + #parser.add_option("-p", '--param', dest="parameter", metavar='/PARAM', default=None, # help="read args from ROS parameter (Bagy format)") - + (options, args) = parser.parse_args(args) if options.rate is not None: if options.once: @@ -1831,7 +1765,7 @@ def _rostopic_cmd_pub(argv): else: # we will default this to 10 for file/stdin later rate = None - + # validate args len if len(args) == 0: parser.error("/topic must be specified") @@ -1839,9 +1773,9 @@ def _rostopic_cmd_pub(argv): parser.error("topic type must be specified") if 0: if len(args) > 2 and options.parameter: - parser.error("args conflict with -p setting") + parser.error("args conflict with -p setting") if len(args) > 2 and options.file: - parser.error("args conflict with -f setting") + parser.error("args conflict with -f setting") topic_name, topic_type = args[0], args[1] # type-case using YAML @@ -1866,19 +1800,17 @@ def _rostopic_cmd_pub(argv): param_publish_once(pub, msg_class, param_name, rate, options.verbose) else: param_publish(pub, msg_class, param_name, rate, options.verbose) - + elif not pub_args and len(msg_class.__slots__): if not options.file and sys.stdin.isatty(): parser.error("Please specify message values") # stdin/file input has a rate by default if rate is None and not options.latch and not options.once: rate = 10. - stdin_publish(pub, msg_class, rate, options.once, options.file, - options.verbose, substitute_keywords=options.substitute_keywords) + stdin_publish(pub, msg_class, rate, options.once, options.file, options.verbose, substitute_keywords=options.substitute_keywords) else: - argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, - substitute_keywords=options.substitute_keywords) - + argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, substitute_keywords=options.substitute_keywords) + def file_yaml_arg(filename): """ @@ -1887,9 +1819,8 @@ def file_yaml_arg(filename): :raises: :exc:`ROSTopicException` If filename is invalid """ if not os.path.isfile(filename): - raise ROSTopicException("file does not exist: %s" % (filename)) + raise ROSTopicException("file does not exist: %s"%(filename)) import yaml - def bagy_iter(): try: with open(filename, 'r') as f: @@ -1898,10 +1829,9 @@ def bagy_iter(): for d in data: yield [d] except yaml.YAMLError as e: - raise ROSTopicException("invalid YAML in file: %s" % (str(e))) + raise ROSTopicException("invalid YAML in file: %s"%(str(e))) return bagy_iter - - + def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywords=False): publish_message(pub, msg_class, pub_args, rate, once, verbose=verbose, substitute_keywords=substitute_keywords) @@ -1911,21 +1841,17 @@ def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywo while not rospy.is_shutdown() and time.time() < timeout_t: rospy.sleep(0.2) - SUBSCRIBER_TIMEOUT = 5. - - def wait_for_subscriber(pub, timeout): timeout_t = time.time() + timeout while pub.get_num_connections() == 0 and timeout_t > time.time(): _sleep(0.01) - def param_publish_once(pub, msg_class, param_name, verbose): if not rospy.has_param(param_name): - raise ROSTopicException("parameter does not exist: %s" % (param_name)) + raise ROSTopicException("parameter does not exist: %s"%(param_name)) pub_args = rospy.get_param(param_name) - argv_publish(pub, msg_class, pub_args, None, True, verbose) + argv_publish(pub, msg_class, pub_args, None, True, verbose) class _ParamNotifier(object): @@ -1955,8 +1881,7 @@ def __call__(self, key, value): self.updates.append(self.value) self.lock.notify_all() - - + def param_publish(pub, msg_class, param_name, rate, verbose): """ :param param_name: ROS parameter name, ``str`` @@ -1966,9 +1891,9 @@ def param_publish(pub, msg_class, param_name, rate, verbose): import rospy import rospy.impl.paramserver import rosgraph - + if not rospy.has_param(param_name): - raise ROSTopicException("parameter does not exist: %s" % (param_name)) + raise ROSTopicException("parameter does not exist: %s"%(param_name)) # reach deep into subscription APIs here. Very unstable stuff # here, don't copy elsewhere! @@ -1982,7 +1907,7 @@ def param_publish(pub, msg_class, param_name, rate, verbose): if type(pub_args) == dict: pub_args = [pub_args] elif type(pub_args) != list: - raise ROSTopicException("Parameter [%s] in not a valid type" % (param_name)) + raise ROSTopicException("Parameter [%s] in not a valid type"%(param_name)) r = rospy.Rate(rate) if rate is not None else None publish = True @@ -1991,7 +1916,7 @@ def param_publish(pub, msg_class, param_name, rate, verbose): if publish: publish_message(pub, msg_class, pub_args, None, True, verbose=verbose) except ValueError as e: - sys.stderr.write("%s\n" % str(e)) + sys.stderr.write("%s\n"%str(e)) break if r is not None: r.sleep() @@ -2010,11 +1935,10 @@ def param_publish(pub, msg_class, param_name, rate, verbose): pub_args = notifier.updates.pop(0) if type(pub_args) == dict: pub_args = [pub_args] - + if rospy.is_shutdown(): break - def stdin_publish(pub, msg_class, rate, once, filename, verbose, substitute_keywords): """ :param filename: name of file to read from instead of stdin, or ``None``, ``str`` @@ -2050,8 +1974,8 @@ def stdin_publish(pub, msg_class, rate, once, filename, verbose, substitute_keyw # None, repeatedly publish it if exactly_one_message and rate is not None: print("Got one message and a rate, publishing repeatedly") - publish_message(pub, msg_class, pub_args, rate=rate, once=once, - verbose=verbose, substitute_keywords=substitute_keywords) + publish_message(pub, msg_class, pub_args, rate=rate, once=once, verbose=verbose, + substitute_keywords=substitute_keywords) # we use 'bool(r) or once' for the once value, which # controls whether or not publish_message blocks and # latches until exit. We want to block if the user @@ -2060,17 +1984,16 @@ def stdin_publish(pub, msg_class, rate, once, filename, verbose, substitute_keyw # but, for now, this is the best re-use of the # underlying methods. else: - publish_message(pub, msg_class, pub_args, None, bool(r) or once, - verbose=verbose, substitute_keywords=substitute_keywords) + publish_message(pub, msg_class, pub_args, None, bool(r) or once, verbose=verbose, + substitute_keywords=substitute_keywords) except ValueError as e: - sys.stderr.write("%s\n" % str(e)) + sys.stderr.write("%s\n"%str(e)) break if r is not None: r.sleep() if rospy.is_shutdown() or once: break - def stdin_yaml_arg(): """ Iterate over YAML documents in stdin @@ -2090,28 +2013,27 @@ def stdin_yaml_arg(): if not val: continue # sys.stdin.readline() returns empty string on EOF - arg = sys.stdin.readline() + arg = sys.stdin.readline() if arg != '' and arg.strip() != '---': buff = buff + arg - if arg.strip() == '---': # End of document + if arg.strip() == '---': # End of document try: loaded = yaml.safe_load(buff.rstrip()) except Exception as e: - sys.stderr.write("Invalid YAML: %s\n" % str(e)) + sys.stderr.write("Invalid YAML: %s\n"%str(e)) if loaded is not None: yield loaded - elif arg == '': # EOF + elif arg == '': #EOF # we don't yield the remaining buffer in this case # because we don't want to publish partial inputs return - - arg = 'x' # reset + + arg = 'x' # reset except select_error: - return # most likely ctrl-c interrupt - - + return # most likely ctrl-c interrupt + def _rostopic_cmd_list(argv): """ Command-line parsing for 'rostopic list' command. @@ -2123,13 +2045,13 @@ def _rostopic_cmd_list(argv): dest="bag", default=None, help="list topics in .bag file", metavar="BAGFILE") parser.add_option("-v", "--verbose", - dest="verbose", default=False, action="store_true", + dest="verbose", default=False,action="store_true", help="list full details about each topic") parser.add_option("-p", - dest="publishers", default=False, action="store_true", + dest="publishers", default=False,action="store_true", help="list only publishers") parser.add_option("-s", - dest="subscribers", default=False, action="store_true", + dest="subscribers", default=False,action="store_true", help="list only subscribers") parser.add_option("--host", dest="hostname", default=False, action="store_true", help="group by host name") @@ -2142,7 +2064,7 @@ def _rostopic_cmd_list(argv): elif len(args) > 1: parser.error("you may only specify one input topic") if options.bag: - if options.subscribers: + if options.subscribers: parser.error("-s option is not valid with bags") elif options.publishers: parser.error("-p option is not valid with bags") @@ -2153,12 +2075,10 @@ def _rostopic_cmd_list(argv): if options.subscribers and options.publishers: parser.error("you may only specify one of -p, -s") - exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, - publishers_only=options.publishers, group_by_host=options.hostname) or 0 + exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, publishers_only=options.publishers, group_by_host=options.hostname) or 0 if exitval != 0: sys.exit(exitval) - def _rostopic_cmd_info(argv): """ Command-line parsing for 'rostopic info' command. @@ -2172,13 +2092,12 @@ def _rostopic_cmd_info(argv): parser.error("you must specify a topic name") elif len(args) > 1: parser.error("you may only specify one topic name") - + topic = rosgraph.names.script_resolve_name('rostopic', args[0]) exitval = _rostopic_info(topic) or 0 if exitval != 0: sys.exit(exitval) - - + def _fullusage(): print("""rostopic is a command-line tool for printing information about ROS Topics. @@ -2197,14 +2116,13 @@ def _fullusage(): """) sys.exit(getattr(os, 'EX_USAGE', 1)) - def rostopicmain(argv=None): import rosbag if argv is None: - argv = sys.argv + argv=sys.argv # filter out remapping arguments in case we are being invoked via roslaunch argv = rospy.myargv(argv) - + # process argv if len(argv) == 1: _fullusage() @@ -2234,14 +2152,13 @@ def rostopicmain(argv=None): sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") sys.exit(1) except rosbag.ROSBagException as e: - sys.stderr.write("ERROR: unable to use bag file: %s\n" % str(e)) + sys.stderr.write("ERROR: unable to use bag file: %s\n"%str(e)) sys.exit(1) except rosgraph.MasterException as e: # mainly for invalid master URI/rosgraph.masterapi - sys.stderr.write("ERROR: %s\n" % str(e)) + sys.stderr.write("ERROR: %s\n"%str(e)) sys.exit(1) except ROSTopicException as e: - sys.stderr.write("ERROR: %s\n" % str(e)) + sys.stderr.write("ERROR: %s\n"%str(e)) sys.exit(1) - except KeyboardInterrupt: - pass + except KeyboardInterrupt: pass From 2afe152d437044ef7a8f4e5905200372e42188a2 Mon Sep 17 00:00:00 2001 From: Prasanna Kannappan Date: Fri, 5 May 2023 11:15:45 -0400 Subject: [PATCH 3/3] Undo irrelevant change --- tools/rostopic/src/rostopic/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/rostopic/src/rostopic/__init__.py b/tools/rostopic/src/rostopic/__init__.py index 4b4a1d7a34..a4a25827f8 100644 --- a/tools/rostopic/src/rostopic/__init__.py +++ b/tools/rostopic/src/rostopic/__init__.py @@ -282,7 +282,7 @@ def _get_ascii_table(header, cols): # sum of col and each 3 spaces width table_width = sum(col_widths) + 3 * (len(header) - 1) n_rows = len(cols[header[0]]) - body = '\n'.join(' '.join(cols[h][i] for h in header) for i in range(n_rows)) + body = '\n'.join(' '.join(cols[h][i] for h in header) for i in xrange(n_rows)) table = '{header}\n{hline}\n{body}\n'.format( header=' '.join(header_aligned), hline='=' * table_width, body=body) return table