[ROS] 读取参数配置文件

在阅读开源代码VINS时，顺带整理一下所用到的读取参数的函数。

参考链接：cnblogs；VINS-Mono

---

1.从ROS节点中读取参数

首先在launch文件中进行配置：

    <arg name="config_path" default = "$(find feature_tracker)/../config/realsense/realsense_color_config.yaml" />
	  <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
    
    <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
        <param name="config_file" type="string" value="$(arg config_path)" />
        <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

可以看到，此处定义了两个string类型的参数：config_file\vins_folder

然后在相应的cpp里，按照模板定义一个读取参数的程序。

template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
    T ans;
    if (n.getParam(name, ans))
    {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    }
    else
    {
        ROS_ERROR_STREAM("Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

最后在函数里调用就行了：

    std::string config_file;
    config_file = readParam<std::string>(n, "config_file");

2.用OpenCV读取yaml文件

    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }

用法：

    fsSettings["image_topic"] >> IMAGE_TOPIC;
    fsSettings["imu_topic"] >> IMU_TOPIC;
    MAX_CNT = fsSettings["max_cnt"];
    MIN_DIST = fsSettings["min_dist"];
    ROW = fsSettings["image_height"];
    COL = fsSettings["image_width"];
    FREQ = fsSettings["freq"];
    F_THRESHOLD = fsSettings["F_threshold"];
    SHOW_TRACK = fsSettings["show_track"];
    EQUALIZE = fsSettings["equalize"];
    FISHEYE = fsSettings["fisheye"];

最后别忘了：

fsSettings.release();

其余的注意事项参考博客已经说了。