@小鱼 谢谢大神回复，我按照您说的改了之后还是没法读取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
