def _get_start_end_time(self, limit):
"""
Returns the start and end time to use (rospy.Time).
also sets self.start_stamp, self.end_stamp
"""
from rospy.rostime import Time #@UnresolvedImport
self.info('limit: %r' % limit)
warnings.warn('Make better code for dealing with unindexed')
if limit is not None and limit != 0:
# try:
chunks = self.bag.__dict__['_chunks']
self.start_stamp = chunks[0].start_time.to_sec()
self.end_stamp = chunks[-1].end_time.to_sec()
start_time = Time.from_sec(self.start_stamp)
end_time = Time.from_sec(self.start_stamp + limit)
return start_time, end_time
# except Exception as e:
# self.error('Perhaps unindexed bag?')
# self.error(traceback.format_exc(e))
# raise
# start_time = None
# end_time = None
#
# self.info('start_stamp: %s' % self.start_stamp)
# self.info('end_stamp: %s' % self.end_stamp)
else:
self.start_stamp = None
self.end_stamp = None
return None, None
def _get_start_end_time(self, limit):
"""
Returns the start and end time to use (rospy.Time).
also sets self.start_stamp, self.end_stamp
"""
from rospy.rostime import Time # @UnresolvedImport
self.info('limit: %r' % limit)
warnings.warn('Make better code for dealing with unindexed')
if limit is not None and limit != 0:
# try:
chunks = self.bag.__dict__['_chunks']
self.start_stamp = chunks[0].start_time.to_sec()
self.end_stamp = chunks[-1].end_time.to_sec()
start_time = Time.from_sec(self.start_stamp)
end_time = Time.from_sec(self.start_stamp + limit)
return start_time, end_time
# except Exception as e:
# self.error('Perhaps unindexed bag?')
# self.error(traceback.format_exc(e))
# raise
# start_time = None
# end_time = None
#
# self.info('start_stamp: %s' % self.start_stamp)
# self.info('end_stamp: %s' % self.end_stamp)
else:
self.start_stamp = None
self.end_stamp = None
return None, None
def gx_msg_stamp(msg):
s = str(msg)
if s.startswith('timestamp: '):
stamp = s.split('\n')[0].split(':')[1].strip()
stamp = stamp[:10] + '.' + stamp[10:]
return Time.from_sec(float(stamp))
else:
print (msg)
exit('Unknown gx type!')
def gxPose2d_to_tf(msg,t,outbag,frame_id,child_frame_id,offset):
str_gx=[x.split(":") for x in str(msg).split("\n")]
br = TransformStamped()
br.transform.translation=Point(float(str_gx[1][1]),float(str_gx[2][1]),0)
br.transform.rotation=Quaternion(*tf.transformations.quaternion_from_euler(0,0,float(str_gx[3][1])))
br.header.stamp=Time.from_sec(gx_msg_stamp(msg).to_sec() + offset)
br.header.frame_id=frame_id
br.child_frame_id=child_frame_id
tfm=TFMessage([br])
outbag.write('/tf',tfm,t)
def _get_start_end_time_t0_t1(self, t0, t1):
"""
Returns the start and end time to use (rospy.Time).
also sets self.start_stamp, self.end_stamp
"""
warnings.warn('Make better code for dealing with unindexed')
from rospy.rostime import Time #@UnresolvedImport
if t0 is not None or t1 is not None:
# try:
chunks = self.bag.__dict__['_chunks']
self.start_stamp = chunks[0].start_time.to_sec()
self.end_stamp = chunks[-1].end_time.to_sec()
if t0 is not None:
start_time = self.start_stamp + t0
else:
start_time = self.start_stamp
if t1 is not None:
end_time = self.start_stamp + t1
else:
end_time = self.end_stamp
start_time = Time.from_sec(start_time)
end_time = Time.from_sec(end_time)
return start_time, end_time
# except Exception as e:
# self.error('Perhaps unindexed bag?')
# self.error(traceback.format_exc(e))
# raise
# start_time = None
# end_time = None
#
# self.info('start_stamp: %s' % self.start_stamp)
# self.info('end_stamp: %s' % self.end_stamp)
else:
self.start_stamp = None
self.end_stamp = None
return None, None
def decode(self, _, data):
""" Generate a rospy.rostime.Time instance based on the given data of
the form 'YYYY-MM-DDTHH:MM:SS.mmmmmm' (ISO 8601).
"""
if '+' in data:
data = data[:data.index('+')]
try:
dt = datetime(year=int(data[0:4]), month=int(data[5:7]),
day=int(data[8:10]), hour=int(data[11:13]),
minute=int(data[14:16]), second=int(data[17:19]),
microsecond=int(data[20:]))
except ValueError:
return Time()
return Time.from_sec(time.mktime(dt.timetuple()))
def decode(self, data):
""" Generate a rospy.rostime.Time instance based on the given data of
the form 'YYYY-MM-DDTHH:MM:SS.mmmmmm' (ISO 8601).
"""
if '+' in data:
data = data[:data.index('+')]
try:
dt = datetime(year=int(data[0:4]), month=int(data[5:7]),
day=int(data[8:10]), hour=int(data[11:13]),
minute=int(data[14:16]), second=int(data[17:19]),
microsecond=int(data[20:]))
except ValueError:
return Time()
return Time.from_sec(time.mktime(dt.timetuple()))
def gxPose2d_to_odom(msg,t,outbag,frame_id,child_frame_id,offset):
'''
turn /gx/RosPose2d topic to /odom topic
change the type of gx_sensor/RosPose2d to nav_msgs/Odometry
/gx/RosPose2d topic message example:
timestamp: 1562430246571000
x: 2.03333210945
y: -0.198542118073
theta: 0.551429688931
linearVelocity: 0.0
angularVelociy: -0.00757710635662
the topic is in base_link coordinate
'''
str_gx=[x.split(":") for x in str(msg).split("\n")]
unit_odom=Odometry()
unit_odom.pose.pose.position=Point(float(str_gx[1][1]),float(str_gx[2][1]),0)
unit_odom.pose.pose.orientation=Quaternion(*tf.transformations.quaternion_from_euler(0,0,float(str_gx[3][1])))
unit_odom.header.stamp=Time.from_sec(gx_msg_stamp(msg).to_sec() + offset)
unit_odom.header.frame_id=frame_id
unit_odom.child_frame_id=child_frame_id
unit_odom.twist.twist.angular=Point(0,0,float(str_gx[5][1]))
unit_odom.twist.twist.linear=Point(float(str_gx[4][1])*math.cos(float(str_gx[3][1])),float(str_gx[4][1])*math.sin(float(str_gx[3][1])),0)
outbag.write('/odom',unit_odom,t)
def increment_clock(clock_pub, cumulative_time):
clock_signal = Clock()
clock_signal.clock = Time.from_sec(cumulative_time)
increment_clock.time = clock_signal.clock
clock_pub.publish(clock_signal)
return cumulative_time
def main():
infiles = ['synced.bag', 'record-d400.bag', 'record-t265.bag'] if len(
sys.argv) == 1 else sys.argv[1:]
outfile = 'final.bag'
outbag = rosbag.Bag(outfile, 'w')
write_exposure_to_seq = True
write_aligned_depth = True
if write_aligned_depth:
aligned_depth_folder = 'aligned_depth'
if not os.path.exists(aligned_depth_folder):
print('Did not find folder of aligned depth images')
aligned_depth_filenames = []
else:
aligned_depth_filenames = sorted([
s for s in os.listdir(aligned_depth_folder)
if s.endswith('.png')
])
print('Found %d images in %s' %
(len(aligned_depth_filenames), aligned_depth_folder))
with rosbag.Bag(infiles[0]) as refbag:
tstart = Time.from_sec(refbag.get_start_time())
add_static_tf(outbag, tstart)
# { in_topic: (out_topic, frame_id) }
image_topics = {
'/device_0/sensor_1/Color_0/image/data':
('/d400/color/image_raw', 'd400_color'),
'/device_0/sensor_0/Depth_0/image/data':
('/d400/depth/image_raw', 'd400_depth'),
'/device_0/sensor_0/Fisheye_1/image/data':
('/t265/fisheye1/image_raw', 't265_fisheye1'),
'/device_0/sensor_0/Fisheye_2/image/data':
('/t265/fisheye2/image_raw', 't265_fisheye2'),
}
imu_topics = {
'/device_0/sensor_2/Accel_0/imu/data':
('/d400/accel/sample', 'd400_imu'),
'/device_0/sensor_2/Gyro_0/imu/data':
('/d400/gyro/sample', 'd400_imu'),
'/device_0/sensor_0/Accel_0/imu/data':
('/t265/accel/sample', 't265_imu'),
'/device_0/sensor_0/Gyro_0/imu/data':
('/t265/gyro/sample', 't265_imu'),
}
camera_info_topics = {
'/device_0/sensor_1/Color_0/info/camera_info':
('/d400/color/camera_info', 'd400_color'),
'/device_0/sensor_0/Depth_0/info/camera_info':
('/d400/depth/camera_info', 'd400_depth'),
'/device_0/sensor_0/Fisheye_1/info/camera_info':
('/t265/fisheye1/camera_info', 't265_fisheye1'),
'/device_0/sensor_0/Fisheye_2/info/camera_info':
('/t265/fisheye2/camera_info', 't265_fisheye2'),
}
imu_info_topics = {
'/device_0/sensor_2/Accel_0/imu_intrinsic':
('/d400/accel/imu_info', 'imu_accel'),
'/device_0/sensor_2/Gyro_0/imu_intrinsic':
('/d400/gyro/imu_info', 'imu_gyro'),
'/device_0/sensor_0/Accel_0/imu_intrinsic':
('/t265/accel/imu_info', 'imu_accel'),
'/device_0/sensor_0/Gyro_0/imu_intrinsic':
('/t265/gyro/imu_info', 'imu_gyro'),
}
def write_image(outbag, topic, msg):
if write_aligned_depth and topic == '/d400/depth/image_raw':
filename = '%f.png' % msg.header.stamp.to_sec()
if filename not in aligned_depth_filenames:
dist = [
abs(float(f.rstrip('.png')) - msg.header.stamp.to_sec())
for f in aligned_depth_filenames
]
stamp_diff_tol = 0.005 # 5ms
if min(dist) < stamp_diff_tol:
filename = aligned_depth_filenames[dist.index(min(dist))]
else:
print(
'Could not find aligned depth image at %.9f. Closest is %s'
% (msg.header.stamp.to_sec(),
aligned_depth_filenames[dist.index(min(dist))]))
return
im = cv2.imread(aligned_depth_folder + '/' + filename,
cv2.IMREAD_UNCHANGED)
msg_aligned = cv_bridge.CvBridge().cv2_to_imgmsg(im,
encoding='mono16')
msg_aligned.header = msg.header
msg_aligned.header.frame_id = 'd400_color'
outbag.write('/d400/aligned_depth_to_color/image_raw', msg_aligned,
msg.header.stamp)
outbag.write(topic, msg, msg.header.stamp)
for infile in infiles:
with rosbag.Bag(infile) as inbag:
print(
"-------------------- Input: %s ------------------------------"
% infile)
print(inbag)
image_buffers = {}
exposure_buffers = {}
for topic, msg, t in inbag.read_messages():
if topic == '/odom' or topic == '/scan' or topic == '/rslidar_packets':
outbag.write(topic, msg, msg.header.stamp)
elif topic == '/rslidar_packets_difop':
outbag.write(topic, msg, msg.stamp)
elif gt_source == 'hector' and topic == '/slam_out_pose':
base_pose = transform_pose_msg(msg, 'base_link',
'base_link')
base_pose.header.frame_id = 'gt_map'
#outbag.write('/gt_base', base_pose, msg.header.stamp)
#outbag.write('/gt_d400_color', transform_pose_msg(base_pose, 'base_link', 'd400_color'), msg.header.stamp)
#outbag.write('/gt_t265_fisheye1', transform_pose_msg(base_pose, 'base_link', 't265_fisheye1'), msg.header.stamp)
outbag.write('/gt', pose_to_tf(base_pose, 'base_link'),
msg.header.stamp)
elif gt_source == 'gaussian' and topic == '/v5_current_pose':
base_pose = PoseStamped()
base_pose.header = msg.header
base_pose.pose = msg.pose.pose
base_pose.header.frame_id = 'gt_map'
#outbag.write('/gt_base', base_pose, msg.header.stamp)
#outbag.write('/gt_d400_color', transform_pose_msg(base_pose, 'base_link', 'd400_color'), msg.header.stamp)
#outbag.write('/gt_t265_fisheye1', transform_pose_msg(base_pose, 'base_link', 't265_fisheye1'), msg.header.stamp)
outbag.write('/gt', pose_to_tf(base_pose, 'base_link'),
msg.header.stamp)
elif gt_source == 'mocap' and topic == '/vrpn_client_node/RigidBody1/pose':
base_pose = transform_pose_msg(msg, 'marker', 'base_link')
base_pose.header.frame_id = 'gt_map'
#outbag.write('/gt_base', base_pose, msg.header.stamp)
#outbag.write('/gt_d400_color', transform_pose_msg(base_pose, 'base_link', 'd400_color'), msg.header.stamp)
#outbag.write('/gt_t265_fisheye1', transform_pose_msg(base_pose, 'base_link', 't265_fisheye1'), msg.header.stamp)
outbag.write('/gt', pose_to_tf(base_pose, 'base_link'),
msg.header.stamp)
elif topic in image_topics:
msg.header.frame_id = image_topics[topic][1]
if write_exposure_to_seq:
if topic in exposure_buffers and t in exposure_buffers[
topic]:
msg.header.seq = exposure_buffers[topic].pop(t)
write_image(outbag, image_topics[topic][0], msg)
else:
image_buffers.setdefault(topic, {})[t] = msg
else:
write_image(outbag, image_topics[topic][0], msg)
elif write_exposure_to_seq and topic.rstrip(
'metadata'
) + 'data' in image_topics and msg.key == 'Actual Exposure':
image_topic = topic.rstrip('metadata') + 'data'
if image_topic in image_buffers and t in image_buffers[
image_topic]:
image = image_buffers[image_topic].pop(t)
image.header.seq = int(msg.value)
write_image(outbag, image_topics[image_topic][0],
image)
else:
exposure_buffers.setdefault(image_topic,
{})[t] = int(msg.value)
elif topic in camera_info_topics:
msg.header.stamp = tstart
msg.header.frame_id = camera_info_topics[topic][1]
msg.P = list(msg.P)
msg.P[0] = msg.K[0]
msg.P[2] = msg.K[2]
msg.P[5] = msg.K[4]
msg.P[6] = msg.K[5]
msg.P[10] = 1.
msg.R = list(msg.R)
msg.R[0] = 1.
msg.R[4] = 1.
msg.R[8] = 1.
outbag.write(camera_info_topics[topic][0], msg, tstart)
if write_aligned_depth and camera_info_topics[topic][
0] == '/d400/color/camera_info':
outbag.write(
'/d400/aligned_depth_to_color/camera_info', msg,
tstart)
elif topic in imu_info_topics:
msg_info = IMUInfo()
if hasattr(msg_info, 'header'):
msg_info.header.stamp = tstart
msg_info.header.frame_id = imu_info_topics[topic][1]
else:
msg_info.frame_id = imu_info_topics[topic][1]
msg_info.data = msg.data
msg_info.noise_variances = msg.noise_variances
msg_info.bias_variances = msg.bias_variances
outbag.write(imu_info_topics[topic][0], msg_info, tstart)
elif topic in imu_topics:
msg.header.frame_id = imu_topics[topic][1]
outbag.write(imu_topics[topic][0], msg, msg.header.stamp)
for topic in image_buffers:
if len(image_buffers[topic]) != 0:
print(
'Error: discarded %d images without exposure data on %s. This should not happen!'
% len(image_buffers[topic]), topic)
outbag.close()
outbag = rosbag.Bag(outfile)
print("------------------- Output: %s ------------------------------" %
outfile)
print(outbag)
def get_stamp_from_gx_msg(msg):
str_gx = [x.split(":") for x in str(msg).split("\n")]
stamp = str_gx[0][1].strip()[:10] + "." + str_gx[0][1].strip()[10:]
stamp = Time.from_sec(numpy.float128(stamp))
return stamp
