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)
