dcl_data_process/archive/pcdtovideo_pji_lidar_bak.py

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