cs add scripts

README.md

 # catkin_ws
-
+常熟二期总的工程仓库

scripts/gaussian.py

+import math
+import numpy as np
+import wgs84
+from rotations import degree2rad, rad2degree
+
+ZoneOffset = 1e6
+ZoneWidth = 3
+EastOffset = 500000.0
+
+def Convert_v2(inLonLat): # BL --> XY
+    Zone = (int)((inLonLat[1] + 1.5) / ZoneWidth)
+    L0 = Zone * ZoneWidth
+
+    dL = inLonLat[1] - L0
+
+    dLRadian = degree2rad(dL)
+    latRadian = degree2rad(inLonLat[0])
+
+    X = wgs84.GetX(latRadian)
+    N = wgs84.GetN(latRadian)
+
+    t = math.tan(latRadian)
+    t2 = t * t
+    sinB = math.sin(latRadian)
+    cosB = math.cos(latRadian)
+    ita2 = wgs84.ep2 * cosB * cosB
+
+    temp = dLRadian * cosB
+    temp2 = temp * temp
+    yPart1 = 0.5 * N * temp * temp * t
+    yPart2 = N * t * (5 - t2 + 9 * ita2 + 4 * ita2 * ita2) * temp2 * temp2 / 24
+    yPart3 = temp2 * temp2 * temp2 * N * t * (61 - 58 * t2 + t2 * t2) / 720
+
+    y = X + yPart1 + yPart2 + yPart3
+
+    xPart1 = N * temp
+    xPart2 = (1 - t2 + ita2) * N * temp * temp2 / 6
+    xPart3 = (5 - 18 * t2 + t2 * t2 + 14 * ita2 - 58 * ita2 * t2) * N * temp * temp2 * temp2 / 120
+
+    x = Zone * ZoneOffset + EastOffset + xPart1 + xPart2 + xPart3
+
+    projPt = np.array([x, y])
+    return projPt
+
+def Inverse_v2(inXY): # XY --> BL
+
+    Zone = (int)(inXY[0] / ZoneOffset)
+
+    L0 = Zone * ZoneWidth
+    L0_radian = degree2rad(L0)
+
+    X0 = Zone * ZoneOffset + EastOffset
+    Y0 = 0
+
+    x = inXY[0] - X0
+    y = inXY[1] - Y0
+
+    Br = wgs84.GetLatByX(y)#
+
+
+    cosB = math.cos(Br)
+    secB = 1 / cosB
+    ita2 = wgs84.ep2 * cosB * cosB
+    t = math.tan(Br)
+    t2 = t * t
+    V = wgs84.GetV(Br)
+    V2 = V * V
+    N = wgs84.c / V
+    M = N/V/V
+    D = x/N
+
+    tmp3 = (1 + 2 * t2 + ita2) * D * D * D / 6
+    tmp5 = (5 + 28 * t2 + 24 * t2 * t2 + 6 * ita2 + 8 * ita2 * t2) * D * D * D * D * D / 120
+    l = secB * (D - tmp3 + tmp5)
+    L_radian = L0_radian + l
+    tmp2 = D * D / 2
+    tmp4 = (5 + 3 * t2 + ita2 - 9 * ita2 * t2) * D * D * D * D / 24
+    tmp6 = (61 + 90 * t2 + 45 * t2 * t2) * D * D * D * D * D * D / 720
+    B_radian = Br - t * V2 * (tmp2 - tmp4 + tmp6)
+
+    B = rad2degree(B_radian)
+    L = rad2degree(L_radian)
+
+    outLonLat = np.array([B, L])
+    return outLonLat
+

scripts/gen_all_configs.py

+#!/usr/bin/env python
+# coding: utf-8
+
+import sys
+import os
+import shutil
+# import yaml
+import ruamel.yaml
+import numpy as np
+import math
+import copy
+import json
+from pyproj import CRS
+from pyproj import Transformer
+
+import gaussian as gauss
+import wgs84
+
+
+class AutoCityV2GenConfig:
+    def __init__(self, origin_yamls_dir, save_yamls_dir):
+        self.origin_yamls_dir = origin_yamls_dir
+        self.save_yamls_dir = save_yamls_dir
+
+    def xy_angle_cal(self, x1, y1, x2, y2):
+        x = abs(x2 - x1)
+        y = abs(y2 - y1)
+
+        # print(math.sqrt(x * x + y * y))
+        tmp_angle = math.atan(y / x) * 57.29578
+
+        # print(tmp_angle)
+
+        if x2 > x1 and y2 > y1:
+            angle = 90 - tmp_angle
+        elif x2 > x1 and y2 < y1:
+            angle = 90 + tmp_angle
+        elif x2 < x1 and y2 > y1:
+            angle = 270 + tmp_angle
+        elif x2 < x1 and y2 < y1:
+            angle = 270 - tmp_angle
+        elif x2 == x1 and y2 < y1:
+            angle = 180
+        elif x2 < x1 and y2 == y1:
+            angle = 270
+        elif x2 > x1 and y2 == y1:
+            angle = 90
+        else:
+            angle = 0
+        return angle
+
+    def gen_lidar_x_angle(self, lidar_pose_lat, lidar_pose_lon, kitti2Gauss):
+        lat1 = lidar_pose_lat
+        lon1 = lidar_pose_lon
+        p1 = np.array([lat1, lon1])
+        p11 = gauss.Convert_v2(p1)
+
+        lidar_x = p11[0]
+        lidar_y = p11[1]
+
+        # 原来的Trans矩阵的上三角旋转矩阵
+        # 原来的Trans矩阵表达的是origin点云坐标到gauss坐标的变换
+        # 现将origin2kitti也考虑在内
+        # 直接给kitti到gauss的变换矩阵
+        # the rotation of kitti to gauss
+        R = kitti2Gauss[:3, :3]
+
+        # 取x轴两个点坐标用来标定lidar的x轴的绝对航向
+        tmp_loc1 = [30, 0, 0]
+        tmp_loc2 = [50, 0, 0]
+
+        tmp_xyz1 = np.dot(R, tmp_loc1)
+        tmp_xyz2 = np.dot(R, tmp_loc2)
+        # 计算lidar x轴的绝对航向
+        lidar_x_angle = self.xy_angle_cal(tmp_xyz1[0], tmp_xyz1[1], tmp_xyz2[0], tmp_xyz2[1]) % 360.0
+
+        return lidar_x_angle
+
+    def WebMercator2WGS84_Single(self, x, y):
+        """
+        web墨卡托坐标转WGS84坐标
+        :param x:  web墨卡托坐标x
+        :param y:  web墨卡托坐标y
+        :return:  纬度,经度
+        """
+        crs_WebMercator = CRS.from_epsg(32651)
+        crs_WGS84 = CRS.from_epsg(4326)
+        transformer = Transformer.from_crs(crs_WebMercator, crs_WGS84)
+        lat, lon = transformer.transform(x, y)
+        return lat, lon
+
+    def compute_trans(self, lidar_install_pose, E0_PROJECT, N0_PROJECT, H0_PROJECT):
+        lidar_xyz_in_cloud = lidar_install_pose[:3, 3]
+        lidar_xyz_in_utm = lidar_xyz_in_cloud + np.array([E0_PROJECT, N0_PROJECT, H0_PROJECT])
+        pos_BL_UTM_B, pos_BL_UTM_L = self.WebMercator2WGS84_Single(lidar_xyz_in_utm[0], lidar_xyz_in_utm[1])
+        pos_gauss_x, pos_gauss_y = gauss.Convert_v2([pos_BL_UTM_B, pos_BL_UTM_L])
+        TRANS = np.zeros((4, 4), dtype=np.float64)
+        TRANS[:3, :3] = lidar_install_pose[:3, :3]
+        TRANS[0, 3] = pos_gauss_x
+        TRANS[1, 3] = pos_gauss_y
+        TRANS[2, 3] = lidar_install_pose[2, 3]
+        TRANS[3, 3] = 1.0
+        return TRANS.tolist()
+
+    def gen_cam_trans(self, cur_camera_node):
+        fusion_node = {}
+        cam_trans = []
+        cam_intrinsics = []
+        cam_dists = []
+        cam_dects = []
+        for cam in cur_camera_node["CameraParams"]:
+            cam_trans.append(cam["cam_trans"])
+            cam_intrinsics.append(cam["cam_intrinsics"])
+            cam_dists.append(cam["cam_dist"])
+            cam_dects.append([0,300])
+        return cam_trans, cam_intrinsics, cam_dists, cam_dects
+
+    def batch_gen(self):
+        yaml = ruamel.yaml.YAML()
+        yaml.preserve_quotes = True
+        yaml.indent(mapping=2, sequence=4, offset=2)
+        yaml.explicit_start = True
+        lidar_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_lidars.yaml")
+        lidar_total_fs = open(lidar_total_yaml, "r")
+        lidar_total_node = yaml.load(lidar_total_fs)
+
+        camera_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_cameras.yaml")
+        camera_total_fs = open(camera_total_yaml, "r")
+        camera_total_node = yaml.load(camera_total_fs)
+
+        pcd_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_pointcloud_detectors.yaml")
+        pcd_total_fs = open(pcd_total_yaml, "r")
+        pcd_total_node = yaml.load(pcd_total_fs)
+
+        cam_models_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_cam_models.yaml")
+        cam_models_total_fs = open(cam_models_total_yaml, "r")
+        cam_models_total_node = yaml.load(cam_models_total_fs)
+
+        common_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_common.yaml")
+        common_total_fs = open(common_total_yaml, "r")
+        common_total_node = yaml.load(common_total_fs)
+
+        track_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_track.yaml")
+        track_total_fs = open(track_total_yaml, "r")
+        track_total_node = yaml.load(track_total_fs)
+
+        fusion_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_fusion.yaml")
+        fusion_total_fs = open(fusion_total_yaml, "r")
+        fusion_total_node = yaml.load(fusion_total_fs)
+
+        event_total_yaml = os.path.join(self.origin_yamls_dir, "total_yamls", "total_event.yaml")
+        event_total_fs = open(event_total_yaml, "r")
+        event_total_node = yaml.load(event_total_fs)
+
+        node_names = ["node" + str(ind + 1) for ind in range(2)]
+        for node_name in node_names:
+            if node_name not in lidar_total_node or not lidar_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " lidar_ros is null")
+                continue
+            if node_name not in camera_total_node or not camera_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " cam_ros is null")
+                continue
+            if node_name not in pcd_total_node or not pcd_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " pointcloud detector is null")
+                continue
+            if node_name not in cam_models_total_node or not cam_models_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " cam model is null")
+                continue
+            if node_name not in common_total_node or not common_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " common is null")
+                continue
+            if node_name not in track_total_node or not track_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " track is null")
+                continue
+            if node_name not in fusion_total_node or not fusion_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " fusion is null")
+                continue
+            if node_name not in event_total_node or not event_total_node[node_name]:
+                print("FATAL: " + str(node_name) + " event is null")
+                continue
+
+            # Flag list for run.sh
+            rep_str = [
+                'RUN_RSLIDAR=1',
+                'RUN_POINT_CLOUD=1',
+            ]
+            Flags_run_sh = [
+                # 0  RUN_RSLIDAR=0
+                1,
+                # 1   RUN_POINT_CLOUD=1
+                1]
+
+            cur_lidar_node = lidar_total_node[node_name]
+            cur_camera_node = camera_total_node[node_name]
+            cur_pcd_node = pcd_total_node[node_name]
+            cur_cam_models_node = cam_models_total_node[node_name]
+            cur_common_node = common_total_node[node_name]
+            cur_track_node = track_total_node[node_name]
+            cur_fusion_node = fusion_total_node[node_name]
+            cur_event_node = event_total_node[node_name]
+
+            cur_node_save_dir = os.path.join(self.save_yamls_dir, node_name)
+
+            # generate lidar launch
+            ####################   generate lidar launch   ######################
+            #suteng usd config/config.yaml
+            cur_config_save_dir = os.path.join(cur_node_save_dir, "config")
+            if not os.path.exists(cur_config_save_dir):
+                os.makedirs(cur_config_save_dir)
+
+            #suteng process
+            lidar_list = cur_lidar_node["LIDAR_TOPIC_LIST"]
+            suteng_last={}
+            common_dict={}
+            common_dict["msg_source"] = 1
+            common_dict["send_packet_ros"] = False
+            common_dict["send_point_cloud_ros"] = True 
+            suteng_last["common"] = common_dict
+
+            sutenglidar_list=[]
+            for lidar_name in lidar_list:
+                if cur_lidar_node[lidar_name]["LIDAR_NAME"] == "robsense":
+                    driver_ros_dict={}
+
+                    drive_dict={}
+                    ros_dict={}
+                    
+                    suteng_topic = cur_lidar_node[lidar_name]["LIDAR_TOPIC"]
+                    suteng_port = cur_lidar_node[lidar_name]["LIDAR_PORT"]
+                    drive_dict["lidar_type"] = "RSM1" 
+                    drive_dict["msop_port"] = suteng_port
+                    drive_dict["difop_port"] = int(str(suteng_port).replace("6699","7788"))
+                    drive_dict["start_angle"] = 0
+                    drive_dict["end_angle"] = 360
+                    drive_dict["wait_for_difop"]=False
+                    drive_dict["min_distance"]=0.2
+                    drive_dict["max_distance"]=200
+                    drive_dict["dense_points"]=True
+                    drive_dict["ts_first_point"]=True
+                    drive_dict["use_lidar_clock"]=False
+                    drive_dict["pcap_path"]="/home/robosense/lidar.pcap"
+
+                    driver_ros_dict["driver"] = drive_dict
+
+                    ros_dict["ros_frame_id"] = cur_lidar_node[lidar_name]["FRAME_ID"]
+                    ros_dict["ros_send_point_cloud_topic"] = suteng_topic
+                    ros_dict["ros_recv_packet_topic"] = suteng_topic+"/packets"
+                    ros_dict["ros_send_packet_topic"] = suteng_topic+"/packets"
+
+                    driver_ros_dict["ros"] = ros_dict
+                    sutenglidar_list.append(driver_ros_dict)
+            
+            suteng_last["lidar"] = sutenglidar_list 
+            cur_tosave_config_path = os.path.join(cur_config_save_dir, "config.yaml")
+            with open(cur_tosave_config_path, "w") as config_tosave_fp:
+                #yaml.dump(suteng_last, config_tosave_fp, Dumper=yaml.RoundTripDumper)
+                yaml.dump(suteng_last, config_tosave_fp)
+
+            ####################   generate lidar launch   ######################
+
+            cur_yaml_save_dir = os.path.join(cur_node_save_dir, "yamls/custom_nodes_yaml/")
+            if not os.path.exists(cur_yaml_save_dir):
+                os.makedirs(cur_yaml_save_dir)
+            cur_tosave_yaml_path = os.path.join(cur_yaml_save_dir, node_name + ".yaml")
+            yaml_dict = {}
+            common_node = {}
+            common_node["NODE_NAME"] = node_name
+
+            # copy the common attributes of total_common.yaml to COMMON node of nodexx.yaml
+            for k, v in cur_common_node.items():
+                common_node[k] = v
+
+            common_node["ORIGIN2KITTI"] = copy.deepcopy(cur_pcd_node["ORIGIN2KITTI"])
+            common_node["KITTI2ORIGIN"] = copy.deepcopy(cur_pcd_node["KITTI2ORIGIN"])
+            del cur_pcd_node["ORIGIN2KITTI"]
+            del cur_pcd_node["KITTI2ORIGIN"]
+            common_node["UTM_STATION"] = [328822.000000, 3462136.000000, 0.000000]
+            common_node["WGS84_STATION"] = [121.2017711, 31.2809747, 0]
+
+            # get the install pose of the main lidar
+            main_lidar_index = cur_lidar_node["LIDAR_MAIN_NODE_INDEX"]
+            main_lidar_install_pose = copy.deepcopy(
+                cur_lidar_node[cur_lidar_node["LIDAR_TOPIC_LIST"][main_lidar_index]]["LIDAR_SELF"])
+            common_node["TRANS"] = self.compute_trans(np.array(main_lidar_install_pose, dtype=np.float64),
+                                                      common_node["UTM_STATION"][0],
+                                                      common_node["UTM_STATION"][1],
+                                                      common_node["UTM_STATION"][2])
+            kitti2Gauss = np.dot(common_node["TRANS"], common_node["KITTI2ORIGIN"])
+            common_node["lidar_x_angle"] = self.gen_lidar_x_angle(common_node["WGS84_STATION"][1],
+                                                                  common_node["WGS84_STATION"][0], kitti2Gauss)
+            common_node["TRANS2STATION"] = copy.deepcopy(main_lidar_install_pose)
+
+            # COMMON
+            yaml_dict["COMMON"] = common_node
+            # LIDAR_ROS
+            yaml_dict["LIDAR_ROS"] = copy.deepcopy(cur_lidar_node)
+            # CAM_ROS
+            yaml_dict["CAM_ROS"] = copy.deepcopy(cur_camera_node)
+            # LIDAR_MODEL
+            yaml_dict["LIDAR_MODEL"] = copy.deepcopy(cur_pcd_node)
+            # CAM_MODEL
+            yaml_dict["CAM_MODEL"] = copy.deepcopy(cur_cam_models_node)
+
+            # FUSION
+            fusion_node = {}
+            fusion_node["match_color"] = 1
+            if "match_color" in cur_fusion_node.keys():
+                fusion_node["match_color"] = cur_fusion_node["match_color"]
+            yaml_dict["FUSION"] = fusion_node
+
+
+            # TRACKING
+            tracking_node = {}
+            tracking_node["is_save_log"] = 0
+            tracking_node["save_log_file"] = "/workspace/data_log/tracking.log"
+            tracking_node["kf_gpu"] = 0
+            if "kf_gpu" in cur_track_node.keys():
+                tracking_node["kf_gpu"] = cur_track_node["kf_gpu"]
+            yaml_dict["TRACKING"] = tracking_node
+
+            #EVENT
+            yaml_dict["EVENT"] = cur_event_node
+
+            with open(cur_tosave_yaml_path, "w") as yaml_tosave_fp:
+                yaml.dump(yaml_dict, yaml_tosave_fp)
+            ####################   generate nodexx.yaml   ######################
+
+            # generate run.sh for each node
+            ####################   generate run.sh   ######################
+            run_sh_path = os.path.join(self.origin_yamls_dir, 'templates/shell/jrun')
+            run_sh_fp = open(run_sh_path, "r")
+            run_sh_cont = str(run_sh_fp.read())
+
+            for ii in range(len(Flags_run_sh)):
+                run_sh_cont = run_sh_cont.replace(rep_str[ii], rep_str[ii][:-1] + str(Flags_run_sh[ii]))
+
+            cur_tosave_shell_path = os.path.join(cur_node_save_dir, "jrun")
+            with open(cur_tosave_shell_path, "w") as run_sh_tosave_fp:
+                run_sh_tosave_fp.write(run_sh_cont)
+
+            ####################   generate run.sh   ######################
+
+            # modify the NODE_NAME of entry.yaml
+            entry_yaml_path = os.path.join(self.origin_yamls_dir, 'templates/yamls/entry.yaml')
+            entry_yaml_fp = open(entry_yaml_path, "r")
+            entry_yaml_cont = str(entry_yaml_fp.read())
+
+            entry_yaml_cont = entry_yaml_cont.replace("NODE_NAME: node17", "NODE_NAME: " + str(node_name))
+
+            cur_tosave_yaml_path = os.path.join(cur_node_save_dir, "entry.yaml")
+            with open(cur_tosave_yaml_path, "w") as entry_yaml_tosave_fp:
+                entry_yaml_tosave_fp.write(entry_yaml_cont)
+
+if __name__ == '__main__':
+    if len(sys.argv) > 1:
+        origin_yamls_dir = sys.argv[1]
+    else:
+        origin_yamls_dir = "/workspace/catkin_ws_v2/scripts/"
+    if len(sys.argv) > 2:
+        save_yamls_dir = sys.argv[2]
+    else:
+        save_yamls_dir = "/workspace/batch_ota/"
+    gen_config = AutoCityV2GenConfig(origin_yamls_dir, save_yamls_dir)
+    # 原始大yaml所在路径
+    print(gen_config.origin_yamls_dir)
+    # 各个点位的配置文件保存路径
+    print(gen_config.save_yamls_dir)
+
+    gen_config.batch_gen()
+    print("True")

