3 Program main interface.
5 ------------------------------------------------------------------------------
6 This file is part of grepros - grep for ROS bag files and live topics.
7 Released under the BSD License.
12 ------------------------------------------------------------------------------
26 from .
import __title__, __version__, __version_date__, api, inputs, outputs, search
27 from . common
import ConsolePrinter, MatchMarkers, parse_datetime
34 "description":
"Searches through messages in ROS bag files or live topics.",
36 PATTERNs use Python regular expression syntax, message matches if all match.
37 * wildcards use simple globbing as zero or more characters,
38 target matches if any value matches.
43 Search for "my text" in all bags under current directory and subdirectories:
44 %(title)s -r "my text"
46 Print 30 lines of the first message from each live ROS topic:
47 %(title)s --max-per-topic 1 --lines-per-message 30 --live
49 Find first message containing "future" (case-sensitive) in my.bag:
50 %(title)s future -I --max-count 1 --name my.bag
52 Find 10 messages, from geometry_msgs package, in "map" frame,
53 from bags in current directory, reindexing any unindexed bags:
54 %(title)s frame_id=map --type geometry_msgs/* --max-count 10 --reindex-if-unindexed
56 Pipe all diagnostics messages with "CPU usage" from live ROS topics to my.bag:
57 %(title)s "CPU usage" --type *DiagnosticArray --no-console-output --write my.bag
59 Find messages with field "key" containing "0xA002",
60 in topics ending with "diagnostics", in bags under "/tmp":
61 %(title)s key=0xA002 --topic *diagnostics --path /tmp
63 Find diagnostics_msgs messages in bags in current directory,
64 containing "navigation" in fields "name" or "message",
65 print only header stamp and values:
66 %(title)s --type diagnostic_msgs/* --select-field name message \\
67 --emit-field header.stamp status.values -- navigation
69 Print first message from each lidar topic on ROS1 host 1.2.3.4, without highlight:
70 ROS_MASTER_URI=http://1.2.3.4::11311 \\
71 %(title)s --live --topic *lidar* --max-per-topic 1 --no-highlight
73 Export all bag messages to SQLite and Postgres, print only export progress:
74 %(title)s -n my.bag --write my.bag.sqlite --no-console-output --no-verbose --progress
76 %(title)s -n my.bag --write postgresql://user@host/dbname \\
77 --no-console-output --no-verbose --progress
78 """ % dict(title=__title__),
81 dict(args=[
"PATTERN"], nargs=
"*", default=[],
82 help=
"pattern(s) to find in message field values,\n"
83 "all messages match if not given,\n"
84 "can specify message field as NAME=PATTERN\n"
85 "(supports nested.paths and * wildcards)"),
87 dict(args=[
"-h",
"--help"],
88 dest=
"HELP", action=
"store_true",
89 help=
"show this help message and exit"),
91 dict(args=[
"-F",
"--fixed-strings"],
92 dest=
"FIXED_STRING", action=
"store_true",
93 help=
"PATTERNs are ordinary strings, not regular expressions"),
95 dict(args=[
"-I",
"--no-ignore-case"],
96 dest=
"CASE", action=
"store_true",
97 help=
"use case-sensitive matching in PATTERNs"),
99 dict(args=[
"-v",
"--invert-match"],
100 dest=
"INVERT", action=
"store_true",
101 help=
"select messages not matching PATTERN"),
103 dict(args=[
"--version"],
104 dest=
"VERSION", action=
"version",
105 version=
"%s: grep for ROS bag files and live topics, v%s (%s)" %
106 (__title__, __version__, __version_date__),
107 help=
"display version information and exit"),
109 dict(args=[
"--live"],
110 dest=
"LIVE", action=
"store_true",
111 help=
"read messages from live ROS topics instead of bagfiles"),
113 dict(args=[
"--publish"],
114 dest=
"PUBLISH", action=
"store_true",
115 help=
"publish matched messages to live ROS topics"),
117 dict(args=[
"--write"],
118 dest=
"WRITE", nargs=
"+", default=[], action=
"append",
119 metavar=
"TARGET [format=bag] [KEY=VALUE ...]",
120 help=
"write matched messages to specified output,\n"
121 "format is autodetected from TARGET if not specified.\n"
122 "Bag or database will be appended to if it already exists.\n"
123 "Keyword arguments are given to output writer."),
125 dict(args=[
"--write-options"],
126 dest=
"WRITE_OPTIONS", default=argparse.SUPPRESS, help=argparse.SUPPRESS),
128 dict(args=[
"--plugin"],
129 dest=
"PLUGIN", nargs=
"+", default=[], action=
"append",
130 help=
"load a Python module or class as plugin"),
132 dict(args=[
"--stop-on-error"],
133 dest=
"STOP_ON_ERROR", action=
"store_true",
134 help=
"stop further execution on any error like unknown message type"),
137 "groups": {
"Filtering": [
139 dict(args=[
"-t",
"--topic"],
140 dest=
"TOPIC", nargs=
"+", default=[], action=
"append",
141 help=
"ROS topics to read if not all (supports * wildcards)"),
143 dict(args=[
"-nt",
"--no-topic"],
144 dest=
"SKIP_TOPIC", metavar=
"TOPIC", nargs=
"+", default=[], action=
"append",
145 help=
"ROS topics to skip (supports * wildcards)"),
147 dict(args=[
"-d",
"--type"],
148 dest=
"TYPE", nargs=
"+", default=[], action=
"append",
149 help=
"ROS message types to read if not all (supports * wildcards)"),
151 dict(args=[
"-nd",
"--no-type"],
152 dest=
"SKIP_TYPE", metavar=
"TYPE", nargs=
"+", default=[], action=
"append",
153 help=
"ROS message types to skip (supports * wildcards)"),
155 dict(args=[
"--condition"],
156 dest=
"CONDITION", nargs=
"+", default=[], action=
"append",
157 help=
"extra conditions to require for matching messages,\n"
158 "as ordinary Python expressions, can refer to last messages\n"
159 "in topics as <topic /my/topic>; topic name can contain wildcards.\n"
160 'E.g. --condition "<topic /robot/enabled>.data" matches\n'
161 "messages only while last message in '/robot/enabled' has data=true."),
163 dict(args=[
"-t0",
"--start-time"],
164 dest=
"START_TIME", metavar=
"TIME",
165 help=
"earliest timestamp of messages to read\n"
166 "as relative seconds if signed,\n"
167 "or epoch timestamp or ISO datetime\n"
168 "(for bag input, relative to bag start time\n"
169 "if positive or end time if negative,\n"
170 "for live input relative to system time,\n"
171 "datetime may be partial like 2021-10-14T12)"),
173 dict(args=[
"-t1",
"--end-time"],
174 dest=
"END_TIME", metavar=
"TIME",
175 help=
"latest timestamp of messages to read\n"
176 "as relative seconds if signed,\n"
177 "or epoch timestamp or ISO datetime\n"
178 "(for bag input, relative to bag start time\n"
179 "if positive or end time if negative,\n"
180 "for live input relative to system time,\n"
181 "datetime may be partial like 2021-10-14T12)"),
183 dict(args=[
"-n0",
"--start-index"],
184 dest=
"START_INDEX", metavar=
"INDEX", type=int,
185 help=
"message index within topic to start from\n"
186 "(1-based if positive, counts back from bag total if negative)"),
188 dict(args=[
"-n1",
"--end-index"],
189 dest=
"END_INDEX", metavar=
"INDEX", type=int,
190 help=
"message index within topic to stop at\n"
191 "(1-based if positive, counts back from bag total if negative)"),
193 dict(args=[
"--every-nth-message"],
194 dest=
"NTH_MESSAGE", metavar=
"NUM", type=int, default=1,
195 help=
"read every Nth message within topic"),
197 dict(args=[
"--every-nth-interval"],
198 dest=
"NTH_INTERVAL", metavar=
"SECONDS", type=int, default=0,
199 help=
"read messages at least N seconds apart within topic"),
201 dict(args=[
"--every-nth-match"],
202 dest=
"NTH_MATCH", metavar=
"NUM", type=int, default=1,
203 help=
"emit every Nth match in topic"),
205 dict(args=[
"-sf",
"--select-field"],
206 dest=
"SELECT_FIELD", metavar=
"FIELD", nargs=
"+", default=[], action=
"append",
207 help=
"message fields to use in matching if not all\n"
208 "(supports nested.paths and * wildcards)"),
210 dict(args=[
"-ns",
"--no-select-field"],
211 dest=
"NOSELECT_FIELD", metavar=
"FIELD", nargs=
"+", default=[], action=
"append",
212 help=
"message fields to skip in matching\n"
213 "(supports nested.paths and * wildcards)"),
215 dict(args=[
"-m",
"--max-count"],
216 dest=
"MAX_COUNT", metavar=
"NUM", default=0, type=int,
217 help=
"number of matched messages to emit (per each file if bag input)"),
219 dict(args=[
"--max-per-topic"],
220 dest=
"MAX_PER_TOPIC", metavar=
"NUM", default=0, type=int,
221 help=
"number of matched messages to emit from each topic\n"
222 "(per each file if bag input)"),
224 dict(args=[
"--max-topics"],
225 dest=
"MAX_TOPICS", metavar=
"NUM", default=0, type=int,
226 help=
"number of topics to emit matches from (per each file if bag input)"),
228 dict(args=[
"--unique-only"],
229 dest=
"UNIQUE", action=
"store_true",
230 help=
"only emit matches that are unique in topic,\n"
231 "taking --select-field and --no-select-field into account\n"
232 "(per each file if bag input)"),
234 ],
"Output control": [
236 dict(args=[
"-B",
"--before-context"],
237 dest=
"BEFORE", metavar=
"NUM", default=0, type=int,
238 help=
"emit NUM messages of leading context before match"),
240 dict(args=[
"-A",
"--after-context"],
241 dest=
"AFTER", metavar=
"NUM", default=0, type=int,
242 help=
"emit NUM messages of trailing context after match"),
244 dict(args=[
"-C",
"--context"],
245 dest=
"CONTEXT", metavar=
"NUM", default=0, type=int,
246 help=
"emit NUM messages of leading and trailing context\n"
249 dict(args=[
"-ef",
"--emit-field"],
250 dest=
"EMIT_FIELD", metavar=
"FIELD", nargs=
"+", default=[], action=
"append",
251 help=
"message fields to emit in console output if not all\n"
252 "(supports nested.paths and * wildcards)"),
254 dict(args=[
"-nf",
"--no-emit-field"],
255 dest=
"NOEMIT_FIELD", metavar=
"FIELD", nargs=
"+", default=[], action=
"append",
256 help=
"message fields to skip in console output\n"
257 "(supports nested.paths and * wildcards)"),
259 dict(args=[
"-mo",
"--matched-fields-only"],
260 dest=
"MATCHED_FIELDS_ONLY", action=
"store_true",
261 help=
"emit only the fields where PATTERNs find a match in console output"),
263 dict(args=[
"-la",
"--lines-around-match"],
264 dest=
"LINES_AROUND_MATCH", metavar=
"NUM", type=int,
265 help=
"emit only matched fields and NUM message lines\n"
266 "around match in console output"),
268 dict(args=[
"-lf",
"--lines-per-field"],
269 dest=
"MAX_FIELD_LINES", metavar=
"NUM", type=int,
270 help=
"maximum number of lines to emit per field in console output"),
272 dict(args=[
"-l0",
"--start-line"],
273 dest=
"START_LINE", metavar=
"NUM", type=int,
274 help=
"message line number to start emitting from in console output\n"
275 "(1-based if positive, counts back from total if negative)"),
277 dict(args=[
"-l1",
"--end-line"],
278 dest=
"END_LINE", metavar=
"NUM", type=int,
279 help=
"message line number to stop emitting at in console output\n"
280 "(1-based if positive, counts back from total if negative)"),
282 dict(args=[
"-lm",
"--lines-per-message"],
283 dest=
"MAX_MESSAGE_LINES", metavar=
"NUM", type=int,
284 help=
"maximum number of lines to emit per message in console output"),
286 dict(args=[
"--match-wrapper"],
287 dest=
"MATCH_WRAPPER", metavar=
"STR", nargs=
"*",
288 help=
"string to wrap around matched values in console output,\n"
289 "both sides if one value, start and end if more than one,\n"
290 "or no wrapping if zero values\n"
291 '(default "**" in colorless output)'),
293 dict(args=[
"--wrap-width"],
294 dest=
"WRAP_WIDTH", metavar=
"NUM", type=int,
295 help=
"character width to wrap message YAML console output at,\n"
296 "0 disables (defaults to detected terminal width)"),
298 dict(args=[
"--color"], dest=
"COLOR",
299 choices=[
"auto",
"always",
"never"], default=
"always",
300 help=
'use color output in console (default "always")'),
302 dict(args=[
"--no-meta"], dest=
"META", action=
"store_false",
303 help=
"do not print source and message metainfo to console"),
305 dict(args=[
"--no-filename"], dest=
"LINE_PREFIX", action=
"store_false",
306 help=
"do not print bag filename prefix on each console message line"),
308 dict(args=[
"--no-highlight"], dest=
"HIGHLIGHT", action=
"store_false",
309 help=
"do not highlight matched values"),
311 dict(args=[
"--no-console-output"], dest=
"CONSOLE", action=
"store_false",
312 help=
"do not print matches to console"),
314 dict(args=[
"--progress"], dest=
"PROGRESS", action=
"store_true",
315 help=
"show progress bar when not printing matches to console"),
317 dict(args=[
"--verbose"], dest=
"VERBOSE", action=
"store_true",
318 help=
"print status messages during console output\n"
319 "for publishing and writing"),
321 dict(args=[
"--no-verbose"], dest=
"SKIP_VERBOSE", action=
"store_true",
322 help=
"do not print status messages during console output\n"
323 "for publishing and writing"),
325 ],
"Bag input control": [
327 dict(args=[
"-n",
"--filename"],
328 dest=
"FILE", nargs=
"+", default=[], action=
"append",
329 help=
"names of ROS bagfiles to read if not all in directory\n"
330 "(supports * wildcards)"),
332 dict(args=[
"-p",
"--path"],
333 dest=
"PATH", nargs=
"+", default=[], action=
"append",
334 help=
"paths to scan if not current directory\n"
335 "(supports * wildcards)"),
337 dict(args=[
"-r",
"--recursive"],
338 dest=
"RECURSE", action=
"store_true",
339 help=
"recurse into subdirectories when looking for bagfiles"),
341 dict(args=[
"--order-bag-by"],
342 dest=
"ORDERBY", choices=[
"topic",
"type"],
343 help=
"order bag messages by topic or type first and then by time"),
345 dict(args=[
"--decompress"],
346 dest=
"DECOMPRESS", action=
"store_true",
347 help=
"decompress archived bagfiles with recognized extensions (.zst .zstd)"),
349 dict(args=[
"--reindex-if-unindexed"],
350 dest=
"REINDEX", action=
"store_true",
351 help=
"reindex unindexed bagfiles (ROS1 only), makes backup copies"),
353 ],
"Live topic control": [
355 dict(args=[
"--publish-prefix"],
356 dest=
"PUBLISH_PREFIX", metavar=
"PREFIX", default=
"",
357 help=
"prefix to prepend to input topic name on publishing match"),
359 dict(args=[
"--publish-suffix"],
360 dest=
"PUBLISH_SUFFIX", metavar=
"SUFFIX", default=
"",
361 help=
"suffix to append to input topic name on publishing match"),
363 dict(args=[
"--publish-fixname"],
364 dest=
"PUBLISH_FIXNAME", metavar=
"TOPIC", default=
"",
365 help=
"single output topic name to publish all matches to,\n"
366 "overrides prefix and suffix"),
368 dict(args=[
"--queue-size-in"],
369 dest=
"QUEUE_SIZE_IN", metavar=
"SIZE", type=int, default=10,
370 help=
"live ROS topic subscriber queue size (default 10)"),
372 dict(args=[
"--queue-size-out"],
373 dest=
"QUEUE_SIZE_OUT", metavar=
"SIZE", type=int, default=10,
374 help=
"output publisher queue size (default 10)"),
376 dict(args=[
"--ros-time-in"],
377 dest=
"ROS_TIME_IN", action=
"store_true",
378 help=
"use ROS time instead of system time for incoming message\n"
379 "timestamps from subsribed live ROS topics"),
389 """RawTextHelpFormatter returning custom metavar for WRITE."""
392 """Returns formatted invocation."""
393 if "WRITE" == action.dest:
394 return " ".join(action.option_strings + [action.metavar])
399 """Returns a configured ArgumentParser instance."""
400 kws = dict(description=ARGUMENTS[
"description"], epilog=ARGUMENTS[
"epilog"],
401 formatter_class=HelpFormatter, add_help=
False)
402 if sys.version_info >= (3, 5): kws.update(allow_abbrev=
False)
403 argparser = argparse.ArgumentParser(**kws)
404 for arg
in map(dict, ARGUMENTS[
"arguments"]):
405 argparser.add_argument(*arg.pop(
"args"), **arg)
406 for group, groupargs
in ARGUMENTS.get(
"groups", {}).items():
407 grouper = argparser.add_argument_group(group)
408 for arg
in map(dict, groupargs):
409 grouper.add_argument(*arg.pop(
"args"), **arg)
415 Converts or combines arguments where necessary, returns full args.
417 @param args arguments object like argparse.Namespace
419 for arg
in sum(ARGUMENTS.get(
"groups", {}).values(), ARGUMENTS[
"arguments"][:]):
420 name = arg.get(
"dest")
or arg[
"args"][0]
421 if "version" != arg.get(
"action")
and argparse.SUPPRESS != arg.get(
"default") \
422 and "HELP" != name
and not hasattr(args, name):
423 value =
False if arg.get(
"store_true")
else True if arg.get(
"store_false")
else None
424 setattr(args, name, arg.get(
"default", value))
427 args.BEFORE = args.AFTER = args.CONTEXT
430 args.VERBOSE =
False if args.SKIP_VERBOSE
else \
431 (args.VERBOSE
or not args.CONSOLE
and bool(CLI_ARGS))
434 args.PROGRESS = args.PROGRESS
and not args.CONSOLE
437 args.LINE_PREFIX = args.LINE_PREFIX
and (args.RECURSE
or len(args.FILE) != 1
438 or args.PATH
or any(
"*" in x
for x
in args.FILE))
440 for k, v
in vars(args).items():
441 if k !=
"WRITE" and isinstance(v, list):
443 setattr(args, k, [x
for xx
in v
for x
in (xx
if isinstance(xx, list)
else [xx])
444 if not (x
in here
or here.add(x))])
446 for n, v
in [(
"START_TIME", args.START_TIME), (
"END_TIME", args.END_TIME)]:
447 if not isinstance(v, (six.binary_type, six.text_type)):
continue
449 except Exception:
pass
450 try:
not isinstance(v, float)
and setattr(args, n,
parse_datetime(v))
451 except Exception:
pass
458 Validates arguments, prints errors, returns success.
460 @param args arguments object like argparse.Namespace
462 errors = collections.defaultdict(list)
465 for opts
in args.WRITE:
468 try: dict([opt.split(
"=", 1)])
469 except Exception: erropts.append(opt)
471 errors[
""].append(
'Invalid KEY=VALUE in "--write %s": %s' %
472 (
" ".join(opts),
" ".join(erropts)))
474 for n, v
in [(
"START_TIME", args.START_TIME), (
"END_TIME", args.END_TIME)]:
475 if v
is None:
continue
477 except Exception:
pass
478 try: isinstance(v, (six.binary_type, six.text_type))
and parse_datetime(v)
479 except Exception: errors[
""].append(
"Invalid ISO datetime for %s: %s" %
480 (n.lower().replace(
"_",
" "), v))
482 for v
in args.PATTERN
if not args.FIXED_STRING
else ():
483 split = v.find(
"=", 1, -1)
484 v = v[split + 1:]
if split > 0
else v
485 try: re.compile(re.escape(v)
if args.FIXED_STRING
else v)
486 except Exception
as e:
487 errors[
"Invalid regular expression"].append(
"'%s': %s" % (v, e))
489 for v
in args.CONDITION:
490 v = inputs.ConditionMixin.TOPIC_RGX.sub(
"dummy", v)
491 try: compile(v,
"",
"eval")
492 except SyntaxError
as e:
493 errors[
"Invalid condition"].append(
"'%s': %s at %schar %s" %
494 (v, e.msg,
"line %s " % e.lineno
if e.lineno > 1
else "", e.offset))
495 except Exception
as e:
496 errors[
"Invalid condition"].append(
"'%s': %s" % (v, e))
498 for err
in errors.get(
"", []):
499 ConsolePrinter.log(logging.ERROR, err)
500 for category
in filter(bool, errors):
501 ConsolePrinter.log(logging.ERROR, category)
502 for err
in errors[category]:
503 ConsolePrinter.log(logging.ERROR,
" %s" % err)
508 """Writes a linefeed to sdtout if nothing has been printed to it so far."""
509 if not ConsolePrinter.PRINTS.get(sys.stdout)
and not sys.stdout.isatty():
511 except (Exception, KeyboardInterrupt):
pass
515 """Imports and initializes plugins from auto-load folder and from arguments."""
517 (
"overwrite=true|false",
"overwrite existing file\nin bag output\n"
518 "instead of appending to if bag or database\n"
519 "or appending unique counter to file name\n"
522 ] + outputs.RolloverSinkMixin.get_write_options(
"bag"))
523 args =
make_parser().parse_known_args(CLI_ARGS)[0]
if "--plugin" in CLI_ARGS
else None
525 except ImportWarning: sys.exit(1)
529 """Parses command-line arguments and runs search."""
531 CLI_ARGS = sys.argv[1:]
532 MatchMarkers.populate(
"%08x" % random.randint(1, 1E9))
536 argparser.print_usage()
539 atexit.register(flush_stdout)
540 args = argparser.parse_args(CLI_ARGS)
542 argparser.print_help()
545 BREAK_EXS = (KeyboardInterrupt, )
546 try: BREAK_EXS += (BrokenPipeError, )
547 except NameError:
pass
549 source, sink =
None,
None
551 ConsolePrinter.configure({
"always":
True,
"never":
False}.get(args.COLOR))
555 source = plugins.load(
"source", args)
or \
557 if not source.validate():
560 sink.sinks.extend(filter(bool, plugins.load(
"sink", args, collect=
True)))
561 if not sink.validate():
564 thread_excepthook =
lambda t, e: (ConsolePrinter.error(t), sys.exit(1))
565 source.thread_excepthook = sink.thread_excepthook = thread_excepthook
567 grepper.work(source, sink)
569 try: source
and source.close()
570 except (Exception, KeyboardInterrupt):
pass
571 try: sink
and sink.close()
572 except (Exception, KeyboardInterrupt):
pass
574 try: os.dup2(os.open(os.devnull, os.O_WRONLY), sys.stdout.fileno())
575 except (Exception, KeyboardInterrupt):
pass
578 sink
and sink.close()
579 source
and source.close()
584 "ARGUMENTS",
"CLI_ARGS",
"HelpFormatter",
585 "make_parser",
"process_args",
"validate_args",
"flush_stdout",
"preload_plugins",
"run",
590 if "__main__" == __name__: