diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index e0f01a8..3da59bb 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -19,6 +19,7 @@ #include +#include #include #include @@ -56,42 +57,49 @@ public: { value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_NOT_SET; } - ParameterVariant(const std::string & name, const bool bool_value) + explicit ParameterVariant(const std::string & name, const bool bool_value) : name_(name) { value_.bool_value = bool_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_BOOL; } - ParameterVariant(const std::string & name, const int64_t int_value) + explicit ParameterVariant(const std::string & name, const int int_value) : name_(name) { value_.integer_value = int_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_INTEGER; } - ParameterVariant(const std::string & name, const double double_value) + explicit ParameterVariant(const std::string & name, const int64_t int_value) + : name_(name) + { + value_.integer_value = int_value; + value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_INTEGER; + } + explicit ParameterVariant(const std::string & name, const float double_value) : name_(name) { value_.double_value = double_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_DOUBLE; } - ParameterVariant(const std::string & name, const std::string & string_value) + explicit ParameterVariant(const std::string & name, const double double_value) + : name_(name) + { + value_.double_value = double_value; + value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_DOUBLE; + } + explicit ParameterVariant(const std::string & name, const std::string & string_value) : name_(name) { value_.string_value = string_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_STRING; } - ParameterVariant(const std::string & name, const std::vector & bytes_value) + explicit ParameterVariant(const std::string & name, const std::vector & bytes_value) : name_(name) { value_.bytes_value = bytes_value; value_.parameter_type = rcl_interfaces::ParameterType::PARAMETER_BYTES; } - /* Templated getter */ - template - T - get_value() const; - inline ParameterType get_type() const {return static_cast(value_.parameter_type); } inline std::string get_name() const & {return name_; } @@ -101,61 +109,78 @@ public: return value_; } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_INTEGER) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.integer_value; + } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_DOUBLE) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.double_value; + } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_STRING) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.string_value; + } + template + typename std::enable_if::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BOOL) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.bool_value; + } + template + typename std::enable_if>::type + get_value() const + { + if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BYTES) { + // TODO: use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.bytes_value; + } + + int64_t as_int() const {return get_value(); } + + double as_double() const {return get_value(); } + + const std::string & as_string() const {return get_value(); } + + bool as_bool() const {return get_value(); } + + std::vector as_bytes() const {return get_value(); } + private: std::string name_; rcl_interfaces::ParameterValue value_; }; -template<> -inline int64_t ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_INTEGER) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.integer_value; -} -template<> -inline double ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_DOUBLE) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.double_value; -} -template<> -inline std::string ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_STRING) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.string_value; -} -template<> -inline bool ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BOOL) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.bool_value; -} -template<> -inline std::vector ParameterVariant::get_value() const -{ - if (value_.parameter_type != rcl_interfaces::ParameterType::PARAMETER_BYTES) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.bytes_value; -} - class AsyncParametersClient { public: + RCLCPP_MAKE_SHARED_DEFINITIONS(AsyncParametersClient); + AsyncParametersClient(const rclcpp::node::Node::SharedPtr & node) : node_(node) { @@ -191,34 +216,34 @@ public: return f; } - std::shared_future> + std::shared_future> set_parameters( std::vector parameters, std::function>)> callback = nullptr) + std::shared_future>)> callback = nullptr) { - std::shared_future> f; + std::shared_future> f; return f; } - std::shared_future + std::shared_future set_parameters_atomically( std::vector parameters, std::function)> callback = nullptr) + std::shared_future)> callback = nullptr) { - std::shared_future f; + std::shared_future f; return f; } - std::shared_future + std::shared_future list_parameters( std::vector parameter_prefixes, uint64_t depth, std::function)> callback = nullptr) + std::shared_future)> callback = nullptr) { - std::shared_future f; + std::shared_future f; return f; } @@ -233,6 +258,70 @@ private: rclcpp::client::Client::SharedPtr describe_parameters_client_; }; +class SyncParametersClient +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(SyncParametersClient); + + SyncParametersClient( + rclcpp::node::Node::SharedPtr & node) + : node_(node) + { + executor_ = std::make_shared(); + async_parameters_client_ = std::make_shared(node); + } + + SyncParametersClient( + rclcpp::executor::Executor::SharedPtr & executor, + rclcpp::node::Node::SharedPtr & node) + : executor_(executor), node_(node) + { + async_parameters_client_ = std::make_shared(node); + } + + std::vector + get_parameters(std::vector parameter_names) + { + auto f = async_parameters_client_->get_parameters(parameter_names); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + std::vector + get_parameter_types(std::vector parameter_names) + { + auto f = async_parameters_client_->get_parameter_types(parameter_names); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + std::vector + set_parameters(std::vector parameters) + { + auto f = async_parameters_client_->set_parameters(parameters); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + rcl_interfaces::SetParametersResult + set_parameters_atomically(std::vector parameters) + { + auto f = async_parameters_client_->set_parameters_atomically(parameters); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + + rcl_interfaces::ListParametersResult + list_parameters( + std::vector parameter_prefixes, + uint64_t depth) + { + auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); + return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + } + +private: + rclcpp::executor::Executor::SharedPtr executor_; + rclcpp::node::Node::SharedPtr node_; + AsyncParametersClient::SharedPtr async_parameters_client_; +}; + } /* namespace parameter */ } /* namespace rclcpp */