#!/usr/bin/env python # -*- coding: utf-8 -*- """ Created on Wed Nov 1 13:38:28 2023 @author: dell """ import subprocess from subprocess import Popen, PIPE import os import sys import math import cv2 import numpy as np import shutil import rosbag def draw_points(pic, filename): f = open(filename, "r") ff = f.readlines()[11:] for i in range(0, len(ff)): point = ff[i].split()[:4] point_x = float(point[0]) point_y = -float(point[1]) if math.isnan(point_x): pass else: cv2.circle(pic, (int(800 - point_x * 20), int(400 - point_y * 20)), 1, (250, 250, 250), 0) if __name__ == '__main__': input_dir = sys.argv[1] # input_dir = '/media/dell/BFAC-F22B/data/pujin_datareturn/pjirobot_20231207_2023-12-07-07-18-46_54.bag' output_dir = sys.argv[2] # output_dir='/media/dell/BFAC-F22B/data/pujin_datareturn' bag_name = input_dir.split('/')[-1].split('.')[0] output_dir = os.path.join(output_dir, bag_name + '_pcd_lidar' + '/pcd') if not os.path.exists(output_dir): os.makedirs(output_dir) with rosbag.Bag(input_dir, 'r') as bag: num_count = 0 for topic, msg, t in bag.read_messages(): if topic == "/scan_map_icp_amcl_node/scan_point_transformed": num_count += 1 bagtime = int(input_dir.split('/')[-1].split('.')[0].split('_')[-1]) hz = str(float(num_count / bagtime)) print(hz) #######################解析rosbag中的点云point_concat话题，获得原始pcd文件####################### command = ['rosrun', 'pcl_ros', 'bag_to_pcd'] + [input_dir] + ['/scan_map_icp_amcl_node/scan_point_transformed'] + [ output_dir] command = ' '.join(command) os.system(command) # Process_rosbag = Popen(command, stdout=subprocess.PIPE, stderr=subprocess.PIPE) # Process_rosbag.wait() file1 = os.path.join(output_dir[0:-4], 'pcd_ascii') file2 = os.path.join(output_dir[0:-4], 'jpg') if not os.path.exists(file1): os.makedirs(file1) if not os.path.exists(file2): os.makedirs(file2) #######################将原始pcd文件转化成pcd-ascii格式,并生成jpg文件####################### for root, dirs, files in os.walk(output_dir[0:-4]): # find .pcd files and do the transfer twice for file in files: if "pcd" == root[-3:] and ".pcd" == file[-4:]: inputFilePath = root + "/" + file tempFilePath = root[:-3] + "pcd_ascii/" + file outputFilePath = root[:-3] + "jpg/" + file[:-3] + "jpg" # transfer pcd_utf-8 to pcd_ascii command = "pcl_convert_pcd_ascii_binary " + \ inputFilePath + \ " " + \ tempFilePath + \ " 0" # print(command) os.system(command) # make sure the temp file exist, invalid input file may cause this issue if not os.path.exists(tempFilePath): continue # transfer pcd_ascii to jpg pic = np.zeros([800, 1600, 3]) draw_points(pic, tempFilePath) cv2.imwrite(outputFilePath, pic) # print("jpg generated: ", outputFilePath) else: break #######################将转化的点云jpg合成视频####################### jpg_list = os.listdir(file2) if not jpg_list == []: command = ['ffmpeg', '-f', 'image2', '-r', hz, '-pattern_type', 'glob', '-i'] + ['"jpg/*.jpg"'] + ['-tag:v', 'avc1', '-y'] + [ 'pcd_lidar.mp4'] result_string = " ".join(command) p = Popen(result_string, shell=True, cwd=output_dir[0:-4] + '/', stdout=subprocess.PIPE, stderr=subprocess.PIPE) p.wait() shutil.rmtree(file1) shutil.rmtree(file2) shutil.rmtree(output_dir) def parse(input_dir, output_dir): bag_name = input_dir.split('/')[-1].split('.')[0] output_dir = os.path.join(output_dir, bag_name + '_pcd_lidar' + '/pcd') if not os.path.exists(output_dir): os.makedirs(output_dir) with rosbag.Bag(input_dir, 'r') as bag: num_count = 0 for topic, msg, t in bag.read_messages(): if topic == "/scan_map_icp_amcl_node/scan_point_transformed": num_count += 1 bagtime = int(input_dir.split('/')[-1].split('.')[0].split('_')[-1]) hz = str(float(num_count / bagtime)) print(hz) #######################解析rosbag中的点云point_concat话题，获得原始pcd文件####################### command = ['rosrun', 'pcl_ros', 'bag_to_pcd'] + [input_dir] + [ '/scan_map_icp_amcl_node/scan_point_transformed'] + [output_dir] command = ' '.join(command) os.system(command) # Process_rosbag = Popen(command, stdout=subprocess.PIPE, stderr=subprocess.PIPE) # Process_rosbag.wait() file1 = os.path.join(output_dir[0:-4], 'pcd_ascii') file2 = os.path.join(output_dir[0:-4], 'jpg') if not os.path.exists(file1): os.makedirs(file1) if not os.path.exists(file2): os.makedirs(file2) #######################将原始pcd文件转化成pcd-ascii格式,并生成jpg文件####################### for root, dirs, files in os.walk(output_dir[0:-4]): # find .pcd files and do the transfer twice for file in files: if "pcd" == root[-3:] and ".pcd" == file[-4:]: inputFilePath = root + "/" + file tempFilePath = root[:-3] + "pcd_ascii/" + file outputFilePath = root[:-3] + "jpg/" + file[:-3] + "jpg" # transfer pcd_utf-8 to pcd_ascii command = "pcl_convert_pcd_ascii_binary " + \ inputFilePath + \ " " + \ tempFilePath + \ " 0" # print(command) os.system(command) # make sure the temp file exist, invalid input file may cause this issue if not os.path.exists(tempFilePath): continue # transfer pcd_ascii to jpg pic = np.zeros([800, 1600, 3]) draw_points(pic, tempFilePath) cv2.imwrite(outputFilePath, pic) # print("jpg generated: ", outputFilePath) else: break #######################将转化的点云jpg合成视频####################### jpg_list = os.listdir(file2) if not jpg_list == []: command = ['ffmpeg', '-f', 'image2', '-r', hz, '-pattern_type', 'glob', '-i'] + ['"jpg/*.jpg"'] + ['-tag:v', 'avc1', '-y'] + [ 'pcd_lidar.mp4'] result_string = " ".join(command) p = Popen(result_string, shell=True, cwd=output_dir[0:-4] + '/', stdout=subprocess.PIPE, stderr=subprocess.PIPE) p.wait() shutil.rmtree(file1) shutil.rmtree(file2) shutil.rmtree(output_dir) return output_dir[0:-4]