#!/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 if __name__ == '__main__': input_dir = sys.argv[1] #input_dir = '/media/dell/BFAC-F22B/data/pujin_datareturn/pujin_robot-01_data_merge_2023-12-20-02-22-18_obstacledetection_10.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_depthcamera' + '/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 == "/camera/depth/points": #/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] + ['/camera/depth/points'] + [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([200, 400, 3]) #pic = np.zeros([800, 1600, 3]) #draw_points(pic, tempFilePath) #cv2.imwrite(outputFilePath, pic) # print("jpg generated: ", outputFilePath) #pcd_path = "D:/文件/2-课题/科技部3.2/影子模式/pcd/data/pcd_ascii/1666841252.226625000.pcd" pic = np.zeros([800, 1600, 3]) f = open(tempFilePath, "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]) #t = np.dot(np.linalg.inv(a),np.array([point_x,point_y,point_z,1])) #x = t[0] #y = t[1] x = -point_x y = -point_y if math.isnan(point_x): pass else: cv2.circle(pic, (int(800 - x * 60), int(400 - y * 60)), 0, (255,255,255), 0) cv2.imwrite(outputFilePath, pic) 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_depthcamera.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_depthcamera' + '/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 == "/camera/depth/points": # /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] + ['/camera/depth/points'] + [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([200, 400, 3]) #pic = np.zeros([800, 1600, 3]) # draw_points(pic, tempFilePath) # cv2.imwrite(outputFilePath, pic) # print("jpg generated: ", outputFilePath) # pcd_path = "D:/文件/2-课题/科技部3.2/影子模式/pcd/data/pcd_ascii/1666841252.226625000.pcd" pic = np.zeros([800, 1600, 3]) f = open(tempFilePath, "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]) # t = np.dot(np.linalg.inv(a),np.array([point_x,point_y,point_z,1])) # x = t[0] # y = t[1] x = -point_x y = -point_y if math.isnan(point_x): pass else: cv2.circle(pic, (int(800 - x * 60), int(400 - y * 60)), 0, (255, 255, 255), 0) cv2.imwrite(outputFilePath, pic) 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_depthcamera.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]