def main(self, *, args): # noqa: D102 # both all and topics cannot be true if args.all and args.topics: return print_error( 'Invalid choice: Can not specify topics and -a at the same time.' ) # both all and topics cannot be false if not (args.all or (args.topics and len(args.topics) > 0)): return print_error('Invalid choice: Must specify topic(s) or -a') uri = args.output or datetime.datetime.now().strftime( 'rosbag2_%Y_%m_%d-%H_%M_%S') if os.path.isdir(uri): return print_error( "Output folder '{}' already exists.".format(uri)) if args.compression_format and args.compression_mode == 'none': return print_error( 'Invalid choice: Cannot specify compression format ' 'without a compression mode.') args.compression_mode = args.compression_mode.upper() qos_profile_overrides = {} # Specify a valid default if args.qos_profile_overrides_path: qos_profile_dict = yaml.safe_load(args.qos_profile_overrides_path) try: qos_profile_overrides = convert_yaml_to_qos_profile( qos_profile_dict) except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) # NOTE(hidmic): in merged install workspaces on Windows, Python entrypoint lookups # combined with constrained environments (as imposed by colcon test) # may result in DLL loading failures when attempting to import a C # extension. Therefore, do not import rosbag2_transport at the module # level but on demand, right before first use. from rosbag2_transport import rosbag2_transport_py rosbag2_transport_py.record( uri=uri, storage_id=args.storage, serialization_format=args.serialization_format, node_prefix=NODE_NAME_PREFIX, compression_mode=args.compression_mode, compression_format=args.compression_format, all=args.all, no_discovery=args.no_discovery, polling_interval=args.polling_interval, max_bagfile_size=args.max_bag_size, max_bagfile_duration=args.max_bag_duration, max_cache_size=args.max_cache_size, topics=args.topics, include_hidden_topics=args.include_hidden_topics, qos_profile_overrides=qos_profile_overrides) if os.path.isdir(uri) and not os.listdir(uri): os.rmdir(uri)
def main(self, *, args): # noqa: D102 if not args.topic: return print_error("topic to export is required") if not os.path.exists(args.bag_file): return print_error("bag file '{}' does not exist!".format( args.bag_file)) filter = StorageFilter(topics=[args.topic]) view = BagView(args.bag_file, filter) exporter = self._exporters[args.exporter]() try: exporter.process(args, view) except ExporterError as e: return print_error(f'export failed: {str(e)}')
def main(self, *, args): # noqa: D102 qos_profile_overrides = {} # Specify a valid default if args.qos_profile_overrides_path: qos_profile_dict = yaml.safe_load(args.qos_profile_overrides_path) try: qos_profile_overrides = convert_yaml_to_qos_profile( qos_profile_dict) except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) storage_config_file = '' if args.storage_config_file: storage_config_file = args.storage_config_file.name # NOTE(hidmic): in merged install workspaces on Windows, Python entrypoint lookups # combined with constrained environments (as imposed by colcon test) # may result in DLL loading failures when attempting to import a C # extension. Therefore, do not import rosbag2_transport at the module # level but on demand, right before first use. from rosbag2_transport import rosbag2_transport_py rosbag2_transport_py.play( uri=args.bag_file, storage_id=args.storage, node_prefix=NODE_NAME_PREFIX, read_ahead_queue_size=args.read_ahead_queue_size, rate=args.rate, topics=args.topics, qos_profile_overrides=qos_profile_overrides, loop=args.loop, topic_remapping=args.remap, storage_config_file=storage_config_file)
def main(self, *, args): if not os.path.isdir(args.bag_directory): return print_error('Must specify a bag directory') storage_options = StorageOptions( uri=args.bag_directory, storage_id=args.storage_id, ) reindexer = Reindexer() reindexer.reindex(storage_options)
def main(self, *, args): # noqa: D102 qos_profile_overrides = {} # Specify a valid default if args.qos_profile_overrides_path: qos_profile_dict = yaml.safe_load(args.qos_profile_overrides_path) try: qos_profile_overrides = convert_yaml_to_qos_profile( qos_profile_dict) except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) storage_config_file = '' if args.storage_config_file: storage_config_file = args.storage_config_file.name topic_remapping = ['--ros-args'] for remap_rule in args.remap: topic_remapping.append('--remap') topic_remapping.append(remap_rule) storage_options = StorageOptions( uri=args.bag_file, storage_id=args.storage, storage_config_uri=storage_config_file, ) play_options = PlayOptions() play_options.read_ahead_queue_size = args.read_ahead_queue_size play_options.node_prefix = NODE_NAME_PREFIX play_options.rate = args.rate play_options.topics_to_filter = args.topics play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = args.loop play_options.topic_remapping_options = topic_remapping play_options.clock_publish_frequency = args.clock play_options.delay = args.delay play_options.playback_duration = args.playback_duration play_options.playback_until_timestamp = self.get_playback_until_from_arg_group( args.playback_until_sec, args.playback_until_nsec) play_options.disable_keyboard_controls = args.disable_keyboard_controls play_options.start_paused = args.start_paused play_options.start_offset = args.start_offset play_options.wait_acked_timeout = args.wait_for_all_acked play_options.disable_loan_message = args.disable_loan_message player = Player() player.play(storage_options, play_options)
def main(self, *, args): # noqa: D102 qos_profile_overrides = {} # Specify a valid default if args.qos_profile_overrides_path: qos_profile_dict = yaml.safe_load(args.qos_profile_overrides_path) try: qos_profile_overrides = convert_yaml_to_qos_profile( qos_profile_dict) except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) storage_config_file = '' if args.storage_config_file: storage_config_file = args.storage_config_file.name topic_remapping = ['--ros-args'] for remap_rule in args.remap: topic_remapping.append('--remap') topic_remapping.append(remap_rule) storage_options = StorageOptions( uri=args.bag_file, storage_id=args.storage, storage_config_uri=storage_config_file, ) play_options = PlayOptions() play_options.read_ahead_queue_size = args.read_ahead_queue_size play_options.node_prefix = NODE_NAME_PREFIX play_options.rate = 1.0 play_options.topics_to_filter = args.topics play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = False play_options.topic_remapping_options = topic_remapping play_options.clock_publish_frequency = 0 play_options.delay = 0.0 play_options.disable_keyboard_controls = False # Give the user control play_options.start_paused = True # Important for allowing the burst play_options.start_offset = args.start_offset play_options.wait_acked_timeout = -1 player = Player() player.burst(storage_options, play_options, args.num_messages)
def main(self, *, args): # noqa: D102 qos_profile_overrides = {} # Specify a valid default if args.qos_profile_overrides_path: qos_profile_dict = yaml.safe_load(args.qos_profile_overrides_path) try: qos_profile_overrides = convert_yaml_to_qos_profile( qos_profile_dict) except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) storage_config_file = '' if args.storage_config_file: storage_config_file = args.storage_config_file.name topic_remapping = ['--ros-args'] for remap_rule in args.remap: topic_remapping.append('--remap') topic_remapping.append(remap_rule) storage_options = StorageOptions( uri=args.bag_file, storage_id=args.storage, storage_config_uri=storage_config_file, ) play_options = PlayOptions() play_options.read_ahead_queue_size = args.read_ahead_queue_size play_options.node_prefix = NODE_NAME_PREFIX play_options.rate = args.rate play_options.topics_to_filter = args.topics play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = args.loop play_options.topic_remapping_options = topic_remapping play_options.clock_publish_frequency = args.clock player = Player() player.play(storage_options, play_options)
def main(self, *, args): # noqa: D102 # both all and topics cannot be true if (args.all and (args.topics or args.regex)) or (args.topics and args.regex): return print_error('Must specify only one option out of topics, --regex or --all') # one out of "all", "topics" and "regex" must be true if not(args.all or (args.topics and len(args.topics) > 0) or (args.regex)): return print_error('Invalid choice: Must specify topic(s), --regex or --all') if args.topics and args.exclude: return print_error('--exclude argument cannot be used when specifying a list ' 'of topics explicitly') if args.exclude and not(args.regex or args.all): return print_error('--exclude argument requires either --all or --regex') uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S') if os.path.isdir(uri): return print_error("Output folder '{}' already exists.".format(uri)) if args.compression_format and args.compression_mode == 'none': return print_error('Invalid choice: Cannot specify compression format ' 'without a compression mode.') if args.compression_queue_size < 1: return print_error('Compression queue size must be at least 1.') args.compression_mode = args.compression_mode.upper() qos_profile_overrides = {} # Specify a valid default if args.qos_profile_overrides_path: qos_profile_dict = yaml.safe_load(args.qos_profile_overrides_path) try: qos_profile_overrides = convert_yaml_to_qos_profile( qos_profile_dict) except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) storage_config_file = '' if args.storage_config_file: storage_config_file = args.storage_config_file.name storage_options = StorageOptions( uri=uri, storage_id=args.storage, max_bagfile_size=args.max_bag_size, max_bagfile_duration=args.max_bag_duration, max_cache_size=args.max_cache_size, storage_preset_profile=args.storage_preset_profile, storage_config_uri=storage_config_file, ) record_options = RecordOptions() record_options.all = args.all record_options.is_discovery_disabled = args.no_discovery record_options.topics = args.topics record_options.rmw_serialization_format = args.serialization_format record_options.topic_polling_interval = datetime.timedelta( milliseconds=args.polling_interval) record_options.regex = args.regex record_options.exclude = args.exclude record_options.node_prefix = NODE_NAME_PREFIX record_options.compression_mode = args.compression_mode record_options.compression_format = args.compression_format record_options.compression_queue_size = args.compression_queue_size record_options.compression_threads = args.compression_threads record_options.topic_qos_profile_overrides = qos_profile_overrides record_options.include_hidden_topics = args.include_hidden_topics recorder = Recorder() try: recorder.record(storage_options, record_options) except KeyboardInterrupt: pass if os.path.isdir(uri) and not os.listdir(uri): os.rmdir(uri)
def main(self, *, args): # noqa: D102 for bag_file in args.bag_files: if not os.path.exists(bag_file): return print_error( "bag file '{}' does not exist!".format(bag_file)) uri = args.output or datetime.now().strftime( 'rosbag2_%Y_%m_%d-%H_%M_%S') if os.path.isdir(uri): return print_error( "Output folder '{}' already exists.".format(uri)) info = Info() metadatas = [ info.read_metadata(f, args.in_storage or '') for f in args.bag_files ] try: self._filter.set_args(metadatas, args) except argparse.ArgumentError as e: return print_error(str(e)) storage_filter = self._filter.get_storage_filter() progress = ProgressTracker() readers = [] for bag_file, metadata in zip(args.bag_files, metadatas): reader = SequentialReader() in_storage_options, in_converter_options = get_rosbag_options( bag_file) if args.in_storage: in_storage_options.storage_id = args.in_storage reader.open(in_storage_options, in_converter_options) if storage_filter: reader.set_filter(storage_filter) if args.progress: progress.add_estimated_work( metadata, self._filter.output_size_factor(metadata)) readers.append(reader) writer = SequentialWriter() out_storage_options = StorageOptions( uri=uri, storage_id=args.out_storage, max_bagfile_size=args.max_bag_size) out_converter_options = ConverterOptions( input_serialization_format=args.serialization_format, output_serialization_format=args.serialization_format) writer.open(out_storage_options, out_converter_options) for reader in readers: for topic_metadata in reader.get_all_topics_and_types(): result = self._filter.filter_topic(topic_metadata) if result: if not isinstance(result, list): result = [result] for item in result: writer.create_topic(item) for reader in readers: while reader.has_next(): msg = reader.read_next() result = self._filter.filter_msg(msg) if args.progress: prog_perc = progress.update(msg[0]) progress.print_update(prog_perc) if result == FilterResult.STOP_CURRENT_BAG: break elif result == FilterResult.DROP_MESSAGE: continue elif isinstance(result, list): for item in result: writer.write(item[0], item[1], item[2]) elif isinstance(result, tuple): writer.write(result[0], result[1], result[2]) else: return print_error( "Filter returned invalid result: '{}'.".format(result)) if args.progress: progress.print_finish()
def main(self, *, args): # noqa: D102 if not os.path.exists(args.bag_file): return print_error("bag file '{}' does not exist!".format( args.bag_file)) if not args.topic: args.topic = [] reader = SequentialReader() in_storage_options = StorageOptions(uri=args.bag_file, storage_id=args.storage) in_converter_options = ConverterOptions( input_serialization_format=args.serialization_format, output_serialization_format=args.serialization_format) reader.open(in_storage_options, in_converter_options) info = Info() metadata = info.read_metadata(args.bag_file, args.storage) message_counts = {} for entry in metadata.topics_with_message_count: message_counts[entry.topic_metadata.name] = entry.message_count bag_duration_s = metadata.duration.total_seconds() type_name_to_type_map = {} topic_to_type_map = {} summaries = {} for topic_metadata in reader.get_all_topics_and_types(): if args.topic and topic_metadata.name not in args.topic: continue if topic_metadata.type not in type_name_to_type_map: try: type_name_to_type_map[topic_metadata.type] = get_message( topic_metadata.type) except (AttributeError, ModuleNotFoundError, ValueError): raise RuntimeError( f"Cannot load message type '{topic_metadata.type}'") topic_to_type_map[topic_metadata.name] = type_name_to_type_map[ topic_metadata.type] summaries[topic_metadata.name] = { 'frame_ids': set(), 'write_delays_ns': [], 'custom': default_summary_output(topic_metadata.type) } reader.set_filter(StorageFilter(topics=args.topic)) progress = ProgressTracker() if args.progress: progress.add_estimated_work(metadata, 1.0) while reader.has_next(): (topic, data, t) = reader.read_next() msg_type = topic_to_type_map[topic] msg = deserialize_message(data, msg_type) for custom in summaries[topic]['custom']: custom.update(msg) if hasattr(msg, 'header'): summaries[topic]['frame_ids'].add(msg.header.frame_id) delay = t - Time.from_msg(msg.header.stamp).nanoseconds summaries[topic]['write_delays_ns'].append(delay) if args.progress: progress.print_update(progress.update(topic), every=100) if args.progress: progress.print_finish() for topic, summary in summaries.items(): print(topic) if not message_counts[topic]: print('\tNo messages') continue frame_id_str = ', '.join(summary['frame_ids']) print(f'\tframe_id: {frame_id_str}') freq = message_counts[topic] / bag_duration_s print(f'\tfrequency: {freq:.2f} hz') if summary['write_delays_ns']: # only messages with header.stamp have delays write_delays = np.array(summary['write_delays_ns']) delay_ms_mean = np.mean(write_delays) / 1000 / 1000 delay_ms_stddev = np.std(write_delays) / 1000 / 1000 print( f'\twrite delay: {delay_ms_mean:.2f}ms (stddev {delay_ms_stddev:.2f})' ) for custom in summaries[topic]['custom']: custom.write()