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 os
42 import sys
43 import math
44 import socket
45 import time
46 import traceback
47 import yaml
48 try:
49 from xmlrpc.client import Fault
50 except ImportError:
51 from xmlrpclib import Fault
52
53 from operator import itemgetter
54 try:
55 from urllib.parse import urlparse
56 except ImportError:
57 from urlparse import urlparse
58
59 import genpy
60
61 import roslib.message
62 import rosgraph
63
64 import rospy
65
67 """
68 Base exception class of rostopic-related errors
69 """
70 pass
72 """
73 rostopic errors related to network I/O failures
74 """
75 pass
76
78 """
79 Make sure that master is available
80 :raises: :exc:`ROSTopicException` If unable to successfully communicate with master
81 """
82 try:
83 rosgraph.Master('/rostopic').getPid()
84 except socket.error:
85 raise ROSTopicIOException("Unable to communicate with master!")
86
88 try:
89 val = master.getTopicTypes()
90 except Fault:
91
92 sys.stderr.write("WARNING: rostopic is being used against an older version of ROS/roscore\n")
93 val = master.getPublishedTopics('/')
94 return val
95
97 """
98 ROSTopicHz receives messages for a topic and computes frequency stats
99 """
100 - def __init__(self, window_size, filter_expr=None, use_wtime=False):
101 import threading
102 self.lock = threading.Lock()
103 self.last_printed_tn = 0
104 self.msg_t0 = -1.
105 self.msg_tn = 0
106 self.times =[]
107 self.filter_expr = filter_expr
108 self.use_wtime = use_wtime
109
110
111 if window_size < 0:
112 window_size = 50000
113 self.window_size = window_size
114
116 """
117 ros sub callback
118 :param m: Message instance
119 """
120
121 if self.filter_expr is not None and not self.filter_expr(m):
122 return
123 with self.lock:
124 curr_rostime = rospy.get_rostime() if not self.use_wtime else \
125 rospy.Time.from_sec(time.time())
126
127
128 if curr_rostime.is_zero():
129 if len(self.times) > 0:
130 print("time has reset, resetting counters")
131 self.times = []
132 return
133
134 curr = curr_rostime.to_sec() if not self.use_wtime else \
135 rospy.Time.from_sec(time.time()).to_sec()
136 if self.msg_t0 < 0 or self.msg_t0 > curr:
137 self.msg_t0 = curr
138 self.msg_tn = curr
139 self.times = []
140 else:
141 self.times.append(curr - self.msg_tn)
142 self.msg_tn = curr
143
144
145 if len(self.times) > self.window_size - 1:
146 self.times.pop(0)
147
149 """
150 print the average publishing rate to screen
151 """
152 if not self.times:
153 return
154 elif self.msg_tn == self.last_printed_tn:
155 print("no new messages")
156 return
157 with self.lock:
158
159
160
161
162
163
164
165
166
167 n = len(self.times)
168
169 mean = sum(self.times) / n
170 rate = 1./mean if mean > 0. else 0
171
172
173 std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) /n)
174
175
176 max_delta = max(self.times)
177 min_delta = min(self.times)
178
179 self.last_printed_tn = self.msg_tn
180 print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, n+1))
181
183 rospy.rostime.wallsleep(duration)
184
185 -def _rostopic_hz(topic, window_size=-1, filter_expr=None, use_wtime=False):
186 """
187 Periodically print the publishing rate of a topic to console until
188 shutdown
189 :param topic: topic name, ``str``
190 :param window_size: number of messages to average over, -1 for infinite, ``int``
191 :param filter_expr: Python filter expression that is called with m, the message instance
192 """
193 msg_class, real_topic, _ = get_topic_class(topic, blocking=True)
194 if rospy.is_shutdown():
195 return
196 rospy.init_node(NAME, anonymous=True)
197 rt = ROSTopicHz(window_size, filter_expr=filter_expr, use_wtime=use_wtime)
198
199
200 if filter_expr is not None:
201
202 sub = rospy.Subscriber(real_topic, msg_class, rt.callback_hz)
203 else:
204 sub = rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback_hz)
205 print("subscribed to [%s]"%real_topic)
206
207 if rospy.get_param('use_sim_time', False):
208 print("WARNING: may be using simulated time",file=sys.stderr)
209
210 while not rospy.is_shutdown():
211 _sleep(1.0)
212 rt.print_hz()
213
214
216
218 import threading
219 self.lock = threading.Lock()
220 self.last_msg_tn = 0
221 self.msg_t0 = -1.
222 self.msg_tn = 0
223 self.delays = []
224
225
226 if window_size < 0:
227 window_size = 50000
228 self.window_size = window_size
229
231 if not msg._has_header:
232 rospy.logerr('msg does not have header')
233 return
234 with self.lock:
235 curr_rostime = rospy.get_rostime()
236
237
238 if curr_rostime.is_zero():
239 if len(self.delays) > 0:
240 print("time has reset, resetting counters")
241 self.delays = []
242 return
243
244 curr = curr_rostime.to_sec()
245 if self.msg_t0 < 0 or self.msg_t0 > curr:
246 self.msg_t0 = curr
247 self.msg_tn = curr
248 self.delays = []
249 else:
250 self.delays.append(curr_rostime.to_time() -
251 msg.header.stamp.to_time())
252 self.msg_tn = curr
253
254 if len(self.delays) > self.window_size - 1:
255 self.delays.pop(0)
256
258 if self.msg_tn == self.last_msg_tn:
259 return
260 with self.lock:
261 if not self.delays:
262 return
263 n = len(self.delays)
264
265 mean = sum(self.delays) / n
266 rate = 1. / mean if mean > 0 else 0
267
268 std_dev = math.sqrt(sum((x - mean)**2 for x in self.delays) / n)
269
270 max_delta = max(self.delays)
271 min_delta = min(self.delays)
272
273 self.last_msg_tn = self.msg_tn
274 return mean, min_delta, max_delta, std_dev, n + 1
275
277 """
278 print the average publishing delay to screen
279 """
280 if not self.delays:
281 return
282 ret = self.get_delay()
283 if ret is None:
284 print("no new messages")
285 return
286 delay, min_delta, max_delta, std_dev, window = ret
287 print("average delay: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(delay, min_delta, max_delta, std_dev, window))
288
289
291 """
292 Periodically print the publishing delay of a topic to console until
293 shutdown
294 :param topic: topic name, ``str``
295 :param window_size: number of messages to average over, -1 for infinite, ``int``
296 """
297
298 msg_class, real_topic, _ = get_topic_class(topic, blocking=True)
299 if rospy.is_shutdown():
300 return
301 rospy.init_node(NAME, anonymous=True)
302 rt = ROSTopicDelay(window_size)
303 sub = rospy.Subscriber(real_topic, msg_class, rt.callback_delay)
304 print("subscribed to [%s]" % real_topic)
305
306 if rospy.get_param('use_sim_time', False):
307 print("WARNING: may be using simulated time",file=sys.stderr)
308
309 while not rospy.is_shutdown():
310 _sleep(1.0)
311 rt.print_delay()
312
313
316 import threading
317 self.lock = threading.Lock()
318 self.last_printed_tn = 0
319 self.sizes =[]
320 self.times =[]
321 self.window_size = window_size or 100
322
324 """ros sub callback"""
325 with self.lock:
326 try:
327 t = time.time()
328 self.times.append(t)
329 self.sizes.append(len(data._buff))
330 assert(len(self.times) == len(self.sizes))
331
332 if len(self.times) > self.window_size:
333 self.times.pop(0)
334 self.sizes.pop(0)
335 except:
336 traceback.print_exc()
337
339 """print the average publishing rate to screen"""
340 if len(self.times) < 2:
341 return
342 with self.lock:
343 n = len(self.times)
344 tn = time.time()
345 t0 = self.times[0]
346
347 total = sum(self.sizes)
348 bytes_per_s = total / (tn - t0)
349 mean = total / n
350
351
352
353
354 max_s = max(self.sizes)
355 min_s = min(self.sizes)
356
357
358 if bytes_per_s < 1000:
359 bw, mean, min_s, max_s = ["%.2fB"%v for v in [bytes_per_s, mean, min_s, max_s]]
360 elif bytes_per_s < 1000000:
361 bw, mean, min_s, max_s = ["%.2fKB"%(v/1000) for v in [bytes_per_s, mean, min_s, max_s]]
362 else:
363 bw, mean, min_s, max_s = ["%.2fMB"%(v/1000000) for v in [bytes_per_s, mean, min_s, max_s]]
364
365 print("average: %s/s\n\tmean: %s min: %s max: %s window: %s"%(bw, mean, min_s, max_s, n))
368 valid_types = [str]
369 try:
370 valid_types.append(unicode)
371 except NameError:
372 pass
373 return t in valid_types
374
376 """
377 periodically print the received bandwidth of a topic to console until
378 shutdown
379 """
380 _check_master()
381 _, real_topic, _ = get_topic_type(topic, blocking=True)
382 if rospy.is_shutdown():
383 return
384
385 rospy.init_node(NAME, anonymous=True, disable_rostime=True)
386 rt = ROSTopicBandwidth(window_size)
387
388
389 sub = rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback)
390 print("subscribed to [%s]"%real_topic)
391 while not rospy.is_shutdown():
392 _sleep(1.0)
393 rt.print_bw()
394
395
397 """
398 Generates a function that returns the relevant field(s) (aka 'subtopic(s)') of a Message object
399 :param pattern: subtopic, e.g. /x[2:]/y[:-1]/z, ``str``
400 :returns: function that converts a message into the desired value, ``fn(Message) -> value``
401 """
402 evals = []
403 fields = [f for f in pattern.split('/') if f]
404 for f in fields:
405 if '[' in f:
406 field_name, rest = f.split('[', 1)
407 if not rest.endswith(']'):
408 print("missing closing ']' in slice spec '%s'" % f, file=sys.stderr)
409 return None
410 rest = rest[:-1]
411 try:
412 array_index_or_slice_object = _get_array_index_or_slice_object(rest)
413 except AssertionError as e:
414 print("field '%s' has invalid slice argument '%s': %s"
415 % (field_name, rest, str(e)), file=sys.stderr)
416 return None
417 evals.append((field_name, array_index_or_slice_object))
418 else:
419 evals.append((f, None))
420
421 def msgeval(msg, evals):
422 for i, (field_name, slice_object) in enumerate(evals):
423 try:
424 msg = getattr(msg, field_name)
425 except AttributeError:
426 print("no field named %s in %s" % (field_name, pattern), file=sys.stderr)
427 return None
428
429 if slice_object is not None:
430 try:
431 msg = msg.__getitem__(slice_object)
432 except IndexError as e:
433 print("%s: %s" % (str(e), pattern), file=sys.stderr)
434 return None
435
436
437
438
439 if isinstance(msg, list):
440 rest = evals[i + 1:]
441 return [msgeval(m, rest) for m in msg]
442 return msg
443
444 return (lambda msg: msgeval(msg, evals)) if evals else None
445
446
448 assert index_string != '', 'empty array index'
449 index_string_parts = index_string.split(':')
450 if len(index_string_parts) == 1:
451 try:
452 array_index = int(index_string_parts[0])
453 except ValueError:
454 assert False, "non-integer array index step '%s'" % index_string_parts[0]
455 return array_index
456
457 slice_args = [None, None, None]
458 if index_string_parts[0] != '':
459 try:
460 slice_args[0] = int(index_string_parts[0])
461 except ValueError:
462 assert False, "non-integer slice start '%s'" % index_string_parts[0]
463 if index_string_parts[1] != '':
464 try:
465 slice_args[1] = int(index_string_parts[1])
466 except ValueError:
467 assert False, "non-integer slice stop '%s'" % index_string_parts[1]
468 if len(index_string_parts) > 2 and index_string_parts[2] != '':
469 try:
470 slice_args[2] = int(index_string_parts[2])
471 except ValueError:
472 assert False, "non-integer slice step '%s'" % index_string_parts[2]
473 if len(index_string_parts) > 3:
474 assert False, 'too many slice arguments'
475 return slice(*slice_args)
476
478 value = msg
479 for attr in nested_attributes.split('/'):
480 value = getattr(value, attr)
481 return value
482
484 """
485 subroutine for getting the topic type
486 :returns: topic type, real topic name and fn to evaluate the message instance
487 if the topic points to a field within a topic, e.g. /rosout/msg, ``(str, str, fn)``
488 """
489 try:
490 val = _master_get_topic_types(rosgraph.Master('/rostopic'))
491 except socket.error:
492 raise ROSTopicIOException("Unable to communicate with master!")
493
494
495 matches = [(t, t_type) for t, t_type in val if t == topic]
496 if not matches:
497 matches = [(t, t_type) for t, t_type in val if topic.startswith(t+'/')]
498
499 matches.sort(key=itemgetter(0), reverse=True)
500
501
502 while matches:
503 t, t_type = matches[0]
504 msg_class = roslib.message.get_message_class(t_type)
505 if not msg_class:
506
507 break
508 msg = msg_class()
509 nested_attributes = topic[len(t) + 1:].rstrip('/')
510 nested_attributes = nested_attributes.split('[')[0]
511 if nested_attributes == '':
512 break
513 try:
514 _get_nested_attribute(msg, nested_attributes)
515 except AttributeError:
516
517 matches.pop(0)
518 continue
519 matches = [(t, t_type)]
520 break
521
522 if matches:
523 t, t_type = matches[0]
524 if t_type == rosgraph.names.ANYTYPE:
525 return None, None, None
526 return t_type, t, msgevalgen(topic[len(t):])
527 else:
528 return None, None, None
529
530
531
533 """
534 Get the topic type.
535
536 :param topic: topic name, ``str``
537 :param blocking: (default False) block until topic becomes available, ``bool``
538
539 :returns: topic type, real topic name and fn to evaluate the message instance
540 if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)``
541 :raises: :exc:`ROSTopicException` If master cannot be contacted
542 """
543 topic_type, real_topic, msg_eval = _get_topic_type(topic)
544 if topic_type:
545 return topic_type, real_topic, msg_eval
546 elif blocking:
547 sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic)
548 while not rospy.is_shutdown():
549 topic_type, real_topic, msg_eval = _get_topic_type(topic)
550 if topic_type:
551 return topic_type, real_topic, msg_eval
552 else:
553 _sleep(0.1)
554 return None, None, None
555
557 """
558 Get the topic message class
559 :returns: message class for topic, real topic
560 name, and function for evaluating message objects into the subtopic
561 (or ``None``). ``(Message, str, str)``
562 :raises: :exc:`ROSTopicException` If topic type cannot be determined or loaded
563 """
564 topic_type, real_topic, msg_eval = get_topic_type(topic, blocking=blocking)
565 if topic_type is None:
566 return None, None, None
567 msg_class = roslib.message.get_message_class(topic_type)
568 if not msg_class:
569 raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % topic_type)
570 return msg_class, real_topic, msg_eval
571
573 """
574 get CSV representation of fields used by _str_plot
575 :returns: list of fields as a CSV string, ``str``
576 """
577 s = _sub_str_plot_fields(val, f, field_filter)
578 if s is not None:
579 return "time,"+s
580 else:
581 return 'time,'
582
584 """recursive helper function for _str_plot_fields"""
585
586 type_ = type(val)
587 if type_ in (bool, int, float) or \
588 isinstance(val, genpy.TVal):
589 return f
590
591 elif hasattr(val, "_slot_types"):
592 if field_filter is not None:
593 fields = list(field_filter(val))
594 else:
595 fields = val.__slots__
596 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)
597 sub = [s for s in sub if s is not None]
598 if sub:
599 return ','.join([s for s in sub])
600 elif _isstring_type(type_):
601 return f
602 elif type_ in (list, tuple):
603 if len(val) == 0:
604 return None
605 val0 = val[0]
606 type0 = type(val0)
607
608 if type0 in (bool, int, float) or \
609 isinstance(val0, genpy.TVal):
610 return ','.join(["%s%s"%(f,x) for x in range(0,len(val))])
611 elif _isstring_type(type0):
612
613 return ','.join(["%s%s"%(f,x) for x in range(0,len(val))])
614 elif hasattr(val0, "_slot_types"):
615 labels = ["%s%s"%(f,x) for x in range(0,len(val))]
616 sub = [s for s in [_sub_str_plot_fields(v, sf, field_filter) for v,sf in zip(val, labels)] if s]
617 if sub:
618 return ','.join([s for s in sub])
619 return None
620
621
622 -def _str_plot(val, time_offset=None, current_time=None, field_filter=None, type_information=None, fixed_numeric_width=None):
623 """
624 Convert value to matlab/octave-friendly CSV string representation.
625
626 :param val: message
627 :param current_time: current :class:`genpy.Time` to use if message does not contain its own timestamp.
628 :param time_offset: (optional) for time printed for message, print as offset against this :class:`genpy.Time`
629 :param field_filter: filter the fields that are strified for Messages, ``fn(Message)->iter(str)``
630 :returns: comma-separated list of field values in val, ``str``
631 """
632
633 s = _sub_str_plot(val, time_offset, field_filter)
634 if s is None:
635 s = ''
636
637 if time_offset is not None:
638 time_offset = time_offset.to_nsec()
639 else:
640 time_offset = 0
641
642 if current_time is not None:
643 return "%s,%s"%(current_time.to_nsec()-time_offset, s)
644 elif getattr(val, "_has_header", False):
645 return "%s,%s"%(val.header.stamp.to_nsec()-time_offset, s)
646 else:
647 return "%s,%s"%(rospy.get_rostime().to_nsec()-time_offset, s)
648
650 """Helper routine for _str_plot."""
651
652 type_ = type(val)
653
654 if type_ == bool:
655 return '1' if val else '0'
656 elif type_ in (int, float) or \
657 isinstance(val, genpy.TVal):
658 if time_offset is not None and isinstance(val, genpy.Time):
659 return str(val-time_offset)
660 else:
661 return str(val)
662 elif hasattr(val, "_slot_types"):
663 if field_filter is not None:
664 fields = list(field_filter(val))
665 else:
666 fields = val.__slots__
667
668 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)
669 sub = [s for s in sub if s is not None]
670 if sub:
671 return ','.join(sub)
672 elif _isstring_type(type_):
673 return val
674 elif type_ in (list, tuple):
675 if len(val) == 0:
676 return None
677 val0 = val[0]
678
679 type0 = type(val0)
680 if type0 == bool:
681 return ','.join([('1' if v else '0') for v in val])
682 elif type0 in (int, float) or \
683 isinstance(val0, genpy.TVal):
684 return ','.join([str(v) for v in val])
685 elif _isstring_type(type0):
686 return ','.join([v for v in val])
687 elif hasattr(val0, "_slot_types"):
688 sub = [s for s in [_sub_str_plot(v, time_offset, field_filter) for v in val] if s is not None]
689 if sub:
690 return ','.join([s for s in sub])
691 return None
692
693
695 """
696 Convert atttribute types on the fly, if necessary. This is mainly
697 to convert uint8[] fields back to an array type.
698 """
699 attr = getattr(val, f)
700 if _isstring_type(type(attr)) and 'uint8[' in t:
701 return [ord(x) for x in attr]
702 else:
703 return attr
704
706 """
707 Callback instance that can print callback data in a variety of
708 formats. Used for all variants of rostopic echo
709 """
710
711 - def __init__(self, topic, msg_eval, plot=False, filter_fn=None,
712 echo_clear=False, echo_all_topics=False,
713 offset_time=False, count=None,
714 field_filter_fn=None, fixed_numeric_width=None):
715 """
716 :param plot: if ``True``, echo in plotting-friendly format (csv), ``bool``
717 :param filter_fn: function that evaluates to ``True`` if message is to be echo'd, ``fn(topic, msg)``
718 :param echo_all_topics: (optional) if ``True``, echo all messages in bag, ``bool``
719 :param offset_time: (optional) if ``True``, display time as offset from current time, ``bool``
720 :param count: number of messages to echo, ``None`` for infinite, ``int``
721 :param field_filter_fn: filter the fields that are strified for Messages, ``fn(Message)->iter(str)``
722 :param fixed_numeric_width: fixed width for numeric values, ``None`` for automatic, ``int``
723 """
724 if topic and topic[-1] == '/':
725 topic = topic[:-1]
726 self.topic = topic
727 self.msg_eval = msg_eval
728 self.plot = plot
729 self.filter_fn = filter_fn
730 self.fixed_numeric_width = fixed_numeric_width
731
732 self.prefix = ''
733 self.suffix = '\n---' if not plot else ''
734
735 self.echo_all_topics = echo_all_topics
736 self.offset_time = offset_time
737
738
739 self.done = False
740 self.max_count = count
741 self.count = 0
742
743
744 if plot:
745
746 self.str_fn = _str_plot
747 self.sep = ''
748 else:
749
750 self.str_fn = self.custom_strify_message
751 if echo_clear:
752 self.prefix = '\033[2J\033[;H'
753
754 self.field_filter=field_filter_fn
755
756
757 self.first = True
758
759
760 self.last_topic = None
761 self.last_msg_eval = None
762
763 - def custom_strify_message(self, val, indent='', time_offset=None, current_time=None, field_filter=None, type_information=None, fixed_numeric_width=None):
764
765 if type_information and type_information.startswith('uint8['):
766 val = [ord(x) for x in val]
767 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)
768
769 - def callback(self, data, callback_args, current_time=None):
770 """
771 Callback to pass to rospy.Subscriber or to call
772 manually. rospy.Subscriber constructor must also pass in the
773 topic name as an additional arg
774 :param data: Message
775 :param topic: topic name, ``str``
776 :param current_time: override calculation of current time, :class:`genpy.Time`
777 """
778 topic = callback_args['topic']
779 type_information = callback_args.get('type_information', None)
780 if self.filter_fn is not None and not self.filter_fn(data):
781 return
782
783 if self.max_count is not None and self.count >= self.max_count:
784 self.done = True
785 return
786
787 try:
788 msg_eval = self.msg_eval
789 if topic == self.topic:
790 pass
791 elif self.topic.startswith(topic + '/'):
792
793 if topic == self.last_topic:
794
795 msg_eval = self.last_msg_eval
796 else:
797
798 self.last_msg_eval = msg_eval = msgevalgen(self.topic[len(topic):])
799 self.last_topic = topic
800 elif not self.echo_all_topics:
801 return
802
803 if msg_eval is not None:
804 data = msg_eval(data)
805
806
807 if data is not None:
808
809
810 self.count += 1
811
812
813 if self.plot and self.first:
814 sys.stdout.write("%"+_str_plot_fields(data, 'field', self.field_filter)+'\n')
815 self.first = False
816
817 if self.offset_time:
818 sys.stdout.write(self.prefix+\
819 self.str_fn(data, time_offset=rospy.get_rostime(),
820 current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width) + \
821 self.suffix + '\n')
822 else:
823 sys.stdout.write(self.prefix+\
824 self.str_fn(data,
825 current_time=current_time, field_filter=self.field_filter, type_information=type_information, fixed_numeric_width=self.fixed_numeric_width) + \
826 self.suffix + '\n')
827
828
829 sys.stdout.flush()
830
831 if self.max_count is not None and self.count >= self.max_count:
832 self.done = True
833
834 except IOError:
835 self.done = True
836 except:
837
838 self.done = True
839 traceback.print_exc()
840
842 """
843 Print ROS message type of topic to screen
844 :param topic: topic name, ``str``
845 """
846 t, _, _ = get_topic_type(topic, blocking=False)
847 if t:
848 print(t)
849 else:
850 sys.stderr.write('unknown topic type [%s]\n'%topic)
851 sys.exit(1)
852
854 """
855 :param callback_echo: :class:`CallbackEcho` instance to invoke on new messages in bag file
856 :param bag_file: name of bag file to echo messages from or ``None``, ``str``
857 """
858 if not os.path.exists(bag_file):
859 raise ROSTopicException("bag file [%s] does not exist"%bag_file)
860 first = True
861
862 import rosbag
863 with rosbag.Bag(bag_file) as b:
864 for t, msg, timestamp in b.read_messages():
865
866
867 if t[0] != '/':
868 t = rosgraph.names.script_resolve_name('rostopic', t)
869 callback_echo.callback(msg, {'topic': t}, current_time=timestamp)
870
871 if callback_echo.done:
872 break
873
874 -def _rostopic_echo(topic, callback_echo, bag_file=None, echo_all_topics=False):
875 """
876 Print new messages on topic to screen.
877
878 :param topic: topic name, ``str``
879 :param bag_file: name of bag file to echo messages from or ``None``, ``str``
880 """
881
882
883 if bag_file:
884
885 rospy.rostime.set_rostime_initialized(True)
886 _rostopic_echo_bag(callback_echo, bag_file)
887 else:
888 _check_master()
889 rospy.init_node(NAME, anonymous=True)
890 msg_class, real_topic, msg_eval = get_topic_class(topic, blocking=True)
891 if msg_class is None:
892
893 return
894 callback_echo.msg_eval = msg_eval
895
896
897 type_information = None
898 if len(topic) > len(real_topic):
899 subtopic = topic[len(real_topic):]
900 subtopic = subtopic.strip('/')
901 if subtopic:
902 fields = subtopic.split('/')
903 submsg_class = msg_class
904 while fields:
905 field = fields[0].split('[')[0]
906 del fields[0]
907 index = submsg_class.__slots__.index(field)
908 type_information = submsg_class._slot_types[index]
909 if fields:
910 submsg_class = roslib.message.get_message_class(type_information.split('[', 1)[0])
911 if not submsg_class:
912 raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?" % type_information)
913
914 use_sim_time = rospy.get_param('/use_sim_time', False)
915 sub = rospy.Subscriber(real_topic, msg_class, callback_echo.callback, {'topic': topic, 'type_information': type_information})
916
917 if use_sim_time:
918
919
920 timeout_t = time.time() + 2.
921 while time.time() < timeout_t and \
922 callback_echo.count == 0 and \
923 not rospy.is_shutdown() and \
924 not callback_echo.done:
925 _sleep(0.1)
926
927 if callback_echo.count == 0 and \
928 not rospy.is_shutdown() and \
929 not callback_echo.done:
930 sys.stderr.write("WARNING: no messages received and simulated time is active.\nIs /clock being published?\n")
931
932 while not rospy.is_shutdown() and not callback_echo.done:
933 _sleep(0.1)
934
935 _caller_apis = {}
937 """
938 Get XML-RPC API of node
939 :param master: XML-RPC handle to ROS Master, :class:`xmlrpclib.ServerProxy`
940 :param caller_id: node name, ``str``
941 :returns: XML-RPC URI of node, ``str``
942 :raises: :exc:`ROSTopicIOException` If unable to communicate with master
943 """
944 caller_api = _caller_apis.get(caller_id, None)
945 if not caller_api:
946 try:
947 caller_api = master.lookupNode(caller_id)
948 _caller_apis[caller_id] = caller_api
949 except socket.error:
950 raise ROSTopicIOException("Unable to communicate with master!")
951 except rosgraph.MasterError:
952 caller_api = 'unknown address %s'%caller_id
953
954 return caller_api
955
957 """
958 Prints topics in bag file to screen
959 :param bag_file: path to bag file, ``str``
960 :param topic: optional topic name to match. Will print additional information just about messagese in this topic, ``str``
961 """
962 import rosbag
963 if not os.path.exists(bag_file):
964 raise ROSTopicException("bag file [%s] does not exist"%bag_file)
965
966 with rosbag.Bag(bag_file) as b:
967 if topic:
968
969 topic_ns = rosgraph.names.make_global_ns(topic)
970 count = 0
971 earliest = None
972 latest = None
973 for top, msg, t in b.read_messages(raw=True):
974 if top == topic or top.startswith(topic_ns):
975 count += 1
976 if earliest == None:
977 earliest = t
978
979 latest = t
980 if rospy.is_shutdown():
981 break
982 import time
983 earliest, latest = [time.strftime("%d %b %Y %H:%M:%S", time.localtime(t.to_time())) for t in (earliest, latest)]
984 print("%s message(s) from %s to %s"%(count, earliest, latest))
985 else:
986 topics = set()
987 for top, msg, _ in b.read_messages(raw=True):
988 if top not in topics:
989 print(top)
990 topics.add(top)
991 if rospy.is_shutdown():
992 break
993
994 -def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, verbose, indent=''):
995 def topic_type(t, topic_types):
996 matches = [t_type for t_name, t_type in topic_types if t_name == t]
997 if matches:
998 return matches[0]
999 return 'unknown type'
1000
1001 if verbose:
1002 topic_types = _master_get_topic_types(master)
1003
1004 if not subscribers_only:
1005 print("\n%sPublished topics:"%indent)
1006 for t, l in pubs:
1007 if len(l) > 1:
1008 print(indent+" * %s [%s] %s publishers"%(t, topic_type(t, topic_types), len(l)))
1009 else:
1010 print(indent+" * %s [%s] 1 publisher"%(t, topic_type(t, topic_types)))
1011
1012 if not publishers_only:
1013 print(indent)
1014 print(indent+"Subscribed topics:")
1015 for t,l in subs:
1016 if len(l) > 1:
1017 print(indent+" * %s [%s] %s subscribers"%(t, topic_type(t, topic_types), len(l)))
1018 else:
1019 print(indent+" * %s [%s] 1 subscriber"%(t, topic_type(t, topic_types)))
1020 print('')
1021 else:
1022 if publishers_only:
1023 topics = [t for t,_ in pubs]
1024 elif subscribers_only:
1025 topics = [t for t,_ in subs]
1026 else:
1027 topics = list(set([t for t,_ in pubs] + [t for t,_ in subs]))
1028 topics.sort()
1029 print('\n'.join(["%s%s"%(indent, t) for t in topics]))
1030
1031
1033 """
1034 Build up maps for hostname to topic list per hostname
1035 :returns: publishers host map, subscribers host map, ``{str: set(str)}, {str: set(str)}``
1036 """
1037 def build_map(master, state, uricache):
1038 tmap = {}
1039 for topic, tnodes in state:
1040 for p in tnodes:
1041 if not p in uricache:
1042 uricache[p] = master.lookupNode(p)
1043 uri = uricache[p]
1044 puri = urlparse(uri)
1045 if not puri.hostname in tmap:
1046 tmap[puri.hostname] = []
1047
1048 matches = [l for x, l in tmap[puri.hostname] if x == topic]
1049 if matches:
1050 matches[0].append(p)
1051 else:
1052 tmap[puri.hostname].append((topic, [p]))
1053 return tmap
1054
1055 uricache = {}
1056 host_pub_topics = build_map(master, pubs, uricache)
1057 host_sub_topics = build_map(master, subs, uricache)
1058 return host_pub_topics, host_sub_topics
1059
1060 -def _rostopic_list(topic, verbose=False,
1061 subscribers_only=False, publishers_only=False,
1062 group_by_host=False):
1063 """
1064 Print topics to screen
1065
1066 :param topic: topic name to list information or None to match all topics, ``str``
1067 :param verbose: print additional debugging information, ``bool``
1068 :param subscribers_only: print information about subscriptions only, ``bool``
1069 :param publishers_only: print information about subscriptions only, ``bool``
1070 :param group_by_host: group topic list by hostname, ``bool``
1071 """
1072
1073 if subscribers_only and publishers_only:
1074 raise ROSTopicException("cannot specify both subscribers- and publishers-only")
1075
1076 master = rosgraph.Master('/rostopic')
1077 try:
1078 state = master.getSystemState()
1079
1080 pubs, subs, _ = state
1081 if topic:
1082
1083 topic_ns = rosgraph.names.make_global_ns(topic)
1084 subs = (x for x in subs if x[0] == topic or x[0].startswith(topic_ns))
1085 pubs = (x for x in pubs if x[0] == topic or x[0].startswith(topic_ns))
1086
1087 except socket.error:
1088 raise ROSTopicIOException("Unable to communicate with master!")
1089
1090 if group_by_host:
1091
1092 host_pub_topics, host_sub_topics = _rostopic_list_group_by_host(master, pubs, subs)
1093 for hostname in set(list(host_pub_topics.keys()) + list(host_sub_topics.keys())):
1094 pubs, subs = host_pub_topics.get(hostname,[]), host_sub_topics.get(hostname, []),
1095 if (pubs and not subscribers_only) or (subs and not publishers_only):
1096 print("Host [%s]:" % hostname)
1097 _sub_rostopic_list(master, pubs, subs,
1098 publishers_only, subscribers_only,
1099 verbose, indent=' ')
1100 else:
1101 _sub_rostopic_list(master, pubs, subs,
1102 publishers_only, subscribers_only,
1103 verbose)
1104
1105 -def get_info_text(topic):
1106 """
1107 Get human-readable topic description
1108
1109 :param topic: topic name, ``str``
1110 """
1111 try:
1112 from cStringIO import StringIO
1113 except ImportError:
1114 from io import StringIO
1115 import itertools
1116 buff = StringIO()
1117 def topic_type(t, topic_types):
1118 matches = [t_type for t_name, t_type in topic_types if t_name == t]
1119 if matches:
1120 return matches[0]
1121 return 'unknown type'
1122
1123 master = rosgraph.Master('/rostopic')
1124 try:
1125 state = master.getSystemState()
1126
1127 pubs, subs, _ = state
1128
1129 subs = [x for x in subs if x[0] == topic]
1130 pubs = [x for x in pubs if x[0] == topic]
1131
1132 topic_types = _master_get_topic_types(master)
1133
1134 except socket.error:
1135 raise ROSTopicIOException("Unable to communicate with master!")
1136
1137 if not pubs and not subs:
1138 raise ROSTopicException("Unknown topic %s"%topic)
1139
1140 buff.write("Type: %s\n\n"%topic_type(topic, topic_types))
1141
1142 if pubs:
1143 buff.write("Publishers: \n")
1144 for p in itertools.chain(*[l for x, l in pubs]):
1145 buff.write(" * %s (%s)\n"%(p, get_api(master, p)))
1146 else:
1147 buff.write("Publishers: None\n")
1148 buff.write('\n')
1149
1150 if subs:
1151 buff.write("Subscribers: \n")
1152 for p in itertools.chain(*[l for x, l in subs]):
1153 buff.write(" * %s (%s)\n"%(p, get_api(master, p)))
1154 else:
1155 buff.write("Subscribers: None\n")
1156 buff.write('\n')
1157 return buff.getvalue()
1158
1160 """
1161 Print topic information to screen.
1162
1163 :param topic: topic name, ``str``
1164 """
1165 print(get_info_text(topic))
1166
1167
1168
1169
1171 def expr_eval(expr):
1172 def eval_fn(m):
1173 return eval(expr)
1174 return eval_fn
1175
1176 args = argv[2:]
1177 from optparse import OptionParser
1178 parser = OptionParser(usage="usage: %prog echo [options] /topic", prog=NAME)
1179 parser.add_option("-b", "--bag",
1180 dest="bag", default=None,
1181 help="echo messages from .bag file", metavar="BAGFILE")
1182 parser.add_option("-p",
1183 dest="plot", default=False,
1184 action="store_true",
1185 help="echo in a plotting friendly format")
1186 parser.add_option("-w",
1187 dest="fixed_numeric_width", default=None, metavar="NUM_WIDTH",
1188 help="fixed width for numeric values")
1189 parser.add_option("--filter",
1190 dest="filter_expr", default=None,
1191 metavar="FILTER-EXPRESSION",
1192 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).")
1193 parser.add_option("--nostr",
1194 dest="nostr", default=False,
1195 action="store_true",
1196 help="exclude string fields")
1197 parser.add_option("--noarr",
1198 dest="noarr", default=False,
1199 action="store_true",
1200 help="exclude arrays")
1201 parser.add_option("-c", "--clear",
1202 dest="clear", default=False,
1203 action="store_true",
1204 help="clear screen before printing next message")
1205 parser.add_option("-a", "--all",
1206 dest="all_topics", default=False,
1207 action="store_true",
1208 help="display all message in bag, only valid with -b option")
1209 parser.add_option("-n",
1210 dest="msg_count", default=None, metavar="COUNT",
1211 help="number of messages to echo")
1212 parser.add_option("--offset",
1213 dest="offset_time", default=False,
1214 action="store_true",
1215 help="display time as offsets from current time (in seconds)")
1216
1217 (options, args) = parser.parse_args(args)
1218 if len(args) > 1:
1219 parser.error("you may only specify one input topic")
1220 if options.all_topics and not options.bag:
1221 parser.error("Display all option is only valid when echoing from bag files")
1222 if options.offset_time and options.bag:
1223 parser.error("offset time option is not valid with bag files")
1224 if options.all_topics:
1225 topic = ''
1226 else:
1227 if len(args) == 0:
1228 parser.error("topic must be specified")
1229 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
1230
1231
1232
1233
1234 filter_fn = None
1235 if options.filter_expr:
1236 filter_fn = expr_eval(options.filter_expr)
1237
1238 try:
1239 msg_count = int(options.msg_count) if options.msg_count else None
1240 except ValueError:
1241 parser.error("COUNT must be an integer")
1242
1243 try:
1244 fixed_numeric_width = int(options.fixed_numeric_width) if options.fixed_numeric_width else None
1245 if fixed_numeric_width is not None and fixed_numeric_width < 2:
1246 parser.error("Fixed width for numeric values must be at least 2")
1247 except ValueError:
1248 parser.error("NUM_WIDTH must be an integer")
1249
1250 field_filter_fn = create_field_filter(options.nostr, options.noarr)
1251 callback_echo = CallbackEcho(topic, None, plot=options.plot,
1252 filter_fn=filter_fn,
1253 echo_clear=options.clear, echo_all_topics=options.all_topics,
1254 offset_time=options.offset_time, count=msg_count,
1255 field_filter_fn=field_filter_fn, fixed_numeric_width=fixed_numeric_width)
1256 try:
1257 _rostopic_echo(topic, callback_echo, bag_file=options.bag)
1258 except socket.error:
1259 sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n")
1260
1262 def field_filter(val):
1263 fields = val.__slots__
1264 field_types = val._slot_types
1265 for f, t in zip(val.__slots__, val._slot_types):
1266 if echo_noarr and '[' in t:
1267 continue
1268 elif echo_nostr and 'string' in t:
1269 continue
1270 yield f
1271 return field_filter
1272
1274 args = argv[2:]
1275 from optparse import OptionParser
1276 parser = OptionParser(usage="usage: %%prog %s /topic"%cmd, prog=NAME)
1277 (options, args) = parser.parse_args(args)
1278 if len(args) == 0:
1279 parser.error("topic must be specified")
1280 if len(args) > 1:
1281 parser.error("you may only specify one input topic")
1282 return rosgraph.names.script_resolve_name('rostopic', args[0])
1283
1285 _rostopic_type(_optparse_topic_only('type', argv))
1286
1288 args = argv[2:]
1289 from optparse import OptionParser
1290 parser = OptionParser(usage="usage: %prog hz /topic", prog=NAME)
1291 parser.add_option("-w", "--window",
1292 dest="window_size", default=-1,
1293 help="window size, in # of messages, for calculating rate", metavar="WINDOW")
1294 parser.add_option("--filter",
1295 dest="filter_expr", default=None,
1296 help="only measure messages matching the specified Python expression", metavar="EXPR")
1297 parser.add_option("--wall-time",
1298 dest="use_wtime", default=False, action="store_true",
1299 help="calculates rate using wall time which can be helpful when clock isnt published during simulation")
1300
1301 (options, args) = parser.parse_args(args)
1302 if len(args) == 0:
1303 parser.error("topic must be specified")
1304 if len(args) > 1:
1305 parser.error("you may only specify one input topic")
1306 try:
1307 if options.window_size != -1:
1308 import string
1309 window_size = string.atoi(options.window_size)
1310 else:
1311 window_size = options.window_size
1312 except:
1313 parser.error("window size must be an integer")
1314 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
1315
1316
1317 if options.filter_expr:
1318 def expr_eval(expr):
1319 def eval_fn(m):
1320 return eval(expr)
1321 return eval_fn
1322 filter_expr = expr_eval(options.filter_expr)
1323 else:
1324 filter_expr = None
1325 _rostopic_hz(topic, window_size=window_size, filter_expr=filter_expr,
1326 use_wtime=options.use_wtime)
1327
1328
1330 args = argv[2:]
1331 import argparse
1332 parser = argparse.ArgumentParser(usage="%(prog)s delay [options] /topic", prog=NAME)
1333 parser.add_argument("topic", help="topic name to be calcurated the delay")
1334 parser.add_argument("-w", "--window",
1335 dest="window_size", default=-1, type=int,
1336 help="window size, in # of messages, for calculating rate")
1337
1338 args = parser.parse_args(args)
1339 topic_name = args.topic
1340 window_size = args.window_size
1341 topic = rosgraph.names.script_resolve_name('rostopic', topic_name)
1342 _rostopic_delay(topic, window_size=window_size)
1343
1344
1346 args = argv[2:]
1347 from optparse import OptionParser
1348 parser = OptionParser(usage="usage: %prog bw /topic", prog=NAME)
1349 parser.add_option("-w", "--window",
1350 dest="window_size", default=None,
1351 help="window size, in # of messages, for calculating rate", metavar="WINDOW")
1352 options, args = parser.parse_args(args)
1353 if len(args) == 0:
1354 parser.error("topic must be specified")
1355 if len(args) > 1:
1356 parser.error("you may only specify one input topic")
1357 try:
1358 if options.window_size:
1359 import string
1360 window_size = string.atoi(options.window_size)
1361 else:
1362 window_size = options.window_size
1363 except:
1364 parser.error("window size must be an integer")
1365 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
1366 _rostopic_bw(topic, window_size=window_size)
1367
1369 """
1370 Lookup topics by topic_type
1371 :param topic_type: type of topic to find, ``str``
1372 :returns: list of topic names that use topic_type, ``[str]``
1373 """
1374 master = rosgraph.Master('/rostopic')
1375 try:
1376 t_list = _master_get_topic_types(master)
1377 except socket.error:
1378 raise ROSTopicIOException("Unable to communicate with master!")
1379 return [t_name for t_name, t_type in t_list if t_type == topic_type]
1380
1382 """
1383 Implements 'rostopic type'
1384 :param argv: command-line args, ``[str]``
1385 """
1386 args = argv[2:]
1387 from optparse import OptionParser
1388 parser = OptionParser(usage="usage: %prog find msg-type", prog=NAME)
1389 options, args = parser.parse_args(args)
1390 if not len(args):
1391 parser.error("please specify a message type")
1392 if len(args) > 1:
1393 parser.error("you may only specify one message type")
1394 print('\n'.join(find_by_type(args[0])))
1395
1396
1398 """
1399 pkg/typeName -> pkg, typeName -> None
1400
1401 :param name: package resource name, e.g. 'std_msgs/String', ``str``
1402 :returns: package name of resource, ``str``
1403 """
1404 if not '/' in name:
1405 return None
1406 return name[:name.find('/')]
1407
1409 """
1410 Create rospy.Publisher instance from the string topic name and
1411 type. This is a powerful method as it allows creation of
1412 rospy.Publisher and Message instances using the topic and type
1413 names. This enables more dynamic publishing from Python programs.
1414
1415 :param topic_name: name of topic, ``str``
1416 :param topic_type: name of topic type, ``str``
1417 :param latch: latching topic, ``bool``
1418 :returns: topic :class:`rospy.Publisher`, :class:`Message` class
1419 """
1420 topic_name = rosgraph.names.script_resolve_name('rostopic', topic_name)
1421 try:
1422 msg_class = roslib.message.get_message_class(topic_type)
1423 except:
1424 raise ROSTopicException("invalid topic type: %s"%topic_type)
1425 if msg_class is None:
1426 pkg = _resource_name_package(topic_type)
1427 raise ROSTopicException("invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'"%(topic_type, pkg))
1428
1429 rospy.init_node('rostopic', anonymous=True, disable_rosout=True, disable_rostime=True)
1430 pub = rospy.Publisher(topic_name, msg_class, latch=latch, queue_size=100)
1431 return pub, msg_class
1432
1433 -def _publish_at_rate(pub, msg, rate, verbose=False, substitute_keywords=False, pub_args=None):
1434 """
1435 Publish message at specified rate. Subroutine of L{publish_message()}.
1436
1437 :param pub: :class:rospy.Publisher` instance for topic
1438 :param msg: message instance to publish
1439 :param rate: publishing rate (hz) or None for just once, ``int``
1440 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1441 """
1442 try:
1443 r = rospy.Rate(float(rate))
1444 except ValueError:
1445 raise ROSTopicException("Rate must be a number")
1446 while not rospy.is_shutdown():
1447 if substitute_keywords:
1448 _fillMessageArgs(msg, pub_args)
1449 if verbose:
1450 print("publishing %s"%msg)
1451 pub.publish(msg)
1452 r.sleep()
1453
1454 _ONCE_DELAY = 3.
1456 """
1457 Publish and latch message. Subroutine of L{publish_message()}.
1458
1459 :param pub: :class:`rospy.Publisher` instance for topic
1460 :param msg: message instance to publish
1461 :param once: if ``True``, publish message once and then exit after sleep interval, ``bool``
1462 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1463 """
1464 try:
1465 pub.publish(msg)
1466 except TypeError as e:
1467 raise ROSTopicException(str(e))
1468
1469 if not once:
1470 rospy.spin()
1471
1472 -def publish_message(pub, msg_class, pub_args, rate=None, once=False, verbose=False, substitute_keywords=False):
1473 """
1474 Create new instance of msg_class, populate with pub_args, and publish. This may
1475 print output to screen.
1476
1477 :param pub: :class:`rospy.Publisher` instance for topic
1478 :param msg_class: Message type, ``Class``
1479 :param pub_args: Arguments to initialize message that is published, ``[val]``
1480 :param rate: publishing rate (hz) or None for just once, ``int``
1481 :param once: publish only once and return, ``bool``
1482 :param verbose: If ``True``, print more verbose output to stdout, ``bool``
1483 """
1484 msg = msg_class()
1485
1486 _fillMessageArgs(msg, pub_args)
1487
1488 try:
1489
1490 if rate is None:
1491 s = "publishing and latching [%s]"%(msg) if verbose else "publishing and latching message"
1492 if once:
1493 s = s + " for %s seconds"%_ONCE_DELAY
1494 else:
1495 s = s + ". Press ctrl-C to terminate"
1496 print(s)
1497
1498 _publish_latched(pub, msg, once, verbose)
1499 else:
1500 _publish_at_rate(pub, msg, rate, verbose=verbose, substitute_keywords=substitute_keywords, pub_args=pub_args)
1501
1502 except rospy.ROSSerializationException as e:
1503 import rosmsg
1504
1505 raise ROSTopicException("Unable to publish message. One of the fields has an incorrect type:\n"+\
1506 " %s\n\nmsg file:\n%s"%(e, rosmsg.get_msg_text(msg_class._type)))
1507
1509 try:
1510
1511
1512
1513
1514
1515
1516
1517
1518
1519
1520
1521 now = rospy.get_rostime()
1522 import std_msgs.msg
1523 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) }
1524 genpy.message.fill_message_args(msg, pub_args, keys=keys)
1525 except genpy.MessageException as e:
1526 raise ROSTopicException(str(e)+"\n\nArgs are: [%s]"%genpy.message.get_printable_message_args(msg))
1527
1529 """
1530 Parse 'pub' command arguments and run command. Will cause a system
1531 exit if command-line argument parsing fails.
1532 :param argv: command-line arguments
1533 :param argv: [str]
1534 :raises: :exc:`ROSTopicException` If call command cannot be executed
1535 """
1536 args = argv[2:]
1537 from optparse import OptionParser
1538 parser = OptionParser(usage="usage: %prog pub /topic type [args...]", prog=NAME)
1539 parser.add_option("-v", dest="verbose", default=False,
1540 action="store_true",
1541 help="print verbose output")
1542 parser.add_option("-r", "--rate", dest="rate", default=None,
1543 help="publishing rate (hz). For -f and stdin input, this defaults to 10. Otherwise it is not set.")
1544 parser.add_option("-1", "--once", action="store_true", dest="once", default=False,
1545 help="publish one message and exit")
1546 parser.add_option("-f", '--file', dest="file", metavar='FILE', default=None,
1547 help="read args from YAML file (Bagy)")
1548 parser.add_option("-l", '--latch', dest="latch", default=False, action="store_true",
1549 help="enable latching for -f, -r and piped input. This latches the first message.")
1550 parser.add_option("-s", '--substitute-keywords', dest="substitute_keywords", default=False, action="store_true",
1551 help="When publishing with a rate, performs keyword ('now' or 'auto') substitution for each message")
1552
1553
1554
1555 (options, args) = parser.parse_args(args)
1556 if options.rate is not None:
1557 if options.once:
1558 parser.error("You cannot select both -r and -1 (--once)")
1559 try:
1560 rate = float(options.rate)
1561 except ValueError:
1562 parser.error("rate must be a number")
1563 if rate <= 0:
1564 parser.error("rate must be greater than zero")
1565 else:
1566
1567 rate = None
1568
1569
1570 if len(args) == 0:
1571 parser.error("/topic must be specified")
1572 if len(args) == 1:
1573 parser.error("topic type must be specified")
1574 if 0:
1575 if len(args) > 2 and options.parameter:
1576 parser.error("args confict with -p setting")
1577 if len(args) > 2 and options.file:
1578 parser.error("args confict with -f setting")
1579 topic_name, topic_type = args[0], args[1]
1580
1581
1582 try:
1583 pub_args = []
1584 for arg in args[2:]:
1585 pub_args.append(yaml.load(arg))
1586 except Exception as e:
1587 parser.error("Argument error: "+str(e))
1588
1589
1590
1591 _check_master()
1592
1593
1594 latch = (rate == None) or options.latch
1595 pub, msg_class = create_publisher(topic_name, topic_type, latch)
1596
1597 if 0 and options.parameter:
1598 param_name = rosgraph.names.script_resolve_name('rostopic', options.parameter)
1599 if options.once:
1600 param_publish_once(pub, msg_class, param_name, rate, options.verbose)
1601 else:
1602 param_publish(pub, msg_class, param_name, rate, options.verbose)
1603
1604 elif not pub_args and len(msg_class.__slots__):
1605 if not options.file and sys.stdin.isatty():
1606 parser.error("Please specify message values")
1607
1608 if rate is None and not options.latch and not options.once:
1609 rate = 10.
1610 stdin_publish(pub, msg_class, rate, options.once, options.file, options.verbose)
1611 else:
1612 argv_publish(pub, msg_class, pub_args, rate, options.once, options.verbose, substitute_keywords=options.substitute_keywords)
1613
1614
1616 """
1617 :param filename: file name, ``str``
1618 :returns: Iterator that yields pub args (list of args), ``iterator``
1619 :raises: :exc:`ROSTopicException` If filename is invalid
1620 """
1621 if not os.path.isfile(filename):
1622 raise ROSTopicException("file does not exist: %s"%(filename))
1623 import yaml
1624 def bagy_iter():
1625 try:
1626 with open(filename, 'r') as f:
1627
1628 data = yaml.load_all(f)
1629 for d in data:
1630 yield [d]
1631 except yaml.YAMLError as e:
1632 raise ROSTopicException("invalid YAML in file: %s"%(str(e)))
1633 return bagy_iter
1634
1635 -def argv_publish(pub, msg_class, pub_args, rate, once, verbose, substitute_keywords=False):
1636 publish_message(pub, msg_class, pub_args, rate, once, verbose=verbose, substitute_keywords=substitute_keywords)
1637
1638 if once:
1639
1640 timeout_t = time.time() + _ONCE_DELAY
1641 while not rospy.is_shutdown() and time.time() < timeout_t:
1642 rospy.sleep(0.2)
1643
1644 SUBSCRIBER_TIMEOUT = 5.
1646 timeout_t = time.time() + timeout
1647 while pub.get_num_connections() == 0 and timeout_t > time.time():
1648 _sleep(0.01)
1649
1651 if not rospy.has_param(param_name):
1652 raise ROSTopicException("parameter does not exist: %s"%(param_name))
1653 pub_args = rospy.get_param(param_name)
1654 argv_publish(pub, msg_class, pub_args, None, True, verbose)
1655
1656
1658
1659 - def __init__(self, param_name, value=None):
1660 import threading
1661 self.lock = threading.Condition()
1662 self.param_name = param_name
1663 self.updates = []
1664 self.value = None
1665
1667 with self.lock:
1668
1669 if key != self.param_name:
1670 subs = [x for x in key[len(self.param_name):].split('/') if x]
1671 idx = self.value
1672 for s in subs[:-1]:
1673 if s in idx:
1674 idx = idx[s]
1675 else:
1676 idx[s] = {}
1677 idx = idx[s]
1678 idx[subs[-1]] = value
1679 else:
1680 self.value = value
1681
1682 self.updates.append(self.value)
1683 self.lock.notify_all()
1684
1686 """
1687 :param param_name: ROS parameter name, ``str``
1688 :returns: List of msg dicts in file, ``[{str: any}]``
1689 :raises: :exc:`ROSTopicException` If parameter is not set
1690 """
1691 import rospy
1692 import rospy.impl.paramserver
1693 import rosgraph
1694
1695 if not rospy.has_param(param_name):
1696 raise ROSTopicException("parameter does not exist: %s"%(param_name))
1697
1698
1699
1700 ps_cache = rospy.impl.paramserver.get_param_server_cache()
1701 notifier = _ParamNotifier(param_name)
1702 ps_cache.set_notifier(notifier)
1703 master = rosgraph.Master(rospy.get_name())
1704 notifier.value = master.subscribeParam(rospy.get_node_uri(), param_name)
1705 pub_args = notifier.value
1706 ps_cache.set(param_name, pub_args)
1707 if type(pub_args) == dict:
1708 pub_args = [pub_args]
1709 elif type(pub_args) != list:
1710 raise ROSTopicException("Parameter [%s] in not a valid type"%(param_name))
1711
1712 r = rospy.Rate(rate) if rate is not None else None
1713 publish = True
1714 while not rospy.is_shutdown():
1715 try:
1716 if publish:
1717 publish_message(pub, msg_class, pub_args, None, True, verbose=verbose)
1718 except ValueError as e:
1719 sys.stderr.write("%s\n"%str(e))
1720 break
1721 if r is not None:
1722 r.sleep()
1723 with notifier.lock:
1724 if notifier.updates:
1725 pub_args = notifier.updates.pop(0)
1726 if type(pub_args) == dict:
1727 pub_args = [pub_args]
1728 else:
1729 publish = False
1730 with notifier.lock:
1731 if not notifier.updates:
1732 notifier.lock.wait(1.)
1733 if notifier.updates:
1734 publish = True
1735 pub_args = notifier.updates.pop(0)
1736 if type(pub_args) == dict:
1737 pub_args = [pub_args]
1738
1739 if rospy.is_shutdown():
1740 break
1741
1742 -def stdin_publish(pub, msg_class, rate, once, filename, verbose):
1743 """
1744 :param filename: name of file to read from instead of stdin, or ``None``, ``str``
1745 """
1746 if filename:
1747 iterator = file_yaml_arg(filename)
1748 else:
1749 iterator = stdin_yaml_arg
1750
1751 r = rospy.Rate(rate) if rate is not None else None
1752
1753
1754
1755
1756 wait_for_subscriber(pub, SUBSCRIBER_TIMEOUT)
1757
1758 for pub_args in iterator():
1759 if rospy.is_shutdown():
1760 break
1761 if pub_args:
1762 if type(pub_args) != list:
1763 pub_args = [pub_args]
1764 try:
1765
1766
1767
1768
1769
1770
1771
1772 publish_message(pub, msg_class, pub_args, None, bool(r) or once, verbose=verbose)
1773 except ValueError as e:
1774 sys.stderr.write("%s\n"%str(e))
1775 break
1776 if r is not None:
1777 r.sleep()
1778 if rospy.is_shutdown() or once:
1779 break
1780
1782 """
1783 Iterate over YAML documents in stdin
1784 :returns: for next list of arguments on stdin. Iterator returns a list of args for each call, ``iterator``
1785 """
1786 import yaml
1787 from select import select
1788 from select import error as select_error
1789 try:
1790 arg = 'x'
1791 rlist = [sys.stdin]
1792 wlist = xlist = []
1793 while not rospy.is_shutdown() and arg != '\n':
1794 buff = ''
1795 while arg != '' and arg.strip() != '---' and not rospy.is_shutdown():
1796 val, _, _ = select(rlist, wlist, xlist, 1.0)
1797 if not val:
1798 continue
1799
1800 arg = sys.stdin.readline()
1801 if arg != '' and arg.strip() != '---':
1802 buff = buff + arg
1803
1804 if arg.strip() == '---':
1805 try:
1806 loaded = yaml.load(buff.rstrip())
1807 except Exception as e:
1808 sys.stderr.write("Invalid YAML: %s\n"%str(e))
1809 if loaded is not None:
1810 yield loaded
1811 elif arg == '':
1812
1813
1814 return
1815
1816 arg = 'x'
1817
1818 except select_error:
1819 return
1820
1822 """
1823 Command-line parsing for 'rostopic list' command.
1824 """
1825 args = argv[2:]
1826 from optparse import OptionParser
1827 parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME)
1828 parser.add_option("-b", "--bag",
1829 dest="bag", default=None,
1830 help="list topics in .bag file", metavar="BAGFILE")
1831 parser.add_option("-v", "--verbose",
1832 dest="verbose", default=False,action="store_true",
1833 help="list full details about each topic")
1834 parser.add_option("-p",
1835 dest="publishers", default=False,action="store_true",
1836 help="list only publishers")
1837 parser.add_option("-s",
1838 dest="subscribers", default=False,action="store_true",
1839 help="list only subscribers")
1840 parser.add_option("--host", dest="hostname", default=False, action="store_true",
1841 help="group by host name")
1842
1843 (options, args) = parser.parse_args(args)
1844 topic = None
1845
1846 if len(args) == 1:
1847 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
1848 elif len(args) > 1:
1849 parser.error("you may only specify one input topic")
1850 if options.bag:
1851 if options.subscribers:
1852 parser.error("-s option is not valid with bags")
1853 elif options.publishers:
1854 parser.error("-p option is not valid with bags")
1855 elif options.hostname:
1856 parser.error("--host option is not valid with bags")
1857 _rostopic_list_bag(options.bag, topic)
1858 else:
1859 if options.subscribers and options.publishers:
1860 parser.error("you may only specify one of -p, -s")
1861
1862 exitval = _rostopic_list(topic, verbose=options.verbose, subscribers_only=options.subscribers, publishers_only=options.publishers, group_by_host=options.hostname) or 0
1863 if exitval != 0:
1864 sys.exit(exitval)
1865
1867 """
1868 Command-line parsing for 'rostopic info' command.
1869 """
1870 args = argv[2:]
1871 from optparse import OptionParser
1872 parser = OptionParser(usage="usage: %prog info /topic", prog=NAME)
1873 (options, args) = parser.parse_args(args)
1874
1875 if len(args) == 0:
1876 parser.error("you must specify a topic name")
1877 elif len(args) > 1:
1878 parser.error("you may only specify one topic name")
1879
1880 topic = rosgraph.names.script_resolve_name('rostopic', args[0])
1881 exitval = _rostopic_info(topic) or 0
1882 if exitval != 0:
1883 sys.exit(exitval)
1884
1886 print("""rostopic is a command-line tool for printing information about ROS Topics.
1887
1888 Commands:
1889 \trostopic bw\tdisplay bandwidth used by topic
1890 \trostopic delay\tdisplay delay of topic from timestamp in header
1891 \trostopic echo\tprint messages to screen
1892 \trostopic find\tfind topics by type
1893 \trostopic hz\tdisplay publishing rate of topic
1894 \trostopic info\tprint information about active topic
1895 \trostopic list\tlist active topics
1896 \trostopic pub\tpublish data to topic
1897 \trostopic type\tprint topic type
1898
1899 Type rostopic <command> -h for more detailed usage, e.g. 'rostopic echo -h'
1900 """)
1901 sys.exit(getattr(os, 'EX_USAGE', 1))
1902
1903 -def rostopicmain(argv=None):
1904 import rosbag
1905 if argv is None:
1906 argv=sys.argv
1907
1908 argv = rospy.myargv(argv)
1909
1910
1911 if len(argv) == 1:
1912 _fullusage()
1913 try:
1914 command = argv[1]
1915 if command == 'echo':
1916 _rostopic_cmd_echo(argv)
1917 elif command == 'hz':
1918 _rostopic_cmd_hz(argv)
1919 elif command == 'type':
1920 _rostopic_cmd_type(argv)
1921 elif command == 'list':
1922 _rostopic_cmd_list(argv)
1923 elif command == 'info':
1924 _rostopic_cmd_info(argv)
1925 elif command == 'pub':
1926 _rostopic_cmd_pub(argv)
1927 elif command == 'bw':
1928 _rostopic_cmd_bw(argv)
1929 elif command == 'find':
1930 _rostopic_cmd_find(argv)
1931 elif command == 'delay':
1932 _rostopic_cmd_delay(argv)
1933 else:
1934 _fullusage()
1935 except socket.error:
1936 sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n")
1937 sys.exit(1)
1938 except rosbag.ROSBagException as e:
1939 sys.stderr.write("ERROR: unable to use bag file: %s\n"%str(e))
1940 sys.exit(1)
1941 except rosgraph.MasterException as e:
1942
1943 sys.stderr.write("ERROR: %s\n"%str(e))
1944 sys.exit(1)
1945 except ROSTopicException as e:
1946 sys.stderr.write("ERROR: %s\n"%str(e))
1947 sys.exit(1)
1948 except KeyboardInterrupt: pass
1949 except rospy.ROSInterruptException: pass
1950