scripts/rotations.py

+# Utitlity file with functions for handling rotations
+#
+# Author: Trevor Ablett
+# University of Toronto Institute for Aerospace Studies
+
+import numpy as np
+from numpy import dot, inner, array, linalg
+
+def skew_symmetric(v):
+    return np.array(
+        [[0, -v[2], v[1]],
+         [v[2], 0, -v[0]],
+         [-v[1], v[0], 0]]
+    )
+
+
+def wraps2pi_pi(rad):
+    while rad > np.pi:
+        rad -= 2 * np.pi
+    while rad < - np.pi:
+        rad += 2 * np.pi
+    return rad
+
+
+class Quaternion():
+    def __init__(self, w=1., x=0., y=0., z=0., axis_angle=None, euler=None):
+        """
+        Allow initialization either with explicit wxyz, axis angle, or euler XYZ (RPY) angles.
+
+        :param w: w (real) of a quaternion.
+        :param x: x (i) of a quaternion.
+        :param y: y (j) of a quaternion.
+        :param z: z (k) of a quaternion.
+        :param axis_angle: Set of three values from axis-angle representation, as list or [3,] or [3,1] np.ndarray.
+                           See C2M5L2 Slide 7 for details.
+        :param euler: Set of three angles from euler XYZ rotating frame angles.
+        """
+        if axis_angle is None and euler is None:
+            self.w = w
+            self.x = x
+            self.y = y
+            self.z = z
+        elif euler is not None and axis_angle is not None:
+            raise AttributeError("Only one of axis_angle and euler can be specified.")
+        elif axis_angle is not None:
+            if not (type(axis_angle) == list or type(axis_angle) == np.ndarray) or len(axis_angle) != 3:
+                raise ValueError("axis_angle must be list or np.ndarray with length 3.")
+            axis_angle = np.array(axis_angle)
+            norm = np.linalg.norm(axis_angle)
+            self.w = np.cos(norm / 2)
+            if norm < 1e-50:  # to avoid instabilities and nans
+                self.x = 0
+                self.y = 0
+                self.z = 0
+            else:
+                imag = axis_angle / norm * np.sin(norm / 2)
+                self.x = imag[0].item()
+                self.y = imag[1].item()
+                self.z = imag[2].item()
+        else:
+            roll = euler[0]
+            pitch = euler[1]
+            yaw = euler[2]
+
+            cy = np.cos(yaw * 0.5)
+            sy = np.sin(yaw * 0.5)
+            cr = np.cos(roll * 0.5)
+            sr = np.sin(roll * 0.5)
+            cp = np.cos(pitch * 0.5)
+            sp = np.sin(pitch * 0.5)
+
+            # static frame
+            self.w = cr * cp * cy + sr * sp * sy
+            self.x = sr * cp * cy - cr * sp * sy
+            self.y = cr * sp * cy + sr * cp * sy
+            self.z = cr * cp * sy - sr * sp * cy
+
+            # rotating frame
+            # self.w = cr * cp * cy - sr * sp * sy
+            # self.x = cr * sp * sy + sr * cp * cy
+            # self.y = cr * sp * cy - sr * cp * sy
+            # self.z = cr * cp * sy + sr * sp * cy
+
+    def __repr__(self):
+        return "Quaternion (wxyz): [%2.5f, %2.5f, %2.5f, %2.5f]" % (self.w, self.x, self.y, self.z)
+
+    def to_mat(self):
+        v = np.array([self.x, self.y, self.z]).reshape(3,1)
+        return (self.w ** 2 - np.dot(v.T, v)) * np.eye(3) + \
+               2 * np.dot(v, v.T) + 2 * self.w * skew_symmetric(v)
+
+    def to_euler(self):
+        """ Return as xyz (roll pitch yaw) Euler angles, with a static frame """
+        roll = np.arctan2(2 * (self.w * self.x + self.y * self.z), 1 - 2 * (self.x**2 + self.y**2))
+        pitch = np.arcsin(2 * (self.w * self.y - self.z * self.x))
+        yaw = np.arctan2(2 * (self.w * self.z + self.x * self.y), 1 - 2 * (self.y**2 + self.z**2))
+        return np.array([roll, pitch, yaw])
+
+    def to_numpy(self):
+        """ Return numpy wxyz representation. """
+        return np.array([self.w, self.x, self.y, self.z])
+
+    def normalize(self):
+        """ Return a normalized version of this quaternion to ensure that it is valid. """
+        norm = np.linalg.norm([self.w, self.x, self.y, self.z])
+        return Quaternion(self.w / norm, self.x / norm, self.y / norm, self.z / norm)
+
+    def to_reverse(self):
+        return Quaternion(self.w, -self.x, -self.y, -self.z)
+
+    def quat_outer_mult(self, q, out='np'):
+        """
+        Give output of multiplying this quaternion by another quaternion.
+        See equation from C2M5L2 Slide 7 for details.
+
+        :param q: The quaternion to multiply by, either a Quaternion or 4x1 ndarray.
+        :param out: Output type, either np or Quaternion.
+        :return: Returns quaternion of desired type
+        """
+
+        v = np.array([self.x, self.y, self.z]).reshape(3, 1)
+        sum_term = np.zeros([4,4])
+        sum_term[0,1:] = -v[:,0]
+        sum_term[1:, 0] = v[:,0]
+        sum_term[1:, 1:] = -skew_symmetric(v)
+        sigma = self.w * np.eye(4) + sum_term
+
+        if type(q).__name__ == "Quaternion":
+            quat_np = np.dot(sigma, q.to_numpy())
+        else:
+            quat_np = np.dot(sigma, q)
+
+        if out == 'np':
+            return quat_np
+        elif out == 'Quaternion':
+            quat_obj = Quaternion(quat_np[0], quat_np[1], quat_np[2], quat_np[3])
+            return quat_obj
+
+
+def euler2quat(eulers):
+  eulers = array(eulers)
+  if len(eulers.shape) > 1:
+    output_shape = (-1,4)
+  else:
+    output_shape = (4,)
+  eulers = np.atleast_2d(eulers)
+  gamma, theta, psi = eulers[:,0],  eulers[:,1],  eulers[:,2]
+
+  q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
+       np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
+  q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
+
+  quats = array([q0, q1, q2, q3]).T
+  for i in range(len(quats)):
+    if quats[i,0] < 0:
+      quats[i] = -quats[i]
+  return quats.reshape(output_shape)
+
+
+def quat2euler(quats):
+  quats = array(quats)
+  if len(quats.shape) > 1:
+    output_shape = (-1,3)
+  else:
+    output_shape = (3,)
+  quats = np.atleast_2d(quats)
+  q0, q1, q2, q3 = quats[:,0], quats[:,1], quats[:,2], quats[:,3]
+
+  gamma = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
+  theta = np.arcsin(2 * (q0 * q2 - q3 * q1))
+  psi = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
+
+  eulers = array([wraps2pi_pi(gamma), wraps2pi_pi(theta), wraps2pi_pi(psi)]).T
+  return eulers.reshape(output_shape)
+
+
+def quat2rot(quats):
+  quats = array(quats)
+  input_shape = quats.shape
+  quats = np.atleast_2d(quats)
+  Rs = np.zeros((quats.shape[0], 3, 3))
+  q0 = quats[:, 0]
+  q1 = quats[:, 1]
+  q2 = quats[:, 2]
+  q3 = quats[:, 3]
+  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
+  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
+  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
+  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
+  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
+  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
+  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
+  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
+  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
+
+  if len(input_shape) < 2:
+    return Rs[0]
+  else:
+    return Rs
+
+
+def rot2quat(rots):
+  input_shape = rots.shape
+  if len(input_shape) < 3:
+    rots = array([rots])
+  K3 = np.empty((len(rots), 4, 4))
+  K3[:, 0, 0] = (rots[:, 0, 0] - rots[:, 1, 1] - rots[:, 2, 2]) / 3.0
+  K3[:, 0, 1] = (rots[:, 1, 0] + rots[:, 0, 1]) / 3.0
+  K3[:, 0, 2] = (rots[:, 2, 0] + rots[:, 0, 2]) / 3.0
+  K3[:, 0, 3] = (rots[:, 1, 2] - rots[:, 2, 1]) / 3.0
+  K3[:, 1, 0] = K3[:, 0, 1]
+  K3[:, 1, 1] = (rots[:, 1, 1] - rots[:, 0, 0] - rots[:, 2, 2]) / 3.0
+  K3[:, 1, 2] = (rots[:, 2, 1] + rots[:, 1, 2]) / 3.0
+  K3[:, 1, 3] = (rots[:, 2, 0] - rots[:, 0, 2]) / 3.0
+  K3[:, 2, 0] = K3[:, 0, 2]
+  K3[:, 2, 1] = K3[:, 1, 2]
+  K3[:, 2, 2] = (rots[:, 2, 2] - rots[:, 0, 0] - rots[:, 1, 1]) / 3.0
+  K3[:, 2, 3] = (rots[:, 0, 1] - rots[:, 1, 0]) / 3.0
+  K3[:, 3, 0] = K3[:, 0, 3]
+  K3[:, 3, 1] = K3[:, 1, 3]
+  K3[:, 3, 2] = K3[:, 2, 3]
+  K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0
+  q = np.empty((len(rots), 4))
+  for i in range(len(rots)):
+    _, eigvecs = linalg.eigh(K3[i].T)
+    eigvecs = eigvecs[:,3:]
+    q[i, 0] = eigvecs[-1]
+    q[i, 1:] = -eigvecs[:-1].flatten()
+    if q[i, 0] < 0:
+      q[i] = -q[i]
+
+  if len(input_shape) < 3:
+    return q[0]
+  else:
+    return q
+
+
+def euler2rot(eulers):
+  return quat2rot(euler2quat(eulers))
+
+
+def rot2euler(rots):
+  return quat2euler(rot2quat(rots))
+
+
+def gpseuler2rot_in_cptframe(gps_euler_degrees):
+  # R2 defines: XYZ right/forward/up -> ENU
+  yaw = degree2rad(gps_euler_degrees[2])
+  pitch = degree2rad(gps_euler_degrees[1])
+  roll = degree2rad(gps_euler_degrees[0])
+
+  sinA = np.sin(yaw);
+  cosA = np.cos(yaw);
+  matA = np.array([[cosA, sinA, 0], [-sinA, cosA, 0], [0, 0, 1]])
+
+  sinP = np.sin(pitch)
+  cosP = np.cos(pitch)
+  matP = np.array([[1, 0, 0], [0, cosP, -sinP], [0, sinP, cosP]])
+
+  sinR = np.sin(roll)
+  cosR = np.cos(roll)
+  matR = np.array([[cosR, 0, sinR], [0, 1, 0], [-sinR, 0, cosR]])
+  return matA.dot(matP.dot(matR))
+
+
+def gpseuler2rot(gps_euler_degrees):
+  # R1 defines: XYZ forward/left/up -> XYZ right/forward/up
+  R1 = np.mat([[0, -1, 0],
+               [1, 0, 0],
+               [0, 0, 1]])
+  R2 = gpseuler2rot_in_cptframe(gps_euler_degrees)
+  # R2 * R1 * XYZ = ENU
+  return R2.dot(R1)
+
+
+def rad2degree(rad=None, list=None):
+  if not (list is None):
+    newlist = np.zeros(len(list))
+    i = 0
+    for rad in list:
+      newlist[i] = rad * 180.0 / np.pi
+      i += 1
+    return newlist
+  else:
+    return rad * 180.0 / np.pi
+
+def degree2rad(angle=None, list=None):
+  if not (list is None):
+    newlist = np.zeros(len(list))
+    i = 0
+    for angle in list:
+      newlist[i] = angle * np.pi / 180.0
+      i += 1
+    return newlist
+  else:
+    return angle * np.pi / 180.0
+
+
+def quat_product(q, r):
+  t = np.zeros(4)
+  t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3]
+  t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2]
+  t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1]
+  t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0]
+  return t
+

