Source code for dump_parameters

#!/usr/bin/python3
import numpy as np
import rospy
import rospkg
from std_msgs.msg import String
from utilities.msg import CalibrationMatrices
import re

# Expression to find Ouster window_start
pattern_ouster_window_start = r"int\s+window_start\s+=\s+(\d+)"

# Expression to find Ouster window_width
pattern_ouster_window_width = r"int\s+window_width\s+=\s+(\d+)"
# Expression to find Ouster limit_lines (64 - value)
pattern_ouster_limit_lines = r"return\s+ls.h\s+-\s+(\d+)"
# Expression to fin limit_lines env variable
pattern_mission_machine_limit_lines = r'<arg name="LIMIT_LINES" value="(\d+)"/>'

class ParameterDumper:
    def __init__(self) -> None:
        self.exclude = ['roslaunch', 'rosdistro', 'rosversion', '/zed2i/zed2i_description', 'run_id']

        self.param_pub = rospy.Publisher('parameters', String, queue_size=10)

        self.matrices_available = rospy.has_param('/local_mapping/matrices_subdirectory')
        if self.matrices_available is True:
            rospack = rospkg.RosPack()
            matrices_subdirectory = rospy.get_param('/local_mapping/matrices_subdirectory')
            matrices_path = f'{rospack.get_path("local_mapping")}/scripts/matrices/{matrices_subdirectory}'

            self.extrinsic_matrix_left = np.load(f'{matrices_path}/extrinsic_matrix_left.npy')
            self.extrinsic_matrix_right = np.load(f'{matrices_path}/extrinsic_matrix_right.npy')
            self.intrinsic_matrix_left = np.load(f'{matrices_path}/intrinsic_matrix_left.npy')
            self.intrinsic_matrix_right = np.load(f'{matrices_path}/intrinsic_matrix_right.npy')
            try:
                self.lidar_to_camera_matrix = np.load(f'{matrices_path}/lidar_to_camera.npy')
            except:
                rospy.loginfo('[Parameter Dumper] No lidar to camera matrix found. Assuming Sensorfusion is not used.')

            self.matrices_pub = rospy.Publisher('calibration_matrices', CalibrationMatrices, queue_size=10)

    def dump_parameters(self):
        param_names = rospy.get_param_names()
        ouster_params = self.get_ouster_params()

        dump_params_msg = String()
        for param_name in param_names:
            if rospy.get_param(param_name) and not any(substring in param_name for substring in self.exclude):
                param_value = rospy.get_param(param_name)
                dump_params_msg.data += f'{param_name}: {param_value}\n'
        dump_params_msg.data += ouster_params
        self.param_pub.publish(dump_params_msg)

        rospy.loginfo('[Parameter Dumper] Saved parameters to rosbag.')

    def dump_calibration_matrices(self):
        matrix_msg = CalibrationMatrices()
        matrix_msg.extrinsic_matrix_left = self.extrinsic_matrix_left.flatten().tolist()
        matrix_msg.extrinsic_matrix_right = self.extrinsic_matrix_right.flatten().tolist()
        matrix_msg.intrinsic_matrix_left = self.intrinsic_matrix_left.flatten().tolist()
        matrix_msg.intrinsic_matrix_right = self.intrinsic_matrix_right.flatten().tolist()
        if hasattr(self, 'lidar_to_camera_matrix'):
            matrix_msg.extrinsic_matrix_lidar_to_camera = self.lidar_to_camera_matrix.flatten().tolist()
        else:
            matrix_msg.extrinsic_matrix_lidar_to_camera = []
        self.matrices_pub.publish(matrix_msg)
        rospy.loginfo('[Parameter Dumper] Saved calibration matrices to rosbag.')

    def get_ouster_params(self):
        try:
            rospack = rospkg.RosPack()
            driver_path = rospack.get_path("ouster_ros") + "/src/point_cloud_compose.h"
            with open(driver_path, "r") as f:
                point_cloud_compose = f.read()
                window_start = re.search(pattern_ouster_window_start, point_cloud_compose).group(1)
                window_width = re.search(pattern_ouster_window_width, point_cloud_compose).group(1)
                limit_lines_subtract = re.search(pattern_ouster_limit_lines, point_cloud_compose).group(1)
                limit_lines = 64 - int(limit_lines_subtract) # Convert reduced number of lines to total number of lines

            mission_machine_path = rospack.get_path("as_missions") + "/launch/mission_machine.launch"
            with open(mission_machine_path, "r") as f:
                mission_machine = f.read()
                limit_lines_m = re.search(pattern_mission_machine_limit_lines, mission_machine).group(1)
                if int(limit_lines_m) > 64 or int(limit_lines_m) < 0:
                    rospy.logwarn(f'[Parameter Dumper] Invalid limit_lines value in mission_machine.launch: {limit_lines_m}')
                else:
                    limit_lines = limit_lines_m

                return f"window_start = {window_start}\nwindow_width = {window_width}\nlimit_lines = {limit_lines}\n"

        except Exception as e:
            rospy.logwarn(f'[Parameter Dumper] Could not find Ouster parameters: {e}')
            return f"No Ouster parameters found."




if __name__ == '__main__':
    rospy.init_node('dump_parameters')
    dumper = ParameterDumper()
    rospy.sleep(5)  # wait 5 seconds until all nodes are started
    dumper.dump_parameters()
    if dumper.matrices_available is True:
        dumper.dump_calibration_matrices()
    else:
        rospy.logwarn('[Parameter_Dumper] Did not save calibration matrices, because the subdirectory was not \
available as a ROS parameter.')