python解析雷达数据_【学习笔记】使用python带时间戳提取rosbag中的图像和雷达数据...

使用sick的TIM561雷达采集数据，topic nam是/scan，摄像头录制图像，topic name是/usb_cam/image_raw。脚本以下：python

# coding:utf-8

#!/usr/bin/python

# Extract images from a bag file.

#PKG = 'beginner_tutorials'

import roslib; #roslib.load_manifest(PKG)

import rosbag

import rospy

import cv2

import os

from sensor_msgs.msg import Image

from cv_bridge import CvBridge

from cv_bridge import CvBridgeError

# Reading bag filename from command line or roslaunch parameter.

#import os

#import sys

rgb_path = '/home/kanghao/bagfiles/bag_image/'

laser_path= '/home/kanghao/bagfiles/laser_data/'

class ImageCreator():

def __init__(self):

self.bridge = CvBridge()

with rosbag.Bag('/home/kanghao/bagfiles/2019-02-19-15-23-09.bag', 'r') as bag: #要读取的bag文件；

for topic,msg,t in bag.read_messages():

if topic == "/usb_cam/image_raw": #图像的topic；

try:

cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")

except CvBridgeError as e:

print(e)

timestr = "%.6f" % msg.header.stamp.to_sec()

#%.6f表示小数点后带有6位，可根据精确度须要修改；

image_name = timestr+ ".png" #图像命名：时间戳.png

cv2.imwrite(rgb_path + image_name, cv_image) #保存；

elif topic == "/scan": #laser的topic；

timestr = "%.6f" % msg.header.stamp.to_sec()

laser_data_name = timestr + ".txt"

laser_data_path = os.path.join(laser_path, laser_data_name)

with open(laser_data_path, "w") as f:

f.write(str(msg))

if __name__ == '__main__':

try:

image_creator = ImageCreator()

except rospy.ROSInterruptException:

pass