.. _program_listing_file__tmp_ws_src_hatchbed_common_include_hatchbed_common_param_handler.h: Program Listing for File param_handler.h ======================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/hatchbed_common/include/hatchbed_common/param_handler.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp #pragma once #include #include #include #include #include #include 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 bool_params_; std::unordered_map double_params_; std::unordered_map int_params_; std::unordered_map string_params_; rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector ¶meters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success"; for (const auto ¶m: 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