python: bag包解析pcd和图像

bag包解析pcd和图像：

---

import glob, os
import rosbag
import cv2
from cv_bridge import CvBridge, CvBridgeError
import sensor_msgs.point_cloud2 as pc2
import numpy as np  

print(cv2.__version__)
bridge = CvBridge()
image_topic = "/usb_cam/image_raw/compressed"
point_topic = "/topic"
bagpath = "path"
for f in glob.glob(bagpath + "/*.bag"):
    print("bagfile:",f)
    img_path = os.path.dirname(f) + "/" + os.path.split(f)[-1][:-4] + "/jpg"
    pcd_path = os.path.dirname(f) + "/" + os.path.split(f)[-1][:-4] + "/pcd"

    if not os.path.exists(img_path):
        os.makedirs(img_path, mode=0o755)

    if not os.path.exists(pcd_path):
        os.makedirs(pcd_path, mode=0o755)

    cmd = "rosrun pcl_ros bag_to_pcd %s %s %s" % (f, point_topic, pcd_path)
    # print("cmd:", cmd)
    os.system(cmd)
    
    with rosbag.Bag(f, 'r') as bag:
        for topic, msg, t in bag.read_messages():
            print(topic, t)
            if topic == image_topic:
                try:
                    cv_image = bridge.compressed_imgmsg_to_cv2(msg, "bgr8")  #sensor_msgs::Image message to an OpenCV: bridege.imgmsg_to_cv2
                    cv2.imshow("window", cv_image)           
                    image_name = "%f" % msg.header.stamp.to_sec() + ".jpg"
                    file_path = os.path.join(img_path, image_name)
                    cv2.imwrite(file_path,cv_image)
                except CvBridgeError as e:
                    print(e)
                print("img saved done", )
            # if topic == point_topic:
            #     data = pc2.read_points(msg)
            #     points = np.array(list(data), dtype=np.float32)
            #     # print(points, len(points))
            #     filename = "%.6f" % msg.header.stamp.to_sec() + '.bin'
            #     file_path = os.path.join(pcd_path, filename)
            #     points.tofile(file_path)
            #     print(file_path)

---