/tools/rostopic/src/rostopic/__init__.py
Python | 1675 lines | 1381 code | 96 blank | 198 comment | 187 complexity | 81d1099697704b01387efa2d632869ca MD5 | raw file
Possible License(s): LGPL-2.1
- #!/usr/bin/env python
- # Software License Agreement (BSD License)
- #
- # Copyright (c) 2008, Willow Garage, Inc.
- # All rights reserved.
- #
- # Redistribution and use in source and binary forms, with or without
- # modification, are permitted provided that the following conditions
- # are met:
- #
- # * Redistributions of source code must retain the above copyright
- # notice, this list of conditions and the following disclaimer.
- # * Redistributions in binary form must reproduce the above
- # copyright notice, this list of conditions and the following
- # disclaimer in the documentation and/or other materials provided
- # with the distribution.
- # * Neither the name of Willow Garage, Inc. nor the names of its
- # contributors may be used to endorse or promote products derived
- # from this software without specific prior written permission.
- #
- # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- # POSSIBILITY OF SUCH DAMAGE.
- #
- # Revision $Id$
- # make sure we aren't using floor division
- from __future__ import division, print_function
- NAME='rostopic'
- import os
- import sys
- import math
- import socket
- import time
- import traceback
- import yaml
- import xmlrpclib
- from operator import itemgetter
- 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
- 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
- :raises: :exc:`ROSTopicException` If unable to successfully communicate with master
- """
- try:
- 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 xmlrpclib.Fault:
- #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):
- import threading
- self.lock = threading.Lock()
- self.last_printed_tn = 0
- self.msg_t0 = -1.
- self.msg_tn = 0
- self.times =[]
- self.filter_expr = filter_expr
-
- # can't have infinite window size due to memory restrictions
- if window_size < 0:
- window_size = 50000
- self.window_size = window_size
-
- def callback_hz(self, m):
- """
- ros sub callback
- :param m: Message instance
- """
- # #694: ignore messages that don't match filter
- if self.filter_expr is not None and not self.filter_expr(m):
- return
- with self.lock:
- curr_rostime = rospy.get_rostime()
- # time reset
- if curr_rostime.is_zero():
- if len(self.times) > 0:
- print("time has reset, resetting counters")
- self.times = []
- return
-
- curr = curr_rostime.to_sec()
- if self.msg_t0 < 0 or self.msg_t0 > curr:
- self.msg_t0 = curr
- self.msg_tn = curr
- self.times = []
- else:
- self.times.append(curr - self.msg_tn)
- self.msg_tn = curr
- #only keep statistics for the last 10000 messages so as not to run out of memory
- if len(self.times) > self.window_size - 1:
- self.times.pop(0)
- def print_hz(self):
- """
- print the average publishing rate to screen
- """
- if not self.times:
- return
- elif self.msg_tn == self.last_printed_tn:
- print("no new messages")
- return
- with self.lock:
- #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.times)
- #rate = (n - 1) / (rospy.get_time() - self.msg_t0)
- mean = sum(self.times) / n
- rate = 1./mean if mean > 0. else 0
- #std dev
- std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) /n)
- # min and max
- max_delta = max(self.times)
- min_delta = min(self.times)
- self.last_printed_tn = self.msg_tn
- print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, n+1))
- def _sleep(duration):
- rospy.rostime.wallsleep(duration)
- def _rostopic_hz(topic, window_size=-1, filter_expr=None):
- """
- Periodically print the publishing rate of a topic to console until
- shutdown
- :param topic: topic name, ``str``
- :param window_size: number of messages to average over, -1 for infinite, ``int``
- :param filter_expr: Python filter expression that is called with m, the message instance
- """
- msg_class, real_topic, _ = get_topic_class(topic, blocking=True) #pause hz until topic is published
- if rospy.is_shutdown():
- return
- rospy.init_node(NAME, anonymous=True)
- rt = ROSTopicHz(window_size, filter_expr=filter_expr)
- # 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:
- # have to subscribe with topic_type
- sub = rospy.Subscriber(real_topic, msg_class, rt.callback_hz)
- else:
- sub = rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback_hz)
- print("subscribed to [%s]"%real_topic)
- while not rospy.is_shutdown():
- _sleep(1.0)
- rt.print_hz()
-
- class ROSTopicBandwidth(object):
- def __init__(self, window_size=100):
- import threading
- self.lock = threading.Lock()
- self.last_printed_tn = 0
- 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))
- if len(self.times) > self.window_size:
- self.times.pop(0)
- self.sizes.pop(0)
- except:
- traceback.print_exc()
- def print_bw(self):
- """print the average publishing rate to screen"""
- if len(self.times) < 2:
- return
- with self.lock:
- n = len(self.times)
- tn = time.time()
- t0 = self.times[0]
-
- total = sum(self.sizes)
- bytes_per_s = total / (tn - t0)
- mean = total / n
- #std_dev = math.sqrt(sum((x - mean)**2 for x in self.sizes) /n)
- # min and max
- 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
- if bytes_per_s < 1000:
- 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]]
- 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))
- 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
- if rospy.is_shutdown():
- return
- # #3543 disable all auto-subscriptions to /clock
- rospy.init_node(NAME, anonymous=True, disable_rostime=True)
- rt = ROSTopicBandwidth(window_size)
- # 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)
- while not rospy.is_shutdown():
- _sleep(1.0)
- rt.print_bw()
- # TODO: port to the version I wrote for rxplot instead as it should be more efficient
- def msgevalgen(pattern):
- """
- Generates a function that returns the relevant field (aka 'subtopic') of a Message object
- :param pattern: subtopic, e.g. /x. Must have a leading '/' if specified, ``str``
- :returns: function that converts a message into the desired value, ``fn(Message) -> value``
- """
- if not pattern or pattern == '/':
- return None
- def msgeval(msg):
- # I will probably replace this with some less beautiful but more efficient
- try:
- return eval('msg'+'.'.join(pattern.split('/')))
- except AttributeError as e:
- sys.stdout.write("no field named [%s]"%pattern+"\n")
- return None
- return msgeval
-
- def _get_topic_type(topic):
- """
- subroutine for getting the topic type
- :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, ``(str, str, fn)``
- """
- try:
- val = _master_get_topic_types(rosgraph.Master('/rostopic'))
- except socket.error:
- raise ROSTopicIOException("Unable to communicate with master!")
- # exact match first, followed by prefix match
- matches = [(t, t_type) for t, t_type in val if t == topic]
- if not matches:
- matches = [(t, t_type) for t, t_type in val if topic.startswith(t+'/')]
- # choose longest match
- matches.sort(key=itemgetter(0), reverse=True)
- if matches:
- t, t_type = matches[0]
- if t_type == rosgraph.names.ANYTYPE:
- return None, None, None
- return t_type, t, msgevalgen(topic[len(t):])
- else:
- 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
- """
- topic_type, real_topic, msg_eval = _get_topic_type(topic)
- 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)
- while not rospy.is_shutdown():
- topic_type, real_topic, msg_eval = _get_topic_type(topic)
- if topic_type:
- return topic_type, real_topic, msg_eval
- else:
- _sleep(0.1)
- return None, None, None
- def get_topic_class(topic, blocking=False):
- """
- Get the topic message class
- :returns: message class for topic, real topic
- name, and function for evaluating message objects into the subtopic
- (or ``None``). ``(Message, str, str)``
- :raises: :exc:`ROSTopicException` If topic type cannot be determined or loaded
- """
- topic_type, real_topic, msg_eval = get_topic_type(topic, blocking=blocking)
- if topic_type is None:
- return None, None, None
- msg_class = roslib.message.get_message_class(topic_type)
- if not msg_class:
- 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
- :returns: list of fields as a CSV string, ``str``
- """
- s = _sub_str_plot_fields(val, f, field_filter)
- if s is not None:
- return "time,"+s
- 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, float) or \
- isinstance(val, genpy.TVal):
- return f
- # duck-type check for messages
- elif hasattr(val, "_slot_types"):
- if field_filter is not None:
- 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 = [s for s in sub if s is not None]
- if sub:
- return ','.join([s for s in sub])
- elif type_ in (str, unicode):
- return f
- elif type_ in (list, tuple):
- if len(val) == 0:
- return None
- val0 = val[0]
- type0 = type(val0)
- # no arrays of arrays
- if type0 in (bool, int, float) or \
- isinstance(val0, genpy.TVal):
- return ','.join(["%s%s"%(f,x) for x in xrange(0,len(val))])
- elif type0 in (str, unicode):
-
- return ','.join(["%s%s"%(f,x) for x in xrange(0,len(val))])
- elif hasattr(val0, "_slot_types"):
- labels = ["%s%s"%(f,x) for x in xrange(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
- def _str_plot(val, time_offset=None, current_time=None, field_filter=None):
- """
- Convert value to matlab/octave-friendly CSV string representation.
- :param val: message
- :param current_time: current :class:`genpy.Time` to use if message does not contain its own timestamp.
- :param time_offset: (optional) for time printed for message, print as offset against this :class:`genpy.Time`
- :param field_filter: filter the fields that are strified for Messages, ``fn(Message)->iter(str)``
- :returns: comma-separated list of field values in val, ``str``
- """
-
- s = _sub_str_plot(val, time_offset, field_filter)
- if s is None:
- s = ''
- if time_offset is not None:
- time_offset = time_offset.to_nsec()
- else:
- time_offset = 0
- if getattr(val, "_has_header", False):
- return "%s,%s"%(val.header.stamp.to_nsec()-time_offset, s)
- elif current_time is not None:
- return "%s,%s"%(current_time.to_nsec()-time_offset, s)
- else:
- 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, float) or \
- isinstance(val, genpy.TVal):
- if time_offset is not None and isinstance(val, genpy.Time):
- return str(val-time_offset)
- else:
- return str(val)
- elif hasattr(val, "_slot_types"):
- if field_filter is not None:
- fields = list(field_filter(val))
- else:
- 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 = [s for s in sub if s is not None]
- if sub:
- return ','.join(sub)
- elif type_ in (str, unicode):
- return val
- elif type_ in (list, tuple):
- if len(val) == 0:
- return None
- val0 = val[0]
- # no arrays of arrays
- type0 = type(val0)
- if type0 == bool:
- return ','.join([('1' if v else '0') for v in val])
- elif type0 in (int, float) or \
- isinstance(val0, genpy.TVal):
- return ','.join([str(v) for v in val])
- elif type0 in (str, unicode):
- 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
- to convert uint8[] fields back to an array type.
- """
- attr = getattr(val, f)
- if type(attr) in (str, unicode) and 'uint8[' in t:
- return [ord(x) for x in attr]
- else:
- return attr
- class CallbackEcho(object):
- """
- Callback instance that can print callback data in a variety of
- formats. Used for all variants of rostopic echo
- """
- def __init__(self, topic, msg_eval, plot=False, filter_fn=None,
- echo_clear=False, echo_all_topics=False,
- offset_time=False, count=None,
- field_filter_fn=None):
- """
- :param plot: if ``True``, echo in plotting-friendly format, ``bool``
- :param filter_fn: function that evaluates to ``True`` if message is to be echo'd, ``fn(topic, msg)``
- :param echo_all_topics: (optional) if ``True``, echo all messages in bag, ``bool``
- :param offset_time: (optional) if ``True``, display time as offset from current time, ``bool``
- :param count: number of messages to echo, ``None`` for infinite, ``int``
- :param field_filter_fn: filter the fields that are strified for Messages, ``fn(Message)->iter(str)``
- """
- if topic and topic[-1] == '/':
- topic = topic[:-1]
- self.topic = topic
- self.msg_eval = msg_eval
- self.plot = plot
- self.filter_fn = filter_fn
- self.prefix = ''
- 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
- # done tracks when we've exceeded the count
- self.done = False
- self.max_count = count
- self.count = 0
- # determine which strifying function to use
- if plot:
- #TODOXXX: need to pass in filter function
- self.str_fn = _str_plot
- self.sep = ''
- else:
- #TODOXXX: need to pass in filter function
- self.str_fn = genpy.message.strify_message
- if echo_clear:
- self.prefix = '\033[2J\033[;H'
- self.field_filter=field_filter_fn
-
- # first tracks whether or not we've printed anything yet. Need this for printing plot fields.
- self.first = True
- # cache
- self.last_topic = None
- self.last_msg_eval = None
- def callback(self, data, topic, current_time=None):
- """
- Callback to pass to rospy.Subscriber or to call
- manually. rospy.Subscriber constructor must also pass in the
- topic name as an additional arg
- :param data: Message
- :param topic: topic name, ``str``
- :param current_time: override calculation of current time, :class:`genpy.Time`
- """
- if self.filter_fn is not None and not self.filter_fn(data):
- return
- 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:
- pass
- elif self.topic.startswith(topic + '/'):
- # self.topic is actually a reference to topic field, generate msgeval
- if topic == self.last_topic:
- # use cached eval
- msg_eval = self.last_msg_eval
- else:
- # generate msg_eval and cache
- self.last_msg_eval = msg_eval = msgevalgen(self.topic[len(topic):])
- self.last_topic = topic
- elif not self.echo_all_topics:
- return
- if msg_eval is not None:
- data = msg_eval(data)
- else:
- val = 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+\
- self.str_fn(data, time_offset=rospy.get_rostime(),
- current_time=current_time, field_filter=self.field_filter) + \
- self.suffix + '\n')
- else:
- sys.stdout.write(self.prefix+\
- self.str_fn(data,
- current_time=current_time, field_filter=self.field_filter) + \
- self.suffix + '\n')
- # we have to flush in order before piping to work
- sys.stdout.flush()
- # #2778 : have to check count after incr to set done flag
- if self.max_count is not None and self.count >= self.max_count:
- self.done = True
- except IOError:
- self.done = True
- except:
- # set done flag so we exit
- self.done = True
- traceback.print_exc()
-
- def _rostopic_type(topic):
- """
- Print ROS message type of topic to screen
- :param topic: topic name, ``str``
- """
- t, _, _ = get_topic_type(topic, blocking=False)
- if t:
- print(t)
- else:
- sys.stderr.write('unknown topic type [%s]\n'%topic)
- sys.exit(1)
- def _rostopic_echo_bag(callback_echo, bag_file):
- """
- :param callback_echo: :class:`CallbackEcho` instance to invoke on new messages in 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)
- 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
- # dynamic renaming
- if t[0] != '/':
- t = rosgraph.names.script_resolve_name('rostopic', t)
- callback_echo.callback(msg, t, current_time=timestamp)
- # done is set if there is a max echo count
- 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``
- """
- # we have to init a node regardless and bag echoing can print timestamps
- if bag_file:
- # initialize rospy time due to potential timestamp printing
- rospy.rostime.set_rostime_initialized(True)
- _rostopic_echo_bag(callback_echo, bag_file)
- else:
- _check_master()
- rospy.init_node(NAME, anonymous=True)
- msg_class, real_topic, msg_eval = get_topic_class(topic, blocking=True)
- if msg_class is None:
- # occurs on ctrl-C
- return
- callback_echo.msg_eval = msg_eval
- use_sim_time = rospy.get_param('/use_sim_time', False)
- sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, topic)
- if use_sim_time:
- # #2950: print warning if nothing received for two seconds
- timeout_t = time.time() + 2.
- while time.time() < timeout_t and \
- callback_echo.count == 0 and \
- not rospy.is_shutdown() and \
- not callback_echo.done:
- _sleep(0.1)
- 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")
- 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
- :param master: XML-RPC handle to ROS Master, :class:`xmlrpclib.ServerProxy`
- :param caller_id: node name, ``str``
- :returns: XML-RPC URI of node, ``str``
- :raises: :exc:`ROSTopicIOException` If unable to communicate with master
- """
- caller_api = _caller_apis.get(caller_id, None)
- if not caller_api:
- try:
- caller_api = master.lookupNode(caller_id)
- _caller_apis[caller_id] = caller_api
- except socket.error:
- raise ROSTopicIOException("Unable to communicate with master!")
- except rosgraph.MasterError:
- 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
- :param bag_file: path to bag file, ``str``
- :param topic: optional topic name to match. Will print additional information just about messagese in this topic, ``str``
- """
- import rosbag
- if not os.path.exists(bag_file):
- raise ROSTopicException("bag file [%s] does not exist"%bag_file)
- with rosbag.Bag(bag_file) as b:
- if topic:
- # create string for namespace comparison
- topic_ns = rosgraph.names.make_global_ns(topic)
- count = 0
- earliest = None
- latest = None
- for top, msg, t in b.read_messages(raw=True):
- if top == topic or top.startswith(topic_ns):
- count += 1
- if earliest == None:
- earliest = t
- latest = t
- 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))
- else:
- topics = set()
- for top, msg, _ in b.read_messages(raw=True):
- if top not in topics:
- print(top)
- topics.add(top)
- if rospy.is_shutdown():
- break
- def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, verbose, indent=''):
- def topic_type(t, topic_types):
- matches = [t_type for t_name, t_type in topic_types if t_name == t]
- if matches:
- return matches[0]
- return 'unknown type'
- if verbose:
- topic_types = _master_get_topic_types(master)
- if not subscribers_only:
- print("\n%sPublished topics:"%indent)
- for t, l in pubs:
- if len(l) > 1:
- print(indent+" * %s [%s] %s publishers"%(t, topic_type(t, topic_types), len(l)))
- else:
- print(indent+" * %s [%s] 1 publisher"%(t, topic_type(t, topic_types)))
- if not publishers_only:
- print(indent)
- print(indent+"Subscribed topics:")
- for t,l in subs:
- if len(l) > 1:
- print(indent+" * %s [%s] %s subscribers"%(t, topic_type(t, topic_types), len(l)))
- else:
- print(indent+" * %s [%s] 1 subscriber"%(t, topic_type(t, topic_types)))
- print('')
- else:
- if publishers_only:
- topics = [t for t,_ in pubs]
- elif subscribers_only:
- topics = [t for t,_ in subs]
- else:
- 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]))
- # #3145
- def _rostopic_list_group_by_host(master, pubs, subs):
- """
- Build up maps for hostname to topic list per hostname
- :returns: publishers host map, subscribers host map, ``{str: set(str)}, {str: set(str)}``
- """
- def build_map(master, state, uricache):
- tmap = {}
- for topic, tnodes in state:
- for p in tnodes:
- if not p in uricache:
- uricache[p] = master.lookupNode(p)
- uri = uricache[p]
- puri = urlparse(uri)
- if not puri.hostname in tmap:
- tmap[puri.hostname] = []
- # recreate the system state data structure, but for a single host
- matches = [l for x, l in tmap[puri.hostname] if x == topic]
- if matches:
- matches[0].append(p)
- else:
- tmap[puri.hostname].append((topic, [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``
- :param publishers_only: print information about subscriptions only, ``bool``
- :param group_by_host: group topic list by hostname, ``bool``
- """
- # #1563
- if subscribers_only and publishers_only:
- raise ROSTopicException("cannot specify both subscribers- and publishers-only")
-
- master = rosgraph.Master('/rostopic')
- try:
- state = master.getSystemState()
- pubs, subs, _ = state
- if topic:
- # filter based on topic
- topic_ns = rosgraph.names.make_global_ns(topic)
- subs = (x for x in subs if x[0] == topic or x[0].startswith(topic_ns))
- pubs = (x for x in pubs if x[0] == topic or x[0].startswith(topic_ns))
-
- except socket.error:
- raise ROSTopicIOException("Unable to communicate with master!")
- 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, []),
- if (pubs and not subscribers_only) or (subs and not publishers_only):
- print("Host [%s]:" % hostname)
- _sub_rostopic_list(master, pubs, subs,
- publishers_only, subscribers_only,
- verbose, indent=' ')
- else:
- _sub_rostopic_list(master, pubs, subs,
- publishers_only, subscribers_only,
- verbose)
- def get_info_text(topic):
- """
- Get human-readable topic description
-
- :param topic: topic name, ``str``
- """
- import cStringIO, itertools
- buff = cStringIO.StringIO()
- def topic_type(t, topic_types):
- matches = [t_type for t_name, t_type in topic_types if t_name == t]
- if matches:
- return matches[0]
- return 'unknown type'
- master = rosgraph.Master('/rostopic')
- try:
- state = master.getSystemState()
- pubs, subs, _ = state
- # filter based on topic
- subs = [x for x in subs if x[0] == topic]
- 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)
- buff.write("Type: %s\n\n"%topic_type(topic, topic_types))
- if pubs:
- buff.write("Publishers: \n")
- for p in itertools.chain(*[l for x, l in pubs]):
- buff.write(" * %s (%s)\n"%(p, get_api(master, p)))
- else:
- buff.write("Publishers: None\n")
- buff.write('\n')
- if subs:
- buff.write("Subscribers: \n")
- for p in itertools.chain(*[l for x, l in subs]):
- 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):
- return eval(expr)
- return eval_fn
- args = argv[2:]
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog echo [options] /topic", prog=NAME)
- parser.add_option("-b", "--bag",
- dest="bag", default=None,
- help="echo messages from .bag file", metavar="BAGFILE")
- parser.add_option("-p",
- dest="plot", default=False,
- action="store_true",
- help="echo in a plotting friendly format")
- 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",
- dest="nostr", default=False,
- action="store_true",
- help="exclude string fields")
- parser.add_option("--noarr",
- dest="noarr", default=False,
- action="store_true",
- help="exclude arrays")
- parser.add_option("-c", "--clear",
- dest="clear", default=False,
- action="store_true",
- help="clear screen before printing next message")
- parser.add_option("-a", "--all",
- dest="all_topics", default=False,
- action="store_true",
- help="display all message in bag, only valid with -b option")
- parser.add_option("-n",
- dest="msg_count", default=None, metavar="COUNT",
- help="number of messages to echo")
- parser.add_option("--offset",
- dest="offset_time", default=False,
- action="store_true",
- help="display time as offsets from current time (in seconds)")
- (options, args) = parser.parse_args(args)
- if len(args) > 1:
- parser.error("you may only specify one input topic")
- if options.all_topics and not options.bag:
- parser.error("Display all option is only valid when echoing from bag files")
- if options.offset_time and options.bag:
- parser.error("offset time option is not valid with bag files")
- if options.all_topics:
- topic = ''
- else:
- if len(args) == 0:
- 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:
- # print "rostopic: topic is [%s]"%topic
-
- filter_fn = None
- if options.filter_expr:
- filter_fn = expr_eval(options.filter_expr)
- try:
- msg_count = int(options.msg_count) if options.msg_count else None
- except ValueError:
- parser.error("COUNT must be an integer")
-
- field_filter_fn = create_field_filter(options.nostr, options.noarr)
- callback_echo = CallbackEcho(topic, None, plot=options.plot,
- filter_fn=filter_fn,
- echo_clear=options.clear, echo_all_topics=options.all_topics,
- offset_time=options.offset_time, count=msg_count,
- field_filter_fn=field_filter_fn)
- try:
- _rostopic_echo(topic, callback_echo, bag_file=options.bag)
- except socket.error:
- sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n")
- def create_field_filter(echo_nostr, echo_noarr):
- def field_filter(val):
- fields = val.__slots__
- field_types = val._slot_types
- for f, t in zip(val.__slots__, val._slot_types):
- if echo_noarr and '[' in t:
- continue
- elif echo_nostr and 'string' in t:
- continue
- 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)
- (options, args) = parser.parse_args(args)
- if len(args) == 0:
- 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):
- _rostopic_type(_optparse_topic_only('type', argv))
-
- def _rostopic_cmd_hz(argv):
- args = argv[2:]
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog hz /topic", prog=NAME)
- parser.add_option("-w", "--window",
- dest="window_size", default=-1,
- help="window size, in # of messages, for calculating rate", metavar="WINDOW")
- parser.add_option("--filter",
- dest="filter_expr", default=None,
- help="only measure messages matching the specified Python expression", metavar="EXPR")
- (options, args) = parser.parse_args(args)
- if len(args) == 0:
- parser.error("topic must be specified")
- if len(args) > 1:
- parser.error("you may only specify one input topic")
- try:
- if options.window_size != -1:
- import string
- window_size = string.atoi(options.window_size)
- else:
- window_size = options.window_size
- except:
- parser.error("window size must be an integer")
- topic = rosgraph.names.script_resolve_name('rostopic', args[0])
- # #694
- if options.filter_expr:
- def expr_eval(expr):
- def eval_fn(m):
- return eval(expr)
- return eval_fn
- filter_expr = expr_eval(options.filter_expr)
- else:
- filter_expr = None
- _rostopic_hz(topic, window_size=window_size, filter_expr=filter_expr)
- def _rostopic_cmd_bw(argv=sys.argv):
- args = argv[2:]
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog bw /topic", prog=NAME)
- parser.add_option("-w", "--window",
- dest="window_size", default=None,
- 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")
- if len(args) > 1:
- parser.error("you may only specify one input topic")
- try:
- if options.window_size:
- import string
- window_size = string.atoi(options.window_size)
- else:
- window_size = options.window_size
- except:
- parser.error("window size must be an integer")
- 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
- :param topic_type: type of topic to find, ``str``
- :returns: list of topic names that use topic_type, ``[str]``
- """
- master = rosgraph.Master('/rostopic')
- try:
- t_list = _master_get_topic_types(master)
- 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'
- :param argv: command-line args, ``[str]``
- """
- args = argv[2:]
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog find msg-type", prog=NAME)
- options, args = parser.parse_args(args)
- if not len(args):
- parser.error("please specify a message type")
- 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):
- """
- Create rospy.Publisher instance from the string topic name and
- type. This is a powerful method as it allows creation of
- rospy.Publisher and Message instances using the topic and type
- names. This enables more dynamic publishing from Python programs.
- :param topic_name: name of topic, ``str``
- :param topic_type: name of topic type, ``str``
- :param latch: latching topic, ``bool``
- :returns: topic :class:`rospy.Publisher`, :class:`Message` class
- """
- topic_name = rosgraph.names.script_resolve_name('rostopic', topic_name)
- try:
- msg_class = roslib.message.get_message_class(topic_type)
- except:
- 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))
- # 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=True)
- pub = rospy.Publisher(topic_name, msg_class, latch=latch)
- return pub, msg_class
- def _publish_at_rate(pub, msg, rate, verbose=False):
- """
- 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``
- :param verbose: If ``True``, print more verbose output to stdout, ``bool``
- """
- try:
- r = rospy.Rate(float(rate))
- except ValueError:
- raise ROSTopicException("Rate must be a number")
- while not rospy.is_shutdown():
- if verbose:
- 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``
- :param verbose: If ``True``, print more verbose output to stdout, ``bool``
- """
- try:
- pub.publish(msg)
- except TypeError as e:
- raise ROSTopicException(str(e))
- if not once:
- rospy.spin()
- def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=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]``
- :param rate: publishing rate (hz) or None for just once, ``int``
- :param once: publish only once and return, ``bool``
- :param verbose: If ``True``, print more verbose output to stdout, ``bool``
- """
- msg = msg_class()
- try:
- # Populate the message and enable substitution keys for 'now'
- # and 'auto'. There is a corner case here: this logic doesn't
- # work if you're publishing a Header only and wish to use
- # 'auto' with it. This isn't a troubling case, but if we start
- # allowing more keys in the future, it could become an actual
- # use case. It greatly complicates logic because we'll have to
- # do more reasoning over types. to avoid ambiguous cases
- # (e.g. a std_msgs/String type, which only has a single string
- # field).
-
- # 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) }
- 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))
- try:
-
- if rate is None:
- s = "publishing and latching [%s]"%(msg) if verbose else "publishing and latching message"
- if once:
- s = s + " for %s seconds"%_ONCE_DELAY
- else:
- s = s + ". Press ctrl-C to terminate"
- print(s)
-
- if rate is None:
- _publish_latched(pub, msg, once, verbose)
- else:
- _publish_at_rate(pub, msg, rate, verbose)
-
- 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)))
-
- def _rostopic_cmd_pub(argv):
- """
- Parse 'pub' command arguments and run command. Will cause a system
- exit if command-line argument parsing fails.
- :param argv: command-line arguments
- :param argv: [str]
- :raises: :exc:`ROSTopicException` If call command cannot be executed
- """
- args = argv[2:]
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog pub /topic type [args...]", prog=NAME)
- parser.add_option("-v", dest="verbose", default=False,
- action="store_true",
- help="print verbose output")
- parser.add_option("-r", "--rate", dest="rate", default=None,
- help="publishing rate (hz). For -f and stdin input, this defaults to 10. Otherwise it is not set.")
- parser.add_option("-1", "--once", action="store_true", dest="once", default=False,
- help="publish one message and exit")
- parser.add_option("-f", '--file', dest="file", metavar='FILE', default=None,
- help="read args from YAML file (Bagy)")
- parser.add_option("-l", '--latch', dest="latch", default=False, action="store_true",
- help="enable latching for -f, -r and piped input. This latches the first message.")
- #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:
- parser.error("You cannot select both -r and -1 (--once)")
- try:
- rate = float(options.rate)
- except ValueError:
- parser.error("rate must be a number")
- if rate <= 0:
- parser.error("rate must be greater than zero")
- 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")
- if len(args) == 1:
- parser.error("topic type must be specified")
- if 0:
- if len(args) > 2 and options.parameter:
- parser.error("args confict with -p setting")
- if len(args) > 2 and options.file:
- parser.error("args confict with -f setting")
- topic_name, topic_type = args[0], args[1]
- # type-case using YAML
- try:
- pub_args = []
- for arg in args[2:]:
- pub_args.append(yaml.load(arg))
- except Exception as e:
- parser.error("Argument error: "+str(e))
- # make sure master is online. we wait until after we've parsed the
- # args to do this so that syntax errors are reported first
- _check_master()
- # if no rate, or explicit latch, we latch
- latch = (rate == None) or options.latch
- pub, msg_class = create_publisher(topic_name, topic_type, latch)
- if 0 and options.parameter:
- param_name = rosgraph.names.script_resolve_name('rostopic', options.parameter)
- if options.once:
- 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)
- else:
- argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose)
-
- def file_yaml_arg(filename):
- """
- :param filename: file name, ``str``
- :returns: Iterator that yields pub args (list of args), ``iterator``
- :raises: :exc:`ROSTopicException` If filename is invalid
- """
- if not os.path.isfile(filename):
- raise ROSTopicException("file does not exist: %s"%(filename))
- import yaml
- def bagy_iter():
- try:
- with open(filename, 'r') as f:
- # load all documents
- data = yaml.load_all(f)
- for d in data:
- yield [d]
- except yaml.YAMLError as e:
- raise ROSTopicException("invalid YAML in file: %s"%(str(e)))
- return bagy_iter
-
- def argv_publish(pub, msg_class, pub_args, rate, once, verbose):
- publish_message(pub, msg_class, pub_args, rate, once, verbose=verbose)
- if once:
- # stick around long enough for others to grab
- timeout_t = time.time() + _ONCE_DELAY
- 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))
- pub_args = rospy.get_param(param_name)
- argv_publish(pub, msg_class, pub_args, None, True, verbose)
- class _ParamNotifier(object):
- def __init__(self, param_name, value=None):
- import threading
- self.lock = threading.Condition()
- self.param_name = param_name
- self.updates = []
- self.value = None
- def __call__(self, key, value):
- with self.lock:
- # have to address downward if we got notification on sub namespace
- if key != self.param_name:
- subs = [x for x in key[len(self.param_name):].split('/') if x]
- idx = self.value
- for s in subs[:-1]:
- if s in idx:
- idx = idx[s]
- else:
- idx[s] = {}
- idx = idx[s]
- idx[subs[-1]] = value
- else:
- self.value = 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``
- :returns: List of msg dicts in file, ``[{str: any}]``
- :raises: :exc:`ROSTopicException` If parameter is not set
- """
- import rospy
- import rospy.impl.paramserver
- import rosgraph
-
- if not rospy.has_param(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!
- ps_cache = rospy.impl.paramserver.get_param_server_cache()
- notifier = _ParamNotifier(param_name)
- ps_cache.set_notifier(notifier)
- master = rosgraph.Master(rospy.get_name())
- notifier.value = master.subscribeParam(rospy.get_node_uri(), param_name)
- pub_args = notifier.value
- ps_cache.set(param_name, pub_args)
- 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))
- r = rospy.Rate(rate) if rate is not None else None
- publish = True
- while not rospy.is_shutdown():
- try:
- if publish:
- publish_message(pub, msg_class, pub_args, None, True, verbose=verbose)
- except ValueError as e:
- sys.stderr.write("%s\n"%str(e))
- break
- if r is not None:
- r.sleep()
- with notifier.lock:
- if notifier.updates:
- pub_args = notifier.updates.pop(0)
- if type(pub_args) == dict:
- pub_args = [pub_args]
- else:
- publish = False
- with notifier.lock:
- if not notifier.updates:
- notifier.lock.wait(1.)
- if notifier.updates:
- publish = True
- 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):
- """
- :param filename: name of file to read from instead of stdin, or ``None``, ``str``
- """
- if filename:
- iterator = file_yaml_arg(filename)
- else:
- iterator = stdin_yaml_arg
- r = rospy.Rate(rate) if rate is not None else None
- # stdin publishing can happen really fast, especially if no rate
- # is set, so try to make sure someone is listening before we
- # publish, though we don't wait too long.
- wait_for_subscriber(pub, SUBSCRIBER_TIMEOUT)
- single_arg = None
- for pub_args in iterator():
- if rospy.is_shutdown():
- break
- if pub_args:
- if type(pub_args) != list:
- pub_args = [pub_args]
- try:
- # 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
- # has enabled latching (i.e. rate is none). It would
- # be good to reorganize this code more conceptually
- # but, for now, this is the best re-use of the
- # underlying methods.
- publish_message(pub, msg_class, pub_args, None, bool(r) or once, verbose=verbose)
- except ValueError as e:
- sys.stderr.write("%s\n"%str(e))
- break
- if r is not None:
- r.sleep()
- if rospy.is_shutdown() or once:
- break
- # Publishing a single message repeatedly
- if single_arg and r and not once:
- while not rospy.is_shutdown():
- try:
- publish_message(pub, msg_class, pub_args, None, True, verbose=verbose)
- if r is not None:
- r.sleep()
- except ValueError as e:
- break
- def stdin_yaml_arg():
- """
- Iterate over YAML documents in stdin
- :returns: for next list of arguments on stdin. Iterator returns a list of args for each call, ``iterator``
- """
- import yaml
- from select import select
- from select import error as select_error
- try:
- arg = 'x'
- rlist = [sys.stdin]
- wlist = xlist = []
- while not rospy.is_shutdown() and arg != '\n':
- buff = ''
- while arg != '' and arg.strip() != '---' and not rospy.is_shutdown():
- val, _, _ = select(rlist, wlist, xlist, 1.0)
- if not val:
- continue
- # sys.stdin.readline() returns empty string on EOF
- arg = sys.stdin.readline()
- if arg != '' and arg.strip() != '---':
- buff = buff + arg
- if arg.strip() == '---': # End of document
- try:
- loaded = yaml.load(buff.rstrip())
- except Exception as e:
- sys.stderr.write("Invalid YAML: %s\n"%str(e))
- if loaded is not None:
- yield loaded
- 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
- except select_error:
- return # most likely ctrl-c interrupt
-
- def _rostopic_cmd_list(argv):
- """
- Command-line parsing for 'rostopic list' command.
- """
- args = argv[2:]
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME)
- parser.add_option("-b", "--bag",
- dest="bag", default=None,
- help="list topics in .bag file", metavar="BAGFILE")
- parser.add_option("-v", "--verbose",
- 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",
- help="list only publishers")
- parser.add_option("-s",
- 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")
- (options, args) = parser.parse_args(args)
- topic = None
- if len(args) == 1:
- topic = rosgraph.names.script_resolve_name('rostopic', args[0])
- elif len(args) > 1:
- parser.error("you may only specify one input topic")
- if options.bag:
- if options.subscribers:
- parser.error("-s option is not valid with bags")
- elif options.publishers:
- parser.error("-p option is not valid with bags")
- elif options.hostname:
- parser.error("--host option is not valid with bags")
- _rostopic_list_bag(options.bag, topic)
- else:
- 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
- if exitval != 0:
- sys.exit(exitval)
- def _rostopic_cmd_info(argv):
- """
- Command-line parsing for 'rostopic info' command.
- """
- args = argv[2:]
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog info /topic", prog=NAME)
- (options, args) = parser.parse_args(args)
- if len(args) == 0:
- 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.
- Commands:
- \trostopic bw\tdisplay bandwidth used by topic
- \trostopic echo\tprint messages to screen
- \trostopic find\tfind topics by type
- \trostopic hz\tdisplay publishing rate of topic
- \trostopic info\tprint information about active topic
- \trostopic list\tlist active topics
- \trostopic pub\tpublish data to topic
- \trostopic type\tprint topic type
- Type rostopic <command> -h for more detailed usage, e.g. 'rostopic echo -h'
- """)
- sys.exit(getattr(os, 'EX_USAGE', 1))
- def rostopicmain(argv=None):
- import rosbag
- if argv is None:
- 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()
- try:
- command = argv[1]
- if command == 'echo':
- _rostopic_cmd_echo(argv)
- elif command == 'hz':
- _rostopic_cmd_hz(argv)
- elif command == 'type':
- _rostopic_cmd_type(argv)
- elif command in 'list':
- _rostopic_cmd_list(argv)
- elif command == 'info':
- _rostopic_cmd_info(argv)
- elif command == 'pub':
- _rostopic_cmd_pub(argv)
- elif command == 'bw':
- _rostopic_cmd_bw(argv)
- elif command == 'find':
- _rostopic_cmd_find(argv)
- else:
- _fullusage()
- except socket.error:
- 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.exit(1)
- except rosgraph.MasterException as e:
- # mainly for invalid master URI/rosgraph.masterapi
- sys.stderr.write("ERROR: %s\n"%str(e))
- sys.exit(1)
- except ROSTopicException as e:
- sys.stderr.write("ERROR: %s\n"%str(e))
- sys.exit(1)
- except KeyboardInterrupt: pass
- except rospy.ROSInterruptException: pass