Package rostopic

Source Code for Package rostopic

   1  #!/usr/bin/env python 
   2  # Software License Agreement (BSD License) 
   3  # 
   4  # Copyright (c) 2008, Willow Garage, Inc. 
   5  # All rights reserved. 
   6  # 
   7  # Redistribution and use in source and binary forms, with or without 
   8  # modification, are permitted provided that the following conditions 
   9  # are met: 
  10  # 
  11  #  * Redistributions of source code must retain the above copyright 
  12  #    notice, this list of conditions and the following disclaimer. 
  13  #  * Redistributions in binary form must reproduce the above 
  14  #    copyright notice, this list of conditions and the following 
  15  #    disclaimer in the documentation and/or other materials provided 
  16  #    with the distribution. 
  17  #  * Neither the name of Willow Garage, Inc. nor the names of its 
  18  #    contributors may be used to endorse or promote products derived 
  19  #    from this software without specific prior written permission. 
  20  # 
  21  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
  22  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
  23  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
  24  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
  25  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
  26  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
  27  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
  28  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
  29  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
  30  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
  31  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
  32  # POSSIBILITY OF SUCH DAMAGE. 
  33  # 
  34  # Revision $Id$ 
  35   
  36  # make sure we aren't using floor division 
  37  from __future__ import division, print_function 
  38   
  39  NAME='rostopic' 
  40   
  41  import argparse 
  42  import os 
  43  import sys 
  44  import math 
  45  import socket 
  46  import time 
  47  import traceback 
  48  import yaml 
  49  try: 
  50      from xmlrpc.client import Fault 
  51  except ImportError: 
  52      from xmlrpclib import Fault 
  53   
  54  from operator import itemgetter 
  55  try: 
  56      from urllib.parse import urlparse 
  57  except ImportError: 
  58      from urlparse import urlparse 
  59   
  60  import genpy 
  61   
  62  import roslib.message 
  63  import rosgraph 
  64  #TODO: lazy-import rospy or move rospy-dependent routines to separate location 
  65  import rospy 
  66   
  67  try: 
  68      long 
  69  except NameError: 
  70      long = int 
  71   
  72   
