from __future__ import print_function, division
from PIL import Image
try:
from cStringIO import StringIO
except ImportError:
from io import StringIO
import rosbag
import sys
if sys.getdefaultencoding() != 'utf-8':
reload(sys)
sys.setdefaultencoding('utf-8')
bag_name = '20180822104624.bag'
dst_dir = '20180822104624/'
if dst_dir[-1] == '/':
img_name = dst_dir + "{:0>10d}.jpg"
imu_info_name = dst_dir + "IMUInformation.txt"
info_name = dst_dir + "Information.txt"
else :
img_name = dst_dir + "/{:0>10d}.jpg"
imu_info_name = dst_dir + "/IMUInformation.txt"
info_name = dst_dir + "/Information.txt"
topics = ["/inspva_info",
"/vehicle_speed",
"/imu_data",
"/camera/compressed"
]
bag = rosbag.Bag(bag_name)
picture_cnt = 0
imu_infos = []
infos = []
front_radar_infos = []
left_radar_infos = []
right_radar_infos = []
front_x_data = [None] * 64
front_y_data = [None] * 64
left_x_data = [None] * 64
left_y_data = [None] * 64
right_x_data = [None] * 64
right_y_data = [None] * 64
for topic, msg, t in bag.read_messages(topics = topics): #print(topic)# print(msg.header)
header = msg.header
header_seq = header.seq
stamp_sec = header.stamp.secs
stamp_nsec = header.stamp.nsecs
frame_id = header.frame_id
#parse topic inspva_infos
if (topic == str('/inspva_info')): #inspva info
time_stamp = msg.time_stamp
latitude = msg.latitude
longitude = msg.longitude
height = msg.height
vehicle_x = msg.vehicle_x
vehicle_y = msg.vehicle_y
roll = msg.roll
pitch = msg.pitch
azimuth = msg.azimuth
acc_x = msg.acc_x
acc_y = msg.acc_y
acc_z = msg.acc_z
valid = msg.valid# Bool#
#parse topic vehicle_speed
if (topic == str('/vehicle_speed')): #vehicle speed# sys_time_us = msg.sys_time_us
vehicle_speed_isvalid = msg.vehicle_speed_isvalid# bool
vehicle_speed = msg.vehicle_speed#
#parse topic imu_data
if (topic == str('/imu_data')): #imu_data# sys_time_us = msg.sys_time_us
measurement_time = msg.measurement_time
status = msg.status
measurement_span = msg.measurement_span
linear_acceleration_x = msg.linear_acceleration_x
linear_acceleration_y = msg.linear_acceleration_y
linear_acceleration_z = msg.linear_acceleration_z
linear_acceleration_covariance = msg.linear_acceleration_covariance# tuple
angular_velocity_x = msg.angular_velocity_x
angular_velocity_y = msg.angular_velocity_y
angular_velocity_z = msg.angular_velocity_z
angular_velocity_covariance = msg.angular_velocity_covariance# tuple
orientation_pitch = msg.orientation_pitch
orientation_roll = msg.orientation_roll
orientation_yaw = msg.orientation_yaw
orientation_quternion_1 = msg.orientation_quternion_1
orientation_quternion_2 = msg.orientation_quternion_2
orientation_quternion_3 = msg.orientation_quternion_3
orientation_quternion_4 = msg.orientation_quternion_4
orientation_covariance = msg.orientation_covariance# tuple
#parse topic camara
if (topic == '/camera/compressed'):
im_format = msg.format
data = msg.data
def imgmsg_to_pil(img_msg, rgba = True):
pil_img = Image.open(StringIO(img_msg))
if pil_img.mode != 'L':
pil_img.convert("RGB")
return pil_img
image = imgmsg_to_pil(msg.data)
image.save(img_name.format(picture_cnt))
## dump info when image
if (topic == '/camera/compressed'):
dum_str = "{:0>10d} {} {} {} {} {} {} {} {} {} {} {}".format(picture_cnt,
header_seq,
stamp_sec,
stamp_nsec,
latitude,
longitude,
# vehicle_speed,
roll,
pitch,
azimuth,
acc_z,
acc_y,
acc_x)# print(picture_cnt)
print (dum_str)
infos.append(dum_str)
#except:
# pass
picture_cnt += 1
with open(info_name, "w") as fp:
for it in infos:
fp.write(it + "\n")
bag.close()
2.使用命令行解析方式:
第一步:开启roscore
roscore
第二步:查看rosbag具体信息
rosbag info + 具体包名
第三步:播放rosbag包
rosbag play ./test3_fei.bag -r 0.5 -l
第四步:将图片从rosbag中解析出来
rosrun image_view extract_images image:=/lane_segment/camera_front/image _image_transport:=compressed
第五步:也可以查看rosbag中的topic
rostopic echo
3. 直接使用rqt_bag进行rosbag的播放
roscore
rqt_bag
会得到这样的界面,直接open你需要打开的rosbag包即可