scripts/scripts.md

+
+This directory consists of the generating scripts of the configuration files, models, binary executable files and maps.

scripts/templates/shell/jrun

+#!/bin/bash
+
+# Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+# Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+# This software is furnished under a license and may be used and copied only in
+# accordance with the terms of such license and with the inclusion of the above
+# copyright notice. This software or any other copies thereof may not be
+# provided or otherwise made available to any other person.
+
+# The information in this software is subject to change without notice and
+# should not be constructed as a commitment by Beijing JUEFX Technology Co.Ltd.
+# JUEFX assumes no responsibility for the use or reliability of its software on
+# equipment which is not supported by JUEFX.
+
+################################################################
+### This script is used for start/stop/restart all ros2 node ###
+################################################################
+
+# TODO: convert to python script
+
+# The project path should include the install folder
+CATKIN_WS_PATH="/home/nvidia/workspace/catkin_ws_v2"
+# The log path prefix is changed to the path where you need to save the log. Do not add / at the end
+LOG_PATH_PREFIX="/home/nvidia/workspace/catkin_ws_v2/data_log"
+
+# Different configurations at different intersections
+LOG_ENABLE=1
+RUN_RSLIDAR=1
+# Programs to be started: set to 1 to start, set to 0 to not start
+RUN_PP=1
+
+# declare a global node list
+declare -g -a NODE_LIST
+declare -g -A NODE_MAP
+declare -g -i HAS_DONE_FIND=0
+declare -g -a FOUND_NODE_LIST
+declare -g -a NOT_FOUND_NODE_LIST
+# struct NODE_LIST {nick_name,ros_pkg_name,ros_launch_name,ros_node_name,bin,whether_need_run}
+#                       0          1             2               3        4         5
+# Usage:
+#   roslaunch ros_pkg_name ros_launch_name
+#   rosnode kill ros_node_name
+#   ps -ef | grep bin
+NODE_LIST[0]="rslidar,rslidar_sdk,start.launch,/rslidar_sdk_node,__name:=rslidar_sdk_node,${RUN_RSLIDAR}"
+NODE_LIST[1]="pp,jfx_point_cloud_detector,point_cloud_detector.launch,/jfx_point_cloud_detector,__name:=jfx_point_cloud_detector,${RUN_PP}"
+
+NODE_MAP[rslidar]=${NODE_LIST[0]}
+NODE_MAP[pp]=${NODE_LIST[1]}
+
+# echo -e "${NODE_MAP[gb28181]}"
+
+# declare a global log path map
+declare -g -A LOG_PATH_LIST
+# struct LOG_PATH_LIST {nick_name,log_absolute_path0,log_abolute_path1...}
+LOG_PATH_LIST["rslidar"]=""
+LOG_PATH_LIST["pp"]=""
+
+# Temporarily reserved until the version update is complete
+function killpid(){
+    local res=`ps -ef | grep -v grep | grep -v bash| grep $1 | sed 's/[ ][ ]*/ /g' | cut -d' ' -f2`
+    #echo $res
+    if [ !  -n "$res" ]; then
+        echo "$1 having been killed"
+    else
+        kill $res
+        echo "$1 been killed"
+    fi
+    return 0
+}
+
+function find_node_by_nick(){
+    if [ $# -lt 1 ];then
+        echo -e "[error] find_node_by_nick needs at least one param"
+        return 1
+    fi
+    local NICK_NAME=$@
+    for NICK in $NICK_NAME
+    do
+        local FOUND=0
+        for FOUND_NODE in $FOUND_NODE_LIST
+        do
+            if [ $FOUND_NODE == $NICK ];then
+                FOUND=1
+            fi
+        done
+        for i in ${!NODE_LIST[@]}
+        do
+            if [ $FOUND == 1 ];then
+                continue
+            fi
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_LIST[$i]})
+            if [ ${node_info_arr[0]} == $NICK ];then
+                FOUND=1
+                FOUND_NODE_LIST[${#FOUND_NODE_LIST[@]}]=$NICK
+            fi
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+        if [ $FOUND == 0 ];then
+            NOT_FOUND_NODE_LIST[${#NOT_FOUND_NODE_LIST[@]}]=$NICK
+        fi
+        FOUND=0
+    done
+    unset $NICK
+    unset $NICK_NAME
+}
+
+function start_func(){
+    param_1_tmp_lower=`echo $1 | tr '[A-Z]' '[a-z]'`
+    if [ $param_1_tmp_lower == "all" ];then
+        start_all_nodes
+        local ret_start_all_nodes=$?
+        return $ret_start_all_nodes
+    elif [ $param_1_tmp_lower == "single" ];then
+        local param_list=($@)
+        local need_param=${param_list[*]:1}
+        if [ $HAS_DONE_FIND == 0 ];then
+            find_node_by_nick $need_param
+            HAS_DONE_FIND=1
+        fi
+        start_single_node
+        local ret_start_single_node=$?
+        return $ret_start_single_node
+    else
+        print_usage
+        return 1
+    fi
+    return 0
+}
+
+function start_all_nodes(){
+    # 程序启动
+    echo -e "\033[45;37mStarting...\033[0m"
+    cd $CATKIN_WS_PATH
+    echo -e "\033[4mProgram works in:\033[0m \033[44;37m$CATKIN_WS_PATH\033[0m"
+    source $CATKIN_WS_PATH/install/setup.bash
+
+    for i in ${!NODE_LIST[@]}
+    do
+        OLD_IFS="$IFS"
+        IFS=","
+        node_info_arr=(${NODE_LIST[$i]})
+        # mkfifo /tmp/${node_info_arr[0]}
+        if [ ${node_info_arr[5]} == 1 ];then
+            if [ ${node_info_arr[0]} == "fusion" ];then
+                sleep 5
+            fi
+            if [ $LOG_ENABLE == 1 ];then
+                rm ${LOG_PATH_PREFIX}/${node_info_arr[0]}.log
+                roslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > ${LOG_PATH_PREFIX}/${node_info_arr[0]}.log 2>&1 &
+                echo -e "\033[36m\troslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > ${LOG_PATH_PREFIX}/${node_info_arr[0]}.log 2>&1 &\033[0m"
+            else
+                roslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > /dev/null 2>&1 &
+                echo -e "\033[36m\troslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > /dev/null 2>&1 &\033[0m"
+            fi
+        else
+            echo -e "\033[36m\t${node_info_arr[0]}: not need to be run in the configuration\033[0m"
+        fi
+        IFS="$OLD_IFS"
+        unset node_info_arr
+        unset OLD_IFS
+    done
+    echo -e "\033[45;37mStarted...\033[0m"
+
+    return 0
+}
+
+function start_single_node(){
+    # 程序启动
+    echo -e "\033[45;37mStarting...\033[0m"
+    cd $CATKIN_WS_PATH
+    echo -e "\033[4mProgram works in:\033[0m \033[44;37m$CATKIN_WS_PATH\033[0m"
+    source $CATKIN_WS_PATH/install/setup.bash
+    for i in ${!FOUND_NODE_LIST[@]}
+    do
+        OLD_IFS="$IFS"
+        IFS=","
+        node_info_arr=(${NODE_MAP[${FOUND_NODE_LIST[$i]}]})
+        if [ ${node_info_arr[5]} == 1 ];then
+            if [ $LOG_ENABLE == 1 ];then
+                rm ${LOG_PATH_PREFIX}/${node_info_arr[0]}.log
+                roslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > ${LOG_PATH_PREFIX}/${node_info_arr[0]}.log 2>&1 &
+                echo -e "\033[36m\troslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > ${LOG_PATH_PREFIX}/${node_info_arr[0]}.log 2>&1 &\033[0m"
+            else
+                roslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > /dev/null 2>&1 &
+                echo -e "\033[36m\troslaunch ${node_info_arr[1]} ${node_info_arr[2]} --wait > /dev/null 2>&1 &\033[0m"
+            fi
+        else
+            echo -e "\033[36m\t${node_info_arr[0]}: not need to be run in the configuration\033[0m"
+        fi
+        IFS="$OLD_IFS"
+        unset node_info_arr
+        unset OLD_IFS
+    done
+    if [ ${#NOT_FOUND_NODE_LIST[@]} != 0 ];then
+        echo -e "\033[45;37m\tDid not find ${NOT_FOUND_NODE_LIST[@]} in node_map. Use \"./jrun.sh node all\" to check your node's nick name\033[0m"
+    fi
+    echo -e "\033[45;37mStarted...\033[0m"
+    return 0
+}
+
+function stop_func(){
+    param_1_tmp_lower=`echo $1 | tr '[A-Z]' '[a-z]'`
+    if [ $param_1_tmp_lower == "all" ];then
+        stop_all_nodes
+        local ret_stop_all_nodes=$?
+        return $ret_stop_all_nodes
+    elif [ $param_1_tmp_lower == "single" ];then
+        local param_list=($@)
+        local need_param=${param_list[*]:1}
+        if [ $HAS_DONE_FIND == 0 ];then
+            find_node_by_nick $need_param
+            HAS_DONE_FIND=1
+        fi
+        stop_single_node
+        local ret_stop_single_node=$?
+        return $ret_stop_single_node
+    else
+        print_usage
+        return 1
+    fi
+    return 0
+}
+
+function stop_all_nodes(){
+    echo -e "\033[45;37mStoping...\033[0m"
+    for i in ${!NODE_LIST[@]}
+    do
+        OLD_IFS="$IFS"
+        IFS=","
+        node_info_arr=(${NODE_LIST[$i]})
+        if [ ${node_info_arr[5]} == 1 ];then
+            a=${node_info_arr[3]}
+            local node_regex=${a/\#/\*\[0\-9\]}
+            local nodes=$( rosnode list )
+            local grep_nodes=$(echo -e $nodes | grep $node_regex)
+            if [ -z "$grep_nodes" ];then
+                echo -e "\033[35m${node_info_arr[0]}: not found $node_regex such ros node\033[0m\n"
+            fi
+            local node_list=($(echo -e $grep_nodes | sed -e ':t;N;s/\n/,/;b t'))
+            for i in ${node_list[@]}
+            do
+                stdout_print=$( ( rosnode kill $i ) 2>&1 )
+                echo -e "\033[36m${node_info_arr[0]}: $stdout_print\033[0m\n"
+            done
+            #using killpid()
+            killpid ${node_info_arr[2]}
+        else
+            echo -e "\033[35m${node_info_arr[0]}: not need to be run in the configuration\033[0m\n"
+        fi
+        IFS="$OLD_IFS"
+        unset node_info_arr
+        unset OLD_IFS
+    done
+    echo "y" | rosnode cleanup
+    ####### log #######
+    clear_old_log_default
+    echo -e "\033[45;37mStopped...\033[0m"
+    return 0
+}
+
+function stop_single_node(){
+    for i in ${!FOUND_NODE_LIST[@]}
+    do
+        OLD_IFS="$IFS"
+        IFS=","
+        node_info_arr=(${NODE_MAP[${FOUND_NODE_LIST[$i]}]})
+        if [ ${node_info_arr[5]} == 1 ];then
+            a=${node_info_arr[3]}
+            local node_regex=${a/\#/\*\[0\-9\]}
+            local nodes=$(rosnode list)
+            local grep_nodes=$(echo -e $nodes | grep $node_regex)
+            if [ -z "$grep_nodes" ];then
+                echo -e "\033[35m${node_info_arr[0]}: not found $node_regex such ros node\033[0m\n"
+            fi
+            local node_list=($(echo -e $grep_nodes | sed -e ':t;N;s/\n/,/;b t'))
+            for i in ${node_list[@]}
+            do
+                stdout_print=$( ( rosnode kill $i ) 2>&1 )
+                echo -e "\033[36m${node_info_arr[0]}: $stdout_print\033[0m\n"
+            done
+            #use killpid()
+            killpid ${node_info_arr[2]}
+        else
+            echo -e "\033[35m${node_info_arr[0]}: not need to be run in the configuration\033[0m\n"
+        fi
+        IFS="$OLD_IFS"
+        unset node_info_arr
+        unset OLD_IFS
+    done
+    echo "y" | rosnode cleanup
+    if [ ${#NOT_FOUND_NODE_LIST[@]} != 0 ];then
+        echo -e "\033[31m\nDid not find [${NOT_FOUND_NODE_LIST[@]}] in node_map. Use \"./jrun.sh node all\" to check your node's nick name\033[0m"
+    fi
+    return 0
+}
+
+function status_func(){
+    param_1_tmp_lower=`echo $1 | tr '[A-Z]' '[a-z]'`
+    if [ $param_1_tmp_lower == "all" ];then
+        status_all_nodes
+        local ret_status_all_nodes=$?
+        return $ret_status_all_nodes
+    elif [ $param_1_tmp_lower == "single" ];then
+        local param_list=($@)
+        local need_param=${param_list[*]:1}
+        if [ $HAS_DONE_FIND == 0 ];then
+            find_node_by_nick $need_param
+            HAS_DONE_FIND=1
+        fi
+        status_single_node
+        local ret_status_single_node=$?
+        return $ret_status_single_node
+    else
+        print_usage
+        return 1
+    fi
+    return 0
+}
+
+function restart_func(){
+    param_1_tmp_lower=`echo $1 | tr '[A-Z]' '[a-z]'`
+    if [ $param_1_tmp_lower == "all" ];then
+        stop_all_nodes
+        sleep 1
+        start_all_nodes
+        local ret_start_all_nodes=$?
+        echo -e "jrun restart all nodes at  $(date)" >> /tmp/ros2app
+        return $ret_start_all_nodes
+    elif [ $param_1_tmp_lower == "single" ];then
+        local param_list=($@)
+        local need_param=${param_list[*]:1}
+        if [ $HAS_DONE_FIND == 0 ];then
+            find_node_by_nick $need_param
+            HAS_DONE_FIND=1
+        fi
+        stop_single_node
+        sleep 1
+        start_single_node
+        local ret_start_single_node=$?
+        echo -e "jrun restart ${FOUND_NODE_LIST[@]} at  $(date)" >> /tmp/ros2app
+        return $ret_start_single_node
+    else
+        print_usage
+        return 1
+    fi
+    return 0
+}
+
+function status_all_nodes(){
+    MAX_DISPLAY_WIDTH=167
+    MIN_DISPLAY_WIDTH=80
+    SHELL_HEIGHT=`stty size|awk '{print $1}'`
+    SHELL_WIDTH=`stty size|awk '{print $2}'`
+    if [ $SHELL_WIDTH -ge $MAX_DISPLAY_WIDTH ];then
+        echo -e "[INFO] Current window size is $SHELL_WIDTH * $SHELL_HEIGHT,"\
+             "the table below could display all infomation\n"
+        echo -e "\033[45;37mApp's status:\033[0m"
+        printf "\033[4m| %-6s | %-10s | %-10s | %-5s | %-3s | %-10s | %-6s | %-20s | %-60s |\033[0m\n"\
+               "TYPE" "NICK" "PID" "PPID" "C" "STIME" "TTY" "TIME" "CMD"
+        for i in ${!NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_LIST[$i]})
+
+            a="${node_info_arr[2]}"
+            local launch_regex=${a/\#/\*\[0\-9\]}
+            local launch_status=$(ps -ef | grep -v grep | grep -w $launch_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$launch_status" ];then
+                printf "| %-6s | %-10s | %-10s | %-5s | %-3s | %-10s | %-6s | %-20s | %-60s |\n"\
+                       "Launch" ${node_info_arr[0]} " " "" " " "" "" "" ""
+            fi
+            local launch_status_list=($launch_status)
+
+            b="${node_info_arr[4]}"
+            local bin_regex=${b/\#/\*\[0\-9\]}
+            local bin_status=$(ps -ef | grep -v grep | grep -w $bin_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$bin_status" ];then
+                printf "| %-6s | %-10s | %-10s | %-5s | %-3s | %-10s | %-6s | %-20s | %-60s |\n"\
+                       "Bin" ${node_info_arr[0]} " " " " " " " " " " " " " "
+            fi
+            local bin_status_list=($bin_status)
+
+            for i in ${!launch_status_list[@]}
+            do
+                local launch_i_status=$(echo -e ${launch_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local launch_i_status_list=(${launch_i_status})
+                # echo -e "---${#launch_i_status_list[@]} ${launch_i_status_list[@]}"
+                printf "| %-6s | %-10s | %-10s | %-5s | %-3s | %-10s | %-6s | %-20s | %-60s |\n"\
+                       "Launch" ${node_info_arr[0]} ${launch_i_status_list[1]}\
+                       ${launch_i_status_list[2]} ${launch_i_status_list[3]}\
+                       ${launch_i_status_list[4]} ${launch_i_status_list[5]}\
+                       ${launch_i_status_list[6]} ${launch_i_status_list[10]}
+            done
+
+            for i in ${!bin_status_list[@]}
+            do
+                local bin_i_status=$(echo -e ${bin_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local bin_i_status_list=(${bin_i_status})
+                # echo -e "+++${#bin_i_status_list[@]} ::: ${bin_i_status_list[@]}"
+                printf "| %-6s | %-10s | %-10s | %-5s | %-3s | %-10s | %-6s | %-20s | %-60s |\n"\
+                       "Bin" "${node_info_arr[0]}" "${bin_i_status_list[1]}"\
+                       "${bin_i_status_list[2]}" "${bin_i_status_list[3]}"\
+                       "${bin_i_status_list[4]}" "${bin_i_status_list[5]}"\
+                       "${bin_i_status_list[6]}" "${bin_i_status_list[8]}"
+            done
+
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+    elif [ $SHELL_WIDTH -ge $MIN_DISPLAY_WIDTH ];then
+        echo -e "[INFO] Current window size is $SHELL_WIDTH * $SHELL_HEIGHT,"\
+             "the table below could display all infomation\n"
+        echo -e "\033[45;37mApp's status:\033[0m"
+        printf "\033[4m| %-6s | %-7s | %-8s | %-8s | %-5s | %-6s | %-16s |\033[0m\n"\
+               "TYPE" "NICK" "PID" "PPID" "STIME" "TTY" "TIME"
+        for i in ${!NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_LIST[$i]})
+
+            a="${node_info_arr[2]}"
+            local launch_regex=${a/\#/\*\[0\-9\]}
+            local launch_status=$(ps -ef | grep -v grep | grep -w $launch_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$launch_status" ];then
+                printf "| %-6s | %-7s | %-8s | %-8s | %-5s | %-6s | %-16s |\n"\
+                       "Launch" ${node_info_arr[0]} "" "" "" "" ""
+            fi
+            local launch_status_list=($launch_status)
+
+            b="${node_info_arr[4]}"
+            local bin_regex=${b/\#/\*\[0\-9\]}
+            local bin_status=$(ps -ef | grep -v grep | grep -w $bin_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$bin_status" ];then
+                printf "| %-6s | %-7s | %-8s | %-8s | %-5s | %-6s | %-16s |\n"\
+                       "Bin" ${node_info_arr[0]} "" "" "" "" ""
+            fi
+            local bin_status_list=($bin_status)
+
+            for i in ${!launch_status_list[@]}
+            do
+                local launch_i_status=$(echo -e ${launch_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local launch_i_status_list=(${launch_i_status})
+                # echo -e "---${#launch_i_status_list[@]} ${launch_i_status_list[@]}"
+                printf "| %-6s | %-7s | %-8s | %-8s | %-5s | %-6s | %-16s |\n"\
+                       "Launch" ${node_info_arr[0]} ${launch_i_status_list[1]}\
+                       ${launch_i_status_list[2]}\
+                       ${launch_i_status_list[4]} ${launch_i_status_list[5]}\
+                       ${launch_i_status_list[6]}
+            done
+
+            for i in ${!bin_status_list[@]}
+            do
+                local bin_i_status=$(echo -e ${bin_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local bin_i_status_list=(${bin_i_status})
+                # echo -e "+++${#bin_i_status_list[@]} ::: ${bin_i_status_list[@]}"
+                printf "| %-6s | %-7s | %-8s | %-8s | %-5s | %-6s | %-16s |\n"\
+                       "Bin" "${node_info_arr[0]}" "${bin_i_status_list[1]}"\
+                       "${bin_i_status_list[2]}"\
+                       "${bin_i_status_list[4]}" "${bin_i_status_list[5]}"\
+                       "${bin_i_status_list[6]}"
+            done
+
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+    else
+        printf "Current window width is %d, could not show all infomation about jfx node, please resize your window width at least 80\n"
+    fi
+    return 0
+}
+
+function status_single_node(){
+    MAX_DISPLAY_WIDTH=167
+    MIN_DISPLAY_WIDTH=80
+    SHELL_HEIGHT=`stty size|awk '{print $1}'`
+    SHELL_WIDTH=`stty size|awk '{print $2}'`
+    if [ $SHELL_WIDTH -ge $MAX_DISPLAY_WIDTH ];then
+        echo -e "[INFO] Current window size is $SHELL_WIDTH * $SHELL_HEIGHT,"\
+             "the table below could display all infomation\n"
+        echo -e "\033[45;37mApp's status:\033[0m"
+        printf "\033[4m| %-6s | %-10s | %-10s | %-10s | %-3s | %-10s | %-6s | %-16s | %-60s |\033[0m\n"\
+               "TYPE" "NICK" "PID" "PPID" "C" "STIME" "TTY" "TIME" "CMD"
+        for i in ${!FOUND_NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_MAP[${FOUND_NODE_LIST[$i]}]})
+
+            a="${node_info_arr[2]}"
+            local launch_regex=${a/\#/\*\[0\-9\]}
+            local launch_status=$(ps -ef | grep -v grep | grep -w $launch_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$launch_status" ];then
+                printf "| %-6s | %-10s | %-10s | %-10s | %-3s | %-10s | %-6s | %-16s | %-60s |\n"\
+                       "Launch" ${node_info_arr[0]} "" "" "" "" "" "" ""
+            fi
+            local launch_status_list=($launch_status)
+
+            b="${node_info_arr[4]}"
+            local bin_regex=${b/\#/\*\[0\-9\]}
+            local bin_status=$(ps -ef | grep -v grep | grep -w $bin_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$bin_status" ];then
+                printf "| %-6s | %-10s | %-10s | %-10s | %-3s | %-10s | %-6s | %-16s | %-60s |\n"\
+                       "Bin" ${node_info_arr[0]} "" "" "" "" "" "" ""
+            fi
+            local bin_status_list=($bin_status)
+
+            for i in ${!launch_status_list[@]}
+            do
+                local launch_i_status=$(echo -e ${launch_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local launch_i_status_list=(${launch_i_status})
+                # echo -e "---${#launch_i_status_list[@]} ${launch_i_status_list[@]}"
+                printf "| %-6s | %-10s | %-10s | %-10s | %-3s | %-10s | %-6s | %-16s | %-60s |\n"\
+                       "Launch" ${node_info_arr[0]} ${launch_i_status_list[1]}\
+                       ${launch_i_status_list[2]} ${launch_i_status_list[3]}\
+                       ${launch_i_status_list[4]} ${launch_i_status_list[5]}\
+                       ${launch_i_status_list[6]} ${launch_i_status_list[10]}
+            done
+
+            for i in ${!bin_status_list[@]}
+            do
+                local bin_i_status=$(echo -e ${bin_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local bin_i_status_list=(${bin_i_status})
+                # echo -e "+++${#bin_i_status_list[@]} ::: ${bin_i_status_list[@]}"
+                printf "| %-6s | %-10s | %-10s | %-10s | %-3s | %-10s | %-6s | %-16s | %-60s |\n"\
+                       "Bin" "${node_info_arr[0]}" "${bin_i_status_list[1]}"\
+                       "${bin_i_status_list[2]}" "${bin_i_status_list[3]}"\
+                       "${bin_i_status_list[4]}" "${bin_i_status_list[5]}"\
+                       "${bin_i_status_list[6]}" "${bin_i_status_list[8]}"
+            done
+
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+    elif [ $SHELL_WIDTH -ge $MIN_DISPLAY_WIDTH ];then
+        echo -e "[INFO] Current window size is $SHELL_WIDTH * $SHELL_HEIGHT,"\
+             "the table below could display all infomation\n"
+        echo -e "\033[45;37mApp's status:\033[0m"
+        printf "\033[4m| %-1s | %-7s | %-8s | %-8s | %-10s | %-6s | %-16s |\033[0m\n"\
+               "T" "NICK" "PID" "PPID" "STIME" "TTY" "TIME"
+        for i in ${!FOUND_NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_MAP[${FOUND_NODE_LIST[$i]}]})
+
+            a="${node_info_arr[2]}"
+            local launch_regex=${a/\#/\*\[0\-9\]}
+            local launch_status=$(ps -ef | grep -v grep | grep -w $launch_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$launch_status" ];then
+                printf "| %-1s | %-7s | %-8s | %-8s | %-10s | %-6s | %-16s |\n"\
+                       "L" ${node_info_arr[0]} "" "" "" "" ""
+            fi
+            local launch_status_list=($launch_status)
+
+            b="${node_info_arr[4]}"
+            local bin_regex=${b/\#/\*\[0\-9\]}
+            local bin_status=$(ps -ef | grep -v grep | grep -w $bin_regex | sed -e ':t;N;s/\n/,/;b t')
+            if [ -z "$bin_status" ];then
+                printf "| %-1s | %-7s | %-8s | %-8s | %-10s | %-6s | %-16s |\n"\
+                       "B" ${node_info_arr[0]} "" "" "" "" ""
+            fi
+            local bin_status_list=($bin_status)
+
+            for i in ${!launch_status_list[@]}
+            do
+                local launch_i_status=$(echo -e ${launch_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local launch_i_status_list=(${launch_i_status})
+                # echo -e "---${#launch_i_status_list[@]} ${launch_i_status_list[@]}"
+                printf "| %-1s | %-7s | %-8s | %-8s | %-10s | %-6s | %-16s |\n"\
+                       "L" ${node_info_arr[0]} ${launch_i_status_list[1]}\
+                       ${launch_i_status_list[2]}\
+                       ${launch_i_status_list[4]} ${launch_i_status_list[5]}\
+                       ${launch_i_status_list[6]}
+            done
+
+            for i in ${!bin_status_list[@]}
+            do
+                local bin_i_status=$(echo -e ${bin_status_list[$i]} | sed -e 's/[ ][ ]*/,/g')
+                local bin_i_status_list=(${bin_i_status})
+                # echo -e "+++${#bin_i_status_list[@]} ::: ${bin_i_status_list[@]}"
+                printf "| %-1s | %-7s | %-8s | %-8s | %-10s | %-6s | %-16s |\n"\
+                       "B" "${node_info_arr[0]}" "${bin_i_status_list[1]}"\
+                       "${bin_i_status_list[2]}"\
+                       "${bin_i_status_list[4]}" "${bin_i_status_list[5]}"\
+                       "${bin_i_status_list[6]}"
+            done
+
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+    else
+        printf "Current window width is %d, could not show all infomation about jfx node, please resize your window width at least 80\n"
+    fi
+    if [ ${#NOT_FOUND_NODE_LIST[@]} != 0 ];then
+        echo -e "\033[31m\nDid not find [${NOT_FOUND_NODE_LIST[@]}] in node_map. Use \"./jrun.sh node all\" to check your node's nick name\033[0m"
+    fi
+    return 0
+}
+
+function clear_old_log_func(){
+    param_1_tmp_lower=`echo $1 | tr '[A-Z]' '[a-z]'`
+    if [ $# != 1 ];then
+        print_usage
+        return 1
+    elif [ $1 == "default" ];then
+        clear_old_log_default
+        ret_clear_old_log_default=$?
+        return $ret_clear_old_log_default
+    else
+        clear_old_log $param_1_tmp_lower
+        ret_clear_old_log_single=$?
+        return $ret_clear_old_log_single
+    fi
+    return 0
+}
+
+# param[in] $1:file_absolute_path $2:days ago $3:file_name(support regex)
+# example: clear_old_log /tmp 3 "*.csv"
+function clear_old_log(){
+    # origin command: find $1 -mtime +$2 -type d,f,l -name $3 -exec rm -rf {} \;
+    # print result to stdout_print variable but not to terminal
+    
+    if [ $3 == "'*'" ];then
+        stdout_print=$((find $1 -mindepth 1 -mtime $2 -type d,f,l -name '*' -exec rm -rf {} \;)2>&1)
+    else
+        stdout_print=$((find $1 -mindepth 1 -mtime $2 -type d,f,l -name $3 -exec rm -rf {} \;)2>&1)
+    fi
+    if [ $? == 0 ];then
+        echo -e "\033[36m[success] path: $1/$3 [$2 days ago] logs cleared\033[0m"
+    else
+        echo -e "\033[35m[failure] what: $stdout_print\033[0m"
+    fi
+    return 0
+}
+
+function clear_old_log_default(){
+    local DEFAULT_EXPIRED_TIME=3
+    echo -e "\033[43;30m------ Default to clear $DEFAULT_EXPIRED_TIME days ago logs ------\033[0m"
+    clear_old_log ~/.ros $DEFAULT_EXPIRED_TIME *.csv
+    clear_old_log ~/.ros/log $DEFAULT_EXPIRED_TIME "'*'"
+    echo -e "\033[43;30m---------- $DEFAULT_EXPIRED_TIME days ago logs cleared -----------\033[0m"
+    return 0
+}
+
+function print_node_info_func(){
+    param_1_tmp_lower=`echo $1 | tr '[A-Z]' '[a-z]'`
+    if [ $param_1_tmp_lower == "all" ];then
+        print_all_nodes_info
+        local ret_print_all_nodes=$?
+        return $ret_print_all_nodes
+    elif [ $param_1_tmp_lower == "show" ];then
+        print_show_nodes_info
+        local ret_show_all_nodes=$?
+        return $ret_show_all_nodes
+    elif [ $param_1_tmp_lower == "single" ];then
+        local param_list=($@)
+        local need_param=${param_list[*]:1}
+        if [ $HAS_DONE_FIND == 0 ];then
+            find_node_by_nick $need_param
+            HAS_DONE_FIND=1
+        fi
+        print_single_node_info
+        local ret_print_single_node=$?
+        return $ret_print_single_node
+    else
+        print_usage
+        return 1
+    fi
+    return 0
+}
+
+function print_all_nodes_info(){
+    MAX_DISPLAY_WIDTH=167
+    MIN_DISPLAY_WIDTH=80
+    SHELL_HEIGHT=`stty size|awk '{print $1}'`
+    SHELL_WIDTH=`stty size|awk '{print $2}'`
+    if [ $SHELL_WIDTH -ge $MAX_DISPLAY_WIDTH ];then
+        echo -e "[INFO] Current window size is $SHELL_WIDTH * $SHELL_HEIGHT,"\
+             "the table below could display all infomation\n"
+        printf "\033[4m| %-7s | %-35s | %-38s | %-38s | %-45s | %-3s |\033[0m\n"\
+               "nick" "pkg name" "launch name" "node name" "bin" "run"
+        for i in ${!NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_LIST[$i]})
+            for ((j=0; j<${#node_info_arr[@]};j++))
+            do
+                if [ $j == 5 ];then
+                    if [ ${node_info_arr[$j]} == 1 ];then
+                        printf "| %-3s |\n" "Y"
+                    else
+                        printf "| %-3s |\n" "N"
+                    fi
+                else
+                    if [ $j == 0 ];then
+                        printf "| %-7s " ${node_info_arr[$j]}
+                    elif [ $j == 1 ];then
+                        printf "| %-35s " ${node_info_arr[$j]}
+                    elif [ $j == 2 ];then
+                        printf "| %-38s " ${node_info_arr[$j]}
+                    elif [ $j == 3 ];then
+                        printf "| %-38s " ${node_info_arr[$j]}
+                    else
+                        printf "| %-45s " ${node_info_arr[$j]}
+                    fi
+                fi
+            done
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+    elif [ $SHELL_WIDTH -ge $MIN_DISPLAY_WIDTH ];then
+        echo -e "Current window size is $SHELL_WIDTH * $SHELL_HEIGHT,"\
+             "the table below could not display all infomation,"\
+             "the best window size is at least 20 * 167\n"
+        printf "\033[4m| %-7s | %-12s | %-13s | %-13s | %-10s | %-3s |\033[0m\n"\
+               "nick" "pkg name" "launch name" "node name" "bin" "run"
+        for i in ${!NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_LIST[$i]})
+            for ((j=0; j<${#node_info_arr[@]};j++))
+            do
+                if [ $j == 5 ];then
+                    if [ ${node_info_arr[$j]} == 1 ];then
+                        printf "| %-3s |\n" "Y"
+                    else
+                        printf "| %-3s |\n" "N"
+                    fi
+                else
+                    if [ $j == 0 ];then
+                        printf "| %-7s " ${node_info_arr[$j]: 0: 7}
+                    elif [ $j == 1 ];then
+                        printf "| %-12s " ${node_info_arr[$j]: 0: 11}
+                    elif [ $j == 2 ];then
+                        printf "| %-13s " ${node_info_arr[$j]: 0: 12}
+                    elif [ $j == 3 ];then
+                        printf "| %-13s " ${node_info_arr[$j]: 0: 12}
+                    else
+                        printf "| %-10s " ${node_info_arr[$j]: 8: 10}
+                    fi
+                fi
+            done
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+    else
+        printf "Current window width is %d, could not show all infomation about jfx node, please resize your window width at least 80\n"
+    fi
+    return 0
+}
+
+function print_show_nodes_info(){
+    printf "\033[4m| %-7s | %-24s | %-37s | %-1s | %-1s|\033[0m\n"\
+           "nick" "pkg name" "launch name" "s" "r"
+    for i in ${!NODE_LIST[@]}
+    do
+        OLD_IFS="$IFS"
+        IFS=","
+        node_info_arr=(${NODE_LIST[$i]})
+        printf "| %-7s " ${node_info_arr[0]:0:7}
+        printf "| %-24s " ${node_info_arr[1]:0:24}
+        printf "| %-37s " ${node_info_arr[2]:0:37}
+        if [ ${node_info_arr[5]} == 1 ];then
+            printf "| %-1s " "Y"
+        else
+            printf "| %-1s " "N"
+        fi
+        local node_exist=$(ps -ef | grep -v grep | grep -w ${node_info_arr[2]} | grep -w ${node_info_arr[1]})
+        #echo ${node_exist}
+        if [ -z "$node_exist" ];then
+            printf "| %-1s| \n" "N" 
+        else
+            printf "| %-1s| \n" "Y" 
+        fi
+        IFS="$OLD_IFS"
+        unset node_info_arr
+        unset OLD_IFS
+    done
+    return 0
+}
+
+function print_single_node_info(){
+    MAX_DISPLAY_WIDTH=167
+    MIN_DISPLAY_WIDTH=80
+    SHELL_HEIGHT=`stty size|awk '{print $1}'`
+    SHELL_WIDTH=`stty size|awk '{print $2}'`
+    if [ $SHELL_WIDTH -ge $MAX_DISPLAY_WIDTH ];then
+        # echo -e "\nCurrent window size is $SHELL_HEIGHT * $SHELL_WIDTH,"\
+        #      "the table below could display all infomation\n"
+        printf "\033[4m| %-10s | %-30s | %-35s | %-30s | %-45s | %-3s |\033[0m\n"\
+               "nick" "pkg name" "launch name" "node name" "bin" "run"
+        for i in ${!FOUND_NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_MAP[${FOUND_NODE_LIST[$i]}]})
+            for ((j=0; j<${#node_info_arr[@]};j++))
+            do
+                if [ $j == 5 ];then
+                    if [ ${node_info_arr[$j]} == 1 ];then
+                        printf "| %-3s |\n" "Y"
+                    else
+                        printf "| %-3s |\n" "N"
+                    fi
+                else
+                    if [ $j == 0 ];then
+                        printf "| %-10s " ${node_info_arr[$j]}
+                    elif [ $j == 1 ];then
+                        printf "| %-30s " ${node_info_arr[$j]}
+                    elif [ $j == 2 ];then
+                        printf "| %-35s " ${node_info_arr[$j]}
+                    elif [ $j == 3 ];then
+                        printf "| %-30s " ${node_info_arr[$j]}
+                    elif [ $j == 4 ];then
+                        printf "| %-45s " ${node_info_arr[$j]}
+                    fi
+                fi
+            done
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+        if [ ${#NOT_FOUND_NODE_LIST[@]} != 0 ];then
+            echo -e "\033[31m\nDid not find [${NOT_FOUND_NODE_LIST[@]}] in node_map. Use \"./jrun.sh node all\" to check your node's nick name\033[0m"
+        fi
+    elif [ $SHELL_WIDTH -ge $MIN_DISPLAY_WIDTH ];then
+        echo -e "\nCurrent window size is $SHELL_HEIGHT * $SHELL_WIDTH,"\
+             "the table below could not display all infomation,"\
+             "the best window size is at least 20 * 167\n"
+        printf "\033[4m| %-7s | %-12s | %-13s | %-13s | %-10s | %-3s |\033[0m\n"\
+               "nick" "pkg name" "launch name" "node name" "bin" "run"
+        for i in ${!FOUND_NODE_LIST[@]}
+        do
+            OLD_IFS="$IFS"
+            IFS=","
+            node_info_arr=(${NODE_MAP[${FOUND_NODE_LIST[$i]}]})
+            for ((j=0; j<${#node_info_arr[@]};j++))
+            do
+                if [ $j == 5 ];then
+                    if [ ${node_info_arr[$j]} == 1 ];then
+                        printf "| %-3s |\n" "Y"
+                    else
+                        printf "| %-3s |\n" "N"
+                    fi
+                else
+                    if [ $j == 0 ];then
+                        printf "| %-7s " ${node_info_arr[$j]: 0: 7}
+                    elif [ $j == 1 ];then
+                        printf "| %-12s " ${node_info_arr[$j]: 0: 11}
+                    elif [ $j == 2 ];then
+                        printf "| %-13s " ${node_info_arr[$j]: 0: 12}
+                    elif [ $j == 3 ];then
+                        printf "| %-13s " ${node_info_arr[$j]: 0: 12}
+                    else
+                        printf "| %-10s " ${node_info_arr[$j]: 8: 10}
+                    fi
+                fi
+            done
+            IFS="$OLD_IFS"
+            unset node_info_arr
+            unset OLD_IFS
+        done
+        if [ ${#NOT_FOUND_NODE_LIST[@]} != 0 ];then
+            echo -e "\033[31m\nDid not find [${NOT_FOUND_NODE_LIST[@]}] in node_map. Use \"./jrun.sh node all\" to check your node's nick name\033[0m"
+        fi
+    else
+        echo -e "\nCurrent window width is %d, could not show all infomation about jfx node, please resize your window width at least 80"
+    fi
+    return 0
+}
+
+function print_usage(){
+    echo -e "Usage: jrun <Action> [Parameters]"
+    echo -e "Action:"
+    echo -e "  start [all,single] <nick name>          Start all jfxnodes or single jfxnode named [nick name]"
+    echo -e "  stop [all,single] <nick name>           Stop all jfxnodes or single jfxnode named [nick name]"
+    echo -e "  restart [all,single] <nick name>        Restart all jfxnodes or single jfxnode named [nick name]"
+    echo -e "  status [all,single] <nick name>         Show all jfxnodes' status or single jfxnode's status named [nick name]"
+    echo -e "  clear_log [default,single] <nick name>  Clear all jfxnodes' logs or single jfxnode's log named [nick name] older than 7 days"
+    echo -e "  node [all, show, single] <nick name>           Show all jfxnodes' node info or single jfxnode's info named [nick name]"
+    echo -e "Parameters:"
+    echo -e "  <nick name>                             You can find all jfxnode's nick name by using command \"./jrun node all\""
+}
+
+function main(){
+    ACTION=""
+    if [ $# -lt 1 ];then
+        echo -e "Error: jrun requires at least one parameter input"
+        print_usage
+        exit 1
+    else
+        ACTION=$1
+    fi
+
+    #start system config,import!!!
+    source /opt/ros/melodic/setup.bash
+    /root/.entry.sh
+    source $CATKIN_WS_PATH/install/setup.bash
+    #start end
+
+    local ALL_PARAM_LIST=($@)
+    local NEED_PARAM_LIST=${ALL_PARAM_LIST[*]:1}
+
+    if [ $ACTION == "stop" ];then
+        stop_func $NEED_PARAM_LIST
+    elif [ $ACTION == "start" ];then
+        start_func $NEED_PARAM_LIST
+    elif [ $ACTION == "status" ];then
+        status_func $NEED_PARAM_LIST
+    elif [ $ACTION == "node" ];then
+        print_node_info_func $NEED_PARAM_LIST
+    elif [ $ACTION == "clear_log" ];then
+        clear_old_log_func $NEED_PARAM_LIST
+    elif [ $ACTION == "restart" ];then
+        restart_func $NEED_PARAM_LIST
+    elif [ $ACTION == "help" ];then
+        print_usage
+    else
+        print_usage
+    fi
+    unset ALL_PARAM_LIST
+    unset NEED_PARAM_LIST
+    exit 0
+}
+
+main $@
+exit 0

scripts/templates/yamls/entry.yaml

+LIDAR_TYPE: hesai
+LIDAR_ROS_YAML: yamls/fix_nodes_yaml/all_lidars.yaml
+CAM_TYPE: flir
+CAM_ROS_YAML: yamls/fix_nodes_yaml/flir_ros.yaml
+LIDAR_MODEL_TYPE: 1
+LIDAR_MODEL_YAML: yamls/fix_nodes_yaml/pointpillars.yaml
+#LIDAR_MODEL_TYPE: 2
+#LIDAR_MODEL_YAML: yamls/fix_nodes_yaml/cp_4stage.yaml
+#LIDAR_MODEL_TYPE: 3
+#LIDAR_MODEL_YAML: yamls/fix_nodes_yaml/cia_ssd.yaml
+CAM_MODEL_TYPE: yolov5
+CAM_MODEL_YAML: yamls/fix_nodes_yaml/yolov5.yaml
+TRACKING_TYPE: tracking_v1
+FUSION_TYPE: lift_2d_to_3d
+FUSION_YAML: yamls/fix_nodes_yaml/lift_2d_to_3d.yaml
+TRACKING_YAML: yamls/fix_nodes_yaml/tracking_v1.yaml
+EVENT_TYPE: Qichecheng
+EVENT_YAML: yamls/fix_nodes_yaml/Qichecheng_event.yaml
+PUBLISH_TYPE: shanghai
+PUBLISH_YAML: yamls/fix_nodes_yaml/all_publish.yaml
+
+NODE_NAME: node17
+ALL_NODES_YAML_DIR: yamls/custom_nodes_yaml/

scripts/total_yamls/total_cam_models.yaml

+node1:
+  yolov5m_enginepath: /models/yolov5/yolov5s_dy_230131.engine
+node2:
+  yolov5m_enginepath: /models/yolov5/yolov5s_dy_230131.engine

scripts/total_yamls/total_cameras.yaml

+node1:
+  GlobalConfig:
+    acquisition_frame_rate: 10
+    utc_leap_nanoseconds: 37000000000
+  CameraParams:
+    - idx: 0
+      url: 10.56.108.124
+      location_lon: 121.188479
+      location_lat: 31.275802
+      location_ele: 0
+      raw_topic: /camera_array/cam0/image_raw
+      cam_trans: [[ 1.51042207e-02, -1.50427234e-02, -1.13173363e+01],
+                  [-7.78807026e-04, -1.45158646e-01,  1.13531205e+02],
+                  [ 1.27580184e-05,  2.20449713e-03, -7.25591444e-01]]
+      cam_intrinsics: [[3.13188623,-1.69726816,3006.77076161],
+                [0.15893756,0.21158008,290.67111259],
+                [0.00054013,0.00059379,1.        ]]
+
+      cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
+    - idx: 1
+      url: 10.56.109.127
+      location_lon: 121.1877493
+      location_lat: 31.2757621
+      location_ele: 0
+      raw_topic: /camera_array/cam1/image_raw
+      cam_trans: [[   -1.64915971  ,  -4.53892942 ,-4759.1907378 ],
+                  [    0.65469646 ,   -0.308533,     731.68354325],
+                  [    0.00106312,    -0.00075274,     1.        ]]
+
+      cam_intrinsics: [[   -1.64915971,    -4.53892942, -4759.1907378 ],
+                 [    0.65469646,    -0.308533,     731.68354325],
+                 [    0.00106312,    -0.00075274,     1.        ]]
+
+      cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
+    - idx: 2
+      url: 10.56.109.128
+      location_lon: 121.1876840
+      location_lat: 31.2756782
+      location_ele: 0
+      raw_topic: /camera_array/cam2/image_raw
+      cam_trans: [[-8.18108420e-04 , 1.09704652e-01, -1.16081430e+02],
+                  [ 1.74596191e-02 , 1.00373102e-02 ,-1.67333506e+01],
+                  [ 6.03293984e-05 , 8.99020631e-04,  2.53539016e-02]]
+      cam_intrinsics: [[-3.17881853,-4.97343073,-7034.53070758],
+                       [0.23636193,0.14470063,495.80985215],
+                       [0.00090617,-0.00047155,1.]]
+
+      cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
+  LogConfig:
+    log_name: node1
+    log_enable: false
+    log_path: /workspace/log/
+    log_to_stderr: false
+    also_log_to_stderr: false
+    stderr_threshold: 0
+    log_buf_secs: 0
+    max_log_size: 100
+    stop_logging_if_full_disk: true
+    log_prefix: true
+    log_clean_days: 0
+    log_color: true
+
+node2:
+  GlobalConfig:
+    acquisition_frame_rate: 10
+    utc_leap_nanoseconds: 37000000000
+  CameraParams:
+    - idx: 0
+      url: 10.56.100.131
+      location_lon: 121.160087
+      location_lat: 31.282610
+      location_ele: 0
+      raw_topic: /camera_array/cam0/image_raw
+      cam_trans: [[1.72715738e-02, 2.95514778e-01, -2.19812153e+02],
+                 [-7.34889367e-03, -3.16259208e-01, 1.38078122e+02],
+                 [ 1.97048960e-04, 2.58610293e-03, -1.32415680e+00]]
+      cam_intrinsics: [[4.3004e+03, 0.0, 968.6373,],
+                      [0.0, 4.3064e+03, 688.2004,],
+                      [0.0, 0.0, 1.0,]]
+      cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
+    - idx: 1
+      url: 10.56.100.129 
+      location_lon: 121.160464
+      location_lat: 31.281601
+      location_ele: 0
+      raw_topic: /camera_array/cam1/image_raw
+      cam_trans: [[1.53258448e-02, -9.78250085e-02, 1.58912911e+01],
+                 [3.65285051e-03, 1.73733978e-01, 4.15968464e+00],
+                 [-4.07695972e-05, -2.54866135e-03, 9.52422567e-01]]
+      cam_intrinsics: [[4.3004e+03, 0.0, 968.6373,],
+                      [0.0, 4.3064e+03, 688.2004,],
+                      [0.0, 0.0, 1.0,]]
+      cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
+  LogConfig:
+    log_name: node2
+    log_enable: false
+    log_path: /workspace/log/
+    log_to_stderr: false
+    also_log_to_stderr: false
+    stderr_threshold: 0
+    log_buf_secs: 0
+    max_log_size: 100
+    stop_logging_if_full_disk: true
+    log_prefix: true
+    log_clean_days: 0
+    log_color: true

scripts/total_yamls/total_common.yaml

+node1:
+  MEC_V4_IP: 10.56.109.1
+  MEC_V6_IP: 2408:860c:5:b01:106:101:a01:1
+  MEC_NAME: JFX_MEC_1
+  MEC_LOCATION_LON: 121.188618
+  MEC_LOCATION_LAT: 31.27368
+  MEC_LOCATION_ELE: 0
+  CROSS_NAME: boyuanlu-anhonglu
+  SCENE_TYPE: 0 # 0表示开放区域(城市道路); 1表示半封闭区域(高速高架,桥梁隧道); 2表示封闭限定区域(园区,机场,停车场); 3为预留
+
+node2:
+  MEC_V4_IP: 10.56.100.1
+  MEC_V6_IP: 2408:860c:5:b01:106:101:101:1
+  MEC_NAME: JFX_MEC_2
+  MEC_LOCATION_LON: 121.160798
+  MEC_LOCATION_LAT: 31.282089
+  MEC_LOCATION_ELE: 0
+  CROSS_NAME: boyuanlu-moyunanlu
+  SCENE_TYPE: 0 # 0表示开放区域(城市道路); 1表示半封闭区域(高速高架,桥梁隧道); 2表示封闭限定区域(园区,机场,停车场); 3为预留

scripts/total_yamls/total_event.yaml

+node1:
+  road_cross_index: 1
+node2:
+  road_cross_index: 2

scripts/total_yamls/total_fusion.yaml

+node1:
+  match_color: 0
+node2:
+  match_color: 0

scripts/total_yamls/total_lidars.yaml

+node1:
+  LIDAR_TOPIC_LIST: [L0,L1,L2]
+  LIDAR_MAIN_NODE_INDEX: 0
+  LIDAR_JOIN_NODE: 7 
+  IS_PUBLISH_POINT: 0
+  IS_NORMALIZATION: 0
+  IS_PACKET_ALL_POINT: 0
+  IS_PACKET_OBJ_POINT: 0
+  IS_PUBLISH_MARK: 1
+
+  L0: # West
+    LIDAR_TYPE: 2
+    LIDAR_NAME: robsense
+    FRAME_ID: robsense0
+    LIDAR_LON: 121.188468
+    LIDAR_LAT: 31.275081
+    LIDAR_ELE: 0
+    LIDAR_TOPIC: /rslidar_packets19
+    LIDAR_IP:   172.16.20.19
+    LIDAR_PORT: 26699
+    LIDAR_INDEX: 0
+    LIDAR_DELAY: 0
+    LIDAR_SELF: [[0.896619498730, -0.442748308182, -0.006880199537, -259.632781982422],
+                 [0.442592650652, 0.896565437317, -0.016799459234, -549.550964355469],
+                 [0.013606481254, 0.012017597444, 0.999835252762, 16.488615036011],
+                 [0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
+  L1: # South
+    LIDAR_TYPE: 2
+    LIDAR_NAME: robsense
+    FRAME_ID: robsense1
+    LIDAR_LON: 121.188479
+    LIDAR_LAT: 31.275802
+    LIDAR_ELE: 0
+    LIDAR_TOPIC: /rslidar_packets18
+    LIDAR_IP:   172.16.20.18
+    LIDAR_PORT: 16699
+    LIDAR_INDEX: 1
+    LIDAR_DELAY: 0
+    LIDAR_SELF: [[-0.607184648514, -0.788420617580, -0.098588868976, -209.208343505859],
+                 [0.778907299042, -0.615131080151, 0.122137434781, -585.203369140625],
+                 [-0.156940743327, -0.002631605603, 0.987604498863, 17.272777557373],
+                 [0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
+  L2: # North
+    LIDAR_TYPE: 2
+    LIDAR_NAME: robsense
+    FRAME_ID: robsense2
+    LIDAR_LON: 121.188583
+    LIDAR_LAT: 31.275165
+    LIDAR_ELE: 0
+    LIDAR_TOPIC: /rslidar_packets20
+    LIDAR_IP:   172.16.20.20
+    LIDAR_PORT: 36699
+    LIDAR_INDEX: 2
+    LIDAR_DELAY: 0
+    LIDAR_SELF: [[0.573620438576, 0.809384703636, 0.125920116901, -274.959991455078],
+                 [-0.789917349815, 0.587276041508, -0.176457703114, -519.970703125000],
+                 [-0.216772064567, 0.001753266319, 0.976220667362, 17.266538619995],
+                 [0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
+
+node2:
+  LIDAR_TOPIC_LIST: [L0,L1,L2,L3]
+  LIDAR_MAIN_NODE_INDEX: 0
+  LIDAR_JOIN_NODE: 15
+  IS_PUBLISH_POINT: 0
+  IS_NORMALIZATION: 0
+  IS_PACKET_ALL_POINT: 0
+  IS_PACKET_OBJ_POINT: 0
+  IS_PUBLISH_MARK: 1
+
+  L0: # South
+    LIDAR_TYPE: 2
+    LIDAR_NAME: robsense
+    FRAME_ID: robsense0
+    LIDAR_LON: 121.160835
+    LIDAR_LAT: 31.282184
+    LIDAR_ELE: 0
+    LIDAR_TOPIC: /rslidar_packets21
+    LIDAR_IP:   172.16.22.21
+    LIDAR_PORT: 46699
+    LIDAR_INDEX: 0
+    LIDAR_DELAY: 0
+    LIDAR_SELF: [[0.997278869152, -0.072646379471, -0.012538959272, -3892.811279296875],
+                 [0.072265379131, 0.996976613998, -0.028550757095, 218.303405761719],
+                 [0.014575158246, 0.027566935867, 0.999513566494, 2.135483741760],
+                 [0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
+  L1: # West
+    LIDAR_TYPE: 2
+    LIDAR_NAME: robsense
+    FRAME_ID: robsense1
+    LIDAR_LON: 121.160820
+    LIDAR_LAT: 31.282287
+    LIDAR_ELE: 0
+    LIDAR_TOPIC: /rslidar_packets20
+    LIDAR_IP:   172.16.22.20
+    LIDAR_PORT: 36699
+    LIDAR_INDEX: 1
+    LIDAR_DELAY: 0
+    LIDAR_SELF: [[-0.050201263279, -0.048703737557, -0.997550964355, -3888.115966796875],
+                 [0.043598838151, 0.997751116753, -0.050907615572, 199.882598876953],
+                 [0.997787058353, -0.046047694981, -0.047964952886, 4.731142520905],
+                 [0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
+  L2: # South
+    LIDAR_TYPE: 2
+    LIDAR_NAME: robsense
+    FRAME_ID: robsense2
+    LIDAR_LON: 121.160299
+    LIDAR_LAT: 31.281535
+    LIDAR_ELE: 0
+    LIDAR_TOPIC: /rslidar_packets19
+    LIDAR_IP:   172.16.22.19
+    LIDAR_PORT: 26699
+    LIDAR_INDEX: 2
+    LIDAR_DELAY: 0
+    LIDAR_SELF: [[-0.063920862973, 0.985661029816, -0.156161770225, -3932.408203125000],
+                 [0.036017764360, 0.158658415079, 0.986676275730, 115.666076660156],
+                 [0.997304737568, 0.057444605976, -0.045642886311, 4.675023555756],
+                 [0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
+
+  L3: # East
+    LIDAR_TYPE: 2
+    LIDAR_NAME: robsense
+    FRAME_ID: robsense3
+    LIDAR_LON: 121.160464
+    LIDAR_LAT: 31.281601
+    LIDAR_ELE: 0
+    LIDAR_TOPIC: /rslidar_packets18
+    LIDAR_IP:   172.16.22.18
+    LIDAR_PORT: 16699
+    LIDAR_INDEX: 3
+    LIDAR_DELAY: 0
+    LIDAR_SELF: [[-0.013388182037, 0.976763427258, -0.213901326060, -3942.002685546875],
+                 [0.039395920932, 0.214269667864, 0.975979804993, 114.107917785645],
+                 [0.999133944511, 0.004639756866, -0.041349183768, 4.373412609100],
+                 [0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]

scripts/total_yamls/total_pointcloud_detectors.yaml

+node1:
+  MAX_POINTS_ALLOWED: 400000
+  MAX_NUM_VOXELS: 150000
+  MAX_ACTIVE: 400000
+  MAX_NUM_POINTS_PER_VOXEL: 12
+  NUM_CLASS: 5
+  SCORE_THRESHOLD: 0.4
+  NMS_OVERLAP_THRESHOLD: 0.1
+  NUM_THREADS: 64
+  BATCH_SIZE: 1
+
+  VOXEL_SIZES: [0.1, 0.1, 0.125]            # cp voxel
+  GRID_SIZES: [3072, 3072, 40]              # xyz
+  PC_RANGE:   [-153.6, -153.6,-7,153.6,153.6, -2]
+  DS_RATIO: 8                               # ratio of GRID_SIZES to BEV_FEATURE_SIZE
+  BEV_FEATURE_SIZE: [384, 384, 256]         # x y c
+  
+  RPN_ONNX_FILE: unify_32_roads_1226.onnx
+  CUTCONV_WEIGHTS: unify_32_roads_1226.weights
+
+  ORIGIN2KITTI: [[0.050645,-0.000283,0.998717,-81.275970,],
+    [-0.011183,-0.999937,0.000283,7.052648,],
+    [0.998654,-0.011183,-0.050645,0.800000,],
+    [0.000000,0.000000,0.000000,1.000000,],
+    ]
+                        
+
+  KITTI2ORIGIN: [[0.050645,-0.011183,0.998654,3.396167,],
+    [-0.000284,-0.999938,-0.011183,7.038095,],
+    [0.998716,0.000284,-0.050645,81.210156,],
+    [0.000000,0.000000,0.000000,1.000000,],
+    ]
+
+  INFER_THRESHOLD:
+    CLASS_0: 0.4    # truck
+    CLASS_1: 0.4    # car
+    CLASS_2: 0.4    # other_v
+    CLASS_3: 0.4    # cyclist
+    CLASS_4: 0.4    # pedestrain
+    
+node2:
+  MAX_POINTS_ALLOWED: 400000
+  MAX_NUM_VOXELS: 150000
+  MAX_ACTIVE: 400000
+  MAX_NUM_POINTS_PER_VOXEL: 12
+  NUM_CLASS: 5
+  SCORE_THRESHOLD: 0.4
+  NMS_OVERLAP_THRESHOLD: 0.1
+  NUM_THREADS: 64
+  BATCH_SIZE: 1
+
+  VOXEL_SIZES: [0.1, 0.1, 0.125]            # cp voxel
+  GRID_SIZES: [3072, 3072, 40]              # xyz
+  PC_RANGE:   [-153.6, -153.6,-7,153.6,153.6, -2]
+  DS_RATIO: 8                               # ratio of GRID_SIZES to BEV_FEATURE_SIZE
+  BEV_FEATURE_SIZE: [384, 384, 256]         # x y c
+  
+  RPN_ONNX_FILE: unify_32_roads_1226.onnx
+  CUTCONV_WEIGHTS: unify_32_roads_1226.weights
+
+  ORIGIN2KITTI: [[0.999907,-0.000186,-0.013612,63.010513,],
+    [-0.000186,0.999629,-0.027250,12.516913,],
+    [0.013612,0.027250,0.999536,-1.600000,],
+    [0.000000,0.000000,0.000000,1.000000,],
+    ]
+                        
+
+  KITTI2ORIGIN: [[0.999907,-0.000186,0.013612,-62.980574,],
+    [-0.000186,0.999629,0.027250,-12.456976,],
+    [-0.013612,-0.027250,0.999536,2.798023,],
+    [0.000000,0.000000,0.000000,1.000000,],
+    ]
+
+  INFER_THRESHOLD:
+    CLASS_0: 0.4    # truck
+    CLASS_1: 0.4    # car
+    CLASS_2: 0.4    # other_v
+    CLASS_3: 0.4    # cyclist
+    CLASS_4: 0.4    # pedestrain

scripts/total_yamls/total_track.yaml

+node1:
+  kf_gpu: 0
+node2:
+  kf_gpu: 0

scripts/wgs84.py

+import math
+import numpy as np
+from rotations import degree2rad, rad2degree
+
+a = 6378137.0
+f = 0.003352810664
+b = 6356752.3142499477
+c = 6399593.6257536924
+e2 = 0.006694379988651241
+ep2 = 0.0067394967407662064
+
+m0 = 6335439.3273023246
+m2 = 63617.757378010137
+m4 = 532.35180239277622
+m6 = 4.1577261283373916
+m8 = 0.031312573415813519
+
+a0 = 6367449.1457686741
+a2 = 32077.017223574985
+a4 = 67.330398573595
+a6 = 0.13188597734903185
+a8 = 0.00024462947981104312
+
+#class Wgs84:
+
+def GetN(radB):
+    cosB = math.cos(radB)
+    V = math.sqrt(1 + ep2 * cosB * cosB)
+    N = c / V
+    return N
+def GetRT_ECEF2ENU(BLH):
+    BLH0 = np.array([BLH[0], BLH[1], 0.])
+    translation = BLH2ECEF(BLH0);
+    B = degree2rad(BLH[0])
+    L = degree2rad(BLH[1])
+    sinL = np.sin(L)
+    cosL = np.cos(L)
+    sinB = np.sin(B)
+    cosB = np.cos(B)
+    rot = np.array([[-sinL, cosL, 0.], [-sinB * cosL, -sinB * sinL, cosB], [cosB * cosL, cosB * sinL, sinB]])
+    return rot, np.dot(rot, -translation)
+
+def GetX(radB):
+    sinB = math.sin(radB)
+    cosB = math.cos(radB)
+    al = a0 * radB
+    sc = sinB * cosB
+    ss = sinB * sinB
+    X = al - sc *((a2 - a4 + a6) + (2 * a4 - a6 * 16 / 3) * ss + a6 * 16 * ss * ss /3)
+    return X
+
+def GetLatByX(X):
+    Bfi0 = X / a0
+    Bfi1 = 0
+    num = 1
+    minAngle = math.pi * 1e-9
+    menus_minAngle = 0 - minAngle
+
+    while (num == 1):
+        num = 0
+        sinB = math.sin(Bfi0)
+        sinB2 = sinB * sinB
+        cosB = math.cos(Bfi0)
+        FBfi = 0 - sinB * cosB *((a2 - a4 + a6) + sinB2 * (2 * a4 - 16 * a6 / 3) + sinB2 * sinB2 * a6 * 16 / 3)
+        Bfi1 = (X - FBfi) / a0
+
+        deltaB = Bfi1 - Bfi0
+        if deltaB < menus_minAngle or deltaB > minAngle:
+            num = 1
+            Bfi0 = Bfi1
+
+    Bf = Bfi1
+    return Bf
+
+def GetV(lat):
+    cosB = math.cos(lat)
+    V = math.sqrt(1 + ep2 * cosB * cosB)
+    return V
+
+
+def BLH2ECEF(ptBLH):
+    cosB = math.cos(degree2rad(ptBLH[0]))
+    sinB = math.sin(degree2rad(ptBLH[0]))
+    cosL = math.cos(degree2rad(ptBLH[1]))
+    sinL = math.sin(degree2rad(ptBLH[1]))
+
+    N = GetN(degree2rad(ptBLH[0]))
+
+    X = (N + ptBLH[2]) * cosB * cosL
+    Y = (N + ptBLH[2]) * cosB * sinL
+    Z = (N * (1 - e2) + ptBLH[2]) * sinB
+    ptXYZ = np.array([X, Y, Z])
+    return ptXYZ
+
+
+
+def ECEF2BLH(ptXYZ):
+    L_radian = math.atan(ptXYZ[1] / ptXYZ[0])
+    if L_radian < 0:
+        L_radian += math.pi
+    L = rad2degree(L_radian)
+    sqrtXY = math.sqrt(ptXYZ[0] * ptXYZ[0] + ptXYZ[1] * ptXYZ[1])
+    t0 = ptXYZ[2] / sqrtXY
+    p = c * e2 / sqrtXY
+    k = 1+ep2
+    ti = t0
+    ti1 = 0
+    loop = 0
+    while loop < 10000:
+        loop += 1
+        ti1 = t0 + p * ti / math.sqrt( k + ti * ti)
+        if math.fabs(ti1 - ti) < 1e-12:
+            break
+        ti = ti1
+    B_radian = math.atan(ti1)
+    N = GetN(B_radian)
+    H = sqrtXY / math.cos(B_radian) - N
+    B = rad2degree(B_radian)
+    geopoint = np.array([B, L, H])
+    return geopoint
+
+
+def GetR_ENU2ECEF(radianL, radianB):
+    sinB = math.sin(radianB)
+    cosB = math.cos(radianB)
+    sinL = math.sin(radianL)
+    cosL = math.cos(radianL)
+    R4 = np.array([[-sinL, -sinB * cosL, cosB * cosL],
+                   [cosL, -sinB * sinL, cosB * sinL],
+                   [0, cosB, sinB]])
+    return R4
+
+
+def GetAngleDif(distance):
+    angleRafian = distance / a
+    return rad2degree(angleRafian)
+