python同步解析ROS BAG包

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包即可

python同步解析ROS BAG包

猜你喜欢