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, 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)
309
310
311 if filter_expr is not None:
312
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
326
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
336 if window_size < 0:
337 window_size = 50000
338 self.window_size = window_size
339
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
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
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
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
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
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
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
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))
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
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
463
464
465 max_s = max(self.sizes)
466 min_s = min(self.sizes)
467
468
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))
479 valid_types = [str]
480 try:
481 valid_types.append(unicode)
482 except NameError:
483 pass
484 return t in valid_types
485
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)
493 if rospy.is_shutdown():
494 return
495
496 rospy.init_node(NAME, anonymous=True, disable_rostime=True)
497 rt = ROSTopicBandwidth(window_size)
498
499
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
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 = []
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]
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:
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:
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
548
549
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
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
589 value = msg
590 for attr in nested_attributes.split('/'):
591 value = getattr(value, attr)
592 return value
593
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
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
610 matches.sort(key=itemgetter(0), reverse=True)
611
612
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
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
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
642
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
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
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
695 """recursive helper function for _str_plot_fields"""
696
697 type_ = type(val)
698 if type_ in (bool, int, long, float) or \
699 isinstance(val, genpy.TVal):
700 return f
701
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
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
762 """Helper routine for _str_plot."""
763
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
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
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
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 ''
848
849 self.echo_all_topics = echo_all_topics
850 self.offset_time = offset_time
851
852
853 self.done = False
854 self.max_count = count
855 self.count = 0
856
857
858 if plot:
859
860 self.str_fn = _str_plot
861 self.sep = ''
862 else:
863
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
872 self.first = True
873
874
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
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
911 if topic == self.last_topic:
912
913 msg_eval = self.last_msg_eval
914 else:
915
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
925 if data is not None:
926
927
928 self.count += 1
929
930
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
951 sys.stdout.flush()
952
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
960 self.done = True
961 traceback.print_exc()
962
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
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
995
996 if t[0] != '/':
997 t = rosgraph.names.script_resolve_name('rostopic', t)
998 callback_echo.callback(msg, {'topic': t}, current_time=timestamp)
999
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
1011
1012 if bag_file:
1013
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
1022 return
1023 callback_echo.msg_eval = msg_eval
1024
1025
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
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 = {}
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
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
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
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
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
1176 return (pubs_out, subs_out)
1177
1178
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
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, [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
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
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())):
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
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
1301 """
1302 Print topic information to screen.
1303
1304 :param topic: topic name, ``str``
1305 """
1306 print(get_info_text(topic))
1307
1308
1309
1310
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
1372
1373
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
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
1430
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
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
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
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
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
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
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
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
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
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
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
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
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 :returns: topic :class:`rospy.Publisher`, :class:`Message` class
1616 """
1617 topic_name = rosgraph.names.script_resolve_name('rostopic', topic_name)
1618 try:
1619 msg_class = roslib.message.get_message_class(topic_type)
1620 except:
1621 raise ROSTopicException("invalid topic type: %s"%topic_type)
1622 if msg_class is None:
1623 pkg = _resource_name_package(topic_type)
1624 raise ROSTopicException("invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'"%(topic_type, pkg))
1625
1626 rospy.init_node('rostopic', anonymous=True, disable_rosout=True, disable_rostime=True)
1627 pub = rospy.Publisher(topic_name, msg_class, latch=latch, queue_size=100)
1628 return pub, msg_class
1629
1630 -def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, pub_args=None):
1631 """
1632 Publish message at specified rate. Subroutine of L{publish_message()}.
1633
1634 :param pub: :class:rospy.Publisher` instance for topic
1635 :param msg: message instance to publish
1636 :param rate: publishing rate (hz) or None for just once, ``int``
1637 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1638 """
1639 try:
1640 r = rospy.Rate(float(rate))
1641 except ValueError:
1642 raise ROSTopicException("Rate must be a number")
1643 while not rospy.is_shutdown():
1644 if substitute_keywords:
1645 _fillMessageArgs(msg, pub_args)
1646 if verbose:
1647 print("publishing %s"%msg)
1648 pub.publish(msg)
1649 r.sleep()
1650
1651 _ONCE_DELAY = 3.
1653 """
1654 Publish and latch message. Subroutine of L{publish_message()}.
1655
1656 :param pub: :class:`rospy.Publisher` instance for topic
1657 :param msg: message instance to publish
1658 :param once: if ``True``, publish message once and then exit after sleep interval, ``bool``
1659 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1660 """
1661 try:
1662 pub.publish(msg)
1663 except TypeError as e:
1664 raise ROSTopicException(str(e))
1665
1666 if not once:
1667 rospy.spin()
1668
1669 -def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=False, substitute_keywords=False):
1670 """
1671 Create new instance of msg_class, populate with pub_args, and publish. This may
1672 print output to screen.
1673
1674 :param pub: :class:`rospy.Publisher` instance for topic
1675 :param msg_class: Message type, ``Class``
1676 :param pub_args: Arguments to initialize message that is published, ``[val]``
1677 :param rate: publishing rate (hz) or None for just once, ``int``
1678 :param once: publish only once and return, ``bool``
1679 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1680 """
1681 msg = msg_class()
1682
1683 _fillMessageArgs(msg, pub_args)
1684
1685 try:
1686
1687 if rate is None:
1688 s = "publishing and latching [%s]"%(msg) if verbose else "publishing and latching message"
1689 if once:
1690 s = s + " for %s seconds"%_ONCE_DELAY
1691 else:
1692 s = s + ". Press ctrl-C to terminate"
1693 print(s)
1694
1695 _publish_latched(pub, msg, once, verbose)
1696 else:
1697 _publish_at_rate(pub, msg, rate, verbose=verbose, substitute_keywords=substitute_keywords, pub_args=pub_args)
1698
1699 except rospy.ROSSerializationException as e:
1700 import rosmsg
1701
1702 raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n"+\
1703 " %s\n\nmsg file:\n%s"%(e, rosmsg.get_msg_text(msg_class._type)))
1704
1706 try:
1707
1708
1709
1710
1711
1712
1713
1714
1715
1716
1717
1718 now = rospy.get_rostime()
1719 import std_msgs.msg
1720 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) }
1721 genpy.message.fill_message_args(msg, pub_args, keys=keys)
1722 except genpy.MessageException as e:
1723 raise ROSTopicException(str(e)+"\n\nArgs are: [%s]"%genpy.message.get_printable_message_args(msg))
1724
1726 """
1727 Parse 'pub' command arguments and run command. Will cause a system
1728 exit if command-line argument parsing fails.
1729 :param argv: command-line arguments
1730 :param argv: [str]
1731 :raises: :exc:`ROSTopicException` If call command cannot be executed
1732 """
1733 args = argv[2:]
1734 from optparse import OptionParser
1735 parser = OptionParser(usage="usage: %prog pub /topic type [args...]", prog=NAME)
1736 parser.add_option("-v", dest="verbose", default=False,
1737 action="store_true",
1738 help="print verbose output")
1739 parser.add_option("-r", "--rate", dest="rate", default=None,
1740 help="publishing rate (hz). For -f and stdin input, this defaults to 10. Otherwise it is not set.")
1741 parser.add_option("-1", "--once", action="store_true", dest="once", default=False,
1742 help="publish one message and exit")
1743 parser.add_option("-f", '--file', dest="file", metavar='FILE', default=None,
1744 help="read args from YAML file (Bagy)")
1745 parser.add_option("-l", '--latch', dest="latch", default=False, action="store_true",
1746 help="enable latching for -f, -r and piped input. This latches the first message.")
1747 parser.add_option("-s", '--substitute-keywords', dest="substitute_keywords", default=False, action="store_true",
1748 help="When publishing with a rate, performs keyword ('now' or 'auto') substitution for each message")
1749
1750
1751
1752 (options, args) = parser.parse_args(args)
1753 if options.rate is not None:
1754 if options.once:
1755 parser.error("You cannot select both -r and -1 (--once)")
1756 try:
1757 rate = float(options.rate)
1758 except ValueError:
1759 parser.error("rate must be a number")
1760 if rate <= 0:
1761 parser.error("rate must be greater than zero")
1762 else:
1763
1764 rate = None
1765
1766
1767 if len(args) == 0:
1768 parser.error("/topic must be specified")
1769 if len(args) == 1:
1770 parser.error("topic type must be specified")
1771 if 0:
1772 if len(args) > 2 and options.parameter:
1773 parser.error("args confict with -p setting")
1774 if len(args) > 2 and options.file:
1775 parser.error("args confict with -f setting")
1776 topic_name, topic_type = args[0], args[1]
1777
1778
1779 try:
1780 pub_args = []
1781 for arg in args[2:]:
1782 pub_args.append(yaml.load(arg))
1783 except Exception as e:
1784 parser.error("Argument error: "+str(e))
1785
1786
1787
1788 _check_master()
1789
1790
1791 latch = (rate == None) or options.latch
1792 pub, msg_class = create_publisher(topic_name, topic_type, latch)
1793
1794 if 0 and options.parameter:
1795 param_name = rosgraph.names.script_resolve_name('rostopic', options.parameter)
1796 if options.once:
1797 param_publish_once(pub, msg_class, param_name, rate, options.verbose)
1798 else:
1799 param_publish(pub, msg_class, param_name, rate, options.verbose)
1800
1801 elif not pub_args and len(msg_class.__slots__):
1802 if not options.file and sys.stdin.isatty():
1803 parser.error("Please specify message values")
1804
1805 if rate is None and not options.latch and not options.once:
1806 rate = 10.
1807 stdin_publish(pub, msg_class, rate, options.once, options.file, options.verbose)
1808 else:
1809 argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, substitute_keywords=options.substitute_keywords)
1810
1811
1813 """
1814 :param filename: file name, ``str``
1815 :returns: Iterator that yields pub args (list of args), ``iterator``
1816 :raises: :exc:`ROSTopicException` If filename is invalid
1817 """
1818 if not os.path.isfile(filename):
1819 raise ROSTopicException("file does not exist: %s"%(filename))
1820 import yaml
1821 def bagy_iter():
1822 try:
1823 with open(filename, 'r') as f:
1824
1825 data = yaml.load_all(f)
1826 for d in data:
1827 yield [d]
1828 except yaml.YAMLError as e:
1829 raise ROSTopicException("invalid YAML in file: %s"%(str(e)))
1830 return bagy_iter
1831
1832 -def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywords=False):
1833 publish_message(pub, msg_class, pub_args, rate, once, verbose=verbose, substitute_keywords=substitute_keywords)
1834
1835 if once:
1836
1837 timeout_t = time.time() + _ONCE_DELAY
1838 while not rospy.is_shutdown() and time.time() < timeout_t:
1839 rospy.sleep(0.2)
1840
1841 SUBSCRIBER_TIMEOUT = 5.
1843 timeout_t = time.time() + timeout
1844 while pub.get_num_connections() == 0 and timeout_t > time.time():
1845 _sleep(0.01)
1846
1848 if not rospy.has_param(param_name):
1849 raise ROSTopicException("parameter does not exist: %s"%(param_name))
1850 pub_args = rospy.get_param(param_name)
1851 argv_publish(pub, msg_class, pub_args, None, True, verbose)
1852
1853
1855
1856 - def __init__(self, param_name, value=None):
1857 import threading
1858 self.lock = threading.Condition()
1859 self.param_name = param_name
1860 self.updates = []
1861 self.value = None
1862
1864 with self.lock:
1865
1866 if key != self.param_name:
1867 subs = [x for x in key[len(self.param_name):].split('/') if x]
1868 idx = self.value
1869 for s in subs[:-1]:
1870 if s in idx:
1871 idx = idx[s]
1872 else:
1873 idx[s] = {}
1874 idx = idx[s]
1875 idx[subs[-1]] = value
1876 else:
1877 self.value = value
1878
1879 self.updates.append(self.value)
1880 self.lock.notify_all()
1881
1883 """
1884 :param param_name: ROS parameter name, ``str``
1885 :returns: List of msg dicts in file, ``[{str: any}]``
1886 :raises: :exc:`ROSTopicException` If parameter is not set
1887 """
1888 import rospy
1889 import rospy.impl.paramserver
1890 import rosgraph
1891
1892 if not rospy.has_param(param_name):
1893 raise ROSTopicException("parameter does not exist: %s"%(param_name))
1894
1895
1896
1897 ps_cache = rospy.impl.paramserver.get_param_server_cache()
1898 notifier = _ParamNotifier(param_name)
1899 ps_cache.set_notifier(notifier)
1900 master = rosgraph.Master(rospy.get_name())
1901 notifier.value = master.subscribeParam(rospy.get_node_uri(), param_name)
1902 pub_args = notifier.value
1903 ps_cache.set(param_name, pub_args)
1904 if type(pub_args) == dict:
1905 pub_args = [pub_args]
1906 elif type(pub_args) != list:
1907 raise ROSTopicException("Parameter [%s] in not a valid type"%(param_name))
1908
1909 r = rospy.Rate(rate) if rate is not None else None
1910 publish = True
1911 while not rospy.is_shutdown():
1912 try:
1913 if publish:
1914 publish_message(pub, msg_class, pub_args, None, True, verbose=verbose)
1915 except ValueError as e:
1916 sys.stderr.write("%s\n"%str(e))
1917 break
1918 if r is not None:
1919 r.sleep()
1920 with notifier.lock:
1921 if notifier.updates:
1922 pub_args = notifier.updates.pop(0)
1923 if type(pub_args) == dict:
1924 pub_args = [pub_args]
1925 else:
1926 publish = False
1927 with notifier.lock:
1928 if not notifier.updates:
1929 notifier.lock.wait(1.)
1930 if notifier.updates:
1931 publish = True
1932 pub_args = notifier.updates.pop(0)
1933 if type(pub_args) == dict:
1934 pub_args = [pub_args]
1935
1936 if rospy.is_shutdown():
1937 break
1938
1939 -def stdin_publish(pub, msg_class, rate, once, filename, verbose):
1940 """
1941 :param filename: name of file to read from instead of stdin, or ``None``, ``str``
1942 """
1943 if filename:
1944 iterator = file_yaml_arg(filename)
1945 else:
1946 iterator = stdin_yaml_arg
1947
1948 r = rospy.Rate(rate) if rate is not None else None
1949
1950
1951
1952
1953 wait_for_subscriber(pub, SUBSCRIBER_TIMEOUT)
1954
1955 for pub_args in iterator():
1956 if rospy.is_shutdown():
1957 break
1958 if pub_args:
1959 if type(pub_args) != list:
1960 pub_args = [pub_args]
1961 try:
1962
1963
1964
1965
1966
1967
1968
1969 publish_message(pub, msg_class, pub_args, None, bool(r) or once, verbose=verbose)
1970 except ValueError as e:
1971 sys.stderr.write("%s\n"%str(e))
1972 break
1973 if r is not None:
1974 r.sleep()
1975 if rospy.is_shutdown() or once:
1976 break
1977
1979 """
1980 Iterate over YAML documents in stdin
1981 :returns: for next list of arguments on stdin. Iterator returns a list of args for each call, ``iterator``
1982 """
1983 import yaml
1984 from select import select
1985 from select import error as select_error
1986 try:
1987 arg = 'x'
1988 rlist = [sys.stdin]
1989 wlist = xlist = []
1990 while not rospy.is_shutdown() and arg != '\n':
1991 buff = ''
1992 while arg != '' and arg.strip() != '---' and not rospy.is_shutdown():
1993 val, _, _ = select(rlist, wlist, xlist, 1.0)
1994 if not val:
1995 continue
1996
1997 arg = sys.stdin.readline()
1998 if arg != '' and arg.strip() != '---':
1999 buff = buff + arg
2000
2001 if arg.strip() == '---':
2002 try:
2003 loaded = yaml.load(buff.rstrip())
2004 except Exception as e:
2005 sys.stderr.write("Invalid YAML: %s\n"%str(e))
2006 if loaded is not None:
2007 yield loaded
2008 elif arg == '':
2009
2010
2011 return
2012
2013 arg = 'x'
2014
2015 except select_error:
2016 return
2017
2019 """
2020 Command-line parsing for 'rostopic list' command.
2021 """
2022 args = argv[2:]
2023 from optparse import OptionParser
2024 parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME)
2025 parser.add_option("-b", "--bag",
2026 dest="bag", default=None,
2027 help="list topics in .bag file", metavar="BAGFILE")
2028 parser.add_option("-v", "--verbose",
2029 dest="verbose", default=False,action="store_true",
2030 help="list full details about each topic")
2031 parser.add_option("-p",
2032 dest="publishers", default=False,action="store_true",
2033 help="list only publishers")
2034 parser.add_option("-s",
2035 dest="subscribers", default=False,action="store_true",
2036 help="list only subscribers")
2037 parser.add_option("--host", dest="hostname", default=False, action="store_true",
2038 help="group by host name")
2039
2040 (options, args) = parser.parse_args(args)
2041 topic = None
2042
2043 if len(args) == 1:
2044 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
2045 elif len(args) > 1:
2046 parser.error("you may only specify one input topic")
2047 if options.bag:
2048 if options.subscribers:
2049 parser.error("-s option is not valid with bags")
2050 elif options.publishers:
2051 parser.error("-p option is not valid with bags")
2052 elif options.hostname:
2053 parser.error("--host option is not valid with bags")
2054 _rostopic_list_bag(options.bag, topic)
2055 else:
2056 if options.subscribers and options.publishers:
2057 parser.error("you may only specify one of -p, -s")
2058
2059 exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, publishers_only=options.publishers, group_by_host=options.hostname) or 0
2060 if exitval != 0:
2061 sys.exit(exitval)
2062
2064 """
2065 Command-line parsing for 'rostopic info' command.
2066 """
2067 args = argv[2:]
2068 from optparse import OptionParser
2069 parser = OptionParser(usage="usage: %prog info /topic", prog=NAME)
2070 (options, args) = parser.parse_args(args)
2071
2072 if len(args) == 0:
2073 parser.error("you must specify a topic name")
2074 elif len(args) > 1:
2075 parser.error("you may only specify one topic name")
2076
2077 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
2078 exitval = _rostopic_info(topic) or 0
2079 if exitval != 0:
2080 sys.exit(exitval)
2081
2083 print("""rostopic is a command-line tool for printing information about ROS Topics.
2084
2085 Commands:
2086 \trostopic bw\tdisplay bandwidth used by topic
2087 \trostopic delay\tdisplay delay of topic from timestamp in header
2088 \trostopic echo\tprint messages to screen
2089 \trostopic find\tfind topics by type
2090 \trostopic hz\tdisplay publishing rate of topic
2091 \trostopic info\tprint information about active topic
2092 \trostopic list\tlist active topics
2093 \trostopic pub\tpublish data to topic
2094 \trostopic type\tprint topic or field type
2095
2096 Type rostopic <command> -h for more detailed usage, e.g. 'rostopic echo -h'
2097 """)
2098 sys.exit(getattr(os, 'EX_USAGE', 1))
2099
2100 -def rostopicmain(argv=None):
2101 import rosbag
2102 if argv is None:
2103 argv=sys.argv
2104
2105 argv = rospy.myargv(argv)
2106
2107
2108 if len(argv) == 1:
2109 _fullusage()
2110 try:
2111 command = argv[1]
2112 if command == 'echo':
2113 _rostopic_cmd_echo(argv)
2114 elif command == 'hz':
2115 _rostopic_cmd_hz(argv)
2116 elif command == 'type':
2117 _rostopic_cmd_type(argv)
2118 elif command == 'list':
2119 _rostopic_cmd_list(argv)
2120 elif command == 'info':
2121 _rostopic_cmd_info(argv)
2122 elif command == 'pub':
2123 _rostopic_cmd_pub(argv)
2124 elif command == 'bw':
2125 _rostopic_cmd_bw(argv)
2126 elif command == 'find':
2127 _rostopic_cmd_find(argv)
2128 elif command == 'delay':
2129 _rostopic_cmd_delay(argv)
2130 else:
2131 _fullusage()
2132 except socket.error:
2133 sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n")
2134 sys.exit(1)
2135 except rosbag.ROSBagException as e:
2136 sys.stderr.write("ERROR: unable to use bag file: %s\n"%str(e))
2137 sys.exit(1)
2138 except rosgraph.MasterException as e:
2139
2140 sys.stderr.write("ERROR: %s\n"%str(e))
2141 sys.exit(1)
2142 except ROSTopicException as e:
2143 sys.stderr.write("ERROR: %s\n"%str(e))
2144 sys.exit(1)
2145 except KeyboardInterrupt: pass
2146