1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
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
65 import rospy
66
67 try:
68 long
69 except NameError:
70 long = int
71
72
74 """
75 Base exception class of rostopic-related errors
76 """
77 pass
79 """
80 rostopic errors related to network I/O failures
81 """
82 pass
83
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
95 try:
96 val = master.getTopicTypes()
97 except Fault:
98
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
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
123 if window_size < 0:
124 window_size = 50000
125 self.window_size = window_size
126
128 if topic is None:
129 return self.last_printed_tn
130 return self._last_printed_tn[topic]
131
133 if topic is None:
134 self.last_printed_tn = value
135 self._last_printed_tn[topic] = value
136
138 if topic is None:
139 return self.msg_t0
140 return self._msg_t0[topic]
141
143 if topic is None:
144 self.msg_t0 = value
145 self._msg_t0[topic] = value
146
148 if topic is None:
149 return self.msg_tn
150 return self._msg_tn[topic]
151
153 if topic is None:
154 self.msg_tn = value
155 self._msg_tn[topic] = value
156
158 if topic is None:
159 return self.times
160 return self._times[topic]
161
163 if topic is None:
164 self.times = value
165 self._times[topic] = value
166
168 """
169 ros sub callback
170 :param m: Message instance
171 :param topic: Topic name
172 """
173
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
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
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
215
216
217
218
219
220
221
222
223 n = len(self.get_times(topic=topic))
224
225 mean = sum(self.get_times(topic=topic)) / n
226 rate = 1./mean if mean > 0. else 0
227
228
229 std_dev = math.sqrt(sum((x - mean)**2 for x in self.get_times(topic=topic)) /n)
230
231
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
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
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
273
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
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
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)
308
309
310 if filter_expr is not None:
311
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
325
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
335 if window_size < 0:
336 window_size = 50000
337 self.window_size = window_size
338
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
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
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
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
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
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
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
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))
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
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
461
462
463 max_s = max(self.sizes)
464 min_s = min(self.sizes)
465
466
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))
477 valid_types = [str]
478 try:
479 valid_types.append(unicode)
480 except NameError:
481 pass
482 return t in valid_types
483
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)
491 if rospy.is_shutdown():
492 return
493
494 rospy.init_node(NAME, anonymous=True, disable_rostime=True)
495 rt = ROSTopicBandwidth(window_size)
496
497
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
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 = []
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]
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:
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:
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
546
547
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
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
587 value = msg
588 for attr in nested_attributes.split('/'):
589 value = getattr(value, attr)
590 return value
591
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
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
608 matches.sort(key=itemgetter(0), reverse=True)
609
610
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
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
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
640
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
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
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
693 """recursive helper function for _str_plot_fields"""
694
695 type_ = type(val)
696 if type_ in (bool, int, long, float) or \
697 isinstance(val, genpy.TVal):
698 return f
699
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
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
760 """Helper routine for _str_plot."""
761
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
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
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
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 ''
846
847 self.echo_all_topics = echo_all_topics
848 self.offset_time = offset_time
849
850
851 self.done = False
852 self.max_count = count
853 self.count = 0
854
855
856 if plot:
857
858 self.str_fn = _str_plot
859 self.sep = ''
860 else:
861
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
870 self.first = True
871
872
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
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
909 if topic == self.last_topic:
910
911 msg_eval = self.last_msg_eval
912 else:
913
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
923 if data is not None:
924
925
926 self.count += 1
927
928
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
949 sys.stdout.flush()
950
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
958 self.done = True
959 traceback.print_exc()
960
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
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
993
994 if t[0] != '/':
995 t = rosgraph.names.script_resolve_name('rostopic', t)
996 callback_echo.callback(msg, {'topic': t}, current_time=timestamp)
997
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
1009
1010 if bag_file:
1011
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
1020 return
1021 callback_echo.msg_eval = msg_eval
1022
1023
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
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 = {}
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
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
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
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
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
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
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
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())):
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
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
1287 """
1288 Print topic information to screen.
1289
1290 :param topic: topic name, ``str``
1291 """
1292 print(get_info_text(topic))
1293
1294
1295
1296
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
1358
1359
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
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
1416
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
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
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
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
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
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
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
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
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
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
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
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
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
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.
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
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
1686 try:
1687
1688
1689
1690
1691
1692
1693
1694
1695
1696
1697
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
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
1730
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
1744 rate = None
1745
1746
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
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
1767
1768 _check_master()
1769
1770
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
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
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
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
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.
1823 timeout_t = time.time() + timeout
1824 while pub.get_num_connections() == 0 and timeout_t > time.time():
1825 _sleep(0.01)
1826
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
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
1844 with self.lock:
1845
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
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
1876
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
1931
1932
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
1943
1944
1945
1946
1947
1948
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
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
1977 arg = sys.stdin.readline()
1978 if arg != '' and arg.strip() != '---':
1979 buff = buff + arg
1980
1981 if arg.strip() == '---':
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 == '':
1989
1990
1991 return
1992
1993 arg = 'x'
1994
1995 except select_error:
1996 return
1997
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
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
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
2085 argv = rospy.myargv(argv)
2086
2087
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
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