def test_sequential_reader(): bag_path = str(Path(__file__).parent.parent / 'resources' / 'talker') storage_options, converter_options = get_rosbag_options(bag_path) reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) topic_types = reader.get_all_topics_and_types() # Create a map for quicker lookup type_map = { topic_types[i].name: topic_types[i].type for i in range(len(topic_types)) } # Set filter for topic of string type storage_filter = rosbag2_py.StorageFilter(topics=['/topic']) reader.set_filter(storage_filter) msg_counter = 0 while reader.has_next(): (topic, data, t) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) assert isinstance(msg, String) assert msg.data == f'Hello, world! {msg_counter}' msg_counter += 1 # No filter reader.reset_filter() reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) msg_counter = 0 while reader.has_next(): (topic, data, t) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) assert isinstance(msg, Log) or isinstance(msg, String) if isinstance(msg, String): assert msg.data == f'Hello, world! {msg_counter}' msg_counter += 1
def test_set_float32(): """Test message serialization/deserialization of float32 type.""" # During (de)serialization we convert to a C float before converting to a PyObject. # This can result in a loss of precision msg = BasicTypes() msg.float32_value = 1.125 # can be represented without rounding msg_serialized = serialize_message(msg) msg_deserialized = deserialize_message(msg_serialized, BasicTypes) assert msg.float32_value == msg_deserialized.float32_value msg = BasicTypes() msg.float32_value = 3.14 # can NOT be represented without rounding msg_serialized = serialize_message(msg) msg_deserialized = deserialize_message(msg_serialized, BasicTypes) assert msg.float32_value == round(msg_deserialized.float32_value, 2)
def get_position_ik(request: GetPositionIK.Request, approximate=False): global ik_fk_state if ik_fk_state is None: ik_fk_state = BitbotsMoveitBindings([]) request_str = serialize_message(request) result_str = ik_fk_state.getPositionIK(request_str, approximate) return deserialize_message(result_str, GetPositionIK.Response)
def test_sequential_reader(self): info = baggie._BagInfo() meta = info.read_metadata(self.s_opt.uri, self.s_opt.storage_id) self.assertEquals(meta.storage_identifier, self.s_opt.storage_id) self.assertEquals(meta.message_count, N_MSGS) self.assertEquals(meta.compression_format, self.comp_opt.compression_format) self.assertEquals(meta.compression_mode, self.comp_opt.mode_to_string()) reader = baggie._Reader(self.comp_opt) reader.open(self.s_opt, self.c_opt) i = 0 while reader.has_next(): topic, ser_msg, ts = reader.read_next() msg = deserialize_message(ser_msg, Int32) self.assertIsInstance(msg, Int32) self.assertEquals(i, msg.data) self.assertEquals(self.stamps[i], ts) i += 1 self.assertEquals(i, N_MSGS)
def filter_msg(self, msg): (topic, data, t) = msg if topic == self._topic: msg = deserialize_message(data, self._message_type) assert hasattr(msg, 'header') msg.header.frame_id = self._frame_id return (topic, serialize_message(msg), t) return msg
def step_open_loop(self, dt: float, cmdvel_msg: Twist): if dt == 0.0: # preventing weird spline interpolation errors on edge case dt = 0.001 stepi = self.py_walk_wrapper.step_open_loop( dt, serialize_message(cmdvel_msg)) result = deserialize_message(stepi, PoseArray) return result
def filter_msg(self, msg): (topic, data, t) = msg if topic == self._topic: msg = deserialize_message(data, self._message_type) result = self.filter_typed_msg((topic, msg, t)) if isinstance(result, FilterResult): return result (topic, msg, t) = result return (topic, serialize_message(msg), t) return msg
def step_relative(self, dt: float, step_msg: Twist, imu_msg, jointstate_msg, pressure_left, pressure_right): if dt == 0.0: # preventing weird spline interpolation errors on edge case dt = 0.001 stepi = self.py_walk_wrapper.step_relative( dt, serialize_message(step_msg), serialize_message(imu_msg), serialize_message(jointstate_msg), serialize_message(pressure_left), serialize_message(pressure_right)) result = deserialize_message(stepi, JointCommand) return result
def test_sequential_writer(tmp_path): """ Test for sequential writer. :return: """ bag_path = str(tmp_path / 'tmp_write_test') storage_options, converter_options = get_rosbag_options(bag_path) writer = rosbag2_py.SequentialWriter() writer.open(storage_options, converter_options) # create topic topic_name = '/chatter' create_topic(writer, topic_name, 'std_msgs/msg/String') for i in range(10): msg = String() msg.data = f'Hello, world! {str(i)}' time_stamp = i * 100 writer.write(topic_name, serialize_message(msg), time_stamp) # close bag and create new storage instance del writer storage_options, converter_options = get_rosbag_options(bag_path) reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) topic_types = reader.get_all_topics_and_types() # Create a map for quicker lookup type_map = { topic_types[i].name: topic_types[i].type for i in range(len(topic_types)) } msg_counter = 0 while reader.has_next(): topic, data, t = reader.read_next() msg_type = get_message(type_map[topic]) msg_deserialized = deserialize_message(data, msg_type) assert isinstance(msg_deserialized, String) assert msg_deserialized.data == f'Hello, world! {msg_counter}' assert t == msg_counter * 100 msg_counter += 1
def read_all_messages_of_topic(bag_path, topic, type): """Read all messages of given topic and type from a rosbag into a list.""" from rosbag2_py import StorageOptions, ConverterOptions, SequentialReader from rclpy.serialization import deserialize_message storage_options = StorageOptions(uri=bag_path, storage_id='sqlite3') converter_options = ConverterOptions(input_serialization_format='cdr', output_serialization_format='cdr') reader = SequentialReader() reader.open(storage_options, converter_options) result = [] while reader.has_next(): (tpc, data, _) = reader.read_next() if tpc == topic: result.append(deserialize_message(data, type)) return result
def test_reframe_filter(): filter = ReframeFilter() parser = argparse.ArgumentParser('reframe') filter.add_arguments(parser) args = parser.parse_args(['-t', '/data', '--frame', 'frame1']) filter.set_args(None, args) topic_metadata = TopicMetadata( '/data', 'diagnostic_msgs/msg/DiagnosticArray', 'cdr') assert(filter.filter_topic(topic_metadata) == topic_metadata) msg = DiagnosticArray() msg.header.frame_id = 'frame0' msg.header.stamp.sec = 0 msg.header.stamp.nanosec = 1 # timestamp within the bag and cut duration bag_msg = ('/data', serialize_message(msg), 1) (_, data, _) = filter.filter_msg(bag_msg) new_msg = deserialize_message(data, DiagnosticArray) assert(new_msg.header.frame_id == 'frame1')
def main(): # hardcoded bag path is bad, but ok since main reccomended running path is Dockerfile bag_path = "/bag.db3" storage_options, converter_options = get_rosbag_options(bag_path) reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) topic_types = reader.get_all_topics_and_types() # Create a map for quicker lookup type_map = { topic_types[i].name: topic_types[i].type for i in range(len(topic_types)) } msg_counter = 0 while reader.has_next(): print("\n-----------------") print("msg id {}".format(msg_counter)) (topic, data, t) = reader.read_next() print(topic) msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) print(msg) msg_counter += 1
def test_replace_filter(): filter = ReplaceFilter() parser = argparse.ArgumentParser('replace') filter.add_arguments(parser) args = parser.parse_args(['-t', '/data', '-v', 'test/data.yaml']) filter.set_args(None, args) string_msg = String() string_msg.data = 'in' msg = ('/data', serialize_message(string_msg), 0) with pytest.raises(RuntimeError): # if topic hasn't been found, filter_msg fails with error filter.filter_msg(msg) topic_metadata = TopicMetadata( '/data', 'example_interfaces/msg/String', 'cdr') assert(filter.filter_topic(topic_metadata) == topic_metadata) (topic, result_data, t) = filter.filter_msg(msg) assert(topic == '/data') result_msg = deserialize_message(result_data, String) assert(result_msg.data == 'out') assert(t == 0)
def main(): path = '../bags/test' dirs = next(walk(path))[1] dirs.sort() print(dirs) avg_MSE_odom = [] avg_MSE_odom_filtered = [] avg_MSE_th = [] avg_MSE_th_filtered = [] plot_number = 0 for dir in dirs: mypath = f'{path}/{dir}' bag_files = [f for f in listdir(mypath) if f.endswith(".db3")] # print(bag_files) for bag_file in bag_files: plot_number += 1 conn = sqlite3.connect(f'{mypath}/{bag_file}') c = conn.cursor() c.execute('SELECT * FROM messages') message_data = c.fetchall() c.execute('SELECT * FROM topics') topic_data = c.fetchall() msg_types = list() topic_names = list() for row in topic_data: msg_types.append(row[2]) topic_names.append(row[1]) time0 = message_data[0][2] dfs = {k: {} for k in topic_names} for df in dfs: dfs[df] = {k: [] for k in ['x', 'y', 'th', 'time']} for row in message_data: id = row[1] - 1 msg_type = get_message(msg_types[id]) x = deserialize_message(row[3], msg_type) pos, th = get_pose(msg_types, topic_names, x, id) dfs[topic_names[id]]['x'].append(pos.x) dfs[topic_names[id]]['y'].append(pos.y) if th == 0: dfs[topic_names[id]]['th'].append( dfs[topic_names[id]]['th'][-1]) else: dfs[topic_names[id]]['th'].append(th) dfs[topic_names[id]]['time'].append((row[2] - time0) / 1e9) fig, ax = plt.subplots() plt.title(f'Trajektoria') plt.xlabel('x [m]') plt.ylabel('y [m]') ax.plot(dfs['/odom/noisy']['x'], dfs['/odom/noisy']['y'], label='odometria') ax.plot(dfs['/odom']['x'], dfs['/odom']['y'], label='odometria przefiltrowana') ax.plot(dfs['/omnivelma/pose']['x'], dfs['/omnivelma/pose']['y'], label='pozycja w symulatorze') # fig.legend() ax.legend(loc=3) plt.grid() axins = zoomed_inset_axes(ax, 15, loc=1) axins.plot(dfs['/odom/noisy']['x'], dfs['/odom/noisy']['y'], label='odometria') axins.plot(dfs['/odom']['x'], dfs['/odom']['y'], label='odometria przefiltrowana') axins.plot(dfs['/omnivelma/pose']['x'], dfs['/omnivelma/pose']['y'], label='pozycja w symulatorze') x1, x2, y1, y2 = -0.1, 0.1, -0.1, 0.1 # specify the limits axins.set_xlim(x1, x2) # apply the x-limits axins.set_ylim(y1, y2) # apply the y-limits axins.grid() plt.yticks(visible=True) plt.xticks(visible=True) mark_inset(ax, axins, loc1=2, loc2=4, fc="none", ec="0.5") plt.savefig(f'{mypath}/trajectory.pdf', bbox_inches='tight') plt.figure(2) plt.grid() plt.plot(dfs['/odom/noisy']['time'], dfs['/odom/noisy']['x'], label='odometria') plt.plot(dfs['/odom']['time'], dfs['/odom']['x'], label='odometria przefiltrowana') plt.plot(dfs['/omnivelma/pose']['time'], dfs['/omnivelma/pose']['x'], label='pozycja w symulatorze') plt.xlabel('czas [s]') plt.ylabel('x [m]') plt.legend() plt.title(f'Wartość położenia w osi x') plt.savefig(f'{mypath}/x.pdf', bbox_inches='tight') plt.figure(3) plt.grid() plt.plot(dfs['/odom/noisy']['time'], dfs['/odom/noisy']['y'], label='odometria') plt.plot(dfs['/odom']['time'], dfs['/odom']['y'], label='odometria przefiltrowana') plt.plot(dfs['/omnivelma/pose']['time'], dfs['/omnivelma/pose']['y'], label='pozycja w symulatorze') plt.xlabel('czas [s]') plt.ylabel('y [m]') plt.legend() plt.title(f'Wartość położenia w osi y') plt.savefig(f'{mypath}/y.pdf', bbox_inches='tight') plt.figure(4) plt.grid() plt.plot(dfs['/odom/noisy']['time'], dfs['/odom/noisy']['th'], label='odometria') plt.plot(dfs['/odom']['time'], dfs['/odom']['th'], label='odometria przefiltrowana') plt.plot(dfs['/omnivelma/pose']['time'], dfs['/omnivelma/pose']['th'], label='pozycja w symulatorze') plt.xlabel('czas [s]') plt.ylabel('kąt [rad]') plt.legend() plt.title(f'Wartość orientacji') plt.savefig(f'{mypath}/theta.pdf', bbox_inches='tight') plt.figure(5) plt.grid() odom_x_error = np.asarray(dfs['/error/odom']['x']) odom_y_error = np.asarray(dfs['/error/odom']['y']) odom_th_error = np.asarray(dfs['/error/odom']['th']) odom_error = np.sqrt( np.add(np.power(odom_x_error, 2), np.power(odom_y_error, 2))) odom_filtered_x_error = np.asarray( dfs['/error/odom_filtered']['x']) odom_filtered_y_error = np.asarray( dfs['/error/odom_filtered']['y']) odom_filtered_th_error = np.asarray( dfs['/error/odom_filtered']['th']) odom_filtered_error = np.sqrt( np.add(np.power(odom_filtered_x_error, 2), np.power(odom_filtered_y_error, 2))) MSE_odom = np.mean(np.power(odom_error, 2)) MSE_Th_odom = np.mean(np.power(odom_th_error, 2)) plt.plot(dfs['/error/odom']['time'], odom_error, label='błąd odometrii') MSE_odom_filtered = np.mean(np.power(odom_filtered_error, 2)) MSE_Th_odom_filtered = np.mean(np.power(odom_filtered_th_error, 2)) plt.plot(dfs['/error/odom_filtered']['time'], odom_filtered_error, label='błąd przefiltrowanej odometrii') avg_MSE_odom.append(MSE_odom) avg_MSE_odom_filtered.append(MSE_odom_filtered) avg_MSE_th.append(MSE_Th_odom) avg_MSE_th_filtered.append(MSE_Th_odom_filtered) # last_x = np.asarray(dfs['/omnivelma/pose']['x'])[-1] # print(f'Last x: {odom_x_error[-1]:.4} , last y: {odom_y_error[-1]:.4} las th: {odom_th_error[-1]:.4}, path length: {last_x:.4}') plt.xlabel('czas [s]') plt.ylabel('błąd [m]') plt.legend() plt.title(f'Absolutny błąd położenia') plt.savefig(f'{mypath}/error_abs.pdf', bbox_inches='tight') plt.figure(6) fig, axs = plt.subplots(2, sharex=True, sharey=False) axs[0].plot(dfs['/error/odom']['time'], dfs['/error/odom']['x'], label='odometria') axs[1].plot(dfs['/error/odom']['time'], dfs['/error/odom']['y'], label='odometria') axs[0].plot(dfs['/error/odom_filtered']['time'], dfs['/error/odom_filtered']['x'], label='przefiltrowana odometria') axs[1].plot(dfs['/error/odom_filtered']['time'], dfs['/error/odom_filtered']['y'], label='przefiltrowana odometria') plt.xlabel('czas [s]') for ax in axs.flat: ax.set(ylabel='błąd [m]') ax.grid() ax.legend() axs[0].set_title('Błąd x') axs[1].set_title('Błąd y') plt.savefig(f'{mypath}/error.pdf', bbox_inches='tight') plt.figure(10) plt.plot(dfs['/error/odom']['time'], dfs['/error/odom']['th'], label='odometria', linewidth=1) plt.plot(dfs['/error/odom_filtered']['time'], dfs['/error/odom_filtered']['th'], label='przefiltrowana odometria', linewidth=1) plt.xlabel('time [s]') plt.xlabel('błąd [rad]') plt.grid() plt.legend() plt.title(f'Błąd orientacji') plt.savefig(f'{mypath}/error_theta.pdf', bbox_inches='tight') plt.figure(11) plt.scatter(dfs['/error/odom']['time'], dfs['/error/odom']['th'], s=1, label='odometria') plt.scatter(dfs['/error/odom_filtered']['time'], dfs['/error/odom_filtered']['th'], s=1, label='przefiltrowana odometria') plt.xlabel('time [s]') plt.xlabel('błąd [rad]') plt.grid() plt.legend() plt.title(f'Błąd orientacji') plt.savefig(f'{mypath}/error_theta_scatter.pdf', bbox_inches='tight') plt.close('all') print("---Polozenie ---") for i in range(len(avg_MSE_odom)): print( f"{i+1} & {avg_MSE_odom[i]:.4} & {avg_MSE_odom_filtered[i]:.4} \\\ \hline" ) print( f"Średnia & {statistics.mean(avg_MSE_odom):.4} & {statistics.mean(avg_MSE_odom_filtered):.4} \\\ \hline" ) print("---Orientacja ---") for i in range(len(avg_MSE_odom)): print( f"{i+1} & {avg_MSE_th[i]:.4} & {avg_MSE_th_filtered[i]:.4} \\\ \hline" ) print( f"Średnia & {statistics.mean(avg_MSE_th):.4} & {statistics.mean(avg_MSE_th_filtered):.4} \\\ \hline" )
def main(): path = '../bags/global_plan/path1' dirs = next(walk(path))[1] dirs.sort() print(dirs) list_x = list() list_y = list() distances = [] n_poses = [] for idx, dir in enumerate(dirs): mypath = f'{path}/{dir}' bag_files = [f for f in listdir(mypath) if f.endswith(".db3")] print(bag_files) for bag_file in bag_files: Dist = 0 y_poses = list() x_poses = list() conn = sqlite3.connect(f'{mypath}/{bag_file}') c = conn.cursor() c.execute('SELECT * FROM messages') message_data = c.fetchall() message = message_data[0][3] msg_type = get_message('nav_msgs/msg/Path') x = deserialize_message(message, msg_type) for i, msg in enumerate(x.poses[:-1]): dx = x.poses[i + 1].pose.position.x - msg.pose.position.x dy = x.poses[i + 1].pose.position.y - msg.pose.position.y Dist = Dist + math.sqrt(dx * dx + dy * dy) x_poses.append(msg.pose.position.x) y_poses.append(msg.pose.position.y) x_poses.append(x.poses[-1].pose.position.x) y_poses.append(x.poses[-1].pose.position.y) list_x.append(x_poses) list_y.append(y_poses) n_poses.append(len(x.poses)) distances.append(Dist) print(n_poses) print(distances) fig, ax = plt.subplots() plt.title(f'trajektoria') plt.xlabel('x [m]') plt.ylabel('y [m]') ax.plot(list_x[0], list_y[0], label='Djikstra') ax.plot(list_x[1], list_y[1], label='A*') # fig.legend() ax.legend(loc=0) plt.grid() plt.savefig(f'{mypath}/trajectory.pdf') plt.close('all')
def get_joint_states(): global current_state if current_state is None: current_state = BitbotsMoveitBindings([]) result_str = current_state.get_joint_states() return deserialize_message(result_str, JointState)
def get_bioik_ik(): global ik_fk_state if ik_fk_state is None: ik_fk_state = BitbotsMoveitBindings([]) result_str = ik_fk_state.getBioIKIK() return deserialize_message(result_str, GetIK.Response)
def get_position_fk(request: GetPositionFK.Request): global ik_fk_state if ik_fk_state is None: ik_fk_state = BitbotsMoveitBindings([]) result_str = ik_fk_state.getPositionFK(serialize_message(request)) return deserialize_message(result_str, GetPositionFK.Response)
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()
def main(): path = '../bags/TEB' dirs = next(walk(path))[1] dirs.sort() print(dirs) iter = 0 avg_x_vel = [] avg_y_vel = [] avg_th_vel = [] list_x = list() list_y = list() plot_number = 0 for dir in dirs: mypath = f'{path}/{dir}' bag_files = [f for f in listdir(mypath) if f.endswith(".db3")] for bag_file in bag_files: plot_number += 1 conn = sqlite3.connect(f'{mypath}/{bag_file}') c = conn.cursor() c.execute('SELECT * FROM messages') message_data = c.fetchall() c.execute('SELECT * FROM topics') topic_data = c.fetchall() msg_types = list() topic_names = list() for row in topic_data: msg_types.append(row[2]) topic_names.append(row[1]) time0 = message_data[0][2] dfs = {k: {} for k in topic_names} for df in dfs: dfs[df] = {k: [] for k in ['x', 'y', 'th', 'time']} for row in message_data: id = row[1] - 1 msg_type = get_message(msg_types[id]) x = deserialize_message(row[3], msg_type) pos, th = get_pose(msg_types, topic_names, x, id) dfs[topic_names[id]]['x'].append(pos.x) dfs[topic_names[id]]['y'].append(pos.y) dfs[topic_names[id]]['th'].append(th) dfs[topic_names[id]]['time'].append((row[2] - time0) / 1e9) list_x.append(dfs['/amcl/filtered']) list_y.append(dfs['/cmd_vel']) iter += 1 fig, ax = plt.subplots() plt.title(f'Trajektoria') plt.xlabel('x [m]') plt.ylabel('y [m]') ax.plot(list_x[0]['x'], list_x[0]['y'], label='DWB') ax.plot(list_x[1]['x'], list_x[1]['y'], label='TEB') # fig.legend() ax.legend(loc=4) plt.grid() axins = zoomed_inset_axes(ax, 5, loc=2, borderpad=3.0) axins.plot(list_x[0]['x'], list_x[0]['y'], label='DWB') axins.plot(list_x[1]['x'], list_x[1]['y'], label='TEB') x1, x2, y1, y2 = 36, 40, 29, 33 # specify the limits axins.set_xlim(x1, x2) # apply the x-limits axins.set_ylim(y1, y2) # apply the y-limits axins.grid() plt.yticks(visible=True) plt.xticks(visible=True) mark_inset(ax, axins, loc1=2, loc2=4, fc="none", ec="0.5") plt.savefig(f'{mypath}/trajectory.pdf', bbox_inches='tight') plt.figure(2) plt.grid() plt.plot(list_y[0]['time'], list_y[0]['x'], label='DWB') plt.plot(list_y[1]['time'], list_y[1]['x'], label='TEB') plt.xlabel('czas [s]') plt.ylabel('v_x [m/s]') plt.legend() plt.title(f'Zadana prędkość liniowa x') plt.savefig(f'{mypath}/x.pdf', bbox_inches='tight') plt.figure(3) plt.grid() plt.plot(list_y[0]['time'], list_y[0]['y'], label='DWB') plt.plot(list_y[1]['time'], list_y[1]['y'], label='TEB') plt.xlabel('czas [s]') plt.ylabel('v_y [m/s]') plt.legend() plt.title(f'Zadana prędkość liniowa y') plt.savefig(f'{mypath}/y.pdf', bbox_inches='tight') plt.figure(4) plt.grid() plt.plot(list_y[0]['time'], list_y[0]['th'], label='DWB') plt.plot(list_y[1]['time'], list_y[1]['th'], label='TEB') plt.xlabel('czas [s]') plt.ylabel('omega [rad/s]') plt.legend() plt.title(f'Zadana prędkość kątowa') plt.savefig(f'{mypath}/theta.pdf', bbox_inches='tight') plt.close('all') avg_x_TEB = statistics.mean(np.absolute(np.asarray(list_y[1]['x']))) avg_y_TEB = statistics.mean(np.absolute(np.asarray(list_y[1]['y']))) avg_th_TEB = statistics.mean(np.absolute(np.asarray(list_y[1]['th']))) time_TEB = (list_y[1]['time'][-1] - list_y[1]['time'][0]) avg_x_DWB = statistics.mean(np.absolute(np.asarray(list_y[0]['x']))) avg_y_DWB = statistics.mean(np.absolute(np.asarray(list_y[0]['y']))) avg_th_DWB = statistics.mean(np.absolute(np.asarray(list_y[0]['th']))) time_DWB = (list_y[0]['time'][-1] - list_y[0]['time'][0]) print(f"time TEB: {time_TEB} time DWB: {time_DWB}") print(f"x TEB: {avg_x_TEB} x DWB: {avg_x_DWB}") print(f"y TEB: {avg_y_TEB} y DWB: {avg_y_DWB}") print(f"th TEB: {avg_th_TEB} th DWB: {avg_th_DWB}")
def test_sequential_reader_seek(): bag_path = str(RESOURCES_PATH / 'talker') storage_options, converter_options = get_rosbag_options(bag_path) reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) topic_types = reader.get_all_topics_and_types() # Create a map for quicker lookup type_map = { topic_types[i].name: topic_types[i].type for i in range(len(topic_types)) } # Seek No Filter reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) reader.seek(1585866237113147888) msg_counter = 5 (topic, data, t) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) assert isinstance(msg, Log) (topic, data, t) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) isinstance(msg, String) assert msg.data == f'Hello, world! {msg_counter}' msg_counter += 1 # Set Filter will continue storage_filter = rosbag2_py.StorageFilter(topics=['/topic']) reader.set_filter(storage_filter) (topic, data, t) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) isinstance(msg, String) assert msg.data == f'Hello, world! {msg_counter}' # Seek will keep filter reader.seek(1585866239113147888) msg_counter = 8 (topic, data, t) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) isinstance(msg, String) assert msg.data == f'Hello, world! {msg_counter}' msg_counter += 1 (topic, data, t) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) isinstance(msg, String) assert msg.data == f'Hello, world! {msg_counter}'
def get_odom(self): odom = self.py_walk_wrapper.get_odom() result = deserialize_message(odom, Odometry) return result
def get_messages(self, topic_name): topic_id = self.topic_id[topic_name] rows = self.cursor.execute( 'SELECT timestamp, data FROM messages WHERE topic_id = {}'.format(topic_id)).fetchall() return [(timestamp, deserialize_message(data, self.topic_msg_message[topic_name])) for timestamp, data in rows]
def get_right_foot_pose(self): foot_pose = self.py_walk_wrapper.get_right_foot_pose() result = deserialize_message(foot_pose, Pose) return result
def filter_msg(self, serialized_msg): (topic, data, t) = serialized_msg msg = deserialize_message(data, self._topic_type_map[topic]) if hasattr(msg, 'header'): t = Time.from_msg(msg.header.stamp).nanoseconds return (topic, data, t)
def test_serialize_deserialize(msgs, msg_type): """Test message serialization/deserialization.""" for msg in msgs: msg_serialized = serialize_message(msg) msg_deserialized = deserialize_message(msg_serialized, msg_type) assert msg == msg_deserialized
def read_messages(self, topics=None, start_time=None, end_time=None): """ Generator function used to produce each message in sequence from a Baggie opened as a reader ("r" mode). Parameters ---------- topics : list of str A list of fully qualified topic names to include in the generated results. If this parameter is specified, topics not listed will be omitted from the generated output. Works together with `start_time` and `end_time`. start_time : rclpy.time.Time Only messages stamped with this start time or later will be included in the generated output. Works together with `topics` and `end_time`. end_time : rclpy.time.Time Only messages stamped with this end time or earlier will be included in the generated output. Works together with the `topics` and `start_time`. Returns ------- 3-tuple : str, msg, rclpy.time.Time [0] is the topic name [1] is the deserialized message [2] is the timestamp on the message If the Baggie is opened as a writer ("w" mode), a baggie.BaggieException is raised. """ if self.reader_ is None: raise (BaggieException("Cannot read in write-only mode")) start_time_filter = Time(nanoseconds=0) if ((start_time is not None) and (isinstance(start_time, Time))): start_time_filter = start_time end_time_filter = Time(nanoseconds=sys.maxsize) if ((end_time is not None) and (isinstance(end_time, Time))): end_time_filter = end_time self.reader_.reset_filter() if topics is not None: filt = _StorageFilter() filt.topics = topics self.reader_.set_filter(filt) type_lut = {} t_meta_list = self.reader_.get_all_topics_and_types() for t_meta in t_meta_list: if ((topics is not None) and (t_meta.name not in topics)): continue type_lut[t_meta.name] = baggie.util.typestr2msgtype(t_meta.type) while self.reader_.has_next(): topic, ser_msg, ts = self.reader_.read_next() msg_time = Time(nanoseconds=ts) if ((msg_time < start_time_filter) or (msg_time > end_time_filter)): continue msg = deserialize_message(ser_msg, type_lut[topic]) yield topic, msg, msg_time
def __next__(self): if not self._reader.has_next(): raise StopIteration() (topic, data, t) = self._reader.read_next() msg = deserialize_message(data, self._topic_type_map[topic]) return (topic, msg, t)
def get_support_state(self): return deserialize_message(self.py_walk_wrapper.get_support_state(), Phase)