#!/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): colormap = np.array([[128, 130, 120], [235, 0, 205], [0, 215, 0], [235, 155, 0]]) / 255.0 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]) point_z = float(point[2]) points_intensity = float(point[2]) if math.isnan(point_x): pass else: cv2.circle(pic, (int(800 - point_x * 10), int(400 - point_y * 10)), 0, colormap[int(points_intensity) % colormap.shape[0]] * 255, 2) # if __name__ == '__main__': # input_dir = sys.argv[1] # # input_dir='/media/dell/BFAC-F22B/data/jinlong_datareturn/test_1116/jinlong_data_merge_2023-12-13-05-59-34_TTC_7.bag' # output_dir = sys.argv[2] # # output_dir = '/media/dell/BFAC-F22B/data/jinlong_datareturn/test_1116' # # bag_name = input_dir.split('/')[-1].split('.')[0] # output_dir = os.path.join(output_dir, bag_name + '_pcd_overlook' + '/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 == "/points_concat": # /camera/depth/points # /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] + ['/points_concat'] + [output_dir] # command = ' '.join(command) # os.system(command) # 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]) # 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_overlook.mp4'] # command = ['ffmpeg', '-f', 'image2', '-t', bagtime, '-pattern_type', 'glob', '-i'] + ['"jpg/*.jpg"'] + [ # '-tag:v', 'avc1', '-y'] + ['pcd_overlook.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_bag_file, output_mp4_dir): input_dir = input_bag_file # input_dir='/media/dell/BFAC-F22B/bizhang_2023-11-15-10-35-21.bag' output_dir = output_mp4_dir # output_dir='/media/dell/BFAC-F22B' bag_name = input_dir.split('/')[-1].split('.')[0] output_dir = os.path.join(output_dir, bag_name + '_pcd_overlook' + '/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 == "/points_concat": # /camera/depth/points # /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)) hz = str(float(num_count) / bagtime) #######################解析rosbag中的点云point_concat话题，获得原始pcd文件####################### command = ['rosrun', 'pcl_ros', 'bag_to_pcd'] + [input_dir] + ['/points_concat'] + [output_dir] command = ' '.join(command) os.system(command) 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]) # 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_overlook.mp4'] # command = ['ffmpeg', '-f', 'image2', '-t', bagtime, '-pattern_type', 'glob', '-i'] + ['"jpg/*.jpg"'] + [ # '-tag:v', 'avc1', '-y'] + ['pcd_overlook.mp4'] result_string = " ".join(command) print '执行视频生成命令：%s' % result_string 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]