Program Listing for File param_handler.h

↰ Return to documentation for file (include/hatchbed_common/param_handler.h)

#pragma once

#include <mutex>
#include <string>
#include <unordered_map>

#include <hatchbed_common/parameter.h>
#include <rclcpp/rclcpp.hpp>
#include <rcutils/logging.h>

namespace hatchbed_common {

class ParamHandler {
  public:
    ParamHandler(rclcpp::Node::SharedPtr node) :
      node_(node),
      namespace_(node_->get_fully_qualified_name()),
      verbose_param_(false)
    {
      params_callback_handle_ = node_->add_on_set_parameters_callback(
        std::bind(&ParamHandler::parametersCallback, this, std::placeholders::_1));
    };

    ~ParamHandler() = default;

    void register_verbose_logging_param() {
        if (verbose_param_) {
            return;
        }
        verbose_param_ = true;

        // determine the default verbose setting based on the current log level
        auto log_level = rcutils_logging_get_logger_level(node_->get_logger().get_name());
        bool is_verbose = log_level == RCUTILS_LOG_SEVERITY_DEBUG;
        if (is_verbose) {
            non_verbose_level_ = log_level;
        }

        param("verbose", is_verbose, "Enable debug logging").callback([this](const bool& value) {
            RCLCPP_INFO_STREAM(node_->get_logger(), "setting verbose logging: " << (value ? "true" : "false"));
            if (value) {
                auto ret = rcutils_logging_set_logger_level(node_->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
                if (ret != RCUTILS_RET_OK) {
                    RCLCPP_WARN_STREAM(node_->get_logger(), "Failed to set log level to debug");
                }
            }
            else {
                auto ret = rcutils_logging_set_logger_level(node_->get_logger().get_name(), non_verbose_level_);
                if (ret != RCUTILS_RET_OK) {
                    RCLCPP_WARN_STREAM(node_->get_logger(), "Failed to set log level");
                }
            }
        }).declare();
    }

    BoolParameter& param(const std::string& name, bool default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        bool_params_[name] = BoolParameter(nullptr, namespace_, name, default_val, description, node_);
        return bool_params_[name];
    }

    BoolParameter& param(bool* param, const std::string& name, bool default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        bool_params_[name] = BoolParameter(param, namespace_, name, default_val, description, node_);
        return bool_params_[name];
    }

    BoolArrayParameter& param(const std::string& name, const std::vector<bool>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        bool_array_params_[name] = BoolArrayParameter(nullptr, namespace_, name, default_val, description, node_);
        return bool_array_params_[name];
    }

    BoolArrayParameter& param(std::vector<bool>* param, const std::string& name, const std::vector<bool>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        bool_array_params_[name] = BoolArrayParameter(param, namespace_, name, default_val, description, node_);
        return bool_array_params_[name];
    }

    SystemIntParameter& param(const std::string& name, int default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        system_int_params_[name] = SystemIntParameter(nullptr, namespace_, name, default_val, description, node_);
        return system_int_params_[name];
    }

    SystemIntParameter& param(int* param, const std::string& name, int default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        system_int_params_[name] = SystemIntParameter(param, namespace_, name, default_val, description, node_);
        return system_int_params_[name];
    }

    IntParameter& param(const std::string& name, int64_t default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        int_params_[name] = IntParameter(nullptr, namespace_, name, default_val, description, node_);
        return int_params_[name];
    }

    IntParameter& param(int64_t* param, const std::string& name, int64_t default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        int_params_[name] = IntParameter(param, namespace_, name, default_val, description, node_);
        return int_params_[name];
    }

    IntArrayParameter& param(const std::string& name, const std::vector<int64_t>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        int_array_params_[name] = IntArrayParameter(nullptr, namespace_, name, default_val, description, node_);
        return int_array_params_[name];
    }

    IntArrayParameter& param(std::vector<int64_t>* param, const std::string& name, const std::vector<int64_t>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        int_array_params_[name] = IntArrayParameter(param, namespace_, name, default_val, description, node_);
        return int_array_params_[name];
    }

    DoubleParameter& param(const std::string& name, double default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        double_params_[name] = DoubleParameter(nullptr, namespace_, name, default_val, description, node_);
        return double_params_[name];
    }

    DoubleParameter& param(double* param, const std::string& name, double default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        double_params_[name] = DoubleParameter(param, namespace_, name, default_val, description, node_);
        return double_params_[name];
    }

    DoubleArrayParameter& param(const std::string& name, const std::vector<double>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        double_array_params_[name] = DoubleArrayParameter(nullptr, namespace_, name, default_val, description, node_);
        return double_array_params_[name];
    }

    DoubleArrayParameter& param(std::vector<double>* param, const std::string& name, const std::vector<double>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        double_array_params_[name] = DoubleArrayParameter(param, namespace_, name, default_val, description, node_);
        return double_array_params_[name];
    }

    StringParameter& param(const std::string& name, const std::string& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        string_params_[name] = StringParameter(nullptr, namespace_, name, default_val, description, node_);
        return string_params_[name];
    }

    StringParameter& param(std::string* param, const std::string& name, const std::string& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        string_params_[name] = StringParameter(param, namespace_, name, default_val, description, node_);
        return string_params_[name];
    }

    StringArrayParameter& param(const std::string& name, const std::vector<std::string>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        string_array_params_[name] = StringArrayParameter(nullptr, namespace_, name, default_val, description, node_);
        return string_array_params_[name];
    }

    StringArrayParameter& param(std::vector<std::string>* param, const std::string& name, const std::vector<std::string>& default_val, const std::string& description) {
        std::scoped_lock lock(mutex_);
        string_array_params_[name] = StringArrayParameter(param, namespace_, name, default_val, description, node_);
        return string_array_params_[name];
    }

    void setCallback(std::function<void()> callback) {
        user_callback_ = callback;
    }

private:
  rclcpp::Node::SharedPtr node_;
  std::string namespace_;
  bool verbose_param_;
  int non_verbose_level_ = RCUTILS_LOG_SEVERITY_INFO;
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;
  std::function<void()> user_callback_;

  std::mutex mutex_;
  std::unordered_map<std::string, BoolParameter> bool_params_;
  std::unordered_map<std::string, BoolArrayParameter> bool_array_params_;
  std::unordered_map<std::string, DoubleParameter> double_params_;
  std::unordered_map<std::string, DoubleArrayParameter> double_array_params_;
  std::unordered_map<std::string, SystemIntParameter> system_int_params_;
  std::unordered_map<std::string, IntParameter> int_params_;
  std::unordered_map<std::string, IntArrayParameter> int_array_params_;
  std::unordered_map<std::string, StringParameter> string_params_;
  std::unordered_map<std::string, StringArrayParameter> string_array_params_;

  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
    rcl_interfaces::msg::SetParametersResult result;
    // default to successful
    // otherwise parameters that weren't created using this handler will print a warning
    // any parameter created using this handler will be checked
    result.successful = true;
    result.reason = "success";

    for (const auto &param: parameters) {
        if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) {
            auto bool_param = bool_params_.find(param.get_name());
            if (bool_param != bool_params_.end()) {
                if (!bool_param->second.update(param.as_bool(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + bool_param->first;
                }
            }
        }
        else if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL_ARRAY) {
            auto bool_array_param = bool_array_params_.find(param.get_name());
            if (bool_array_param != bool_array_params_.end()) {
                if (!bool_array_param->second.update(param.as_bool_array(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + bool_array_param->first;
                }
            }
        }
        else if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) {
            auto double_param = double_params_.find(param.get_name());
            if (double_param != double_params_.end()) {
                if (!double_param->second.update(param.as_double(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + double_param->first;
                }
            }
        }
        else if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY) {
            auto double_array_param = double_array_params_.find(param.get_name());
            if (double_array_param != double_array_params_.end()) {
                if (!double_array_param->second.update(param.as_double_array(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + double_array_param->first;
                }
            }
        }
        else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) {
            auto int_param = int_params_.find(param.get_name());
            if (int_param != int_params_.end()) {
                if (!int_param->second.update(param.as_int(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + int_param->first;
                }
            } else {
                // the parameter is not registered as int64_t, so we need to check if it's registered as system int
                auto int_system_param = system_int_params_.find(param.get_name());
                if (int_system_param != system_int_params_.end()) {
                    if (param.as_int() >= std::numeric_limits<int>::min() && param.as_int() <= std::numeric_limits<int>::max()) {
                        if (!int_system_param->second.update(static_cast<int>(param.as_int()), true)) {
                            result.successful = false;
                            result.reason = "Failed to update parameter: " + int_system_param->first;
                        }
                    } else {
                        // unsafe to cast to int
                        result.successful = false;
                        result.reason = "Failed to update parameter: " + int_system_param->first + " (value too large to cast from 'int64_t' to 'int')";
                    }
                }
            }
        }
        else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) {
            auto int_array_param = int_array_params_.find(param.get_name());
            if (int_array_param != int_array_params_.end()) {
                if (!int_array_param->second.update(param.as_integer_array(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + int_array_param->first;
                }
            }
        }
        else if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) {
            auto string_param = string_params_.find(param.get_name());
            if (string_param != string_params_.end()) {
                if (!string_param->second.update(param.as_string(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + string_param->first;
                }
            }
        }
        else if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) {
            auto string_array_param = string_array_params_.find(param.get_name());
            if (string_array_param != string_array_params_.end()) {
                if (!string_array_param->second.update(param.as_string_array(), true)) {
                    result.successful = false;
                    result.reason = "Failed to update parameter: " + string_array_param->first;
                }
            }
        }
    }

    if (user_callback_) {
        user_callback_();
    }

    if (!result.successful) {
        RCLCPP_WARN_STREAM(node_->get_logger(), result.reason);
    }

    return result;
  }
};

}  // hatchbed_common