73 -class ROSTopicException(Exception):
74 """ 75 Base exception class of rostopic-related errors 76 """ 77 pass
78 -class ROSTopicIOException(ROSTopicException):
79 """ 80 rostopic errors related to network I/O failures 81 """ 82 pass
83
84 -def _check_master():
85 """ 86 Make sure that master is available 87 :raises: :exc:`ROSTopicException` If unable to successfully communicate with master 88 """ 89 try: 90 rosgraph.Master('/rostopic').getPid() 91 except socket.error: 92 raise ROSTopicIOException("Unable to communicate with master!")
93
94 -def _master_get_topic_types(master):
95 try: 96 val = master.getTopicTypes() 97 except Fault: 98 #TODO: remove, this is for 1.1 99 sys.stderr.write("WARNING: rostopic is being used against an older version of ROS/roscore\n") 100 val = master.getPublishedTopics('/') 101 return val
102
103 -class ROSTopicHz(object):
104 """ 105 ROSTopicHz receives messages for a topic and computes frequency stats 106 """
107 - def __init__(self, window_size, filter_expr=None, use_wtime=False):
108 import threading 109 from collections import defaultdict 110 self.lock = threading.Lock() 111 self.last_printed_tn = 0 112 self.msg_t0 = -1 113 self.msg_tn = 0 114 self.times = [] 115 self._last_printed_tn = defaultdict(int) 116 self._msg_t0 = defaultdict(lambda: -1) 117 self._msg_tn = defaultdict(int) 118 self._times = defaultdict(list) 119 self.filter_expr = filter_expr 120 self.use_wtime = use_wtime 121 122 # can't have infinite window size due to memory restrictions 123 if window_size < 0: 124 window_size = 50000 125 self.window_size = window_size
126
127 - def get_last_printed_tn(self, topic=None):
128 if topic is None: 129 return self.last_printed_tn 130 return self._last_printed_tn[topic]
131
132 - def set_last_printed_tn(self, value, topic=None):
133 if topic is None: 134 self.last_printed_tn = value 135 self._last_printed_tn[topic] = value
136
137 - def get_msg_t0(self, topic=None):
138 if topic is None: 139 return self.msg_t0 140 return self._msg_t0[topic]
141
142 - def set_msg_t0(self, value, topic=None):
143 if topic is None: 144 self.msg_t0 = value 145 self._msg_t0[topic] = value
146
147 - def get_msg_tn(self, topic=None):
148 if topic is None: 149 return self.msg_tn 150 return self._msg_tn[topic]
151
152 - def set_msg_tn(self, value, topic=None):
153 if topic is None: 154 self.msg_tn = value 155 self._msg_tn[topic] = value
156
157 - def get_times(self, topic=None):
158 if topic is None: 159 return self.times 160 return self._times[topic]
161
162 - def set_times(self, value, topic=None):
163 if topic is None: 164 self.times = value 165 self._times[topic] = value
166
167 - def callback_hz(self, m, topic=None):
168 """ 169 ros sub callback 170 :param m: Message instance 171 :param topic: Topic name 172 """ 173 # #694: ignore messages that don't match filter 174 if self.filter_expr is not None and not self.filter_expr(m): 175 return 176 with self.lock: 177 curr_rostime = rospy.get_rostime() if not self.use_wtime else \ 178 rospy.Time.from_sec(time.time()) 179 180 # time reset 181 if curr_rostime.is_zero(): 182 if len(self.get_times(topic=topic)) > 0: 183 print("time has reset, resetting counters") 184 self.set_times([], topic=topic) 185 return 186 187 curr = curr_rostime.to_sec() if not self.use_wtime else \ 188 rospy.Time.from_sec(time.time()).to_sec() 189 if self.get_msg_t0(topic=topic) < 0 or self.get_msg_t0(topic=topic) > curr: 190 self.set_msg_t0(curr, topic=topic) 191 self.set_msg_tn(curr, topic=topic) 192 self.set_times([], topic=topic) 193 else: 194 self.get_times(topic=topic).append(curr - self.get_msg_tn(topic=topic)) 195 self.set_msg_tn(curr, topic=topic) 196 197 #only keep statistics for the last 10000 messages so as not to run out of memory 198 if len(self.get_times(topic=topic)) > self.window_size - 1: 199 self.get_times(topic=topic).pop(0)
200
201 - def get_hz(self, topic=None):
202 """ 203 calculate the average publising rate 204 205 @returns: tuple of stat results 206 (rate, min_delta, max_delta, standard deviation, window number) 207 None when waiting for the first message or there is no new one 208 """ 209 if not self.get_times(topic=topic): 210 return 211 elif self.get_msg_tn(topic=topic) == self.get_last_printed_tn(topic=topic): 212 return 213 with self.lock: 214 #frequency 215 216 # kwc: In the past, the rate decayed when a publisher 217 # dies. Now, we use the last received message to perform 218 # the calculation. This change was made because we now 219 # report a count and keep track of last_printed_tn. This 220 # makes it easier for users to see when a publisher dies, 221 # so the decay is no longer necessary. 222 223 n = len(self.get_times(topic=topic)) 224 #rate = (n - 1) / (rospy.get_time() - self.msg_t0) 225 mean = sum(self.get_times(topic=topic)) / n 226 rate = 1./mean if mean > 0. else 0 227 228 #std dev 229 std_dev = math.sqrt(sum((x - mean)**2 for x in self.get_times(topic=topic)) /n) 230 231 # min and max 232 max_delta = max(self.get_times(topic=topic)) 233 min_delta = min(self.get_times(topic=topic)) 234 235 self.set_last_printed_tn(self.get_msg_tn(topic=topic), topic=topic) 236 237 return rate, min_delta, max_delta, std_dev, n+1
238
239 - def print_hz(self, topics=(None,)):
240 """ 241 print the average publishing rate to screen 242 """ 243 if len(topics) == 1: 244 ret = self.get_hz(topics[0]) 245 if ret is None: 246 print("no new messages") 247 return 248 rate, min_delta, max_delta, std_dev, window = ret 249 print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, window)) 250 return 251 252 # monitoring multiple topics' hz 253 header = ['topic', 'rate', 'min_delta', 'max_delta', 'std_dev', 'window'] 254 stats = {h: [] for h in header} 255 for topic in topics: 256 hz_stat = self.get_hz(topic) 257 if hz_stat is None: 258 continue 259 rate, min_delta, max_delta, std_dev, window = hz_stat 260 stats['window'].append(str(window)) 261 stats['topic'].append(topic) 262 stats['rate'].append('{:.4}'.format(rate)) 263 stats['min_delta'].append('{:.4}'.format(min_delta)) 264 stats['max_delta'].append('{:.4}'.format(max_delta)) 265 stats['std_dev'].append('{:.4}'.format(std_dev)) 266 stats['window'].append(str(window)) 267 if not stats['topic']: 268 print('no new messages') 269 return 270 print(_get_ascii_table(header, stats))
271
272 -def _get_ascii_table(header, cols):
273 # compose table with left alignment 274 header_aligned = [] 275 col_widths = [] 276 for h in header: 277 col_width = max(len(h), max(len(el) for el in cols[h])) 278 col_widths.append(col_width) 279 header_aligned.append(h.center(col_width)) 280 for i, el in enumerate(cols[h]): 281 cols[h][i] = str(cols[h][i]).ljust(col_width) 282 # sum of col and each 3 spaces width 283 table_width = sum(col_widths) + 3 * (len(header) - 1) 284 n_rows = len(cols[header[0]]) 285 body = '\n'.join(' '.join(cols[h][i] for h in header) for i in xrange(n_rows)) 286 table = '{header}\n{hline}\n{body}\n'.format( 287 header=' '.join(header_aligned), hline='=' * table_width, body=body) 288 return table
289
290 -def _sleep(duration):
291 rospy.rostime.wallsleep(duration)
292
293 -def _rostopic_hz(topics, window_size=-1, filter_expr=None, use_wtime=False):
294 """ 295 Periodically print the publishing rate of a topic to console until 296 shutdown 297 :param topics: topic names, ``list`` of ``str`` 298 :param window_size: number of messages to average over, -1 for infinite, ``int`` 299 :param filter_expr: Python filter expression that is called with m, the message instance 300 """ 301 _check_master() 302 if rospy.is_shutdown(): 303 return 304 rospy.init_node(NAME, anonymous=True) 305 rt = ROSTopicHz(window_size, filter_expr=filter_expr, use_wtime=use_wtime) 306 for topic in topics: 307 msg_class, real_topic, _ = get_topic_class(topic, blocking=True) # pause hz until topic is published 308 # we use a large buffer size as we don't know what sort of messages we're dealing with. 309 # may parameterize this in the future 310 if filter_expr is not None: 311 # have to subscribe with topic_type 312 rospy.Subscriber(real_topic, msg_class, rt.callback_hz, callback_args=topic) 313 else: 314 rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback_hz, callback_args=topic) 315 print("subscribed to [%s]" % real_topic) 316 317 if rospy.get_param('use_sim_time', False): 318 print("WARNING: may be using simulated time",file=sys.stderr) 319 320 while not rospy.is_shutdown(): 321 _sleep(1.0) 322 rt.print_hz(topics)
323
324 -class ROSTopicDelay(object):
325
326 - def __init__(self, window_size):
327 import threading 328 self.lock = threading.Lock() 329 self.last_msg_tn = 0 330 self.msg_t0 = -1. 331 self.msg_tn = 0 332 self.delays = [] 333 334 # can't have infinite window size due to memory restrictions 335 if window_size < 0: 336 window_size = 50000 337 self.window_size = window_size
338
339 - def callback_delay(self, msg):
340 if not msg._has_header: 341 rospy.logerr('msg does not have header') 342 return 343 with self.lock: 344 curr_rostime = rospy.get_rostime() 345 346 # time reset 347 if curr_rostime.is_zero(): 348 if len(self.delays) > 0: 349 print("time has reset, resetting counters") 350 self.delays = [] 351 return 352 353 curr = curr_rostime.to_sec() 354 if self.msg_t0 < 0 or self.msg_t0 > curr: 355 self.msg_t0 = curr 356 self.msg_tn = curr 357 self.delays = [] 358 else: 359 self.delays.append(curr_rostime.to_time() - 360 msg.header.stamp.to_time()) 361 self.msg_tn = curr 362 363 if len(self.delays) > self.window_size - 1: 364 self.delays.pop(0)
365
366 - def get_delay(self):
367 if self.msg_tn == self.last_msg_tn: 368 return 369 with self.lock: 370 if not self.delays: 371 return 372 n = len(self.delays) 373 374 mean = sum(self.delays) / n 375 rate = 1. / mean if mean > 0 else 0 376 377 std_dev = math.sqrt(sum((x - mean)**2 for x in self.delays) / n) 378 379 max_delta = max(self.delays) 380 min_delta = min(self.delays) 381 382 self.last_msg_tn = self.msg_tn 383 return mean, min_delta, max_delta, std_dev, n + 1
384
385 - def print_delay(self):
386 """ 387 print the average publishing delay to screen 388 """ 389 if not self.delays: 390 return 391 ret = self.get_delay() 392 if ret is None: 393 print("no new messages") 394 return 395 delay, min_delta, max_delta, std_dev, window = ret 396 print("average delay: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(delay, min_delta, max_delta, std_dev, window))
397 398
399 -def _rostopic_delay(topic, window_size=-1):
400 """ 401 Periodically print the publishing delay of a topic to console until 402 shutdown 403 :param topic: topic name, ``str`` 404 :param window_size: number of messages to average over, -1 for infinite, ``int`` 405 """ 406 # pause hz until topic is published 407 msg_class, real_topic, _ = get_topic_class(topic, blocking=True) 408 if rospy.is_shutdown(): 409 return 410 rospy.init_node(NAME, anonymous=True) 411 rt = ROSTopicDelay(window_size) 412 sub = rospy.Subscriber(real_topic, msg_class, rt.callback_delay) 413 print("subscribed to [%s]" % real_topic) 414 415 if rospy.get_param('use_sim_time', False): 416 print("WARNING: may be using simulated time",file=sys.stderr) 417 418 while not rospy.is_shutdown(): 419 _sleep(1.0) 420 rt.print_delay()
421 422
423 -class ROSTopicBandwidth(object):
424 - def __init__(self, window_size=100):
425 import threading 426 self.lock = threading.Lock() 427 self.last_printed_tn = 0 428 self.sizes =[] 429 self.times =[] 430 self.window_size = window_size or 100
431
432 - def callback(self, data):
433 """ros sub callback""" 434 with self.lock: 435 try: 436 t = time.time() 437 self.times.append(t) 438 self.sizes.append(len(data._buff)) #AnyMsg instance 439 assert(len(self.times) == len(self.sizes)) 440 441 if len(self.times) > self.window_size: 442 self.times.pop(0) 443 self.sizes.pop(0) 444 except: 445 traceback.print_exc()
446
447 - def print_bw(self):
448 """print the average publishing rate to screen""" 449 if len(self.times) < 2: 450 return 451 with self.lock: 452 n = len(self.times) 453 tn = time.time() 454 t0 = self.times[0] 455 456 total = sum(self.sizes) 457 bytes_per_s = total / (tn - t0) 458 mean = total / n 459 460 #std_dev = math.sqrt(sum((x - mean)**2 for x in self.sizes) /n) 461 462 # min and max 463 max_s = max(self.sizes) 464 min_s = min(self.sizes) 465 466 #min/max and even mean are likely to be much smaller, but for now I prefer unit consistency 467 if bytes_per_s < 1000: 468 bw, mean, min_s, max_s = ["%.2fB"%v for v in [bytes_per_s, mean, min_s, max_s]] 469 elif bytes_per_s < 1000000: 470 bw, mean, min_s, max_s = ["%.2fKB"%(v/1000) for v in [bytes_per_s, mean, min_s, max_s]] 471 else: 472 bw, mean, min_s, max_s = ["%.2fMB"%(v/1000000) for v in [bytes_per_s, mean, min_s, max_s]] 473 474 print("average: %s/s\n\tmean: %s min: %s max: %s window: %s"%(bw, mean, min_s, max_s, n))
475
476 -def _isstring_type(t):
477 valid_types = [str] 478 try: 479 valid_types.append(unicode) 480 except NameError: 481 pass 482 return t in valid_types
483
484 -def _rostopic_bw(topic, window_size=-1):
485 """ 486 periodically print the received bandwidth of a topic to console until 487 shutdown 488 """ 489 _check_master() 490 _, real_topic, _ = get_topic_type(topic, blocking=True) #pause hz until topic is published 491 if rospy.is_shutdown(): 492 return 493 # #3543 disable all auto-subscriptions to /clock 494 rospy.init_node(NAME, anonymous=True, disable_rostime=True) 495 rt = ROSTopicBandwidth(window_size) 496 # we use a large buffer size as we don't know what sort of messages we're dealing with. 497 # may parameterize this in the future 498 sub = rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback) 499 print("subscribed to [%s]"%real_topic) 500 while not rospy.is_shutdown(): 501 _sleep(1.0) 502 rt.print_bw()
503 504 # code adapted from rqt_plot
505 -def msgevalgen(pattern):
506 """ 507 Generates a function that returns the relevant field(s) (aka 'subtopic(s)') of a Message object 508 :param pattern: subtopic, e.g. /x[2:]/y[:-1]/z, ``str`` 509 :returns: function that converts a message into the desired value, ``fn(Message) -> value`` 510 """ 511 evals = [] # list of (field_name, slice_object) pairs 512 fields = [f for f in pattern.split('/') if f] 513 for f in fields: 514 if '[' in f: 515 field_name, rest = f.split('[', 1) 516 if not rest.endswith(']'): 517 print("missing closing ']' in slice spec '%s'" % f, file=sys.stderr) 518 return None 519 rest = rest[:-1] # slice content, removing closing bracket 520 try: 521 array_index_or_slice_object = _get_array_index_or_slice_object(rest) 522 except AssertionError as e: 523 print("field '%s' has invalid slice argument '%s': %s" 524 % (field_name, rest, str(e)), file=sys.stderr) 525 return None 526 evals.append((field_name, array_index_or_slice_object)) 527 else: 528 evals.append((f, None)) 529 530 def msgeval(msg, evals): 531 for i, (field_name, slice_object) in enumerate(evals): 532 try: # access field first 533 msg = getattr(msg, field_name) 534 except AttributeError: 535 print("no field named %s in %s" % (field_name, pattern), file=sys.stderr) 536 return None 537 538 if slice_object is not None: # access slice 539 try: 540 msg = msg.__getitem__(slice_object) 541 except IndexError as e: 542 print("%s: %s" % (str(e), pattern), file=sys.stderr) 543 return None 544 545 # if a list is returned here (i.e. not only a single element accessed), 546 # we need to recursively call msg_eval() with the rest of evals 547 # in order to handle nested slices 548 if isinstance(msg, list): 549 rest = evals[i + 1:] 550 return [msgeval(m, rest) for m in msg] 551 return msg
552 553 return (lambda msg: msgeval(msg, evals)) if evals else None 554 555
556 -def _get_array_index_or_slice_object(index_string):
557 assert index_string != '', 'empty array index' 558 index_string_parts = index_string.split(':') 559 if len(index_string_parts) == 1: 560 try: 561 array_index = int(index_string_parts[0]) 562 except ValueError: 563 assert False, "non-integer array index step '%s'" % index_string_parts[0] 564 return array_index 565 566 slice_args = [None, None, None] 567 if index_string_parts[0] != '': 568 try: 569 slice_args[0] = int(index_string_parts[0]) 570 except ValueError: 571 assert False, "non-integer slice start '%s'" % index_string_parts[0] 572 if index_string_parts[1] != '': 573 try: 574 slice_args[1] = int(index_string_parts[1]) 575 except ValueError: 576 assert False, "non-integer slice stop '%s'" % index_string_parts[1] 577 if len(index_string_parts) > 2 and index_string_parts[2] != '': 578 try: 579 slice_args[2] = int(index_string_parts[2]) 580 except ValueError: 581 assert False, "non-integer slice step '%s'" % index_string_parts[2] 582 if len(index_string_parts) > 3: 583 assert False, 'too many slice arguments' 584 return slice(*slice_args)
585
586 -def _get_nested_attribute(msg, nested_attributes):
587 value = msg 588 for attr in nested_attributes.split('/'): 589 value = getattr(value, attr) 590 return value
591
592 -def _get_topic_type(topic):
593 """ 594 subroutine for getting the topic type 595 :returns: topic type, real topic name and fn to evaluate the message instance 596 if the topic points to a field within a topic, e.g. /rosout/msg, ``(str, str, fn)`` 597 """ 598 try: 599 val = _master_get_topic_types(rosgraph.Master('/rostopic')) 600 except socket.error: 601 raise ROSTopicIOException("Unable to communicate with master!") 602 603 # exact match first, followed by prefix match 604 matches = [(t, t_type) for t, t_type in val if t == topic] 605 if not matches: 606 matches = [(t, t_type) for t, t_type in val if topic.startswith(t+'/')] 607 # choose longest match 608 matches.sort(key=itemgetter(0), reverse=True) 609 610 # try to ignore messages which don't have the field specified as part of the topic name 611 while matches: 612 t, t_type = matches[0] 613 msg_class = roslib.message.get_message_class(t_type) 614 if not msg_class: 615 # if any class is not fetchable skip ignoring any message types 616 break 617 msg = msg_class() 618 nested_attributes = topic[len(t) + 1:].rstrip('/') 619 nested_attributes = nested_attributes.split('[')[0] 620 if nested_attributes == '': 621 break 622 try: 623 _get_nested_attribute(msg, nested_attributes) 624 except AttributeError: 625 # ignore this type since it does not have the requested field 626 matches.pop(0) 627 continue 628 matches = [(t, t_type)] 629 break 630 631 if matches: 632 t, t_type = matches[0] 633 if t_type == rosgraph.names.ANYTYPE: 634 return None, None, None 635 return t_type, t, msgevalgen(topic[len(t):]) 636 else: 637 return None, None, None
638 639 # NOTE: this is used externally by rxplot 640
641 -def get_topic_type(topic, blocking=False):
642 """ 643 Get the topic type. 644 645 :param topic: topic name, ``str`` 646 :param blocking: (default False) block until topic becomes available, ``bool`` 647 648 :returns: topic type, real topic name and fn to evaluate the message instance 649 if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)`` 650 :raises: :exc:`ROSTopicException` If master cannot be contacted 651 """ 652 topic_type, real_topic, msg_eval = _get_topic_type(topic) 653 if topic_type: 654 return topic_type, real_topic, msg_eval 655 elif blocking: 656 sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic) 657 while not rospy.is_shutdown(): 658 topic_type, real_topic, msg_eval = _get_topic_type(topic) 659 if topic_type: 660 return topic_type, real_topic, msg_eval 661 else: 662 _sleep(0.1) 663 return None, None, None
664
665 -def get_topic_class(topic, blocking=False):
666 """ 667 Get the topic message class 668 :returns: message class for topic, real topic 669 name, and function for evaluating message objects into the subtopic 670 (or ``None``). ``(Message, str, str)`` 671 :raises: :exc:`ROSTopicException` If topic type cannot be determined or loaded 672 """ 673 topic_type, real_topic, msg_eval = get_topic_type(topic, blocking=blocking) 674 if topic_type is None: 675 return None, None, None 676 msg_class = roslib.message.get_message_class(topic_type) 677 if not msg_class: 678 raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % topic_type) 679 return msg_class, real_topic, msg_eval
680
681 -def _str_plot_fields(val, f, field_filter):
682 """ 683 get CSV representation of fields used by _str_plot 684 :returns: list of fields as a CSV string, ``str`` 685 """ 686 s = _sub_str_plot_fields(val, f, field_filter) 687 if s is not None: 688 return "time,"+s 689 else: 690 return 'time,'
691
692 -def _sub_str_plot_fields(val, f, field_filter):
693 """recursive helper function for _str_plot_fields""" 694 # CSV 695 type_ = type(val) 696 if type_ in (bool, int, long, float) or \ 697 isinstance(val, genpy.TVal): 698 return f 699 # duck-type check for messages 700 elif hasattr(val, "_slot_types"): 701 if field_filter is not None: 702 fields = list(field_filter(val)) 703 else: 704 fields = val.__slots__ 705 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) 706 sub = [s for s in sub if s is not None] 707 if sub: 708 return ','.join([s for s in sub]) 709 elif _isstring_type(type_): 710 return f 711 elif type_ in (list, tuple): 712 if len(val) == 0: 713 return None 714 val0 = val[0] 715 type0 = type(val0) 716 # no arrays of arrays 717 if type0 in (bool, int, long, float) or \ 718 isinstance(val0, genpy.TVal): 719 return ','.join(["%s%s"%(f,x) for x in range(0,len(val))]) 720 elif _isstring_type(type0): 721 722 return ','.join(["%s%s"%(f,x) for x in range(0,len(val))]) 723 elif hasattr(val0, "_slot_types"): 724 labels = ["%s%s"%(f,x) for x in range(0,len(val))] 725 sub = [s for s in [_sub_str_plot_fields(v, sf, field_filter) for v,sf in zip(val, labels)] if s] 726 if sub: 727 return ','.join([s for s in sub]) 728 return None
729 730
731 -def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_information=None, fixed_numeric_width=None, value_transform=None):
732 """ 733 Convert value to matlab/octave-friendly CSV string representation. 734 735 :param val: message 736 :param current_time: current :class:`genpy.Time` to use if message does not contain its own timestamp. 737 :param time_offset: (optional) for time printed for message, print as offset against this :class:`genpy.Time` 738 :param field_filter: filter the fields that are stringified for Messages, ``fn(Message)->iter(str)`` 739 :param value_transform: Not used but for same API as CallbackEcho.custom_strify_message 740 :returns: comma-separated list of field values in val, ``str`` 741 """ 742 743 s = _sub_str_plot(val, time_offset, field_filter) 744 if s is None: 745 s = '' 746 747 if time_offset is not None: 748 time_offset = time_offset.to_nsec() 749 else: 750 time_offset = 0 751 752 if current_time is not None: 753 return "%s,%s"%(current_time.to_nsec()-time_offset, s) 754 elif getattr(val, "_has_header", False): 755 return "%s,%s"%(val.header.stamp.to_nsec()-time_offset, s) 756 else: 757 return "%s,%s"%(rospy.get_rostime().to_nsec()-time_offset, s)
758
759 -def _sub_str_plot(val, time_offset, field_filter):
760 """Helper routine for _str_plot.""" 761 # CSV 762 type_ = type(val) 763 764 if type_ == bool: 765 return '1' if val else '0' 766 elif type_ in (int, long, float) or \ 767 isinstance(val, genpy.TVal): 768 if time_offset is not None and isinstance(val, genpy.Time): 769 return str(val-time_offset) 770 else: 771 return str(val) 772 elif hasattr(val, "_slot_types"): 773 if field_filter is not None: 774 fields = list(field_filter(val)) 775 else: 776 fields = val.__slots__ 777 778 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) 779 sub = [s for s in sub if s is not None] 780 if sub: 781 return ','.join(sub) 782 elif _isstring_type(type_): 783 return val 784 elif type_ in (list, tuple): 785 if len(val) == 0: 786 return None 787 val0 = val[0] 788 # no arrays of arrays 789 type0 = type(val0) 790 if type0 == bool: 791 return ','.join([('1' if v else '0') for v in val]) 792 elif type0 in (int, long, float) or \ 793 isinstance(val0, genpy.TVal): 794 return ','.join([str(v) for v in val]) 795 elif _isstring_type(type0): 796 return ','.join([v for v in val]) 797 elif hasattr(val0, "_slot_types"): 798 sub = [s for s in [_sub_str_plot(v, time_offset, field_filter) for v in val] if s is not None] 799 if sub: 800 return ','.join([s for s in sub]) 801 return None
802 803 # copied from roslib.message
804 -def _convert_getattr(val, f, t):
805 """ 806 Convert atttribute types on the fly, if necessary. This is mainly 807 to convert uint8[] fields back to an array type. 808 """ 809 attr = getattr(val, f) 810 if _isstring_type(type(attr)) and 'uint8[' in t: 811 return [ord(x) for x in attr] 812 else: 813 return attr
814
815 -class CallbackEcho(object):
816 """ 817 Callback instance that can print callback data in a variety of 818 formats. Used for all variants of rostopic echo 819 """ 820
821 - def __init__(self, topic, msg_eval, plot=False, filter_fn=None, 822 echo_clear=False, echo_all_topics=False, 823 offset_time=False, count=None, 824 field_filter_fn=None, fixed_numeric_width=None, 825 value_transform_fn=None):
826 """ 827 :param plot: if ``True``, echo in plotting-friendly format (csv), ``bool`` 828 :param filter_fn: function that evaluates to ``True`` if message is to be echo'd, ``fn(topic, msg)`` 829 :param echo_all_topics: (optional) if ``True``, echo all messages in bag, ``bool`` 830 :param offset_time: (optional) if ``True``, display time as offset from current time, ``bool`` 831 :param count: number of messages to echo, ``None`` for infinite, ``int`` 832 :param field_filter_fn: filter the fields that are stringified for Messages, ``fn(Message)->iter(str)`` 833 :param fixed_numeric_width: fixed width for numeric values, ``None`` for automatic, ``int`` 834 :param value_transform_fn: transform the values of Messages, ``fn(Message)->Message`` 835 """ 836 if topic and topic[-1] == '/': 837 topic = topic[:-1] 838 self.topic = topic 839 self.msg_eval = msg_eval 840 self.plot = plot 841 self.filter_fn = filter_fn 842 self.fixed_numeric_width = fixed_numeric_width 843 844 self.prefix = '' 845 self.suffix = '\n---' if not plot else ''# same as YAML document separator, bug #3291 846 847 self.echo_all_topics = echo_all_topics 848 self.offset_time = offset_time 849 850 # done tracks when we've exceeded the count 851 self.done = False 852 self.max_count = count 853 self.count = 0 854 855 # determine which strifying function to use 856 if plot: 857 #TODOXXX: need to pass in filter function 858 self.str_fn = _str_plot 859 self.sep = '' 860 else: 861 #TODOXXX: need to pass in filter function 862 self.str_fn = self.custom_strify_message 863 if echo_clear: 864 self.prefix = '\033[2J\033[;H' 865 866 self.field_filter=field_filter_fn 867 self.value_transform=value_transform_fn 868 869 # first tracks whether or not we've printed anything yet. Need this for printing plot fields. 870 self.first = True 871 872 # cache 873 self.last_topic = None 874 self.last_msg_eval = None
875
876 - def custom_strify_message(self, val, indent='', time_offset=None, current_time=None, field_filter=None, 877 type_information=None, fixed_numeric_width=None, value_transform=None):
878 # ensure to print uint8[] as array of numbers instead of string 879 if type_information and type_information.startswith('uint8['): 880 val = [ord(x) for x in val] 881 if value_transform is not None: 882 val = value_transform(val, type_information) 883 return genpy.message.strify_message(val, indent=indent, time_offset=time_offset, current_time=current_time, field_filter=field_filter, fixed_numeric_width=fixed_numeric_width)
884
885 - def callback(self, data, callback_args, current_time=None):
886 """ 887 Callback to pass to rospy.Subscriber or to call 888 manually. rospy.Subscriber constructor must also pass in the 889 topic name as an additional arg 890 :param data: Message 891 :param topic: topic name, ``str`` 892 :param current_time: override calculation of current time, :class:`genpy.Time` 893 """ 894 topic = callback_args['topic'] 895 type_information = callback_args.get('type_information', None) 896 if self.filter_fn is not None and not self.filter_fn(data): 897 return 898 899 if self.max_count is not None and self.count >= self.max_count: 900 self.done = True 901 return 902 903 try: 904 msg_eval = self.msg_eval 905 if topic == self.topic: 906 pass 907 elif self.topic.startswith(topic + '/'): 908 # self.topic is actually a reference to topic field, generate msgeval 909 if topic == self.last_topic: 910 # use cached eval 911 msg_eval = self.last_msg_eval 912 else: 913 # generate msg_eval and cache 914 self.last_msg_eval = msg_eval = msgevalgen(self.topic[len(topic):]) 915 self.last_topic = topic 916 elif not self.echo_all_topics: 917 return 918 919 if msg_eval is not None: 920 data = msg_eval(data) 921 922 # data can be None if msg_eval returns None 923 if data is not None: 924 # NOTE: we do all prints using direct writes to sys.stdout, which works better with piping 925 926 self.count += 1 927 928 # print fields header for plot 929 if self.plot and self.first: 930 sys.stdout.write("%"+_str_plot_fields(data, 'field', self.field_filter)+'\n') 931 self.first = False 932 933 if self.offset_time: 934 sys.stdout.write(self.prefix+\ 935 self.str_fn(data, time_offset=rospy.get_rostime(), 936 current_time=current_time, field_filter=self.field_filter, 937 type_information=type_information, fixed_numeric_width=self.fixed_numeric_width, 938 value_transform=self.value_transform) + \ 939 self.suffix + '\n') 940 else: 941 sys.stdout.write(self.prefix+\ 942 self.str_fn(data, 943 current_time=current_time, field_filter=self.field_filter, 944 type_information=type_information, fixed_numeric_width=self.fixed_numeric_width, 945 value_transform=self.value_transform) + \ 946 self.suffix + '\n') 947 948 # we have to flush in order before piping to work 949 sys.stdout.flush() 950 # #2778 : have to check count after incr to set done flag 951 if self.max_count is not None and self.count >= self.max_count: 952 self.done = True 953 954 except IOError: 955 self.done = True 956 except: 957 # set done flag so we exit 958 self.done = True 959 traceback.print_exc()
960
961 -def _rostopic_type(topic):
962 """ 963 Print ROS message type of topic to screen 964 :param topic: topic name, ``str`` 965 """ 966 topic_type, topic_real_name, _ = get_topic_type(topic, blocking=False) 967 if topic_type is None: 968 sys.stderr.write('unknown topic type [%s]\n'%topic) 969 sys.exit(1) 970 elif topic == topic_real_name: 971 print(topic_type) 972 else: 973 field = topic[len(topic_real_name)+1:] 974 field_type = topic_type 975 for current_field in field.split('/'): 976 msg_class = roslib.message.get_message_class(field_type) 977 field_type = msg_class._slot_types[msg_class.__slots__.index(current_field)] 978 print('%s %s %s'%(topic_type, field, field_type))
979
980 -def _rostopic_echo_bag(callback_echo, bag_file):
981 """ 982 :param callback_echo: :class:`CallbackEcho` instance to invoke on new messages in bag file 983 :param bag_file: name of bag file to echo messages from or ``None``, ``str`` 984 """ 985 if not os.path.exists(bag_file): 986 raise ROSTopicException("bag file [%s] does not exist"%bag_file) 987 first = True 988 989 import rosbag 990 with rosbag.Bag(bag_file) as b: 991 for t, msg, timestamp in b.read_messages(): 992 # bag files can have relative paths in them, this respects any 993 # dynamic renaming 994 if t[0] != '/': 995 t = rosgraph.names.script_resolve_name('rostopic', t) 996 callback_echo.callback(msg, {'topic': t}, current_time=timestamp) 997 # done is set if there is a max echo count 998 if callback_echo.done: 999 break
1000
1001 -def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False):
1002 """ 1003 Print new messages on topic to screen. 1004 1005 :param topic: topic name, ``str`` 1006 :param bag_file: name of bag file to echo messages from or ``None``, ``str`` 1007 """ 1008 # we have to init a node regardless and bag echoing can print timestamps 1009 1010 if bag_file: 1011 # initialize rospy time due to potential timestamp printing 1012 rospy.rostime.set_rostime_initialized(True) 1013 _rostopic_echo_bag(callback_echo, bag_file) 1014 else: 1015 _check_master() 1016 rospy.init_node(NAME, anonymous=True) 1017 msg_class, real_topic, msg_eval = get_topic_class(topic, blocking=True) 1018 if msg_class is None: 1019 # occurs on ctrl-C 1020 return 1021 callback_echo.msg_eval = msg_eval 1022 1023 # extract type information for submessages 1024 type_information = None 1025 if len(topic) > len(real_topic): 1026 subtopic = topic[len(real_topic):] 1027 subtopic = subtopic.strip('/') 1028 if subtopic: 1029 fields = subtopic.split('/') 1030 submsg_class = msg_class 1031 while fields: 1032 field = fields[0].split('[')[0] 1033 del fields[0] 1034 index = submsg_class.__slots__.index(field) 1035 type_information = submsg_class._slot_types[index] 1036 if fields: 1037 submsg_class = roslib.message.get_message_class(type_information.split('[', 1)[0]) 1038 if not submsg_class: 1039 raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % type_information) 1040 1041 use_sim_time = rospy.get_param('/use_sim_time', False) 1042 sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, {'topic': topic, 'type_information': type_information}) 1043 1044 if use_sim_time: 1045 # #2950: print warning if nothing received for two seconds 1046 1047 timeout_t = time.time() + 2. 1048 while time.time() < timeout_t and \ 1049 callback_echo.count == 0 and \ 1050 not rospy.is_shutdown() and \ 1051 not callback_echo.done: 1052 _sleep(0.1) 1053 1054 if callback_echo.count == 0 and \ 1055 not rospy.is_shutdown() and \ 1056 not callback_echo.done: 1057 sys.stderr.write("WARNING: no messages received and simulated time is active.\nIs /clock being published?\n") 1058 1059 while not rospy.is_shutdown() and not callback_echo.done: 1060 _sleep(0.1)
1061 1062 _caller_apis = {}
1063 -def get_api(master, caller_id):
1064 """ 1065 Get XML-RPC API of node 1066 :param master: XML-RPC handle to ROS Master, :class:`xmlrpclib.ServerProxy` 1067 :param caller_id: node name, ``str`` 1068 :returns: XML-RPC URI of node, ``str`` 1069 :raises: :exc:`ROSTopicIOException` If unable to communicate with master 1070 """ 1071 caller_api = _caller_apis.get(caller_id, None) 1072 if not caller_api: 1073 try: 1074 caller_api = master.lookupNode(caller_id) 1075 _caller_apis[caller_id] = caller_api 1076 except socket.error: 1077 raise ROSTopicIOException("Unable to communicate with master!") 1078 except rosgraph.MasterError: 1079 caller_api = 'unknown address %s'%caller_id 1080 1081 return caller_api
1082
1083 -def _rostopic_list_bag(bag_file, topic=None):
1084 """ 1085 Prints topics in bag file to screen 1086 :param bag_file: path to bag file, ``str`` 1087 :param topic: optional topic name to match. Will print additional information just about messagese in this topic, ``str`` 1088 """ 1089 import rosbag 1090 if not os.path.exists(bag_file): 1091 raise ROSTopicException("bag file [%s] does not exist"%bag_file) 1092 1093 with rosbag.Bag(bag_file) as b: 1094 if topic: 1095 # create string for namespace comparison 1096 topic_ns = rosgraph.names.make_global_ns(topic) 1097 count = 0 1098 earliest = None 1099 latest = None 1100 for top, msg, t in b.read_messages(raw=True): 1101 if top == topic or top.startswith(topic_ns): 1102 count += 1 1103 if earliest == None: 1104 earliest = t 1105 1106 latest = t 1107 if rospy.is_shutdown(): 1108 break 1109 import time 1110 earliest, latest = [time.strftime("%d %b %Y %H:%M:%S", time.localtime(t.to_time())) for t in (earliest, latest)] 1111 print("%s message(s) from %s to %s"%(count, earliest, latest)) 1112 else: 1113 topics = set() 1114 for top, msg, _ in b.read_messages(raw=True): 1115 if top not in topics: 1116 print(top) 1117 topics.add(top) 1118 if rospy.is_shutdown(): 1119 break
1120
1121 -def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, verbose, indent=''):
1122 def topic_type(t, topic_types): 1123 matches = [t_type for t_name, t_type in topic_types if t_name == t] 1124 if matches: 1125 return matches[0] 1126 return 'unknown type'
1127 1128 if verbose: 1129 topic_types = _master_get_topic_types(master) 1130 1131 if not subscribers_only: 1132 print("\n%sPublished topics:"%indent) 1133 for t, l in pubs: 1134 if len(l) > 1: 1135 print(indent+" * %s [%s] %s publishers"%(t, topic_type(t, topic_types), len(l))) 1136 else: 1137 print(indent+" * %s [%s] 1 publisher"%(t, topic_type(t, topic_types))) 1138 1139 if not publishers_only: 1140 print(indent) 1141 print(indent+"Subscribed topics:") 1142 for t,l in subs: 1143 if len(l) > 1: 1144 print(indent+" * %s [%s] %s subscribers"%(t, topic_type(t, topic_types), len(l))) 1145 else: 1146 print(indent+" * %s [%s] 1 subscriber"%(t, topic_type(t, topic_types))) 1147 print('') 1148 else: 1149 if publishers_only: 1150 topics = [t for t,_ in pubs] 1151 elif subscribers_only: 1152 topics = [t for t,_ in subs] 1153 else: 1154 topics = list(set([t for t,_ in pubs] + [t for t,_ in subs])) 1155 topics.sort() 1156 print('\n'.join(["%s%s"%(indent, t) for t in topics])) 1157 1158 # #3145
1159 -def _rostopic_list_group_by_host(master, pubs, subs):
1160 """ 1161 Build up maps for hostname to topic list per hostname 1162 :returns: publishers host map, subscribers host map, ``{str: set(str)}, {str: set(str)}`` 1163 """ 1164 def build_map(master, state, uricache): 1165 tmap = {} 1166 for topic, tnodes in state: 1167 for p in tnodes: 1168 if not p in uricache: 1169 uricache[p] = master.lookupNode(p) 1170 uri = uricache[p] 1171 puri = urlparse(uri) 1172 if not puri.hostname in tmap: 1173 tmap[puri.hostname] = [] 1174 # recreate the system state data structure, but for a single host 1175 matches = [l for x, _, l in tmap[puri.hostname] if x == topic] 1176 if matches: 1177 matches[0].append(p) 1178 else: 1179 tmap[puri.hostname].append((topic, ttype, [p])) 1180 return tmap
1181 1182 uricache = {} 1183 host_pub_topics = build_map(master, pubs, uricache) 1184 host_sub_topics = build_map(master, subs, uricache) 1185 return host_pub_topics, host_sub_topics 1186
1187 -def _rostopic_list(topic, verbose=False, 1188 subscribers_only=False, publishers_only=False, 1189 group_by_host=False):
1190 """ 1191 Print topics to screen 1192 1193 :param topic: topic name to list information or None to match all topics, ``str`` 1194 :param verbose: print additional debugging information, ``bool`` 1195 :param subscribers_only: print information about subscriptions only, ``bool`` 1196 :param publishers_only: print information about subscriptions only, ``bool`` 1197 :param group_by_host: group topic list by hostname, ``bool`` 1198 """ 1199 # #1563 1200 if subscribers_only and publishers_only: 1201 raise ROSTopicException("cannot specify both subscribers- and publishers-only") 1202 1203 master = rosgraph.Master('/rostopic') 1204 try: 1205 state = master.getSystemState() 1206 1207 pubs, subs, _ = state 1208 if topic: 1209 # filter based on topic 1210 topic_ns = rosgraph.names.make_global_ns(topic) 1211 subs = (x for x in subs if x[0] == topic or x[0].startswith(topic_ns)) 1212 pubs = (x for x in pubs if x[0] == topic or x[0].startswith(topic_ns)) 1213 1214 except socket.error: 1215 raise ROSTopicIOException("Unable to communicate with master!") 1216 1217 if group_by_host: 1218 # #3145 1219 host_pub_topics, host_sub_topics = _rostopic_list_group_by_host(master, pubs, subs) 1220 for hostname in set(list(host_pub_topics.keys()) + list(host_sub_topics.keys())): #py3k 1221 pubs, subs = host_pub_topics.get(hostname,[]), host_sub_topics.get(hostname, []), 1222 if (pubs and not subscribers_only) or (subs and not publishers_only): 1223 print("Host [%s]:" % hostname) 1224 _sub_rostopic_list(master, pubs, subs, 1225 publishers_only, subscribers_only, 1226 verbose, indent=' ') 1227 else: 1228 _sub_rostopic_list(master, pubs, subs, 1229 publishers_only, subscribers_only, 1230 verbose)
1231
1232 -def get_info_text(topic):
1233 """ 1234 Get human-readable topic description 1235 1236 :param topic: topic name, ``str`` 1237 """ 1238 try: 1239 from cStringIO import StringIO 1240 except ImportError: 1241 from io import StringIO 1242 import itertools 1243 buff = StringIO() 1244 def topic_type(t, topic_types): 1245 matches = [t_type for t_name, t_type in topic_types if t_name == t] 1246 if matches: 1247 return matches[0] 1248 return 'unknown type'
1249 1250 master = rosgraph.Master('/rostopic') 1251 try: 1252 state = master.getSystemState() 1253 1254 pubs, subs, _ = state 1255 # filter based on topic 1256 subs = [x for x in subs if x[0] == topic] 1257 pubs = [x for x in pubs if x[0] == topic] 1258 1259 topic_types = _master_get_topic_types(master) 1260 1261 except socket.error: 1262 raise ROSTopicIOException("Unable to communicate with master!") 1263 1264 if not pubs and not subs: 1265 raise ROSTopicException("Unknown topic %s"%topic) 1266 1267 buff.write("Type: %s\n\n"%topic_type(topic, topic_types)) 1268 1269 if pubs: 1270 buff.write("Publishers: \n") 1271 for p in itertools.chain(*[l for x, l in pubs]): 1272 buff.write(" * %s (%s)\n"%(p, get_api(master, p))) 1273 else: 1274 buff.write("Publishers: None\n") 1275 buff.write('\n') 1276 1277 if subs: 1278 buff.write("Subscribers: \n") 1279 for p in itertools.chain(*[l for x, l in subs]): 1280 buff.write(" * %s (%s)\n"%(p, get_api(master, p))) 1281 else: 1282 buff.write("Subscribers: None\n") 1283 buff.write('\n') 1284 return buff.getvalue() 1285
1286 -def _rostopic_info(topic):
1287 """ 1288 Print topic information to screen. 1289 1290 :param topic: topic name, ``str`` 1291 """ 1292 print(get_info_text(topic))
1293 1294 ########################################################################################## 1295 # COMMAND PROCESSING ##################################################################### 1296
1297 -def _rostopic_cmd_echo(argv):
1298 def expr_eval(expr): 1299 def eval_fn(m): 1300 return eval(expr)
1301 return eval_fn 1302 1303 args = argv[2:] 1304 from optparse import OptionParser 1305 parser = OptionParser(usage="usage: %prog echo [options] /topic", prog=NAME) 1306 parser.add_option("-b", "--bag", 1307 dest="bag", default=None, 1308 help="echo messages from .bag file", metavar="BAGFILE") 1309 parser.add_option("-p", 1310 dest="plot", default=False, 1311 action="store_true", 1312 help="echo in a plotting friendly format") 1313 parser.add_option("-w", 1314 dest="fixed_numeric_width", default=None, metavar="NUM_WIDTH", 1315 help="fixed width for numeric values") 1316 parser.add_option("--filter", 1317 dest="filter_expr", default=None, 1318 metavar="FILTER-EXPRESSION", 1319 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).") 1320 parser.add_option("--nostr", 1321 dest="nostr", default=False, 1322 action="store_true", 1323 help="exclude string fields") 1324 parser.add_option("--noarr", 1325 dest="noarr", default=False, 1326 action="store_true", 1327 help="exclude arrays") 1328 parser.add_option("-c", "--clear", 1329 dest="clear", default=False, 1330 action="store_true", 1331 help="clear screen before printing next message") 1332 parser.add_option("-a", "--all", 1333 dest="all_topics", default=False, 1334 action="store_true", 1335 help="display all message in bag, only valid with -b option") 1336 parser.add_option("-n", 1337 dest="msg_count", default=None, metavar="COUNT", 1338 help="number of messages to echo") 1339 parser.add_option("--offset", 1340 dest="offset_time", default=False, 1341 action="store_true", 1342 help="display time as offsets from current time (in seconds)") 1343 1344 (options, args) = parser.parse_args(args) 1345 if len(args) > 1: 1346 parser.error("you may only specify one input topic") 1347 if options.all_topics and not options.bag: 1348 parser.error("Display all option is only valid when echoing from bag files") 1349 if options.offset_time and options.bag: 1350 parser.error("offset time option is not valid with bag files") 1351 if options.all_topics: 1352 topic = '' 1353 else: 1354 if len(args) == 0: 1355 parser.error("topic must be specified") 1356 topic = rosgraph.names.script_resolve_name('rostopic', args[0]) 1357 # suppressing output to keep it clean 1358 #if not options.plot: 1359 # print "rostopic: topic is [%s]"%topic 1360 1361 filter_fn = None 1362 if options.filter_expr: 1363 filter_fn = expr_eval(options.filter_expr) 1364 1365 try: 1366 msg_count = int(options.msg_count) if options.msg_count else None 1367 except ValueError: 1368 parser.error("COUNT must be an integer") 1369 1370 try: 1371 fixed_numeric_width = int(options.fixed_numeric_width) if options.fixed_numeric_width else None 1372 if fixed_numeric_width is not None and fixed_numeric_width < 2: 1373 parser.error("Fixed width for numeric values must be at least 2") 1374 except ValueError: 1375 parser.error("NUM_WIDTH must be an integer") 1376 1377 if options.plot: 1378 field_filter_fn = create_field_filter(options.nostr, options.noarr) 1379 value_transform_fn = None 1380 else: 1381 field_filter_fn = None 1382 value_transform_fn = create_value_transform(options.nostr, options.noarr) 1383 1384 callback_echo = CallbackEcho(topic, None, plot=options.plot, 1385 filter_fn=filter_fn, 1386 echo_clear=options.clear, echo_all_topics=options.all_topics, 1387 offset_time=options.offset_time, count=msg_count, 1388 field_filter_fn=field_filter_fn, 1389 value_transform_fn=value_transform_fn, 1390 fixed_numeric_width=fixed_numeric_width) 1391 try: 1392 _rostopic_echo(topic, callback_echo, bag_file=options.bag) 1393 except socket.error: 1394 sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") 1395
1396 -def create_value_transform(echo_nostr, echo_noarr):
1397 def value_transform(val, type_information=None): 1398 def transform_field_value(value, value_type, echo_nostr, echo_noarr): 1399 if echo_noarr and '[' in value_type: 1400 return '<array type: %s, length: %s>' % \ 1401 (value_type.rstrip('[]'), len(value)) 1402 elif echo_nostr and value_type == 'string': 1403 return '<string length: %s>' % len(value) 1404 elif echo_nostr and value_type == 'string[]': 1405 return '<array type: string, length: %s>' % len(value) 1406 return value
1407 1408 if not isinstance(val, genpy.Message): 1409 if type_information is None: 1410 return val 1411 return transform_field_value(val, type_information, 1412 echo_nostr, echo_noarr) 1413 1414 class TransformedMessage(genpy.Message): 1415 # These should be copy because changing these variables 1416 # in transforming is problematic without its untransforming. 1417 __slots__ = val.__slots__[:] 1418 _slot_types = val._slot_types[:] 1419 1420 val_trans = TransformedMessage() 1421 1422 fields = val.__slots__ 1423 field_types = val._slot_types 1424 for index, (f, t) in enumerate(zip(fields, field_types)): 1425 f_val = getattr(val, f) 1426 f_val_trans = transform_field_value(f_val, t, 1427 echo_nostr, echo_noarr) 1428 if f_val_trans != f_val: 1429 setattr(val_trans, f, f_val_trans) 1430 val_trans._slot_types[index] = 'string' 1431 else: 1432 try: 1433 msg_class = genpy.message.get_message_class(t) 1434 if msg_class is None: 1435 # happens for list of ROS messages like std_msgs/String[] 1436 raise ValueError 1437 nested_transformed = value_transform(f_val) 1438 setattr(val_trans, f, nested_transformed) 1439 except ValueError: 1440 setattr(val_trans, f, f_val) 1441 return val_trans 1442 return value_transform 1443
1444 -def create_field_filter(echo_nostr, echo_noarr):
1445 def field_filter(val): 1446 fields = val.__slots__ 1447 field_types = val._slot_types 1448 for f, t in zip(val.__slots__, val._slot_types): 1449 if echo_noarr and '[' in t: 1450 continue 1451 elif echo_nostr and 'string' in t: 1452 continue 1453 yield f
1454 return field_filter 1455
1456 -def _optparse_topic_only(cmd, argv):
1457 args = argv[2:] 1458 from optparse import OptionParser 1459 parser = OptionParser(usage="usage: %%prog %s /topic"%cmd, prog=NAME) 1460 (options, args) = parser.parse_args(args) 1461 if len(args) == 0: 1462 parser.error("topic must be specified") 1463 if len(args) > 1: 1464 parser.error("you may only specify one input topic") 1465 return rosgraph.names.script_resolve_name('rostopic', args[0])
1466
1467 -def _rostopic_cmd_type(argv):
1468 parser = argparse.ArgumentParser(prog='%s type' % NAME) 1469 parser.add_argument('topic_or_field', help='Topic or field name') 1470 args = parser.parse_args(argv[2:]) 1471 _rostopic_type(rosgraph.names.script_resolve_name('rostopic', args.topic_or_field))
1472
1473 -def _rostopic_cmd_hz(argv):
1474 args = argv[2:] 1475 from optparse import OptionParser 1476 parser = OptionParser(usage="usage: %prog hz [options] /topic_0 [/topic_1 [topic_2 [..]]]", prog=NAME) 1477 parser.add_option("-w", "--window", 1478 dest="window_size", default=-1, 1479 help="window size, in # of messages, for calculating rate", metavar="WINDOW") 1480 parser.add_option("--filter", 1481 dest="filter_expr", default=None, 1482 help="only measure messages matching the specified Python expression", metavar="EXPR") 1483 parser.add_option("--wall-time", 1484 dest="use_wtime", default=False, action="store_true", 1485 help="calculates rate using wall time which can be helpful when clock isnt published during simulation") 1486 1487 (options, args) = parser.parse_args(args) 1488 if len(args) == 0: 1489 parser.error("topic must be specified") 1490 try: 1491 window_size = int(options.window_size) 1492 except: 1493 parser.error("window size must be an integer") 1494 1495 topics = [rosgraph.names.script_resolve_name('rostopic', t) for t in args] 1496 1497 # #694 1498 if options.filter_expr: 1499 def expr_eval(expr): 1500 def eval_fn(m): 1501 return eval(expr)
1502 return eval_fn 1503 filter_expr = expr_eval(options.filter_expr) 1504 else: 1505 filter_expr = None 1506 _rostopic_hz(topics, window_size=window_size, filter_expr=filter_expr, 1507 use_wtime=options.use_wtime) 1508 1509
1510 -def _rostopic_cmd_delay(argv):
1511 args = argv[2:] 1512 import argparse 1513 parser = argparse.ArgumentParser(usage="%(prog)s delay [options] /topic", prog=NAME) 1514 parser.add_argument("topic", help="topic name to be calcurated the delay") 1515 parser.add_argument("-w", "--window", 1516 dest="window_size", default=-1, type=int, 1517 help="window size, in # of messages, for calculating rate") 1518 1519 args = parser.parse_args(args) 1520 topic_name = args.topic 1521 window_size = args.window_size 1522 topic = rosgraph.names.script_resolve_name('rostopic', topic_name) 1523 _rostopic_delay(topic, window_size=window_size)
1524 1525
1526 -def _rostopic_cmd_bw(argv=sys.argv):
1527 args = argv[2:] 1528 from optparse import OptionParser 1529 parser = OptionParser(usage="usage: %prog bw /topic", prog=NAME) 1530 parser.add_option("-w", "--window", 1531 dest="window_size", default=None, 1532 help="window size, in # of messages, for calculating rate", metavar="WINDOW") 1533 options, args = parser.parse_args(args) 1534 if len(args) == 0: 1535 parser.error("topic must be specified") 1536 if len(args) > 1: 1537 parser.error("you may only specify one input topic") 1538 try: 1539 window_size = int(options.window_size) if options.window_size is not None else None 1540 except: 1541 parser.error("window size must be an integer") 1542 topic = rosgraph.names.script_resolve_name('rostopic', args[0]) 1543 _rostopic_bw(topic, window_size=window_size)
1544
1545 -def find_by_type(topic_type):
1546 """ 1547 Lookup topics by topic_type 1548 :param topic_type: type of topic to find, ``str`` 1549 :returns: list of topic names that use topic_type, ``[str]`` 1550 """ 1551 master = rosgraph.Master('/rostopic') 1552 try: 1553 t_list = _master_get_topic_types(master) 1554 except socket.error: 1555 raise ROSTopicIOException("Unable to communicate with master!") 1556 return [t_name for t_name, t_type in t_list if t_type == topic_type]
1557
1558 -def _rostopic_cmd_find(argv=sys.argv):
1559 """ 1560 Implements 'rostopic type' 1561 :param argv: command-line args, ``[str]`` 1562 """ 1563 args = argv[2:] 1564 from optparse import OptionParser 1565 parser = OptionParser(usage="usage: %prog find msg-type", prog=NAME) 1566 options, args = parser.parse_args(args) 1567 if not len(args): 1568 parser.error("please specify a message type") 1569 if len(args) > 1: 1570 parser.error("you may only specify one message type") 1571 print('\n'.join(find_by_type(args[0])))
1572 1573
1574 -def _resource_name_package(name):
1575 """ 1576 pkg/typeName -> pkg, typeName -> None 1577 1578 :param name: package resource name, e.g. 'std_msgs/String', ``str`` 1579 :returns: package name of resource, ``str`` 1580 """ 1581 if not '/' in name: 1582 return None 1583 return name[:name.find('/')]
1584
1585 -def create_publisher(topic_name, topic_type, latch):
1586 """ 1587 Create rospy.Publisher instance from the string topic name and 1588 type. This is a powerful method as it allows creation of 1589 rospy.Publisher and Message instances using the topic and type 1590 names. This enables more dynamic publishing from Python programs. 1591 1592 :param topic_name: name of topic, ``str`` 1593 :param topic_type: name of topic type, ``str`` 1594 :param latch: latching topic, ``bool`` 1595 :returns: topic :class:`rospy.Publisher`, :class:`Message` class 1596 """ 1597 topic_name = rosgraph.names.script_resolve_name('rostopic', topic_name) 1598 try: 1599 msg_class = roslib.message.get_message_class(topic_type) 1600 except: 1601 raise ROSTopicException("invalid topic type: %s"%topic_type) 1602 if msg_class is None: 1603 pkg = _resource_name_package(topic_type) 1604 raise ROSTopicException("invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'"%(topic_type, pkg)) 1605 # disable /rosout and /rostime as this causes blips in the pubsub network due to rostopic pub often exiting quickly 1606 rospy.init_node('rostopic', anonymous=True, disable_rosout=True, disable_rostime=True) 1607 pub = rospy.Publisher(topic_name, msg_class, latch=latch, queue_size=100) 1608 return pub, msg_class
1609
1610 -def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, pub_args=None):
1611 """ 1612 Publish message at specified rate. Subroutine of L{publish_message()}. 1613 1614 :param pub: :class:rospy.Publisher` instance for topic 1615 :param msg: message instance to publish 1616 :param rate: publishing rate (hz) or None for just once, ``int`` 1617 :param verbose: If ``True``, print more verbose output to stdout, ``bool`` 1618 """ 1619 try: 1620 r = rospy.Rate(float(rate)) 1621 except ValueError: 1622 raise ROSTopicException("Rate must be a number") 1623 while not rospy.is_shutdown(): 1624 if substitute_keywords: 1625 _fillMessageArgs(msg, pub_args) 1626 if verbose: 1627 print("publishing %s"%msg) 1628 pub.publish(msg) 1629 r.sleep()
1630 1631 _ONCE_DELAY = 3.
1632 -def _publish_latched(pub, msg, once=False, verbose=False):
1633 """ 1634 Publish and latch message. Subroutine of L{publish_message()}. 1635 1636 :param pub: :class:`rospy.Publisher` instance for topic 1637 :param msg: message instance to publish 1638 :param once: if ``True``, publish message once and then exit after sleep interval, ``bool`` 1639 :param verbose: If ``True``, print more verbose output to stdout, ``bool`` 1640 """ 1641 try: 1642 pub.publish(msg) 1643 except TypeError as e: 1644 raise ROSTopicException(str(e)) 1645 1646 if not once: 1647 rospy.spin()
1648
1649 -def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=False, substitute_keywords=False):
1650 """ 1651 Create new instance of msg_class, populate with pub_args, and publish. This may 1652 print output to screen. 1653 1654 :param pub: :class:`rospy.Publisher` instance for topic 1655 :param msg_class: Message type, ``Class`` 1656 :param pub_args: Arguments to initialize message that is published, ``[val]`` 1657 :param rate: publishing rate (hz) or None for just once, ``int`` 1658 :param once: publish only once and return, ``bool`` 1659 :param verbose: If ``True``, print more verbose output to stdout, ``bool`` 1660 """ 1661 msg = msg_class() 1662 1663 _fillMessageArgs(msg, pub_args) 1664 1665 try: 1666 1667 if rate is None: 1668 s = "publishing and latching [%s]"%(msg) if verbose else "publishing and latching message" 1669 if once: 1670 s = s + " for %s seconds"%_ONCE_DELAY 1671 else: 1672 s = s + ". Press ctrl-C to terminate" 1673 print(s) 1674 1675 _publish_latched(pub, msg, once, verbose) 1676 else: 1677 _publish_at_rate(pub, msg, rate, verbose=verbose, substitute_keywords=substitute_keywords, pub_args=pub_args) 1678 1679 except rospy.ROSSerializationException as e: 1680 import rosmsg 1681 # we could just print the message definition, but rosmsg is more readable 1682 raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n"+\ 1683 " %s\n\nmsg file:\n%s"%(e, rosmsg.get_msg_text(msg_class._type)))
1684
1685 -def _fillMessageArgs(msg, pub_args):
1686 try: 1687 # Populate the message and enable substitution keys for 'now' 1688 # and 'auto'. There is a corner case here: this logic doesn't 1689 # work if you're publishing a Header only and wish to use 1690 # 'auto' with it. This isn't a troubling case, but if we start 1691 # allowing more keys in the future, it could become an actual 1692 # use case. It greatly complicates logic because we'll have to 1693 # do more reasoning over types. to avoid ambiguous cases 1694 # (e.g. a std_msgs/String type, which only has a single string 1695 # field). 1696 1697 # allow the use of the 'now' string with timestamps and 'auto' with header 1698 now = rospy.get_rostime() 1699 import std_msgs.msg 1700 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) } 1701 genpy.message.fill_message_args(msg, pub_args, keys=keys) 1702 except genpy.MessageException as e: 1703 raise ROSTopicException(str(e)+"\n\nArgs are: [%s]"%genpy.message.get_printable_message_args(msg))
1704
1705 -def _rostopic_cmd_pub(argv):
1706 """ 1707 Parse 'pub' command arguments and run command. Will cause a system 1708 exit if command-line argument parsing fails. 1709 :param argv: command-line arguments 1710 :param argv: [str] 1711 :raises: :exc:`ROSTopicException` If call command cannot be executed 1712 """ 1713 args = argv[2:] 1714 from optparse import OptionParser 1715 parser = OptionParser(usage="usage: %prog pub /topic type [args...]", prog=NAME) 1716 parser.add_option("-v", dest="verbose", default=False, 1717 action="store_true", 1718 help="print verbose output") 1719 parser.add_option("-r", "--rate", dest="rate", default=None, 1720 help="publishing rate (hz). For -f and stdin input, this defaults to 10. Otherwise it is not set.") 1721 parser.add_option("-1", "--once", action="store_true", dest="once", default=False, 1722 help="publish one message and exit") 1723 parser.add_option("-f", '--file', dest="file", metavar='FILE', default=None, 1724 help="read args from YAML file (Bagy)") 1725 parser.add_option("-l", '--latch', dest="latch", default=False, action="store_true", 1726 help="enable latching for -f, -r and piped input. This latches the first message.") 1727 parser.add_option("-s", '--substitute-keywords', dest="substitute_keywords", default=False, action="store_true", 1728 help="When publishing with a rate, performs keyword ('now' or 'auto') substitution for each message") 1729 #parser.add_option("-p", '--param', dest="parameter", metavar='/PARAM', default=None, 1730 # help="read args from ROS parameter (Bagy format)") 1731 1732 (options, args) = parser.parse_args(args) 1733 if options.rate is not None: 1734 if options.once: 1735 parser.error("You cannot select both -r and -1 (--once)") 1736 try: 1737 rate = float(options.rate) 1738 except ValueError: 1739 parser.error("rate must be a number") 1740 if rate <= 0: 1741 parser.error("rate must be greater than zero") 1742 else: 1743 # we will default this to 10 for file/stdin later 1744 rate = None 1745 1746 # validate args len 1747 if len(args) == 0: 1748 parser.error("/topic must be specified") 1749 if len(args) == 1: 1750 parser.error("topic type must be specified") 1751 if 0: 1752 if len(args) > 2 and options.parameter: 1753 parser.error("args confict with -p setting") 1754 if len(args) > 2 and options.file: 1755 parser.error("args confict with -f setting") 1756 topic_name, topic_type = args[0], args[1] 1757 1758 # type-case using YAML 1759 try: 1760 pub_args = [] 1761 for arg in args[2:]: 1762 pub_args.append(yaml.load(arg)) 1763 except Exception as e: 1764 parser.error("Argument error: "+str(e)) 1765 1766 # make sure master is online. we wait until after we've parsed the 1767 # args to do this so that syntax errors are reported first 1768 _check_master() 1769 1770 # if no rate, or explicit latch, we latch 1771 latch = (rate == None) or options.latch 1772 pub, msg_class = create_publisher(topic_name, topic_type, latch) 1773 1774 if 0 and options.parameter: 1775 param_name = rosgraph.names.script_resolve_name('rostopic', options.parameter) 1776 if options.once: 1777 param_publish_once(pub, msg_class, param_name, rate, options.verbose) 1778 else: 1779 param_publish(pub, msg_class, param_name, rate, options.verbose) 1780 1781 elif not pub_args and len(msg_class.__slots__): 1782 if not options.file and sys.stdin.isatty(): 1783 parser.error("Please specify message values") 1784 # stdin/file input has a rate by default 1785 if rate is None and not options.latch and not options.once: 1786 rate = 10. 1787 stdin_publish(pub, msg_class, rate, options.once, options.file, options.verbose) 1788 else: 1789 argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, substitute_keywords=options.substitute_keywords)
1790 1791
1792 -def file_yaml_arg(filename):
1793 """ 1794 :param filename: file name, ``str`` 1795 :returns: Iterator that yields pub args (list of args), ``iterator`` 1796 :raises: :exc:`ROSTopicException` If filename is invalid 1797 """ 1798 if not os.path.isfile(filename): 1799 raise ROSTopicException("file does not exist: %s"%(filename)) 1800 import yaml 1801 def bagy_iter(): 1802 try: 1803 with open(filename, 'r') as f: 1804 # load all documents 1805 data = yaml.load_all(f) 1806 for d in data: 1807 yield [d] 1808 except yaml.YAMLError as e: 1809 raise ROSTopicException("invalid YAML in file: %s"%(str(e)))
1810 return bagy_iter 1811
1812 -def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywords=False):
1813 publish_message(pub, msg_class, pub_args, rate, once, verbose=verbose, substitute_keywords=substitute_keywords) 1814 1815 if once: 1816 # stick around long enough for others to grab 1817 timeout_t = time.time() + _ONCE_DELAY 1818 while not rospy.is_shutdown() and time.time() < timeout_t: 1819 rospy.sleep(0.2)
1820 1821 SUBSCRIBER_TIMEOUT = 5.
1822 -def wait_for_subscriber(pub, timeout):
1823 timeout_t = time.time() + timeout 1824 while pub.get_num_connections() == 0 and timeout_t > time.time(): 1825 _sleep(0.01)
1826
1827 -def param_publish_once(pub, msg_class, param_name, verbose):
1828 if not rospy.has_param(param_name): 1829 raise ROSTopicException("parameter does not exist: %s"%(param_name)) 1830 pub_args = rospy.get_param(param_name) 1831 argv_publish(pub, msg_class, pub_args, None, True, verbose)
1832 1833
1834 -class _ParamNotifier(object):
1835
1836 - def __init__(self, param_name, value=None):
1837 import threading 1838 self.lock = threading.Condition() 1839 self.param_name = param_name 1840 self.updates = [] 1841 self.value = None
1842
1843 - def __call__(self, key, value):
1844 with self.lock: 1845 # have to address downward if we got notification on sub namespace 1846 if key != self.param_name: 1847 subs = [x for x in key[len(self.param_name):].split('/') if x] 1848 idx = self.value 1849 for s in subs[:-1]: 1850 if s in idx: 1851 idx = idx[s] 1852 else: 1853 idx[s] = {} 1854 idx = idx[s] 1855 idx[subs[-1]] = value 1856 else: 1857 self.value = value 1858 1859 self.updates.append(self.value) 1860 self.lock.notify_all()
1861
1862 -def param_publish(pub, msg_class, param_name, rate, verbose):
1863 """ 1864 :param param_name: ROS parameter name, ``str`` 1865 :returns: List of msg dicts in file, ``[{str: any}]`` 1866 :raises: :exc:`ROSTopicException` If parameter is not set 1867 """ 1868 import rospy 1869 import rospy.impl.paramserver 1870 import rosgraph 1871 1872 if not rospy.has_param(param_name): 1873 raise ROSTopicException("parameter does not exist: %s"%(param_name)) 1874 1875 # reach deep into subscription APIs here. Very unstable stuff 1876 # here, don't copy elsewhere! 1877 ps_cache = rospy.impl.paramserver.get_param_server_cache() 1878 notifier = _ParamNotifier(param_name) 1879 ps_cache.set_notifier(notifier) 1880 master = rosgraph.Master(rospy.get_name()) 1881 notifier.value = master.subscribeParam(rospy.get_node_uri(), param_name) 1882 pub_args = notifier.value 1883 ps_cache.set(param_name, pub_args) 1884 if type(pub_args) == dict: 1885 pub_args = [pub_args] 1886 elif type(pub_args) != list: 1887 raise ROSTopicException("Parameter [%s] in not a valid type"%(param_name)) 1888 1889 r = rospy.Rate(rate) if rate is not None else None 1890 publish = True 1891 while not rospy.is_shutdown(): 1892 try: 1893 if publish: 1894 publish_message(pub, msg_class, pub_args, None, True, verbose=verbose) 1895 except ValueError as e: 1896 sys.stderr.write("%s\n"%str(e)) 1897 break 1898 if r is not None: 1899 r.sleep() 1900 with notifier.lock: 1901 if notifier.updates: 1902 pub_args = notifier.updates.pop(0) 1903 if type(pub_args) == dict: 1904 pub_args = [pub_args] 1905 else: 1906 publish = False 1907 with notifier.lock: 1908 if not notifier.updates: 1909 notifier.lock.wait(1.) 1910 if notifier.updates: 1911 publish = True 1912 pub_args = notifier.updates.pop(0) 1913 if type(pub_args) == dict: 1914 pub_args = [pub_args] 1915 1916 if rospy.is_shutdown(): 1917 break
1918
1919 -def stdin_publish(pub, msg_class, rate, once, filename, verbose):
1920 """ 1921 :param filename: name of file to read from instead of stdin, or ``None``, ``str`` 1922 """ 1923 if filename: 1924 iterator = file_yaml_arg(filename) 1925 else: 1926 iterator = stdin_yaml_arg 1927 1928 r = rospy.Rate(rate) if rate is not None else None 1929 1930 # stdin publishing can happen really fast, especially if no rate 1931 # is set, so try to make sure someone is listening before we 1932 # publish, though we don't wait too long. 1933 wait_for_subscriber(pub, SUBSCRIBER_TIMEOUT) 1934 1935 for pub_args in iterator(): 1936 if rospy.is_shutdown(): 1937 break 1938 if pub_args: 1939 if type(pub_args) != list: 1940 pub_args = [pub_args] 1941 try: 1942 # we use 'bool(r) or once' for the once value, which 1943 # controls whether or not publish_message blocks and 1944 # latches until exit. We want to block if the user 1945 # has enabled latching (i.e. rate is none). It would 1946 # be good to reorganize this code more conceptually 1947 # but, for now, this is the best re-use of the 1948 # underlying methods. 1949 publish_message(pub, msg_class, pub_args, None, bool(r) or once, verbose=verbose) 1950 except ValueError as e: 1951 sys.stderr.write("%s\n"%str(e)) 1952 break 1953 if r is not None: 1954 r.sleep() 1955 if rospy.is_shutdown() or once: 1956 break
1957
1958 -def stdin_yaml_arg():
1959 """ 1960 Iterate over YAML documents in stdin 1961 :returns: for next list of arguments on stdin. Iterator returns a list of args for each call, ``iterator`` 1962 """ 1963 import yaml 1964 from select import select 1965 from select import error as select_error 1966 try: 1967 arg = 'x' 1968 rlist = [sys.stdin] 1969 wlist = xlist = [] 1970 while not rospy.is_shutdown() and arg != '\n': 1971 buff = '' 1972 while arg != '' and arg.strip() != '---' and not rospy.is_shutdown(): 1973 val, _, _ = select(rlist, wlist, xlist, 1.0) 1974 if not val: 1975 continue 1976 # sys.stdin.readline() returns empty string on EOF 1977 arg = sys.stdin.readline() 1978 if arg != '' and arg.strip() != '---': 1979 buff = buff + arg 1980 1981 if arg.strip() == '---': # End of document 1982 try: 1983 loaded = yaml.load(buff.rstrip()) 1984 except Exception as e: 1985 sys.stderr.write("Invalid YAML: %s\n"%str(e)) 1986 if loaded is not None: 1987 yield loaded 1988 elif arg == '': #EOF 1989 # we don't yield the remaining buffer in this case 1990 # because we don't want to publish partial inputs 1991 return 1992 1993 arg = 'x' # reset 1994 1995 except select_error: 1996 return # most likely ctrl-c interrupt
1997
1998 -def _rostopic_cmd_list(argv):
1999 """ 2000 Command-line parsing for 'rostopic list' command. 2001 """ 2002 args = argv[2:] 2003 from optparse import OptionParser 2004 parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME) 2005 parser.add_option("-b", "--bag", 2006 dest="bag", default=None, 2007 help="list topics in .bag file", metavar="BAGFILE") 2008 parser.add_option("-v", "--verbose", 2009 dest="verbose", default=False,action="store_true", 2010 help="list full details about each topic") 2011 parser.add_option("-p", 2012 dest="publishers", default=False,action="store_true", 2013 help="list only publishers") 2014 parser.add_option("-s", 2015 dest="subscribers", default=False,action="store_true", 2016 help="list only subscribers") 2017 parser.add_option("--host", dest="hostname", default=False, action="store_true", 2018 help="group by host name") 2019 2020 (options, args) = parser.parse_args(args) 2021 topic = None 2022 2023 if len(args) == 1: 2024 topic = rosgraph.names.script_resolve_name('rostopic', args[0]) 2025 elif len(args) > 1: 2026 parser.error("you may only specify one input topic") 2027 if options.bag: 2028 if options.subscribers: 2029 parser.error("-s option is not valid with bags") 2030 elif options.publishers: 2031 parser.error("-p option is not valid with bags") 2032 elif options.hostname: 2033 parser.error("--host option is not valid with bags") 2034 _rostopic_list_bag(options.bag, topic) 2035 else: 2036 if options.subscribers and options.publishers: 2037 parser.error("you may only specify one of -p, -s") 2038 2039 exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, publishers_only=options.publishers, group_by_host=options.hostname) or 0 2040 if exitval != 0: 2041 sys.exit(exitval)
2042
2043 -def _rostopic_cmd_info(argv):
2044 """ 2045 Command-line parsing for 'rostopic info' command. 2046 """ 2047 args = argv[2:] 2048 from optparse import OptionParser 2049 parser = OptionParser(usage="usage: %prog info /topic", prog=NAME) 2050 (options, args) = parser.parse_args(args) 2051 2052 if len(args) == 0: 2053 parser.error("you must specify a topic name") 2054 elif len(args) > 1: 2055 parser.error("you may only specify one topic name") 2056 2057 topic = rosgraph.names.script_resolve_name('rostopic', args[0]) 2058 exitval = _rostopic_info(topic) or 0 2059 if exitval != 0: 2060 sys.exit(exitval)
2061
2062 -def _fullusage():
2063 print("""rostopic is a command-line tool for printing information about ROS Topics. 2064 2065 Commands: 2066 \trostopic bw\tdisplay bandwidth used by topic 2067 \trostopic delay\tdisplay delay of topic from timestamp in header 2068 \trostopic echo\tprint messages to screen 2069 \trostopic find\tfind topics by type 2070 \trostopic hz\tdisplay publishing rate of topic 2071 \trostopic info\tprint information about active topic 2072 \trostopic list\tlist active topics 2073 \trostopic pub\tpublish data to topic 2074 \trostopic type\tprint topic or field type 2075 2076 Type rostopic <command> -h for more detailed usage, e.g. 'rostopic echo -h' 2077 """) 2078 sys.exit(getattr(os, 'EX_USAGE', 1))
2079
2080 -def rostopicmain(argv=None):
2081 import rosbag 2082 if argv is None: 2083 argv=sys.argv 2084 # filter out remapping arguments in case we are being invoked via roslaunch 2085 argv = rospy.myargv(argv) 2086 2087 # process argv 2088 if len(argv) == 1: 2089 _fullusage() 2090 try: 2091 command = argv[1] 2092 if command == 'echo': 2093 _rostopic_cmd_echo(argv) 2094 elif command == 'hz': 2095 _rostopic_cmd_hz(argv) 2096 elif command == 'type': 2097 _rostopic_cmd_type(argv) 2098 elif command == 'list': 2099 _rostopic_cmd_list(argv) 2100 elif command == 'info': 2101 _rostopic_cmd_info(argv) 2102 elif command == 'pub': 2103 _rostopic_cmd_pub(argv) 2104 elif command == 'bw': 2105 _rostopic_cmd_bw(argv) 2106 elif command == 'find': 2107 _rostopic_cmd_find(argv) 2108 elif command == 'delay': 2109 _rostopic_cmd_delay(argv) 2110 else: 2111 _fullusage() 2112 except socket.error: 2113 sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n") 2114 sys.exit(1) 2115 except rosbag.ROSBagException as e: 2116 sys.stderr.write("ERROR: unable to use bag file: %s\n"%str(e)) 2117 sys.exit(1) 2118 except rosgraph.MasterException as e: 2119 # mainly for invalid master URI/rosgraph.masterapi 2120 sys.stderr.write("ERROR: %s\n"%str(e)) 2121 sys.exit(1) 2122 except ROSTopicException as e: 2123 sys.stderr.write("ERROR: %s\n"%str(e)) 2124 sys.exit(1) 2125 except KeyboardInterrupt: pass 2126 except rospy.ROSInterruptException: pass
2127