Program Listing for File param_handler.h

Return to documentation for file (/tmp/ws/src/hatchbed_common/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);
            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, const 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, const 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];
    }

    IntParameter& param(const std::string& name, const int& 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(int* param, const std::string& name, const int& 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];
    }

    DoubleParameter& param(const std::string& name, const 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, const 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];
    }

    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];
    }

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::mutex mutex_;
  std::unordered_map<std::string, BoolParameter> bool_params_;
  std::unordered_map<std::string, DoubleParameter> double_params_;
  std::unordered_map<std::string, IntParameter> int_params_;
  std::unordered_map<std::string, StringParameter> string_params_;

  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
    rcl_interfaces::msg::SetParametersResult result;
    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_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_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 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;
                }
            }
        }
    }

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

    return result;
  }
};

}  // hatchbed_common