@小鱼 谢谢大神回复，我按照您说的改了之后还是没法读取yaml文件，这几天我又改了几个版本的launch.py文件，发现都无法读取配置文件。我思考了一下，感觉是yaml文件直接用了ros1中格式的问题，不知道这个方向是否正确？
以下是我改写的readParameter函数，其中ros1和ros2不一样的地方是由getParam()函数变成了get_parameter()函数，不知是否是这个原因。

... template <typename T> T readParam(rclcpp::Node::SharedPtr n, std::string name) { T ans; if (n->get_parameter(name, ans)) { //ROS_INFO("Loaded " << name << ": " << ans); std::cout<<"Loaded "<< name << ": " << ans << std::endl; } else { //ROS_ERROR_STREAM("Failed to load " << name); std::cout<< "failed to load " << name <<std::endl;//终端输出该语句后shutdown std::cout << "ans=" << ans << std::endl;//目前调试的结果来看ans输出为空 rclcpp::shutdown(); } return ans; } void readParameters(rclcpp::Node::SharedPtr n)//std::string config_file)// { /* FILE *fh = fopen(config_file.c_str(),"r"); if(fh == NULL){ ROS_WARN("config_file doesn't exist; wrong config_file path"); return; } fclose(fh); */ std::string config_file; config_file = readParam<std::string>(n, "config_file"); cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); if(!fsSettings.isOpened()) { std::cerr << "ERROR: Wrong path to settings" << std::endl; } fsSettings["imu_topic"] >> IMU_TOPIC; std::cout << "imu topic: " << IMU_TOPIC << std::endl; SOLVER_TIME = fsSettings["max_solver_time"]; NUM_ITERATIONS = fsSettings["max_num_iterations"]; MIN_PARALLAX = fsSettings["keyframe_parallax"]; MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH; std::string OUTPUT_PATH; fsSettings["output_path"] >> OUTPUT_PATH; VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv"; std::cout << "result path " << VINS_RESULT_PATH << std::endl; // create folder if not exists FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str()); std::ofstream fout(VINS_RESULT_PATH, std::ios::out); fout.close(); ACC_N = fsSettings["acc_n"]; ACC_W = fsSettings["acc_w"]; GYR_N = fsSettings["gyr_n"]; GYR_W = fsSettings["gyr_w"]; G.z() = fsSettings["g_norm"]; ROW = fsSettings["image_height"]; COL = fsSettings["image_width"]; ROS_INFO("ROW: %f COL: %f ", ROW, COL); ROS_INFO("acc_n: %f acc_w: %f ", ACC_N, ACC_W); ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; if (ESTIMATE_EXTRINSIC == 2) { ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); RIC.push_back(Eigen::Matrix3d::Identity()); TIC.push_back(Eigen::Vector3d::Zero()); EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; } else { if ( ESTIMATE_EXTRINSIC == 1) { ROS_WARN(" Optimize extrinsic param around initial guess!"); EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; } if (ESTIMATE_EXTRINSIC == 0) ROS_WARN(" fix extrinsic param "); cv::Mat cv_R, cv_T; fsSettings["extrinsicRotation"] >> cv_R; fsSettings["extrinsicTranslation"] >> cv_T; //std::cout << "cv_R:" << cv_R << std::endl; Eigen::Matrix3d eigen_R; Eigen::Vector3d eigen_T; cv::cv2eigen(cv_R, eigen_R); cv::cv2eigen(cv_T, eigen_T); Eigen::Quaterniond Q(eigen_R); eigen_R = Q.normalized(); RIC.push_back(eigen_R); TIC.push_back(eigen_T); ROS_INFO("Extrinsic_R : {}", RIC[0]); std::cout << "Extrinsic_R :" << std::endl << RIC[0] << std::endl; std::cout << "Extrinsic_T :" << std::endl << TIC[0] << std::endl; //ROS_INFO("Extrinsic_T : ", TIC[0].transpose()); } INIT_DEPTH = 5.0; BIAS_ACC_THRESHOLD = 0.1; BIAS_GYR_THRESHOLD = 0.1; TD = fsSettings["td"]; ESTIMATE_TD = fsSettings["estimate_td"]; if (ESTIMATE_TD) ROS_INFO("Unsynchronized sensors, online estimate time offset, initial td: ", TD); else ROS_INFO("Synchronized sensors, fix time offset: ", TD); ROLLING_SHUTTER = fsSettings["rolling_shutter"]; if (ROLLING_SHUTTER) { TR = fsSettings["rolling_shutter_tr"]; ROS_INFO("rolling shutter camera, read out time per line: ", TR); } else { TR = 0; } fsSettings.release(); }

试图读取的yaml文件如下：

%YAML:1.0 #common parameters imu_topic: "/imu0" image_topic: "/cam0/image_raw" output_path: "/home/ylab/output/" #camera calibration model_type: PINHOLE camera_name: camera image_width: 752 image_height: 480 distortion_parameters: k1: -2.917e-01 k2: 8.228e-02 p1: 5.333e-05 p2: -1.578e-04 projection_parameters: fx: 4.616e+02 fy: 4.603e+02 cx: 3.630e+02 cy: 2.481e+02 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [0.0148655429818, -0.999880929698, 0.00414029679422, 0.999557249008, 0.0149672133247, 0.025715529948, -0.0257744366974, 0.00375618835797, 0.999660727178] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [-0.0216401454975,-0.064676986768, 0.00981073058949] #feature traker paprameters max_cnt: 150 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 1 # publish tracking image as topic equalize: 1 # if image is too dark or light, trun on equalize to find enough features fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04 gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02 gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.81007 # gravity magnitude #loop closure parameters loop_closure: 1 # start loop closure load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' fast_relocalization: 0 # useful in real-time and large project pose_graph_save_path: "/home/ylab/output/pose_graph/" # save and load path #unsynchronization parameters estimate_td: 0 # online estimate time offset between camera and imu td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) #rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). #visualization parameters save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results visualize_camera_size: 0.4 # size of camera marker in RVIZ