dcl_data_process/archive/python2-kinglong-module/pcdtovideo_jilong_overlook.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):
    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]