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