def test_cut_filter(): filter = CutFilter() parser = argparse.ArgumentParser('cut') filter.add_arguments(parser) args = parser.parse_args(['--duration', '0.5']) filter.set_args([read_metadata('test/test.bag')], args) string_msg = String() string_msg.data = 'in' # timestamp before bag start msg = ('/data', serialize_message(string_msg), 0) assert(filter.filter_msg(msg) == FilterResult.DROP_MESSAGE) # exact start timestamp msg = ('/data', serialize_message(string_msg), 1000) assert(filter.filter_msg(msg) == msg) # timestamp within the bag and cut duration msg = ('/data', serialize_message(string_msg), 500 * 1000 * 1000 - 1000) assert(filter.filter_msg(msg) == msg) # timestamp after the requested duration, but before the last message in the test bag msg = ('/data', serialize_message(string_msg), 1000 * 1000 * 1000 + 1000) assert(filter.filter_msg(msg) == FilterResult.STOP_CURRENT_BAG)
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic_name, initial_time): print(f"Exporting camera {camera}") if kitti_type.find("raw") != -1: camera_pad = '{0:02d}'.format(camera) image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad)) image_path = os.path.join(image_dir, 'data') image_filenames = sorted(os.listdir(image_path)) with open(os.path.join(image_dir, 'timestamps.txt')) as f: image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines()) calib = CameraInfo() calib.header.frame_id = camera_frame_id calib.width, calib.height = tuple([int(x) for x in util[f'S_rect_{camera_pad}']]) calib.distortion_model = 'plumb_bob' calib.k = util[f'K_{camera_pad}'] calib.r = util[f'R_rect_{camera_pad}'] calib.d = [float(x) for x in util[f'D_{camera_pad}']] calib.p = util[f'P_rect_{camera_pad}'] elif kitti_type.find("odom") != -1: camera_pad = '{0:01d}'.format(camera) image_path = os.path.join(kitti.sequence_path, f'image_{camera_pad}') image_filenames = sorted(os.listdir(image_path)) image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) calib = CameraInfo() calib.header.frame_id = camera_frame_id calib.p = util[f'P{camera_pad}'] iterable = zip(image_datetimes, image_filenames) bar = progressbar.ProgressBar() for dt, filename in bar(iterable): image_filename = os.path.join(image_path, filename) cv_image = cv2.imread(image_filename) calib.height, calib.width = cv_image.shape[:2] if camera in (0, 1): cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) encoding = "mono8" if camera in (0, 1) else "bgr8" image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding) image_message.header.frame_id = camera_frame_id if kitti_type.find("raw") != -1: image_message.header.stamp = Time(nanoseconds=int(float(datetime.strftime(dt, "%s.%f")) * 1e+9)).to_msg() topic_ext = "/image_raw" elif kitti_type.find("odom") != -1: image_message.header.stamp = Time(seconds=dt).to_msg() topic_ext = "/image_rect" calib.header.stamp = image_message.header.stamp camera_info_topic = create_topic(topic_name + '/camera_info', "sensor_msgs/msg/CameraInfo", "cdr") bag.create_topic(camera_info_topic) camera_topic = create_topic(topic_name + topic_ext, "sensor_msgs/msg/Image", "cdr") bag.create_topic(camera_topic) bag.write((topic_name + topic_ext, serialize_message(image_message), timestamp_to_nanoseconds_from_epoch(image_message.header.stamp))) bag.write((topic_name + '/camera_info', serialize_message(calib), timestamp_to_nanoseconds_from_epoch(calib.header.stamp)))
def create_test_bag(path): writer = SequentialWriter() storage_options, converter_options = get_rosbag_options(path) writer.open(storage_options, converter_options) topic = TopicMetadata('/data', 'example_interfaces/msg/String', 'cdr') writer.create_topic(topic) msg = String() msg.data = 'test_start' writer.write('/data', serialize_message(msg), 1000) msg.data = 'test_end' writer.write('/data', serialize_message(msg), CONVERSION_CONSTANT + 2000)
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_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 test_add_filter(): filter = AddFilter() parser = argparse.ArgumentParser('add') filter.add_arguments(parser) args = parser.parse_args( ['-t', '/data', '--type', 'example_interfaces/msg/String', '-v', 'test/data.yaml', '--align-to', '/align']) filter.set_args(None, args) topic_metadata = TopicMetadata( '/align', 'example_interfaces/msg/String', 'cdr') topics = filter.filter_topic(topic_metadata) assert(len(topics) == 2) assert(topics[0].name == '/align') assert(topics[0].type == 'example_interfaces/msg/String') assert(topics[1].name == '/data') assert(topics[1].type == 'example_interfaces/msg/String') msg = String() msg.data = 'align' msgs = filter.filter_msg(('/align', serialize_message(msg), 1)) assert(len(msgs) == 2) (topic0, data0, t0) = msgs[0] (topic1, data1, t1) = msgs[1] assert(topic0 == '/align') assert(topic1 == '/data') assert(t0 == t1) assert(deserialize_message(data0, String).data == 'align') assert(deserialize_message(data1, String).data == 'out')
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 check_collision(joint_state): global collision_state if collision_state is None: collision_state = BitbotsMoveitBindings([]) set_joint_states(serialize_message(joint_state)) collision = collision_state.check_collision() return collision
def setUp(self): self.tmp_dir = tempfile.TemporaryDirectory() self.s_opt = baggie._StorageOptions() self.s_opt.uri = "{}{}{}".format(self.tmp_dir.name, os.sep, BAGNAME) self.s_opt.storage_id = "sqlite3" os.makedirs(self.s_opt.uri) self.c_opt = baggie._ConverterOptions() self.c_opt.input_serialization_format = SERFMT self.c_opt.output_serialization_format = SERFMT self.comp_opt = baggie._CompressionOptions() self.comp_opt.compression_format = COMPFMT self.comp_opt.compression_mode = baggie._CompressionMode.FILE writer = baggie._Writer(self.comp_opt) writer.open(self.s_opt, self.c_opt) t_meta = baggie._TopicMetadata() t_meta.name = TOPIC_NAME t_meta.serialization_format = SERFMT t_meta.type = MSGTYPE writer.create_topic(t_meta) self.stamps = [] for i in range(N_MSGS): msg = Int32() msg.data = i ts = baggie._stamp() self.stamps.append(ts) writer.write(t_meta.name, serialize_message(msg), ts) time.sleep(1. / 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 write(self, topic, msg, t=None): """ Write a message to Baggie opened in "w" mode. Parameters ---------- topic : str The topic name to write to msg : Message The unserialzed message instance (of any message type) to be written on the topic. t : rclpy.time.Time or datetime.datetime or None The timestamp for the message. If its value is `None` the current system time is used. Exceptions ---------- baggie.BaggieException if an attempt to write is made on a Baggie opened as a reader ("r" mode). Exceptions encountered by the underlying C++ library are propogated up should they be encountered while writing. """ try: self.writer_.write(topic, serialize_message(msg), baggie.util.stamp(t)) except IndexError as idx_err: t_meta = _TopicMetadata() t_meta.name = topic t_meta.serialization_format = self.c_opt_.input_serialization_format t_meta.type = baggie.util.msg2typestr(msg) self.writer_.create_topic(t_meta) self.writer_.write(topic, serialize_message(msg), baggie.util.stamp(t)) except Exception as ex: if self.writer_ is None: raise (BaggieException("Cannot write in read-only mode")) raise (ex)
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 create_day_time_bag(path): writer = SequentialWriter() storage_options, converter_options = get_rosbag_options(path) writer.open(storage_options, converter_options) topic = TopicMetadata('/data', 'example_interfaces/msg/String', 'cdr') writer.create_topic(topic) HOUR_TO_NS = 60 * 60 * CONVERSION_CONSTANT msg = String() msg.data = 'msg0' writer.write('/data', serialize_message(msg), 13 * HOUR_TO_NS - 1000) msg.data = 'msg1' writer.write('/data', serialize_message(msg), 13 * HOUR_TO_NS) msg.data = 'msg2' writer.write('/data', serialize_message(msg), 14 * HOUR_TO_NS) msg.data = 'msg2' writer.write('/data', serialize_message(msg), 14 * HOUR_TO_NS + 1000)
def create_diagnostics_bag(path): writer = SequentialWriter() storage_options, converter_options = get_rosbag_options(path) writer.open(storage_options, converter_options) topic = TopicMetadata('/diagnostics', 'diagnostic_msgs/msg/DiagnosticArray', 'cdr') writer.create_topic(topic) msg = DiagnosticArray() writer.write('/diagnostics', serialize_message(msg), 1000)
def create_range_bag(path): writer = SequentialWriter() storage_options = StorageOptions(uri=path, storage_id='sqlite3') converter_options = ConverterOptions( input_serialization_format='cdr', output_serialization_format='cdr') writer.open(storage_options, converter_options) topic = TopicMetadata('/range', 'sensor_msgs/msg/Range', 'cdr') writer.create_topic(topic) msg = Range() msg.header.stamp.sec = 0 msg.header.stamp.nanosec = 90 msg.range = 10.0 writer.write('/range', serialize_message(msg), 100) msg.header.stamp.sec = 0 msg.header.stamp.nanosec = 190 msg.range = 20.0 writer.write('/range', serialize_message(msg), 200)
def __init__(self, namespace="", parameters: [Parameter] = [], setForceSmoothStepTransition=False): serialized_parameters = [] for parameter in parameters: serialized_parameters.append(serialize_message(parameter)) if parameter.value.type == 2: print( f"Gave parameter {parameter.name} of integer type. If the code crashes it is maybe because this " f"should be a float. You may need to add an .0 in some yaml file." ) self.py_walk_wrapper = PyWalkWrapper(namespace, serialized_parameters, setForceSmoothStepTransition)
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 save_gps_fix_data(bag, kitti, gps_frame_id, topic_name): topic = create_topic(topic_name, "sensor_msgs/msg/NavSatFix", "cdr") bag.create_topic(topic) for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): navsatfix_msg = NavSatFix() navsatfix_msg.header.frame_id = gps_frame_id navsatfix_msg.header.stamp = Time(nanoseconds=int(float(timestamp.strftime("%s.%f")) * 1e+9)).to_msg() navsatfix_msg.latitude = oxts.packet.lat navsatfix_msg.longitude = oxts.packet.lon navsatfix_msg.altitude = oxts.packet.alt navsatfix_msg.status.service = 1 bag.write((topic_name, serialize_message(navsatfix_msg), timestamp_to_nanoseconds_from_epoch(navsatfix_msg.header.stamp)))
def save_static_transforms(bag, transforms, timestamps): print("Exporting static transformations") topic_name = '/tf_static' topic = create_topic(topic_name, "tf2_msgs/msg/TFMessage", "cdr") bag.create_topic(topic) tfm = TFMessage() for transform in transforms: t = get_static_transform(from_frame_id=transform[0], to_frame_id=transform[1], transform=transform[2]) tfm.transforms.append(t) for timestamp in timestamps: time = Time(nanoseconds=int(float(timestamp.strftime("%s.%f")) * 1e+9)).to_msg() for i in range(len(tfm.transforms)): tfm.transforms[i].header.stamp = time bag.write((topic_name, serialize_message(tfm), timestamp_to_nanoseconds_from_epoch(time)))
def special_reset(self, state: String, phase: float, cmd_vel_msg: Twist, reset_odometry: bool): state_dict = { "PAUSED": 0, "WALKING": 1, "IDLE": 2, "START_MOVEMENT": 3, "STOP_MOVEMENT": 4, "START_STEP": 5, "STOP_STEP": 6, "KICK": 7 } self.py_walk_wrapper.special_reset(state_dict[state], phase, serialize_message(cmd_vel_msg), reset_odometry)
def save_gps_vel_data(bag, kitti, gps_frame_id, topic_name): topic = create_topic(topic_name, "geometry_msgs/msg/TwistStamped", 'cdr') bag.create_topic(topic) for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): twist_msg = TwistStamped() twist_msg.header.frame_id = gps_frame_id twist_msg.header.stamp = Time(nanoseconds=int(float(timestamp.strftime("%s.%f")) * 1e+9)).to_msg() twist_msg.twist.linear.x = oxts.packet.vf twist_msg.twist.linear.y = oxts.packet.vl twist_msg.twist.linear.z = oxts.packet.vu twist_msg.twist.angular.x = oxts.packet.wf twist_msg.twist.angular.y = oxts.packet.wl twist_msg.twist.angular.z = oxts.packet.wu bag.write( (topic_name, serialize_message(twist_msg), timestamp_to_nanoseconds_from_epoch(twist_msg.header.stamp)))
def save_velo_data(bag, kitti, velo_frame_id, topic_name): print("Exporting velodyne data") topic = create_topic(topic_name + '/pointcloud', "sensor_msgs/msg/PointCloud2", "cdr") bag.create_topic(topic) velo_path = os.path.join(kitti.data_path, 'velodyne_points') velo_data_dir = os.path.join(velo_path, 'data') velo_filenames = sorted(os.listdir(velo_data_dir)) with open(os.path.join(velo_path, 'timestamps.txt')) as f: lines = f.readlines() velo_datetimes = [] for line in lines: if len(line) == 1: continue dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f') velo_datetimes.append(dt) iterable = zip(velo_datetimes, velo_filenames) bar = progressbar.ProgressBar() for dt, filename in bar(iterable): if dt is None: continue velo_filename = os.path.join(velo_data_dir, filename) # read binary data points = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4) # create header header = Header() header.frame_id = velo_frame_id header.stamp = Time(nanoseconds=int(float(datetime.strftime(dt, "%s.%f")) * 1e+9)).to_msg() # fill pcl msg fields = [PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), PointField(name='i', offset=12, datatype=PointField.FLOAT32, count=1)] pcl_msg = create_cloud(header, fields, points) bag.write((topic_name + '/pointcloud', serialize_message(pcl_msg), timestamp_to_nanoseconds_from_epoch(pcl_msg.header.stamp)))
def set_moveit_parameters(parameters: [Parameter], type): """ Allows to set the parameters manually instead of copying them from the move_group node. :param parameters: List of Parameter messages :return: """ global ik_fk_state, collision_state, current_state serialized_parameters = [] for parameter in parameters: serialized_parameters.append(serialize_message(parameter)) if type == "ikfk": ik_fk_state = BitbotsMoveitBindings(serialized_parameters) elif type == "collision": collision_state = BitbotsMoveitBindings(serialized_parameters) elif type == "current": current_state = BitbotsMoveitBindings(serialized_parameters) else: print("Type for bitbots_moveit_bindings state not correctly defined") exit(1)
def test_rename_filter(): filter = RenameFilter() parser = argparse.ArgumentParser('rename') filter.add_arguments(parser) args = parser.parse_args(['-t', '/data', '--name', '/renamed']) filter.set_args(None, args) topic_metadata = TopicMetadata( '/data', 'example_interfaces/msg/String', 'cdr') assert(filter.filter_topic(topic_metadata).name == '/renamed') msg = String() msg.data = 'test' # timestamp within the bag and cut duration bag_msg = ('/data', serialize_message(msg), 1) (topic, _, _) = filter.filter_msg(bag_msg) assert(topic == '/renamed')
def save_imu_data(bag, kitti, imu_frame_id, topic_name): print("Exporting IMU") topic = create_topic(topic_name, "sensor_msgs/msg/Imu", "cdr") bag.create_topic(topic) for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): q = quaternion_from_euler(oxts.packet.roll, oxts.packet.pitch, oxts.packet.yaw) imu = Imu() imu.header.frame_id = imu_frame_id imu.header.stamp = Time(nanoseconds=int(float(timestamp.strftime("%s.%f")) * 1e+9)).to_msg() imu.orientation.x = q[0] imu.orientation.y = q[1] imu.orientation.z = q[2] imu.orientation.w = q[3] imu.linear_acceleration.x = oxts.packet.af imu.linear_acceleration.y = oxts.packet.al imu.linear_acceleration.z = oxts.packet.au imu.angular_velocity.x = oxts.packet.wf imu.angular_velocity.y = oxts.packet.wl imu.angular_velocity.z = oxts.packet.wu bag.write((topic_name, serialize_message(imu), timestamp_to_nanoseconds_from_epoch(imu.header.stamp)))
def test_restamp_filter(): filter = RestampFilter() parser = argparse.ArgumentParser('restamp') filter.add_arguments(parser) args = parser.parse_args([]) filter.set_args(None, args) topic_metadata = TopicMetadata( '/data', 'diagnostic_msgs/msg/DiagnosticArray', 'cdr') assert(filter.filter_topic(topic_metadata) == topic_metadata) ns_stamp = 500 * 1000 * 1000 - 100 msg = DiagnosticArray() msg.header.stamp.sec = 0 msg.header.stamp.nanosec = ns_stamp # timestamp within the bag and cut duration bag_msg = ('/data', serialize_message(msg), 500 * 1000 * 1000) (_, _, t) = filter.filter_msg(bag_msg) assert(t == ns_stamp)
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 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 test_serialized_publish(self): msg = Strings() msg.string_value = 'ñu' pub = self.node.create_publisher(BasicTypes, 'chatter', 1) pub.publish(serialize_message(msg)) self.node.destroy_publisher(pub)