diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 13332e94bd..5930554d67 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -46,9 +46,7 @@ class ClientBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase); RCLCPP_PUBLIC - ClientBase( - std::shared_ptr node_handle, - const std::string & service_name); + ClientBase(const std::string & service_name); RCLCPP_PUBLIC virtual ~ClientBase(); @@ -61,86 +59,44 @@ class ClientBase const rcl_client_t * get_client_handle() const; + virtual void handle_response(std::shared_ptr request_header, + std::shared_ptr response) = 0; virtual std::shared_ptr create_response() = 0; virtual std::shared_ptr create_request_header() = 0; - virtual void handle_response( - std::shared_ptr request_header, std::shared_ptr response) = 0; protected: RCLCPP_DISABLE_COPY(ClientBase); - std::shared_ptr node_handle_; - rcl_client_t client_handle_ = rcl_get_zero_initialized_client(); std::string service_name_; }; -template -class Client : public ClientBase +template +class ClientPattern { public: - using SharedRequest = typename ServiceT::Request::SharedPtr; - using SharedResponse = typename ServiceT::Response::SharedPtr; + RCLCPP_SMART_PTR_DEFINITIONS(ClientPattern); + using SharedRequest = std::shared_ptr; + using SharedResponse = std::shared_ptr; - using Promise = std::promise; - using PromiseWithRequest = std::promise>; + using Promise = std::promise; + using PromiseWithRequest = std::promise>; using SharedPromise = std::shared_ptr; using SharedPromiseWithRequest = std::shared_ptr; - using SharedFuture = std::shared_future; - using SharedFutureWithRequest = std::shared_future>; + using SharedFuture = std::shared_future; + using SharedFutureWithRequest = std::shared_future>; using CallbackType = std::function; using CallbackWithRequestType = std::function; - RCLCPP_SMART_PTR_DEFINITIONS(Client); + using SendRequestFunctionT = std::function; - Client( - std::shared_ptr node_handle, - const std::string & service_name, - rcl_client_options_t & client_options) - : ClientBase(node_handle, service_name) - { - using rosidl_generator_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); - if (rcl_client_init(&client_handle_, this->node_handle_.get(), - service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK) - { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create client: ") + - rcl_get_error_string_safe()); - // *INDENT-ON* - } - } - - virtual ~Client() - { - if (rcl_client_fini(&client_handle_, node_handle_.get()) != RCL_RET_OK) { - fprintf(stderr, - "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); - } - } - - std::shared_ptr create_response() - { - return std::shared_ptr(new typename ServiceT::Response()); - } - - std::shared_ptr create_request_header() - { - // TODO(wjwwood): This should probably use rmw_request_id's allocator. - // (since it is a C type) - return std::shared_ptr(new rmw_request_id_t); - } - - void handle_response(std::shared_ptr request_header, - std::shared_ptr response) + virtual void handle_response(std::shared_ptr request_header, + ResponseT & response) { std::lock_guard lock(pending_requests_mutex_); - auto typed_response = std::static_pointer_cast(response); int64_t sequence_number = request_header->sequence_number; // TODO(esteve) this should throw instead since it is not expected to happen in the first place if (this->pending_requests_.count(sequence_number) == 0) { @@ -152,15 +108,10 @@ class Client : public ClientBase auto callback = std::get<1>(tuple); auto future = std::get<2>(tuple); this->pending_requests_.erase(sequence_number); - call_promise->set_value(typed_response); + call_promise->set_value(response); callback(future); } - SharedFuture async_send_request(SharedRequest request) - { - return async_send_request(request, [](SharedFuture) {}); - } - template< typename CallbackT, typename std::enable_if< @@ -170,17 +121,11 @@ class Client : public ClientBase >::value >::type * = nullptr > - SharedFuture async_send_request(SharedRequest request, CallbackT && cb) + SharedFuture async_send_request(const RequestT & request, CallbackT && cb) { std::lock_guard lock(pending_requests_mutex_); int64_t sequence_number; - if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to send request: ") + rcl_get_error_string_safe()); - // *INDENT-ON* - } - + send_request_function_(request, sequence_number); SharedPromise call_promise = std::make_shared(); SharedFuture f(call_promise->get_future()); pending_requests_[sequence_number] = @@ -197,7 +142,7 @@ class Client : public ClientBase >::value >::type * = nullptr > - SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT && cb) + SharedFutureWithRequest async_send_request(const RequestT & request, CallbackT && cb) { SharedPromiseWithRequest promise = std::make_shared(); SharedFutureWithRequest future_with_request(promise->get_future()); @@ -213,13 +158,96 @@ class Client : public ClientBase return future_with_request; } -private: - RCLCPP_DISABLE_COPY(Client); + SharedFuture async_send_request(const RequestT & request) + { + return async_send_request(request, [](SharedFuture) {}); + } + virtual void set_send_request_function(SendRequestFunctionT && fn) + { + send_request_function_ = fn; + } +protected: + SendRequestFunctionT send_request_function_; + +private: std::map> pending_requests_; std::mutex pending_requests_mutex_; }; +template +class Client : public ClientPattern, + public ClientBase +{ + using ClientPatternT = ClientPattern; +public: + RCLCPP_SMART_PTR_DEFINITIONS(Client); + using SharedRequest = std::shared_ptr; + + Client( + std::shared_ptr node_handle, + const std::string & service_name, + rcl_client_options_t & client_options) + : ClientBase(service_name), node_handle_(node_handle) + { + using rosidl_generator_cpp::get_service_type_support_handle; + auto service_type_support_handle = + get_service_type_support_handle(); + if (rcl_client_init(&client_handle_, this->node_handle_.get(), + service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK) + { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create client: ") + + rcl_get_error_string_safe()); + // *INDENT-ON* + } + + this->set_send_request_function([this](SharedRequest request, int64_t & sequence_number) + { + if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("failed to send request: ") + rcl_get_error_string_safe()); + // *INDENT-ON* + } + }); + } + + virtual ~Client() + { + if (rcl_client_fini(&client_handle_, node_handle_.get()) != RCL_RET_OK) { + fprintf(stderr, + "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); + } + } + + std::shared_ptr create_response() + { + return std::shared_ptr(new typename ServiceT::Response()); + } + + virtual void handle_response(std::shared_ptr request_header, + std::shared_ptr response) + { + auto typed_response = std::static_pointer_cast(response); + ClientPatternT::handle_response(request_header, typed_response); + } + + virtual std::shared_ptr create_request_header() + { + // TODO(wjwwood): This should probably use rmw_request_id's allocator. + // (since it is a C type) + return std::shared_ptr(new rmw_request_id_t); + } + +private: + RCLCPP_DISABLE_COPY(Client); + std::shared_ptr node_handle_; + +}; + + } // namespace client } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 988fbaa5e5..769efa9018 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -260,6 +260,9 @@ class Node RCLCPP_PUBLIC const rcl_guard_condition_t * get_notify_guard_condition() const; + RCLCPP_PUBLIC + const rcl_node_t * get_rcl_handle() const; + std::atomic_bool has_executor; private: diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 9f1ef1bb27..54b480ebdd 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -27,6 +27,9 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "rcl_interfaces/srv/set_parameters.hpp" #include "rcl_interfaces/srv/set_parameters_atomically.hpp" + +#include "rcl/parameter_client.h" + #include "rclcpp/executors.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" @@ -40,9 +43,15 @@ namespace rclcpp namespace parameter_client { +using NameVector = std::vector; +using ParameterVector = std::vector; +using ParameterTypeVector = std::vector; +using SetParametersResultVector = std::vector; + class AsyncParametersClient { public: + RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient); RCLCPP_PUBLIC @@ -51,33 +60,27 @@ class AsyncParametersClient const std::string & remote_node_name = ""); RCLCPP_PUBLIC - std::shared_future> + std::shared_future get_parameters( - const std::vector & names, - std::function< - void(std::shared_future>) - > callback = nullptr); + const NameVector & names, + std::function)> callback = nullptr); RCLCPP_PUBLIC - std::shared_future> + std::shared_future get_parameter_types( - const std::vector & names, - std::function< - void(std::shared_future>) - > callback = nullptr); + const NameVector & names, + std::function)> callback = nullptr); RCLCPP_PUBLIC - std::shared_future> + std::shared_future set_parameters( - const std::vector & parameters, - std::function< - void(std::shared_future>) - > callback = nullptr); + const ParameterVector & parameters, + std::function)> callback = nullptr); RCLCPP_PUBLIC std::shared_future set_parameters_atomically( - const std::vector & parameters, + const ParameterVector & parameters, std::function< void(std::shared_future) > callback = nullptr); @@ -85,7 +88,7 @@ class AsyncParametersClient RCLCPP_PUBLIC std::shared_future list_parameters( - const std::vector & prefixes, + const NameVector & prefixes, uint64_t depth, std::function< void(std::shared_future) @@ -101,15 +104,23 @@ class AsyncParametersClient private: const rclcpp::node::Node::SharedPtr node_; - rclcpp::client::Client::SharedPtr get_parameters_client_; - rclcpp::client::Client::SharedPtr - get_parameter_types_client_; - rclcpp::client::Client::SharedPtr set_parameters_client_; - rclcpp::client::Client::SharedPtr - set_parameters_atomically_client_; - rclcpp::client::Client::SharedPtr list_parameters_client_; - rclcpp::client::Client::SharedPtr - describe_parameters_client_; + rcl_parameter_client_t parameter_client_handle_ = rcl_get_zero_initialized_parameter_client(); + +#define RCLCPP_PARAMETER_CLIENT(REQUEST, RESPONSE) \ + rclcpp::client::ClientPattern + +#define RCLCPP_PARAMETER_CLIENT_SRV(SERVICE_TYPE) \ + rclcpp::client::ClientPattern + + + // Storage for promise/future patterns + RCLCPP_PARAMETER_CLIENT(NameVector, ParameterVector) get_parameters_client_; + RCLCPP_PARAMETER_CLIENT(NameVector, ParameterTypeVector) get_parameter_types_client_; + RCLCPP_PARAMETER_CLIENT(ParameterVector, SetParametersResultVector) set_parameters_client_; + RCLCPP_PARAMETER_CLIENT(ParameterVector, rcl_interfaces::msg::SetParametersResult) set_parameters_atomically_client_; + // TODO interface is a little strange + RCLCPP_PARAMETER_CLIENT_SRV(rcl_interfaces::srv::ListParameters) list_parameters_client_; + std::string remote_node_name_; }; @@ -128,25 +139,25 @@ class SyncParametersClient rclcpp::node::Node::SharedPtr node); RCLCPP_PUBLIC - std::vector - get_parameters(const std::vector & parameter_names); + ParameterVector + get_parameters(const NameVector & parameter_names); RCLCPP_PUBLIC - std::vector - get_parameter_types(const std::vector & parameter_names); + ParameterTypeVector + get_parameter_types(const NameVector & parameter_names); RCLCPP_PUBLIC - std::vector - set_parameters(const std::vector & parameters); + SetParametersResultVector + set_parameters(const ParameterVector & parameters); RCLCPP_PUBLIC rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + set_parameters_atomically(const ParameterVector & parameters); RCLCPP_PUBLIC rcl_interfaces::msg::ListParametersResult list_parameters( - const std::vector & parameter_prefixes, + const NameVector & parameter_prefixes, uint64_t depth); template diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index dbb1ce0b3f..2e5288e0cd 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -23,6 +23,9 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "rcl_interfaces/srv/set_parameters.hpp" #include "rcl_interfaces/srv/set_parameters_atomically.hpp" + +#include "rcl/parameter_service.h" + #include "rclcpp/executors.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" @@ -45,15 +48,7 @@ class ParameterService private: const rclcpp::node::Node::SharedPtr node_; - rclcpp::service::Service::SharedPtr get_parameters_service_; - rclcpp::service::Service::SharedPtr - get_parameter_types_service_; - rclcpp::service::Service::SharedPtr set_parameters_service_; - rclcpp::service::Service::SharedPtr - set_parameters_atomically_service_; - rclcpp::service::Service::SharedPtr - describe_parameters_service_; - rclcpp::service::Service::SharedPtr list_parameters_service_; + rcl_parameter_service_t parameter_service_handle; }; } // namespace parameter_service diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 10510b1773..0d8d5cf621 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -303,6 +303,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy service_handles_.erase(it); return; } + // Try again, checking if it's a parameter service + // Else, the service is no longer valid, remove it and continue service_handles_.erase(it); } diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 0fd26a80fb..1aeedd4a9b 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -22,9 +22,8 @@ using rclcpp::client::ClientBase; ClientBase::ClientBase( - std::shared_ptr node_handle, const std::string & service_name) -: node_handle_(node_handle), service_name_(service_name) +: service_name_(service_name) {} ClientBase::~ClientBase() diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index b068e6880e..b0bfdcfc4d 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -95,6 +95,16 @@ MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle, return nullptr; } +/* +rclcpp::parameter_client::ParameterClient::SharedPtr +MemoryStrategy::get_parameter_client_by_handle( + const WeakNodeVector & weak_nodes) +{ + // iterate through the parameter clients of each node and see if the + // need to also indicate which verb was available +} +*/ + rclcpp::node::Node::SharedPtr MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, const WeakNodeVector & weak_nodes) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index ce4c7acb2e..9063f66eda 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -372,3 +372,8 @@ const rcl_guard_condition_t * Node::get_notify_guard_condition() const { return ¬ify_guard_condition_; } + +const rcl_node_t * Node::get_rcl_handle() const +{ + return node_handle_.get(); +} diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index f3b88f317b..c5b9f4176d 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -12,14 +12,71 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rosidl_generator_c/string_functions.h" +#include "rcl_interfaces/msg/parameter.h" +#include "rcl_interfaces/msg/parameter_type__struct.h" + #include "rclcpp/parameter_client.hpp" #include +#include #include #include using rclcpp::parameter_client::AsyncParametersClient; using rclcpp::parameter_client::SyncParametersClient; +using rclcpp::parameter_client::NameVector; +using rclcpp::parameter_client::ParameterVector; +using rclcpp::parameter_client::ParameterTypeVector; +using rclcpp::parameter_client::SetParametersResultVector; + + +// probably need to be in utilities.hpp +// Expects array to be initialized to the same size as str_vector +void convert_to_rosidl_generator_c_string( + const std::vector & input, rosidl_generator_c__String__Array & output) +{ + for (size_t i = 0; i < input.size(); ++i) { + rosidl_generator_c__String__assign(&output.data[i], input[i].c_str()); + } +} + + +void convert_to_c_array( + const std::vector & input, + rcl_interfaces__msg__Parameter__Array & output) +{ + for (size_t i = 0; i < input.size(); ++i) { + rosidl_generator_c__String__assign(&output.data[i].name, input[i].get_name().c_str()); + // TODO this map idea is yet implemented, but it' + // output.data[i].value.type = ParameterTypeMap[input[i].get_type()]; + // switch on enums... + switch(input[i].get_type()) { + case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: + // TODO Throw + throw std::runtime_error("parameter type not set"); + case rclcpp::parameter::ParameterType::PARAMETER_BOOL: + output.data[i].value.type = rcl_interfaces__msg__ParameterType__PARAMETER_BOOL; + output.data[i].value.bool_value = input[i].as_bool(); + break; + case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: + output.data[i].value.type = rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER; + output.data[i].value.integer_value = input[i].as_int(); + break; + case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: + output.data[i].value.type = rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE; + output.data[i].value.double_value = input[i].as_double(); + break; + case rclcpp::parameter::ParameterType::PARAMETER_STRING: + output.data[i].value.type = rcl_interfaces__msg__ParameterType__PARAMETER_STRING; + rosidl_generator_c__String__assign(&output.data[i].value.string_value, input[i].as_string().c_str()); + break; + default: + // TODO Throw + throw std::runtime_error("Unexpected type received"); + } + } +} AsyncParametersClient::AsyncParametersClient( const rclcpp::node::Node::SharedPtr node, @@ -31,136 +88,119 @@ AsyncParametersClient::AsyncParametersClient( } else { remote_node_name_ = node_->get_name(); } - get_parameters_client_ = node_->create_client( - remote_node_name_ + "__get_parameters"); - get_parameter_types_client_ = node_->create_client( - remote_node_name_ + "__get_parameter_types"); - set_parameters_client_ = node_->create_client( - remote_node_name_ + "__set_parameters"); - list_parameters_client_ = node_->create_client( - remote_node_name_ + "__list_parameters"); - describe_parameters_client_ = node_->create_client( - remote_node_name_ + "__describe_parameters"); + rcl_parameter_client_options_t options = rcl_parameter_client_get_default_options(); + memcpy(options.remote_node_name, remote_node_name.c_str(), remote_node_name.length()); + rcl_ret_t ret = rcl_parameter_client_init(¶meter_client_handle_, node->get_rcl_handle(), &options); + if (ret != RCL_RET_OK) { + // TODO error handling + } + + // TODO move semantics for moving lambdas into the clients + auto get_parameters_request_function = [this](const NameVector & names, int64_t & sequence_number) { + rosidl_generator_c__String__Array names_array; + rosidl_generator_c__String__Array__init(&names_array, names.size()); + convert_to_rosidl_generator_c_string(names, names_array); + + rcl_ret_t ret = rcl_parameter_client_send_get_request( + ¶meter_client_handle_, &names_array, &sequence_number); + }; + get_parameters_client_.set_send_request_function(get_parameters_request_function); + + auto get_parameter_types_request_function = [this](const NameVector & names, int64_t & sequence_number) { + rosidl_generator_c__String__Array names_array; + rosidl_generator_c__String__Array__init(&names_array, names.size()); + convert_to_rosidl_generator_c_string(names, names_array); + + rcl_ret_t ret = rcl_parameter_client_send_get_types_request( + ¶meter_client_handle_, &names_array, &sequence_number); + }; + get_parameter_types_client_.set_send_request_function(get_parameter_types_request_function); + + auto set_parameters_request_function = [this](const ParameterVector & params, int64_t & sequence_number) { + rcl_interfaces__msg__Parameter__Array param_array; + rcl_interfaces__msg__Parameter__Array__init(¶m_array, params.size()); + convert_to_c_array(params, param_array); + + rcl_ret_t ret = rcl_parameter_client_send_set_request( + ¶meter_client_handle_, ¶m_array, &sequence_number); + }; + set_parameters_client_.set_send_request_function(set_parameters_request_function); + } std::shared_future> AsyncParametersClient::get_parameters( - const std::vector & names, - std::function< - void(std::shared_future>) - > callback) + const NameVector & names, + std::function)> callback) { auto promise_result = std::make_shared>>(); auto future_result = promise_result->get_future().share(); - auto request = std::make_shared(); - request->names = names; - - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - get_parameters_client_->async_send_request( - request, - [request, promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) + get_parameters_client_.async_send_request( + names, + [promise_result, future_result, &callback]( + RCLCPP_PARAMETER_CLIENT(NameVector, ParameterVector)::SharedFuture cb_f) { - std::vector parameter_variants; - auto & pvalues = cb_f.get()->values; - - for (auto & pvalue : pvalues) { - auto i = &pvalue - &pvalues[0]; - rcl_interfaces::msg::Parameter parameter; - parameter.name = request->names[i]; - parameter.value = pvalue; - parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter( - parameter)); - } - - promise_result->set_value(parameter_variants); + promise_result->set_value(cb_f.get()); if (callback != nullptr) { callback(future_result); } } ); - // *INDENT-ON* return future_result; } std::shared_future> AsyncParametersClient::get_parameter_types( - const std::vector & names, - std::function< - void(std::shared_future>) - > callback) + const NameVector & names, + std::function)> callback) { - auto promise_result = - std::make_shared>>(); + auto promise_result = std::make_shared>(); auto future_result = promise_result->get_future().share(); - auto request = std::make_shared(); - request->names = names; - - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - get_parameter_types_client_->async_send_request( - request, + get_parameter_types_client_.async_send_request( + names, [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) + RCLCPP_PARAMETER_CLIENT(NameVector, ParameterTypeVector)::SharedFuture cb_f) { - std::vector types; - auto & pts = cb_f.get()->types; - for (auto & pt : pts) { - pts.push_back(static_cast(pt)); - } - promise_result->set_value(types); + promise_result->set_value(cb_f.get()); if (callback != nullptr) { callback(future_result); } } ); - // *INDENT-ON* return future_result; } -std::shared_future> +std::shared_future AsyncParametersClient::set_parameters( - const std::vector & parameters, - std::function< - void(std::shared_future>) - > callback) + const ParameterVector & parameters, + std::function)> callback) { - auto promise_result = - std::make_shared>>(); + auto promise_result = std::make_shared>(); auto future_result = promise_result->get_future().share(); - auto request = std::make_shared(); - - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters), - [](rclcpp::parameter::ParameterVariant p) { - return p.to_parameter(); - } - ); - - set_parameters_client_->async_send_request( - request, + set_parameters_client_.async_send_request( + parameters, [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) + RCLCPP_PARAMETER_CLIENT(ParameterVector, SetParametersResultVector)::SharedFuture cb_f) { - promise_result->set_value(cb_f.get()->results); + promise_result->set_value(cb_f.get()); if (callback != nullptr) { callback(future_result); } } ); - // *INDENT-ON* return future_result; } std::shared_future AsyncParametersClient::set_parameters_atomically( - const std::vector & parameters, + const ParameterVector & parameters, std::function< void(std::shared_future) > callback) @@ -169,6 +209,7 @@ AsyncParametersClient::set_parameters_atomically( std::make_shared>(); auto future_result = promise_result->get_future().share(); +/* auto request = std::make_shared(); // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) @@ -190,6 +231,7 @@ AsyncParametersClient::set_parameters_atomically( } ); // *INDENT-ON* +*/ return future_result; } @@ -206,6 +248,7 @@ AsyncParametersClient::list_parameters( std::make_shared>(); auto future_result = promise_result->get_future().share(); +/* auto request = std::make_shared(); request->prefixes = prefixes; request->depth = depth; @@ -223,6 +266,7 @@ AsyncParametersClient::list_parameters( } ); // *INDENT-ON* +*/ return future_result; } diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index e1efa7072a..ffea9f7ee1 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -24,6 +24,7 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node) { std::weak_ptr captured_node = node_; // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) +/* get_parameters_service_ = node_->create_service( node_->get_name() + "__get_parameters", [captured_node]( @@ -137,4 +138,5 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node) } ); // *INDENT-ON* +*/ }