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))
477
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, ttype, [p]))
1200 return tmap
1201
1202 uricache = {}
1203 host_pub_topics = build_map(master, pubs, uricache)
1204 host_sub_topics = build_map(master, subs, uricache)
1205 return host_pub_topics, host_sub_topics
1206
1207 -def _rostopic_list(topic, verbose=False,
1208 subscribers_only=False, publishers_only=False,
1209 group_by_host=False):
1210 """
1211 Print topics to screen
1212
1213 :param topic: topic name to list information or None to match all topics, ``str``
1214 :param verbose: print additional debugging information, ``bool``
1215 :param subscribers_only: print information about subscriptions only, ``bool``
1216 :param publishers_only: print information about subscriptions only, ``bool``
1217 :param group_by_host: group topic list by hostname, ``bool``
1218 """
1219
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 :param latch: disable_rostime: whether to disable rostime (use walltime instead), ``bool``
1616 :returns: topic :class:`rospy.Publisher`, :class:`Message` class
1617 """
1618 topic_name = rosgraph.names.script_resolve_name('rostopic', topic_name)
1619 try:
1620 msg_class = roslib.message.get_message_class(topic_type)
1621 except:
1622 raise ROSTopicException("invalid topic type: %s"%topic_type)
1623 if msg_class is None:
1624 pkg = _resource_name_package(topic_type)
1625 raise ROSTopicException("invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'"%(topic_type, pkg))
1626
1627 rospy.init_node('rostopic', anonymous=True, disable_rosout=True, disable_rostime=disable_rostime)
1628 pub = rospy.Publisher(topic_name, msg_class, latch=latch, queue_size=100)
1629 return pub, msg_class
1630
1631 -def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, pub_args=None):
1632 """
1633 Publish message at specified rate. Subroutine of L{publish_message()}.
1634
1635 :param pub: :class:rospy.Publisher` instance for topic
1636 :param msg: message instance to publish
1637 :param rate: publishing rate (hz) or None for just once, ``int``
1638 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1639 """
1640 try:
1641 r = rospy.Rate(float(rate))
1642 except ValueError:
1643 raise ROSTopicException("Rate must be a number")
1644 while not rospy.is_shutdown():
1645 if substitute_keywords:
1646 _fillMessageArgs(msg, pub_args)
1647 if verbose:
1648 print("publishing %s"%msg)
1649 pub.publish(msg)
1650 r.sleep()
1651
1652 _ONCE_DELAY = 3.
1654 """
1655 Publish and latch message. Subroutine of L{publish_message()}.
1656
1657 :param pub: :class:`rospy.Publisher` instance for topic
1658 :param msg: message instance to publish
1659 :param once: if ``True``, publish message once and then exit after sleep interval, ``bool``
1660 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1661 """
1662 try:
1663 pub.publish(msg)
1664 except TypeError as e:
1665 raise ROSTopicException(str(e))
1666
1667 if not once:
1668 rospy.spin()
1669
1670 -def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=False, substitute_keywords=False):
1671 """
1672 Create new instance of msg_class, populate with pub_args, and publish. This may
1673 print output to screen.
1674
1675 :param pub: :class:`rospy.Publisher` instance for topic
1676 :param msg_class: Message type, ``Class``
1677 :param pub_args: Arguments to initialize message that is published, ``[val]``
1678 :param rate: publishing rate (hz) or None for just once, ``int``
1679 :param once: publish only once and return, ``bool``
1680 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1681 """
1682 msg = msg_class()
1683
1684 _fillMessageArgs(msg, pub_args)
1685
1686 try:
1687
1688 if rate is None:
1689 s = "publishing and latching [%s]"%(msg) if verbose else "publishing and latching message"
1690 if once:
1691 s = s + " for %s seconds"%_ONCE_DELAY
1692 else:
1693 s = s + ". Press ctrl-C to terminate"
1694 print(s)
1695
1696 _publish_latched(pub, msg, once, verbose)
1697 else:
1698 _publish_at_rate(pub, msg, rate, verbose=verbose, substitute_keywords=substitute_keywords, pub_args=pub_args)
1699
1700 except rospy.ROSSerializationException as e:
1701 import rosmsg
1702
1703 raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n"+\
1704 " %s\n\nmsg file:\n%s"%(e, rosmsg.get_msg_text(msg_class._type)))
1705
1707 try:
1708
1709
1710
1711
1712
1713
1714
1715
1716
1717
1718
1719 now = rospy.get_rostime()
1720 import std_msgs.msg
1721 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) }
1722 genpy.message.fill_message_args(msg, pub_args, keys=keys)
1723 except genpy.MessageException as e:
1724 raise ROSTopicException(str(e)+"\n\nArgs are: [%s]"%genpy.message.get_printable_message_args(msg))
1725
1727 """
1728 Parse 'pub' command arguments and run command. Will cause a system
1729 exit if command-line argument parsing fails.
1730 :param argv: command-line arguments
1731 :param argv: [str]
1732 :raises: :exc:`ROSTopicException` If call command cannot be executed
1733 """
1734 args = argv[2:]
1735 from optparse import OptionParser
1736 parser = OptionParser(usage="usage: %prog pub /topic type [args...]", prog=NAME)
1737 parser.add_option("-v", dest="verbose", default=False,
1738 action="store_true",
1739 help="print verbose output")
1740 parser.add_option("-r", "--rate", dest="rate", default=None,
1741 help="publishing rate (hz). For -f and stdin input, this defaults to 10. Otherwise it is not set.")
1742 parser.add_option("-1", "--once", action="store_true", dest="once", default=False,
1743 help="publish one message and exit")
1744 parser.add_option("-f", '--file', dest="file", metavar='FILE', default=None,
1745 help="read args from YAML file (Bagy)")
1746 parser.add_option("-l", '--latch', dest="latch", default=False, action="store_true",
1747 help="enable latching for -f, -r and piped input. This latches the first message.")
1748 parser.add_option("-s", '--substitute-keywords', dest="substitute_keywords", default=False, action="store_true",
1749 help="When publishing with a rate, performs keyword ('now' or 'auto') substitution for each message")
1750 parser.add_option('--use-rostime', dest="use_rostime", default=False, action="store_true",
1751 help="use rostime for time stamps, else walltime is used")
1752
1753
1754
1755 (options, args) = parser.parse_args(args)
1756 if options.rate is not None:
1757 if options.once:
1758 parser.error("You cannot select both -r and -1 (--once)")
1759 try:
1760 rate = float(options.rate)
1761 except ValueError:
1762 parser.error("rate must be a number")
1763 if rate <= 0:
1764 parser.error("rate must be greater than zero")
1765 else:
1766
1767 rate = None
1768
1769
1770 if len(args) == 0:
1771 parser.error("/topic must be specified")
1772 if len(args) == 1:
1773 parser.error("topic type must be specified")
1774 if 0:
1775 if len(args) > 2 and options.parameter:
1776 parser.error("args conflict with -p setting")
1777 if len(args) > 2 and options.file:
1778 parser.error("args conflict with -f setting")
1779 topic_name, topic_type = args[0], args[1]
1780
1781
1782 try:
1783 pub_args = []
1784 for arg in args[2:]:
1785 pub_args.append(yaml.safe_load(arg))
1786 except Exception as e:
1787 parser.error("Argument error: "+str(e))
1788
1789
1790
1791 _check_master()
1792
1793
1794 latch = (rate == None) or options.latch
1795 pub, msg_class = create_publisher(topic_name, topic_type, latch, disable_rostime=not options.use_rostime)
1796
1797 if 0 and options.parameter:
1798 param_name = rosgraph.names.script_resolve_name('rostopic', options.parameter)
1799 if options.once:
1800 param_publish_once(pub, msg_class, param_name, rate, options.verbose)
1801 else:
1802 param_publish(pub, msg_class, param_name, rate, options.verbose)
1803
1804 elif not pub_args and len(msg_class.__slots__):
1805 if not options.file and sys.stdin.isatty():
1806 parser.error("Please specify message values")
1807
1808 if rate is None and not options.latch and not options.once:
1809 rate = 10.
1810 stdin_publish(pub, msg_class, rate, options.once, options.file, options.verbose)
1811 else:
1812 argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, substitute_keywords=options.substitute_keywords)
1813
1814
1816 """
1817 :param filename: file name, ``str``
1818 :returns: Iterator that yields pub args (list of args), ``iterator``
1819 :raises: :exc:`ROSTopicException` If filename is invalid
1820 """
1821 if not os.path.isfile(filename):
1822 raise ROSTopicException("file does not exist: %s"%(filename))
1823 import yaml
1824 def bagy_iter():
1825 try:
1826 with open(filename, 'r') as f:
1827
1828 data = yaml.safe_load_all(f)
1829 for d in data:
1830 yield [d]
1831 except yaml.YAMLError as e:
1832 raise ROSTopicException("invalid YAML in file: %s"%(str(e)))
1833 return bagy_iter
1834
1835 -def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywords=False):
1836 publish_message(pub, msg_class, pub_args, rate, once, verbose=verbose, substitute_keywords=substitute_keywords)
1837
1838 if once:
1839
1840 timeout_t = time.time() + _ONCE_DELAY
1841 while not rospy.is_shutdown() and time.time() < timeout_t:
1842 rospy.sleep(0.2)
1843
1844 SUBSCRIBER_TIMEOUT = 5.
1846 timeout_t = time.time() + timeout
1847 while pub.get_num_connections() == 0 and timeout_t > time.time():
1848 _sleep(0.01)
1849
1851 if not rospy.has_param(param_name):
1852 raise ROSTopicException("parameter does not exist: %s"%(param_name))
1853 pub_args = rospy.get_param(param_name)
1854 argv_publish(pub, msg_class, pub_args, None, True, verbose)
1855
1856
1858
1859 - def __init__(self, param_name, value=None):
1860 import threading
1861 self.lock = threading.Condition()
1862 self.param_name = param_name
1863 self.updates = []
1864 self.value = None
1865
1867 with self.lock:
1868
1869 if key != self.param_name:
1870 subs = [x for x in key[len(self.param_name):].split('/') if x]
1871 idx = self.value
1872 for s in subs[:-1]:
1873 if s in idx:
1874 idx = idx[s]
1875 else:
1876 idx[s] = {}
1877 idx = idx[s]
1878 idx[subs[-1]] = value
1879 else:
1880 self.value = value
1881
1882 self.updates.append(self.value)
1883 self.lock.notify_all()
1884
1886 """
1887 :param param_name: ROS parameter name, ``str``
1888 :returns: List of msg dicts in file, ``[{str: any}]``
1889 :raises: :exc:`ROSTopicException` If parameter is not set
1890 """
1891 import rospy
1892 import rospy.impl.paramserver
1893 import rosgraph
1894
1895 if not rospy.has_param(param_name):
1896 raise ROSTopicException("parameter does not exist: %s"%(param_name))
1897
1898
1899
1900 ps_cache = rospy.impl.paramserver.get_param_server_cache()
1901 notifier = _ParamNotifier(param_name)
1902 ps_cache.set_notifier(notifier)
1903 master = rosgraph.Master(rospy.get_name())
1904 notifier.value = master.subscribeParam(rospy.get_node_uri(), param_name)
1905 pub_args = notifier.value
1906 ps_cache.set(param_name, pub_args)
1907 if type(pub_args) == dict:
1908 pub_args = [pub_args]
1909 elif type(pub_args) != list:
1910 raise ROSTopicException("Parameter [%s] in not a valid type"%(param_name))
1911
1912 r = rospy.Rate(rate) if rate is not None else None
1913 publish = True
1914 while not rospy.is_shutdown():
1915 try:
1916 if publish:
1917 publish_message(pub, msg_class, pub_args, None, True, verbose=verbose)
1918 except ValueError as e:
1919 sys.stderr.write("%s\n"%str(e))
1920 break
1921 if r is not None:
1922 r.sleep()
1923 with notifier.lock:
1924 if notifier.updates:
1925 pub_args = notifier.updates.pop(0)
1926 if type(pub_args) == dict:
1927 pub_args = [pub_args]
1928 else:
1929 publish = False
1930 with notifier.lock:
1931 if not notifier.updates:
1932 notifier.lock.wait(1.)
1933 if notifier.updates:
1934 publish = True
1935 pub_args = notifier.updates.pop(0)
1936 if type(pub_args) == dict:
1937 pub_args = [pub_args]
1938
1939 if rospy.is_shutdown():
1940 break
1941
1942 -def stdin_publish(pub, msg_class, rate, once, filename, verbose):
1943 """
1944 :param filename: name of file to read from instead of stdin, or ``None``, ``str``
1945 """
1946 exactly_one_message = False
1947
1948 if filename:
1949 iterator = file_yaml_arg(filename)
1950
1951 for _ in iterator():
1952 if exactly_one_message:
1953 exactly_one_message = False
1954 break
1955 exactly_one_message = True
1956 else:
1957 iterator = stdin_yaml_arg
1958
1959 r = rospy.Rate(rate) if rate is not None else None
1960
1961
1962
1963
1964 wait_for_subscriber(pub, SUBSCRIBER_TIMEOUT)
1965
1966 for pub_args in iterator():
1967 if rospy.is_shutdown():
1968 break
1969 if pub_args:
1970 if type(pub_args) != list:
1971 pub_args = [pub_args]
1972 try:
1973
1974
1975 if exactly_one_message and rate is not None:
1976 print("Got one message and a rate, publishing repeatedly")
1977 publish_message(pub, msg_class, pub_args, rate=rate, once=once, verbose=verbose)
1978
1979
1980
1981
1982
1983
1984
1985 else:
1986 publish_message(pub, msg_class, pub_args, None, bool(r) or once, verbose=verbose)
1987 except ValueError as e:
1988 sys.stderr.write("%s\n"%str(e))
1989 break
1990 if r is not None:
1991 r.sleep()
1992 if rospy.is_shutdown() or once:
1993 break
1994
1996 """
1997 Iterate over YAML documents in stdin
1998 :returns: for next list of arguments on stdin. Iterator returns a list of args for each call, ``iterator``
1999 """
2000 import yaml
2001 from select import select
2002 from select import error as select_error
2003 try:
2004 arg = 'x'
2005 rlist = [sys.stdin]
2006 wlist = xlist = []
2007 while not rospy.is_shutdown() and arg != '\n':
2008 buff = ''
2009 while arg != '' and arg.strip() != '---' and not rospy.is_shutdown():
2010 val, _, _ = select(rlist, wlist, xlist, 1.0)
2011 if not val:
2012 continue
2013
2014 arg = sys.stdin.readline()
2015 if arg != '' and arg.strip() != '---':
2016 buff = buff + arg
2017
2018 if arg.strip() == '---':
2019 try:
2020 loaded = yaml.safe_load(buff.rstrip())
2021 except Exception as e:
2022 sys.stderr.write("Invalid YAML: %s\n"%str(e))
2023 if loaded is not None:
2024 yield loaded
2025 elif arg == '':
2026
2027
2028 return
2029
2030 arg = 'x'
2031
2032 except select_error:
2033 return
2034
2036 """
2037 Command-line parsing for 'rostopic list' command.
2038 """
2039 args = argv[2:]
2040 from optparse import OptionParser
2041 parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME)
2042 parser.add_option("-b", "--bag",
2043 dest="bag", default=None,
2044 help="list topics in .bag file", metavar="BAGFILE")
2045 parser.add_option("-v", "--verbose",
2046 dest="verbose", default=False,action="store_true",
2047 help="list full details about each topic")
2048 parser.add_option("-p",
2049 dest="publishers", default=False,action="store_true",
2050 help="list only publishers")
2051 parser.add_option("-s",
2052 dest="subscribers", default=False,action="store_true",
2053 help="list only subscribers")
2054 parser.add_option("--host", dest="hostname", default=False, action="store_true",
2055 help="group by host name")
2056
2057 (options, args) = parser.parse_args(args)
2058 topic = None
2059
2060 if len(args) == 1:
2061 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
2062 elif len(args) > 1:
2063 parser.error("you may only specify one input topic")
2064 if options.bag:
2065 if options.subscribers:
2066 parser.error("-s option is not valid with bags")
2067 elif options.publishers:
2068 parser.error("-p option is not valid with bags")
2069 elif options.hostname:
2070 parser.error("--host option is not valid with bags")
2071 _rostopic_list_bag(options.bag, topic)
2072 else:
2073 if options.subscribers and options.publishers:
2074 parser.error("you may only specify one of -p, -s")
2075
2076 exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, publishers_only=options.publishers, group_by_host=options.hostname) or 0
2077 if exitval != 0:
2078 sys.exit(exitval)
2079
2081 """
2082 Command-line parsing for 'rostopic info' command.
2083 """
2084 args = argv[2:]
2085 from optparse import OptionParser
2086 parser = OptionParser(usage="usage: %prog info /topic", prog=NAME)
2087 (options, args) = parser.parse_args(args)
2088
2089 if len(args) == 0:
2090 parser.error("you must specify a topic name")
2091 elif len(args) > 1:
2092 parser.error("you may only specify one topic name")
2093
2094 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
2095 exitval = _rostopic_info(topic) or 0
2096 if exitval != 0:
2097 sys.exit(exitval)
2098
2100 print("""rostopic is a command-line tool for printing information about ROS Topics.
2101
2102 Commands:
2103 \trostopic bw\tdisplay bandwidth used by topic
2104 \trostopic delay\tdisplay delay of topic from timestamp in header
2105 \trostopic echo\tprint messages to screen
2106 \trostopic find\tfind topics by type
2107 \trostopic hz\tdisplay publishing rate of topic
2108 \trostopic info\tprint information about active topic
2109 \trostopic list\tlist active topics
2110 \trostopic pub\tpublish data to topic
2111 \trostopic type\tprint topic or field type
2112
2113 Type rostopic <command> -h for more detailed usage, e.g. 'rostopic echo -h'
2114 """)
2115 sys.exit(getattr(os, 'EX_USAGE', 1))
2116
2117 -def rostopicmain(argv=None):
2118 import rosbag
2119 if argv is None:
2120 argv=sys.argv
2121
2122 argv = rospy.myargv(argv)
2123
2124
2125 if len(argv) == 1:
2126 _fullusage()
2127 try:
2128 command = argv[1]
2129 if command == 'echo':
2130 _rostopic_cmd_echo(argv)
2131 elif command == 'hz':
2132 _rostopic_cmd_hz(argv)
2133 elif command == 'type':
2134 _rostopic_cmd_type(argv)
2135 elif command == 'list':
2136 _rostopic_cmd_list(argv)
2137 elif command == 'info':
2138 _rostopic_cmd_info(argv)
2139 elif command == 'pub':
2140 _rostopic_cmd_pub(argv)
2141 elif command == 'bw':
2142 _rostopic_cmd_bw(argv)
2143 elif command == 'find':
2144 _rostopic_cmd_find(argv)
2145 elif command == 'delay':
2146 _rostopic_cmd_delay(argv)
2147 else:
2148 _fullusage()
2149 except socket.error:
2150 sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n")
2151 sys.exit(1)
2152 except rosbag.ROSBagException as e:
2153 sys.stderr.write("ERROR: unable to use bag file: %s\n"%str(e))
2154 sys.exit(1)
2155 except rosgraph.MasterException as e:
2156
2157 sys.stderr.write("ERROR: %s\n"%str(e))
2158 sys.exit(1)
2159 except ROSTopicException as e:
2160 sys.stderr.write("ERROR: %s\n"%str(e))
2161 sys.exit(1)
2162 except KeyboardInterrupt: pass
2163