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)
python:bag包解析pcd和图像
于 2021-08-10 15:38:13 首次发布