saturn_ros2/saturn_controller/src/config_handler.cpp

#include "saturn_controller/config_handler.hpp"
#include <spdlog/spdlog.h>
#include <spdlog/async.h>
#include <spdlog/sinks/rotating_file_sink.h>
#include <spdlog/sinks/stdout_color_sinks.h>
#include <spdlog/spdlog.h>

namespace saturn_ros2 {
ConfigHandler::ConfigHandler(std::shared_ptr<rclcpp::Node> node)
    : node_(node) {
    loadConfig();
}

template <typename T>
T declareOrGetParam(rclcpp::Node::SharedPtr node_, const std::string &param_name, T default_value)
{
    T output_value;
    try
    {
        if (node_->has_parameter(param_name))
            node_->get_parameter<T>(param_name, output_value);
        else
            output_value = node_->declare_parameter<T>(param_name, default_value);
    }
    catch (const rclcpp::exceptions::InvalidParameterTypeException &e)
    {
        RCLCPP_WARN_STREAM(node_->get_logger(), "InvalidParameterTypeException(" << param_name << "): " << e.what());
        RCLCPP_ERROR_STREAM(node_->get_logger(), "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
        throw e;
    }
    RCLCPP_INFO_STREAM(node_->get_logger(), "Found parameter - " << param_name << ": " << output_value);
    return output_value;
}


void ConfigHandler::loadConfig() {
    // 声明参数
    motion_computer_ip_address_ = declareOrGetParam<std::string>(
        node_, "motion_computer_ip_address", "192.168.144.103");
    main_timer_frequency_ =
        declareOrGetParam<float>(node_, "main_timer_frequency", 50.0);
    imu_timer_frequency_ =
        declareOrGetParam<float>(node_, "imu_timer_frequency", 200.0);
    legged_odom_timer_frequency_ =
        declareOrGetParam<float>(node_, "legged_odom_timer_frequency", 100.0);
    joints_state_timer_frequency_ =
        declareOrGetParam<float>(node_, "joints_state_timer_frequency", 5.0);
}
} // namespace saturn_ros2