#!/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]