From 9e4fc05f6b3f5e8b83c644ad35dbb356946d8f9f Mon Sep 17 00:00:00 2001 From: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com> Date: Thu, 14 Apr 2022 13:16:01 +0200 Subject: [PATCH] Upgrade parameters (#274) * Initial parameters upgrade * Fix memory init/fini * Delete update * Fix name on describe service * Fix var name * Update parameter example * Fix add parameter undeclared * Update test cases * Uncrustify and lint tests * Update tests teardown * Update tests and add low mem mode * Update example with new API * Uncrustify * Fix low mem fini * Add sleep for event pubsub match * Update notify_changed_over_dds * Update string init and deleted event pub * Disable set API on callback * Aux param review (#275) * Update Signed-off-by: Pablo Garrido * Fix typos Co-authored-by: acuadros95 * Fix int tests types * Update readme * Minor readme fix * Apply suggestions from code review Co-authored-by: Ralph Lange * Apply suggestions from code review * Fix example * Requested changes * Apply suggestions from code review Co-authored-by: Jan Staschulat * Add NULL checks * Apply review comments * Apply review comments * Fix line break * Add test cases Co-authored-by: Pablo Garrido Co-authored-by: Ralph Lange Co-authored-by: Jan Staschulat --- README.md | 5 +- rclc_examples/src/example_parameter_server.c | 60 +- rclc_parameter/README.md | 297 ++++ .../include/rclc_parameter/rclc_parameter.h | 214 ++- .../src/rclc_parameter/parameter_server.c | 1190 ++++++++++++++--- .../src/rclc_parameter/parameter_utils.c | 170 ++- .../src/rclc_parameter/parameter_utils.h | 34 +- .../test/rclc_parameter/test_parameter.cpp | 1114 ++++++++++++--- 8 files changed, 2716 insertions(+), 368 deletions(-) create mode 100644 rclc_parameter/README.md diff --git a/README.md b/README.md index 5f745a26..050e4759 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,12 @@ # The rclc repository -This repository provides the rclc package, which complements the [ROS Client Support Library (rcl)](https://github.com/ros2/rcl/) to make up a complete ROS 2 client library for the C programming language. That is, rclc does not add a new layer of types on top of rcl (like rclcpp and rclpy do) but only provides convenience functions that ease the programming with the rcl types. New types are introduced only for concepts that are missing in rcl, most important an Executor and a Lifecycle Node. +This repository provides the rclc package, which complements the [ROS Client Support Library (rcl)](https://github.com/ros2/rcl/) to make up a complete ROS 2 client library for the C programming language. That is, rclc does not add a new layer of types on top of rcl (like rclcpp and rclpy do) but only provides convenience functions that ease the programming with the rcl types. New types are introduced only for concepts that are missing in rcl, most important an Executor, Lifecycle Node and the Parameter server. -In detail, this repository contains three packages: +In detail, this repository contains four packages: - [rclc](rclc/) provides the mentioned convenience functions for creating instances of publishers, subscriptions, nodes, etc. with the corresponding [rcl types](https://github.com/ros2/rcl/tree/master/rcl/include/rcl). Furthermore, it provides the rclc Executor for C, analogously to rclcpp's [Executor class](https://github.com/ros2/rclcpp/blob/master/rclcpp/include/rclcpp/executor.hpp) for C++. A key feature compared to the rclcpp Executor is that it includes features for implementing deterministic timing behavior. - [rclc_lifecycle](rclc_lifecycle/) introduces an rclc Lifecycle Node, bundling an rcl Node and the [lifecycle state machine](http://design.ros2.org/articles/node_lifecycle.html) from the [rcl_lifecycle package](https://github.com/ros2/rcl/tree/master/rcl_lifecycle). - [rclc_examples](rclc_examples/) provides small examples for the use of the convenience functions and the rclc Executor, as well as a small example for the use of the rclc Lifecycle Node. +- [rclc_parameter](rclc_parameter/) provides convenience functions for creating parameter server instances with full ROS2 parameters client compatibility. Technical information on the interfaces and the usage of these packages is given in the README.md files in the corresponding subfolders. diff --git a/rclc_examples/src/example_parameter_server.c b/rclc_examples/src/example_parameter_server.c index 11b3233e..848cce35 100644 --- a/rclc_examples/src/example_parameter_server.c +++ b/rclc_examples/src/example_parameter_server.c @@ -36,23 +36,44 @@ void timer_callback(rcl_timer_t * timer, int64_t last_call_time) rclc_parameter_set_int(¶m_server, "param2", value); } -void on_parameter_changed(Parameter * param) +bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context) { - printf("Parameter %s modified.", param->name.data); - switch (param->value.type) { - case RCLC_PARAMETER_BOOL: - printf(" New value: %d (bool)", param->value.bool_value); - break; - case RCLC_PARAMETER_INT: - printf(" New value: %ld (int)", param->value.integer_value); - break; - case RCLC_PARAMETER_DOUBLE: - printf(" New value: %f (double)", param->value.double_value); - break; - default: - break; + (void) context; + + if (old_param == NULL && new_param == NULL) { + printf("Callback error, both parameters are NULL\n"); + return false; + } + + if (old_param == NULL) { + printf("Creating new parameter %s\n", new_param->name.data); + } else if (new_param == NULL) { + printf("Deleting parameter %s\n", old_param->name.data); + } else { + printf("Parameter %s modified.", old_param->name.data); + switch (old_param->value.type) { + case RCLC_PARAMETER_BOOL: + printf( + " Old value: %d, New value: %d (bool)", old_param->value.bool_value, + new_param->value.bool_value); + break; + case RCLC_PARAMETER_INT: + printf( + " Old value: %ld, New value: %ld (int)", old_param->value.integer_value, + new_param->value.integer_value); + break; + case RCLC_PARAMETER_DOUBLE: + printf( + " Old value: %f, New value: %f (double)", old_param->value.double_value, + new_param->value.double_value); + break; + default: + break; + } + printf("\n"); } - printf("\n"); + + return true; } int main() @@ -82,7 +103,7 @@ int main() // Create executor rclc_executor_t executor; rclc_executor_init( - &executor, &support.context, RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1, + &executor, &support.context, RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES + 1, &allocator); rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed); rclc_executor_add_timer(&executor, &timer); @@ -96,6 +117,13 @@ int main() rclc_parameter_set_int(¶m_server, "param2", 10); rclc_parameter_set_double(¶m_server, "param3", 0.01); + // Add parameters constraints + rclc_add_parameter_description(¶m_server, "param2", "Second parameter", "Only even numbers"); + rclc_add_parameter_constraint_integer(¶m_server, "param2", -10, 120, 2); + + rclc_add_parameter_description(¶m_server, "param3", "Third parameter", ""); + rclc_set_parameter_read_only(¶m_server, "param3", true); + bool param1; int64_t param2; double param3; diff --git a/rclc_parameter/README.md b/rclc_parameter/README.md new file mode 100644 index 00000000..93b5cdac --- /dev/null +++ b/rclc_parameter/README.md @@ -0,0 +1,297 @@ +# The rclc parameter package + +ROS 2 parameters allow the user to create variables on a node and manipulate/read them with different ROS 2 commands. Further information about ROS 2 parameters can be found [here](https://docs.ros.org/en/rolling/Tutorials/Parameters/Understanding-ROS2-Parameters.html) + +This package provides the rclc API with parameter server instances with full ROS 2 parameter client compatibility. A parameter client has not been implemented for rclc (yet). +Ready to use code examples related to this tutorial can be found in [`rclc/rclc_examples/src/example_parameter_server.c`](../rclc_examples/src/example_parameter_server.c). + +## Table of contents +* [Initialization](#initialization) +* [Memory requirements](#memory-requirements) +* [Callback](#callback) +* [Add a parameter](#add-a-parameter) +* [Delete a parameter](#delete-a-parameter) +* [Parameter description](#parameter-description) +* [Cleaning up](#cleaning-up) + +## Initialization + +- Default initialization: + ```c + // Parameter server object + rclc_parameter_server_t param_server; + // Initialize parameter server with default configuration + rcl_ret_t rc = rclc_parameter_server_init_default(¶m_server, &node); + + if (RCL_RET_OK != rc) { + ... // Handle error + return -1; + } + ``` + +- Custom options: + + The following options can be configured: + - notify_changed_over_dds: Publish parameter events to other ROS 2 nodes as well. + - max_params: Maximum number of parameters allowed on the `rclc_parameter_server_t` object. + - allow_undeclared_parameters: Allows creation of parameters from external parameter clients. A new parameter will be created if a `set` operation is requested on a non-existing parameter. + - low_mem_mode: Reduces the memory used by the parameter server, functionality constrains are applied. + + ```c + // Parameter server object + rclc_parameter_server_t param_server; + + // Initialize parameter server options + const rclc_parameter_options_t options = { + .notify_changed_over_dds = true, + .max_params = 4, + .allow_undeclared_parameters = true, + .low_mem_mode = false; }; + + // Initialize parameter server with configured options + rcl_ret_t rc = rclc_parameter_server_init_with_option(¶m_server, &node, &options); + if (RCL_RET_OK != rc) { + ... // Handle error + return -1; + } + ``` + +- Low memory mode: + + This mode ports the parameter functionality to memory constrained devices. The following constrains are applied: + - Request size limited to one parameter on Set, Get, Get types and Describe services. + - List parameter request has no prefixes enabled nor depth. + - Parameter description strings not allowed, `rclc_add_parameter_description` is disabled. + + Memory benchmark results on `STM32F4` for 7 parameters with `RCLC_PARAMETER_MAX_STRING_LENGTH = 50` and `notify_changed_over_dds = true`: + - Full mode: 11736 B + - Low memory mode: 4160 B + +## Memory requirements + +The parameter server uses five services and an optional publisher. These need to be taken into account on the `rmw-microxrcedds` package memory configuration: + +```yaml +# colcon.meta example with memory requirements to use a parameter server +{ + "names": { + "rmw_microxrcedds": { + "cmake-args": [ + "-DRMW_UXRCE_MAX_NODES=1", + "-DRMW_UXRCE_MAX_PUBLISHERS=1", + "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0", + "-DRMW_UXRCE_MAX_SERVICES=5", + "-DRMW_UXRCE_MAX_CLIENTS=0" + ] + } + } +} +``` + +At runtime, the variable `RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES` defines the necessary number of handles required by a parameter server for the rclc Executor: + +```c +// Executor init example with the minimum RCLC executor handles required +rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); +rc = rclc_executor_init( + &executor, &support.context, + RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES, &allocator); +``` + +## Callback + +When adding the parameter server to the executor, a callback could to be configured. This callback would then be executed on the following events: +- Parameter value change: Internal and external parameter set on existing parameters. +- Parameter creation: External parameter set on unexisting parameter if `allow_undeclared_parameters` is set. +- Parameter delete: External parameter delete on existing parameter. +- The user can allow or reject this operations using the `bool` return value. + +Callback parameters: +- `old_param`: Parameter actual value, `NULL` for new parameter request. +- `new_param`: Parameter new value, `NULL` for parameter removal request. +- `context`: User context, configured on `rclc_executor_add_parameter_server_with_context`. + +```c +bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context) +{ + (void) context; + + if (old_param == NULL && new_param == NULL) { + printf("Callback error, both parameters are NULL\n"); + return false; + } + + if (old_param == NULL) { + printf("Creating new parameter %s\n", new_param->name.data); + } else if (new_param == NULL) { + printf("Deleting parameter %s\n", old_param->name.data); + } else { + printf("Parameter %s modified.", old_param->name.data); + switch (old_param->value.type) { + case RCLC_PARAMETER_BOOL: + printf( + " Old value: %d, New value: %d (bool)", old_param->value.bool_value, + new_param->value.bool_value); + break; + case RCLC_PARAMETER_INT: + printf( + " Old value: %ld, New value: %ld (int)", old_param->value.integer_value, + new_param->value.integer_value); + break; + case RCLC_PARAMETER_DOUBLE: + printf( + " Old value: %f, New value: %f (double)", old_param->value.double_value, + new_param->value.double_value); + break; + default: + break; + } + printf("\n"); + } + + return true; +} +``` + +Parameters modifications are disabled while the callback `on_parameter_changed` is executed, causing the following methods to return `RCLC_PARAMETER_DISABLED_ON_CALLBACK` if they are invoked: +- rclc_add_parameter +- rclc_delete_parameter +- rclc_parameter_set_bool +- rclc_parameter_set_int +- rclc_parameter_set_double +- rclc_set_parameter_read_only +- rclc_add_parameter_constraint_double +- rclc_add_parameter_constraint_integer + +Once the parameter server and the executor are initialized, the parameter server must be added to the executor in order to accept parameter commands from ROS 2: +```c +// Add parameter server to the executor including defined callback +rc = rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed); +``` + +Note that this callback is optional as its just an event information for the user. To use the parameter server without a callback: +```c +// Add parameter server to the executor without a callback +rc = rclc_executor_add_parameter_server(&executor, ¶m_server, NULL); +``` + +Configuration of the callback context: +```c +// Add parameter server to the executor including defined callback and a context +rc = rclc_executor_add_parameter_server_with_context(&executor, ¶m_server, on_parameter_changed, &context); +``` + +## Add a parameter + +The micro-ROS parameter server supports the following parameter types: + +- Boolean: + ```c + const char * parameter_name = "parameter_bool"; + bool param_value = true; + + // Add parameter to the server + rcl_ret_t rc = rclc_add_parameter(¶m_server, parameter_name, RCLC_PARAMETER_BOOL); + + // Set parameter value (Triggers `on_parameter_changed` callback, if defined) + rc = rclc_parameter_set_bool(¶m_server, parameter_name, param_value); + + // Get parameter value and store it in "param_value" + rc = rclc_parameter_get_bool(¶m_server, "param1", ¶m_value); + + if (RCL_RET_OK != rc) { + ... // Handle error + return -1; + } + ``` + +- Integer: + ```c + const char * parameter_name = "parameter_int"; + int param_value = 100; + + // Add parameter to the server + rcl_ret_t rc = rclc_add_parameter(¶m_server, parameter_name, RCLC_PARAMETER_INT); + + // Set parameter value + rc = rclc_parameter_set_int(¶m_server, parameter_name, param_value); + + // Get parameter value on param_value + rc = rclc_parameter_get_int(¶m_server, parameter_name, ¶m_value); + ``` + +- Double: + ```c + const char * parameter_name = "parameter_double"; + double param_value = 0.15; + + // Add parameter to the server + rcl_ret_t rc = rclc_add_parameter(¶m_server, parameter_name, RCLC_PARAMETER_DOUBLE); + + // Set parameter value + rc = rclc_parameter_set_double(¶m_server, parameter_name, param_value); + + // Get parameter value on param_value + rc = rclc_parameter_get_double(¶m_server, parameter_name, ¶m_value); + ``` + +Parameters can also be created by external clients if the `allow_undeclared_parameters` flag is set. +The client just needs to set a value on a non-existing parameter. Then this parameter will be created if the server has still capacity and the callback allows the operation. + +*Max name size is controlled by the compile-time option `RCLC_PARAMETER_MAX_STRING_LENGTH`, default value is 50.* + +## Delete a parameter +Parameters can be deleted by both, the parameter server and external clients: +```c +rclc_delete_parameter(¶m_server, "param2"); +``` + +Note that for external delete requests, the server callback will be executed, allowing the node to reject the operation. + +## Parameter description + +- Parameter description + Adds a description of a parameter and its constrains, which will be returned on a describe parameter requests: + ```c + rclc_add_parameter_description(¶m_server, "param2", "Second parameter", "Only even numbers"); + ``` + + *The maximum string size is controlled by the compilation time option `RCLC_PARAMETER_MAX_STRING_LENGTH`, default value is 50.* + +- Parameter constraints + Informative numeric constraints can be added to int and double parameters, returning this values on describe parameter requests: + - `from_value`: Start value for valid values, inclusive. + - `to_value`: End value for valid values, inclusive. + - `step`: Size of valid steps between the from and to bound. + + ```c + int64_t int_from = 0; + int64_t int_to = 20; + uint64_t int_step = 2; + rclc_add_parameter_constraint_integer(¶m_server, "param2", int_from, int_to, int_step); + + double double_from = -0.5; + double double_to = 0.5; + double double_step = 0.01; + rclc_add_parameter_constraint_double(¶m_server, "param3", double_from, double_to, double_step); + ``` + + *This constrains will not be applied by the parameter server, leaving values filtering to the user callback.* + +- Read-only parameters: + The new API offers a read-only flag. This flag blocks parameter changes from external clients, but allows changes on the server side: + ```c + bool read_only = true; + rclc_set_parameter_read_only(¶m_server, "param3", read_only); + ``` + +## Cleaning up + +To destroy an initialized parameter server: + +```c +// Delete parameter server +rclc_parameter_server_fini(¶m_server, &node); +``` + +This will delete any automatically created infrastructure on the agent (if possible) and deallocate used memory on the parameter server side. \ No newline at end of file diff --git a/rclc_parameter/include/rclc_parameter/rclc_parameter.h b/rclc_parameter/include/rclc_parameter/rclc_parameter.h index 9c60bdf8..1f4aa2b2 100644 --- a/rclc_parameter/include/rclc_parameter/rclc_parameter.h +++ b/rclc_parameter/include/rclc_parameter/rclc_parameter.h @@ -62,13 +62,43 @@ typedef struct rcl_interfaces__srv__ListParameters_Response ListParameters_Respo typedef struct rcl_interfaces__msg__Parameter Parameter; typedef struct rcl_interfaces__msg__ParameterValue ParameterValue; typedef struct rcl_interfaces__msg__Parameter__Sequence Parameter__Sequence; +typedef struct rcl_interfaces__msg__ParameterDescriptor ParameterDescriptor; +typedef struct rcl_interfaces__msg__ParameterDescriptor__Sequence ParameterDescriptor__Sequence; typedef struct rcl_interfaces__msg__ParameterEvent ParameterEvent; // Number of RCLC executor handles required for a parameter server -#define RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER 5 +#define RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES 5 +#define RCLC_PARAMETER_MODIFICATION_REJECTED 4001 +#define RCLC_PARAMETER_TYPE_MISMATCH 4002 +#define RCLC_PARAMETER_UNSUPORTED_ON_LOW_MEM 4003 +#define RCLC_PARAMETER_DISABLED_ON_CALLBACK 40004 -// On parameter modified callback -typedef void (* ModifiedParameter_Callback)(Parameter * param); +/** + * Parameter event callback. + * This callback will allow the user to allow or reject the following events: + * - Parameter value change: Internal and external parameter set on existing parameters. + * - Parameter creation: External parameter set on unexisting parameter if `allow_undeclared_parameters` is set. + * - Parameter delete: External parameter delete on existing parameter. + * + * Parameters modifications are disabled while this callback is executed. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] old_param Parameter actual value, `NULL` for new parameter request. + * \param[in] new_param Parameter new value, `NULL` for parameter removal request. + * \param[in] context Context of the callback. + * \return `true` to accept the parameter event. If the operation was rejected, `false` is returned. + */ +typedef bool (* rclc_parameter_callback_t)( + const Parameter * old_param, + const Parameter * new_param, + void * context); // Allowed RCLC parameter types typedef enum rclc_parameter_type_t @@ -84,6 +114,8 @@ typedef struct rclc_parameter_options_t { bool notify_changed_over_dds; size_t max_params; + bool allow_undeclared_parameters; + bool low_mem_mode; } rclc_parameter_options_t; // Container for RCLC parameter server @@ -112,12 +144,17 @@ typedef struct rclc_parameter_server_t DescribeParameters_Response describe_response; Parameter__Sequence parameter_list; + ParameterDescriptor__Sequence parameter_descriptors; ParameterEvent event_list; - ModifiedParameter_Callback on_modification; + rclc_parameter_callback_t on_modification; + void * context; + bool on_callback; bool notify_changed_over_dds; + bool allow_undeclared_parameters; + bool low_mem_mode; } rclc_parameter_server_t; /** @@ -202,10 +239,35 @@ RCLC_PARAMETER_PUBLIC rcl_ret_t rclc_executor_add_parameter_server( rclc_executor_t * executor, rclc_parameter_server_t * parameter_server, - ModifiedParameter_Callback on_modification); + rclc_parameter_callback_t on_modification); + +/** + * Adds a RCLC parameter server to an RCLC executor + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] executor RCLC executor + * \param[in] parameter_server preallocated rclc_parameter_server_t + * \param[in] on_modification on parameter modification callback + * \param[in] context context of the parameter modification callback + * \return `RCL_RET_OK` if success + */ +RCLC_PARAMETER_PUBLIC +rcl_ret_t rclc_executor_add_parameter_server_with_context( + rclc_executor_t * executor, + rclc_parameter_server_t * parameter_server, + rclc_parameter_callback_t on_modification, + void * context); /** * Adds a RCLC parameter to a server + * This method is disabled on user callback execution. * *
* Attribute | Adherence @@ -228,7 +290,30 @@ rclc_add_parameter( rclc_parameter_type_t type); /** - * Sets the value of an existing a RCLC bool parameter + * Deletes a RCLC parameter from the server. + * This method is disabled on user callback execution. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] parameter_server preallocated rclc_parameter_server_t + * \param[in] parameter_name name of the parameter + * \return `RCL_RET_OK` if success + */ +RCLC_PARAMETER_PUBLIC +rcl_ret_t +rclc_delete_parameter( + rclc_parameter_server_t * parameter_server, + const char * parameter_name); + +/** + * Sets the value of an existing a RCLC bool parameter. + * This method is disabled on user callback execution. * *
* Attribute | Adherence @@ -251,7 +336,8 @@ rclc_parameter_set_bool( bool value); /** - * Sets the value of an existing a RCLC integer parameter + * Sets the value of an existing a RCLC integer parameter. + * This method is disabled on user callback execution. * *
* Attribute | Adherence @@ -274,7 +360,8 @@ rclc_parameter_set_int( int64_t value); /** - * Sets the value of an existing a RCLC double parameter + * Sets the value of an existing a RCLC double parameter. + * This method is disabled on user callback execution. * *
* Attribute | Adherence @@ -365,6 +452,117 @@ rclc_parameter_get_double( const char * parameter_name, double * output); + +/** + * Add a description to a parameter. (This feature is disabled in low memory mode.) + * This description will be returned on describe parameters requests. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] parameter_server preallocated rclc_parameter_server_t + * \param[in] parameter_name name of the parameter + * \param[in] parameter_description description of the parameter + * \param[in] additional_constraints constraints description + * \param[in] read_only read only flag + * \return `RCL_RET_OK` if success + */ +RCLC_PARAMETER_PUBLIC +rcl_ret_t +rclc_add_parameter_description( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, + const char * parameter_description, + const char * additional_constraints); + +/** + * Sets a parameter read only flag. + * Read only parameters can only be modified from the `rclc_parameter_set` API. + * This method is disabled on user callback execution. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] parameter_server preallocated rclc_parameter_server_t + * \param[in] parameter_name name of the parameter + * \param[in] read_only read only flag + * \return `RCL_RET_OK` if success + */ +RCLC_PARAMETER_PUBLIC +rcl_ret_t +rclc_set_parameter_read_only( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, + bool read_only); + +/** + * Sets a constraint on an integer parameter. + * This constraint specification will be returned on describe parameters requests. + * This method is disabled on user callback execution. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] parameter_server preallocated rclc_parameter_server_t + * \param[in] parameter_name name of the parameter + * \param[in] from_value start value for valid values, inclusive. + * \param[in] to_value end value for valid values, inclusive. + * \param[in] step size of valid steps between the from and to bound. + * \return `RCL_RET_OK` if success + */ +RCLC_PARAMETER_PUBLIC +rcl_ret_t +rclc_add_parameter_constraint_integer( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, + int64_t from_value, + int64_t to_value, + uint64_t step); + +/** + * Sets a constraint on an double parameter. + * This constraint specification will be returned on describe parameters requests. + * This method is disabled on user callback execution. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] parameter_server preallocated rclc_parameter_server_t + * \param[in] parameter_name name of the parameter + * \param[in] from_value start value for valid values, inclusive. + * \param[in] to_value end value for valid values, inclusive. + * \param[in] step size of valid steps between the from and to bound. + * \return `RCL_RET_OK` if success + */ +RCLC_PARAMETER_PUBLIC +rcl_ret_t +rclc_add_parameter_constraint_double( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, + double from_value, + double to_value, + double step); + #if __cplusplus } #endif // if __cplusplus diff --git a/rclc_parameter/src/rclc_parameter/parameter_server.c b/rclc_parameter/src/rclc_parameter/parameter_server.c index ab97b089..4cbdd646 100644 --- a/rclc_parameter/src/rclc_parameter/parameter_server.c +++ b/rclc_parameter/src/rclc_parameter/parameter_server.c @@ -21,9 +21,13 @@ extern "C" #include #include +#include +#include #include "./parameter_utils.h" +#define RCLC_SET_ERROR_MAX_STRING_LENGTH 25 + rcl_ret_t rclc_parameter_server_init_service( rcl_service_t * service, @@ -34,34 +38,69 @@ rclc_parameter_server_init_service( rcl_ret_t rclc_parameter_service_publish_event( rclc_parameter_server_t * parameter_server); +rcl_ret_t rclc_add_parameter_undeclared( + rclc_parameter_server_t * parameter_server, + Parameter * parameter); + +rcl_ret_t rclc_parameter_execute_callback( + rclc_parameter_server_t * parameter_server, + const Parameter * old_param, + const Parameter * new_param); + void rclc_parameter_server_describe_service_callback( const void * req, void * res, void * parameter_server) { + RCL_CHECK_FOR_NULL_WITH_MSG( + req, "req is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + res, "res is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return ); + rclc_parameter_server_t * param_server = (rclc_parameter_server_t *) parameter_server; DescribeParameters_Request * request = (DescribeParameters_Request *) req; DescribeParameters_Response * response = (DescribeParameters_Response *) res; - size_t size = (request->names.size > param_server->parameter_list.size) ? - param_server->parameter_list.size : - request->names.size; - - response->descriptors.size = size; + if (request->names.size > response->descriptors.capacity) { + response->descriptors.size = 0; + return; + } - for (size_t i = 0; i < size; i++) { - rclc_parameter_set_string( - &response->descriptors.data[i].name, - request->names.data[i].data); + response->descriptors.size = request->names.size; - Parameter * parameter = rclc_parameter_search( + for (size_t i = 0; i < request->names.size; ++i) { + size_t index = rclc_parameter_search_index( ¶m_server->parameter_list, request->names.data[i].data); - response->descriptors.data[i].type = (parameter != NULL) ? - parameter->value.type : - RCLC_PARAMETER_NOT_SET; + ParameterDescriptor * response_descriptor = &response->descriptors.data[i]; + + // Reset response values + response_descriptor->type = RCLC_PARAMETER_NOT_SET; + response_descriptor->read_only = false; + response_descriptor->floating_point_range.size = 0; + response_descriptor->integer_range.size = 0; + + // Copy request name + if (param_server->low_mem_mode) { + response_descriptor->name.data = request->names.data[i].data; + response_descriptor->name.size = request->names.data[i].size; + response_descriptor->name.capacity = request->names.data[i].capacity; + } + + if (index < param_server->parameter_descriptors.size) { + ParameterDescriptor * parameter_descriptor = ¶m_server->parameter_descriptors.data[index]; + rclc_parameter_descriptor_copy( + response_descriptor, parameter_descriptor, + param_server->low_mem_mode); + } else if (!param_server->low_mem_mode) { + rclc_parameter_set_string(&response_descriptor->name, request->names.data[i].data); + rclc_parameter_set_string(&response_descriptor->description, ""); + rclc_parameter_set_string(&response_descriptor->additional_constraints, ""); + } } } @@ -71,6 +110,11 @@ rclc_parameter_server_list_service_callback( void * res, void * parameter_server) { + RCL_CHECK_FOR_NULL_WITH_MSG( + res, "res is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return ); + (void) req; rclc_parameter_server_t * param_server = (rclc_parameter_server_t *) parameter_server; @@ -78,10 +122,14 @@ rclc_parameter_server_list_service_callback( response->result.names.size = param_server->parameter_list.size; - for (size_t i = 0; i < response->result.names.size; i++) { - rclc_parameter_set_string( - &response->result.names.data[i], - param_server->parameter_list.data[i].name.data); + for (size_t i = 0; i < response->result.names.size; ++i) { + if (!param_server->low_mem_mode) { + rclc_parameter_set_string( + &response->result.names.data[i], + param_server->parameter_list.data[i].name.data); + } else { + response->result.names.data[i].size = param_server->parameter_list.data[i].name.size; + } } } @@ -91,17 +139,29 @@ rclc_parameter_server_get_service_callback( void * res, void * parameter_server) { + RCL_CHECK_FOR_NULL_WITH_MSG( + req, "req is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + res, "res is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return ); + const GetParameters_Request * request = (const GetParameters_Request *) req; GetParameters_Response * response = (GetParameters_Response *) res; rclc_parameter_server_t * param_server = (rclc_parameter_server_t *) parameter_server; + if (request->names.size > response->values.capacity) { + response->values.size = 0; + return; + } + size_t size = (request->names.size > param_server->parameter_list.size) ? param_server->parameter_list.size : request->names.size; response->values.size = size; - for (size_t i = 0; i < size; i++) { + for (size_t i = 0; i < size; ++i) { Parameter * parameter = rclc_parameter_search( ¶m_server->parameter_list, request->names.data[i].data); @@ -120,6 +180,13 @@ rclc_parameter_server_get_types_service_callback( void * res, void * parameter_server) { + RCL_CHECK_FOR_NULL_WITH_MSG( + req, "req is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + res, "res is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return ); + const GetParameterTypes_Request * request = (const GetParameterTypes_Request *) req; GetParameterTypes_Response * response = (GetParameterTypes_Response *) res; rclc_parameter_server_t * param_server = (rclc_parameter_server_t *) parameter_server; @@ -131,7 +198,7 @@ rclc_parameter_server_get_types_service_callback( response->types.size = request->names.size; - for (size_t i = 0; i < response->types.size; i++) { + for (size_t i = 0; i < response->types.size; ++i) { Parameter * parameter = rclc_parameter_search( ¶m_server->parameter_list, request->names.data[i].data); @@ -150,6 +217,13 @@ rclc_parameter_server_set_service_callback( void * res, void * parameter_server) { + RCL_CHECK_FOR_NULL_WITH_MSG( + req, "req is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + res, "res is a null pointer", return ); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return ); + const SetParameters_Request * request = (const SetParameters_Request *) req; SetParameters_Response * response = (SetParameters_Response *) res; rclc_parameter_server_t * param_server = (rclc_parameter_server_t *) parameter_server; @@ -161,70 +235,103 @@ rclc_parameter_server_set_service_callback( response->results.size = request->parameters.size; - for (size_t i = 0; i < response->results.size; i++) { + for (size_t i = 0; i < response->results.size; ++i) { rosidl_runtime_c__String * message = (rosidl_runtime_c__String *) &response->results.data[i].reason; - Parameter * parameter = rclc_parameter_search( + size_t index = rclc_parameter_search_index( ¶m_server->parameter_list, request->parameters.data[i].name.data); + rcl_ret_t ret = RCL_RET_OK; + // Clean previous response msg + response->results.data[i].successful = false; rclc_parameter_set_string(message, ""); - if (parameter != NULL) { - response->results.data[i].successful = true; - - if (parameter->value.type != request->parameters.data[i].value.type) { - rclc_parameter_set_string(message, "Type mismatch"); - response->results.data[i].successful = false; + if (index < param_server->parameter_list.size) { + if (param_server->parameter_descriptors.data[index].read_only) { + rclc_parameter_set_string(message, "Read only parameter"); continue; } + Parameter * parameter = ¶m_server->parameter_list.data[index]; + switch (request->parameters.data[i].value.type) { case RCLC_PARAMETER_NOT_SET: - rclc_parameter_set_string(message, "Type not set"); - response->results.data[i].successful = false; + ret = rclc_parameter_execute_callback(param_server, parameter, NULL); + if (ret == RCL_RET_OK) { + ret = rclc_delete_parameter(param_server, parameter->name.data); + } break; case RCLC_PARAMETER_BOOL: ret = rclc_parameter_set_bool( - parameter_server, parameter->name.data, + param_server, parameter->name.data, request->parameters.data[i].value.bool_value); break; case RCLC_PARAMETER_INT: ret = rclc_parameter_set_int( - parameter_server, parameter->name.data, + param_server, parameter->name.data, request->parameters.data[i].value.integer_value); break; case RCLC_PARAMETER_DOUBLE: ret = rclc_parameter_set_double( - parameter_server, parameter->name.data, + param_server, parameter->name.data, request->parameters.data[i].value.double_value); break; default: rclc_parameter_set_string(message, "Type not supported"); - response->results.data[i].successful = false; break; } if (ret == RCL_RET_INVALID_ARGUMENT) { rclc_parameter_set_string(message, "Set parameter error"); - response->results.data[i].successful = false; + } else if (ret == RCLC_PARAMETER_MODIFICATION_REJECTED) { + rclc_parameter_set_string(message, "Rejected by server"); + } else if (ret == RCLC_PARAMETER_TYPE_MISMATCH) { + rclc_parameter_set_string(message, "Type mismatch"); + } else { + response->results.data[i].successful = true; + } + } else if (param_server->allow_undeclared_parameters && // NOLINT + request->parameters.data[i].value.type != RCLC_PARAMETER_NOT_SET) + { + size_t remaining_capacity = param_server->parameter_list.capacity - + param_server->parameter_list.size; + + if (0 == remaining_capacity) { + // Check parameter server capacity + rclc_parameter_set_string(message, "Parameter server is full"); + + } else if (RCL_RET_OK != // NOLINT + rclc_parameter_execute_callback(param_server, NULL, &request->parameters.data[i])) + { + // Check server callback + rclc_parameter_set_string(message, "New parameter rejected"); + } else if (RCL_RET_OK != // NOLINT + rclc_add_parameter_undeclared(param_server, &request->parameters.data[i])) + { + // Check add parameter + rclc_parameter_set_string(message, "Add parameter failed"); + } else { + rclc_parameter_set_string(message, "New parameter added"); + response->results.data[i].successful = true; } } else { rclc_parameter_set_string(message, "Parameter not found"); - response->results.data[i].successful = false; } } } const rclc_parameter_options_t DEFAULT_PARAMETER_SERVER_OPTIONS = { .notify_changed_over_dds = true, - .max_params = 4 + .max_params = 4, + .allow_undeclared_parameters = false, + .low_mem_mode = false }; rcl_ret_t rclc_parameter_server_init_default( @@ -241,7 +348,7 @@ rcl_ret_t rclc_parameter_server_init_default( } rcl_ret_t -rclc_parameter_server_init_with_option( +init_parameter_server_memory( rclc_parameter_server_t * parameter_server, rcl_node_t * node, const rclc_parameter_options_t * options) @@ -255,58 +362,6 @@ rclc_parameter_server_init_with_option( rcl_ret_t ret = RCL_RET_OK; - const rosidl_service_type_support_t * get_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( - rcl_interfaces, srv, - GetParameters); - ret |= rclc_parameter_server_init_service( - ¶meter_server->get_service, node, "/get_parameters", - get_ts); - - const rosidl_service_type_support_t * get_types_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( - rcl_interfaces, - srv, - GetParameterTypes); - ret |= rclc_parameter_server_init_service( - ¶meter_server->get_types_service, node, - "/get_parameter_types", get_types_ts); - - const rosidl_service_type_support_t * set_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( - rcl_interfaces, srv, - SetParameters); - ret |= rclc_parameter_server_init_service( - ¶meter_server->set_service, node, "/set_parameters", - set_ts); - - const rosidl_service_type_support_t * list_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( - rcl_interfaces, srv, - ListParameters); - ret |= rclc_parameter_server_init_service( - ¶meter_server->list_service, node, - "/list_parameters", list_ts); - - const rosidl_service_type_support_t * describe_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( - rcl_interfaces, srv, - DescribeParameters); - ret |= rclc_parameter_server_init_service( - ¶meter_server->describe_service, node, - "/describe_parameters", describe_ts); - - parameter_server->notify_changed_over_dds = options->notify_changed_over_dds; - if (parameter_server->notify_changed_over_dds) { - const rosidl_message_type_support_t * event_ts = ROSIDL_GET_MSG_TYPE_SUPPORT( - rcl_interfaces, - msg, - ParameterEvent); - ret |= rclc_publisher_init( - ¶meter_server->event_publisher, node, event_ts, - "/parameter_events", - &rmw_qos_profile_parameter_events); - } - - static char empty_string[RCLC_PARAMETER_MAX_STRING_LENGTH]; - memset(empty_string, ' ', RCLC_PARAMETER_MAX_STRING_LENGTH); - empty_string[RCLC_PARAMETER_MAX_STRING_LENGTH - 1] = '\0'; - bool mem_allocs_ok = true; mem_allocs_ok &= rcl_interfaces__msg__Parameter__Sequence__init( ¶meter_server->parameter_list, @@ -314,12 +369,12 @@ rclc_parameter_server_init_with_option( parameter_server->parameter_list.size = 0; // Pre-init strings - for (size_t i = 0; i < options->max_params; i++) { - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->parameter_list.data[i].name, - (const char *) empty_string); + for (size_t i = 0; i < options->max_params; ++i) { + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->parameter_list.data[i].name); } + // Init list service msgs mem_allocs_ok &= rcl_interfaces__srv__ListParameters_Request__init(¶meter_server->list_request); mem_allocs_ok &= rcl_interfaces__srv__ListParameters_Response__init( @@ -330,12 +385,12 @@ rclc_parameter_server_init_with_option( parameter_server->list_response.result.names.size = 0; // Pre-init strings - for (size_t i = 0; i < options->max_params; i++) { - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->list_response.result.names.data[i], - (const char *) empty_string); + for (size_t i = 0; i < options->max_params; ++i) { + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->list_response.result.names.data[i]); } + // Init Get service msgs mem_allocs_ok &= rcl_interfaces__srv__GetParameters_Request__init(¶meter_server->get_request); mem_allocs_ok &= rcl_interfaces__srv__GetParameters_Response__init(¶meter_server->get_response); @@ -349,12 +404,12 @@ rclc_parameter_server_init_with_option( parameter_server->get_response.values.size = 0; // Pre-init strings - for (size_t i = 0; i < options->max_params; i++) { - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->get_request.names.data[i], - (const char *) empty_string); + for (size_t i = 0; i < options->max_params; ++i) { + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->get_request.names.data[i]); } + // Init Set service msgs mem_allocs_ok &= rcl_interfaces__srv__SetParameters_Request__init(¶meter_server->set_request); mem_allocs_ok &= rcl_interfaces__srv__SetParameters_Response__init(¶meter_server->set_response); @@ -368,15 +423,14 @@ rclc_parameter_server_init_with_option( parameter_server->set_response.results.size = 0; // Pre-init strings - for (size_t i = 0; i < options->max_params; i++) { - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->set_request.parameters.data[i].name, - (const char *) empty_string); - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->set_response.results.data[i].reason, - (const char *) empty_string); + for (size_t i = 0; i < options->max_params; ++i) { + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->set_request.parameters.data[i].name); + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->set_response.results.data[i].reason); } + // Init Get types service msgs mem_allocs_ok &= rcl_interfaces__srv__GetParameterTypes_Request__init( ¶meter_server->get_types_request); mem_allocs_ok &= rcl_interfaces__srv__GetParameterTypes_Response__init( @@ -390,12 +444,12 @@ rclc_parameter_server_init_with_option( options->max_params); parameter_server->get_types_response.types.size = 0; - for (size_t i = 0; i < options->max_params; i++) { - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->get_types_request.names.data[i], - (const char *) empty_string); + for (size_t i = 0; i < options->max_params; ++i) { + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->get_types_request.names.data[i]); } + // Init Describe service msgs mem_allocs_ok &= rcl_interfaces__srv__DescribeParameters_Request__init( ¶meter_server->describe_request); mem_allocs_ok &= rcl_interfaces__srv__DescribeParameters_Response__init( @@ -404,24 +458,62 @@ rclc_parameter_server_init_with_option( ¶meter_server->describe_request.names, options->max_params); parameter_server->describe_request.names.size = 0; + mem_allocs_ok &= rcl_interfaces__msg__ParameterDescriptor__Sequence__init( ¶meter_server->describe_response.descriptors, options->max_params); parameter_server->describe_response.descriptors.size = 0; - for (size_t i = 0; i < options->max_params; i++) { - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->describe_request.names.data[i], - (const char *) empty_string); - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->describe_response.descriptors.data[i].name, - (const char *) empty_string); + mem_allocs_ok &= rcl_interfaces__msg__ParameterDescriptor__Sequence__init( + ¶meter_server->parameter_descriptors, + options->max_params); + parameter_server->parameter_descriptors.size = 0; + + for (size_t i = 0; i < options->max_params; ++i) { + // Init describe_request members + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->describe_request.names.data[i]); + + // Init describe_response members + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->describe_response.descriptors.data[i].name); + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->describe_response.descriptors.data[i].description); + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->describe_response.descriptors.data[i].additional_constraints); + mem_allocs_ok &= rcl_interfaces__msg__FloatingPointRange__Sequence__init( + ¶meter_server->describe_response.descriptors.data[i].floating_point_range, + rcl_interfaces__msg__ParameterDescriptor__floating_point_range__MAX_SIZE); + parameter_server->describe_response.descriptors.data[i].floating_point_range.size = 0; + mem_allocs_ok &= rcl_interfaces__msg__IntegerRange__Sequence__init( + ¶meter_server->describe_response.descriptors.data[i].integer_range, + rcl_interfaces__msg__ParameterDescriptor__integer_range__MAX_SIZE); + parameter_server->describe_response.descriptors.data[i].integer_range.size = 0; + + // Init parameter_descriptors members + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->parameter_descriptors.data[i].name); + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->parameter_descriptors.data[i].description); + mem_allocs_ok &= rclc_parameter_descriptor_initialize_string( + ¶meter_server->parameter_descriptors.data[i].additional_constraints); + mem_allocs_ok &= rcl_interfaces__msg__FloatingPointRange__Sequence__init( + ¶meter_server->parameter_descriptors.data[i].floating_point_range, + rcl_interfaces__msg__ParameterDescriptor__floating_point_range__MAX_SIZE); + parameter_server->parameter_descriptors.data[i].floating_point_range.size = 0; + mem_allocs_ok &= rcl_interfaces__msg__IntegerRange__Sequence__init( + ¶meter_server->parameter_descriptors.data[i].integer_range, + rcl_interfaces__msg__ParameterDescriptor__integer_range__MAX_SIZE); + parameter_server->parameter_descriptors.data[i].integer_range.size = 0; } - mem_allocs_ok &= rcl_interfaces__msg__ParameterEvent__init(¶meter_server->event_list); - mem_allocs_ok &= rosidl_runtime_c__String__assign( - ¶meter_server->event_list.node, - rcl_node_get_name(node)); + // Init event publisher msgs + if (parameter_server->notify_changed_over_dds) { + mem_allocs_ok &= rcl_interfaces__msg__ParameterEvent__init(¶meter_server->event_list); + mem_allocs_ok &= rosidl_runtime_c__String__assign( + ¶meter_server->event_list.node, + rcl_node_get_name(node)); + } if (!mem_allocs_ok) { ret = RCL_RET_ERROR; @@ -430,33 +522,420 @@ rclc_parameter_server_init_with_option( return ret; } +rcl_ret_t init_parameter_server_memory_low( + rclc_parameter_server_t * parameter_server, + rcl_node_t * node, + const rclc_parameter_options_t * options) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + options, "options is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = RCL_RET_OK; + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + // Init a parameter sequence + parameter_server->parameter_list.data = + allocator.zero_allocate(options->max_params, sizeof(Parameter), allocator.state); + parameter_server->parameter_list.size = 0; + parameter_server->parameter_list.capacity = options->max_params; + + // Init a parameter descriptor sequence + parameter_server->parameter_descriptors.data = allocator.zero_allocate( + options->max_params, + sizeof(ParameterDescriptor), + allocator.state); + parameter_server->parameter_descriptors.size = 0; + parameter_server->parameter_descriptors.capacity = options->max_params; + + for (size_t i = 0; i < options->max_params; ++i) { + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->parameter_list.data[i].name, + RCLC_PARAMETER_MAX_STRING_LENGTH); + ret |= + rclc_parameter_initialize_empty_string( + ¶meter_server->parameter_list.data[i].value.string_value, 1); + ret |= + rclc_parameter_initialize_empty_string( + ¶meter_server->parameter_descriptors.data[i].description, 1); + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->parameter_descriptors.data[i].additional_constraints, 1); + + parameter_server->parameter_descriptors.data[i].floating_point_range.data = + allocator.zero_allocate( + 1, sizeof(rcl_interfaces__msg__FloatingPointRange__Sequence), + allocator.state); + parameter_server->parameter_descriptors.data[i].floating_point_range.capacity = 1; + parameter_server->parameter_descriptors.data[i].floating_point_range.size = 0; + + parameter_server->parameter_descriptors.data[i].integer_range.data = allocator.zero_allocate( + 1, + sizeof( + rcl_interfaces__msg__IntegerRange__Sequence), allocator.state); + parameter_server->parameter_descriptors.data[i].integer_range.capacity = 1; + parameter_server->parameter_descriptors.data[i].integer_range.size = 0; + } + + // Parameter descriptors + + // Initialize empty string value + + // List parameters: + // - The request has no prefixes enabled nor depth. + // - The response has a sequence of names taken from the names of each parameter + parameter_server->list_request.prefixes.data = NULL; + parameter_server->list_request.prefixes.size = 0; + parameter_server->list_request.prefixes.capacity = 0; + + parameter_server->list_response.result.names.data = + allocator.allocate(sizeof(rosidl_runtime_c__String) * options->max_params, allocator.state); + parameter_server->list_response.result.names.size = 0; + parameter_server->list_response.result.names.capacity = options->max_params; + for (size_t i = 0; i < options->max_params; ++i) { + parameter_server->list_response.result.names.data[i].data = + parameter_server->parameter_list.data[i].name.data; + parameter_server->list_response.result.names.data[i].capacity = + parameter_server->parameter_list.data[i].name.capacity; + parameter_server->list_response.result.names.data[i].size = + parameter_server->parameter_list.data[i].name.size; + } + + parameter_server->list_response.result.prefixes.data = NULL; + parameter_server->list_response.result.prefixes.size = 0; + parameter_server->list_response.result.prefixes.capacity = 0; + + // Get parameters: + // - Only one parameter can be retrieved per request + parameter_server->get_request.names.data = allocator.allocate( + sizeof(rosidl_runtime_c__String), + allocator.state); + parameter_server->get_request.names.size = 0; + parameter_server->get_request.names.capacity = 1; + + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->get_request.names.data[0], + RCLC_PARAMETER_MAX_STRING_LENGTH); + + parameter_server->get_response.values.data = allocator.zero_allocate( + 1, sizeof(ParameterValue), + allocator.state); + parameter_server->get_response.values.size = 0; + parameter_server->get_response.values.capacity = 1; + + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->get_response.values.data[0].string_value, 1); + + // Set parameters: + // - Only one parameter can be set, created or deleted per request + // TODO(acuadros95): Check if alloc ParameterValue string_value + parameter_server->set_request.parameters.data = allocator.zero_allocate( + 1, sizeof(Parameter), + allocator.state); + parameter_server->set_request.parameters.size = 0; + parameter_server->set_request.parameters.capacity = 1; + + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->set_request.parameters.data[0].name, + RCLC_PARAMETER_MAX_STRING_LENGTH); + + parameter_server->set_response.results.data = + allocator.zero_allocate(1, sizeof(SetParameters_Result), allocator.state); + parameter_server->set_response.results.size = 0; + parameter_server->set_response.results.capacity = 1; + + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->set_response.results.data[0].reason, + RCLC_SET_ERROR_MAX_STRING_LENGTH); + + // Get parameter types: + // - Only one parameter type can be retrieved per request + parameter_server->get_types_request.names.data = + allocator.allocate(sizeof(rosidl_runtime_c__String), allocator.state); + parameter_server->get_types_request.names.size = 0; + parameter_server->get_types_request.names.capacity = 1; + + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->get_types_request.names.data[0], + RCLC_PARAMETER_MAX_STRING_LENGTH); + + parameter_server->get_types_response.types.data = allocator.zero_allocate( + 1, sizeof(uint8_t), + allocator.state); + parameter_server->get_types_response.types.size = 0; + parameter_server->get_types_response.types.capacity = 1; + + // Describe parameters: + // - Only one description can be retrieved per request + parameter_server->describe_request.names.data = allocator.allocate( + sizeof(rosidl_runtime_c__String), allocator.state); + parameter_server->describe_request.names.size = 0; + parameter_server->describe_request.names.capacity = 1; + + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->describe_request.names.data[0], + RCLC_PARAMETER_MAX_STRING_LENGTH); + + parameter_server->describe_response.descriptors.data = + allocator.zero_allocate(1, sizeof(ParameterDescriptor), allocator.state); + parameter_server->describe_response.descriptors.size = 0; + parameter_server->describe_response.descriptors.capacity = 1; + + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->describe_response.descriptors.data[0].description, + RCLC_PARAMETER_MAX_STRING_LENGTH); + ret |= rclc_parameter_initialize_empty_string( + ¶meter_server->describe_response.descriptors.data[0].additional_constraints, 1); + + parameter_server->describe_response.descriptors.data[0].floating_point_range.data = + allocator.zero_allocate( + 1, + sizeof(rcl_interfaces__msg__FloatingPointRange__Sequence), + allocator.state); + parameter_server->describe_response.descriptors.data[0].floating_point_range.capacity = 1; + parameter_server->describe_response.descriptors.data[0].floating_point_range.size = 0; + + parameter_server->describe_response.descriptors.data[0].integer_range.data = + allocator.zero_allocate( + 1, + sizeof( + rcl_interfaces__msg__IntegerRange__Sequence), allocator.state); + parameter_server->describe_response.descriptors.data[0].integer_range.capacity = 1; + parameter_server->describe_response.descriptors.data[0].integer_range.size = 0; + + // Parameter events msg + if (parameter_server->notify_changed_over_dds) { + // Parameter node info + parameter_server->event_list.node.data = (char *) rcl_node_get_name(node); + parameter_server->event_list.node.size = strlen(rcl_node_get_name(node)); + parameter_server->event_list.node.capacity = parameter_server->event_list.node.size + 1; + + // Parameters event + parameter_server->event_list.new_parameters.size = 0; + parameter_server->event_list.new_parameters.capacity = 1; + parameter_server->event_list.changed_parameters.size = 0; + parameter_server->event_list.changed_parameters.capacity = 1; + parameter_server->event_list.deleted_parameters.size = 0; + parameter_server->event_list.deleted_parameters.capacity = 1; + } + + return ret; +} + rcl_ret_t -rclc_parameter_server_fini( +rclc_parameter_server_init_with_option( rclc_parameter_server_t * parameter_server, - rcl_node_t * node) + rcl_node_t * node, + const rclc_parameter_options_t * options) { RCL_CHECK_FOR_NULL_WITH_MSG( parameter_server, "parameter is a null pointer", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + options, "options is a null pointer", return RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; - ret |= rcl_service_fini(¶meter_server->list_service, node); - ret |= rcl_service_fini(¶meter_server->set_service, node); - ret |= rcl_service_fini(¶meter_server->get_service, node); - ret |= rcl_service_fini(¶meter_server->get_types_service, node); - ret |= rcl_service_fini(¶meter_server->describe_service, node); + const rosidl_service_type_support_t * get_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + rcl_interfaces, + srv, + GetParameters); + ret |= rclc_parameter_server_init_service( + ¶meter_server->get_service, node, "/get_parameters", + get_ts); + + const rosidl_service_type_support_t * get_types_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + rcl_interfaces, + srv, + GetParameterTypes); + ret |= rclc_parameter_server_init_service( + ¶meter_server->get_types_service, node, + "/get_parameter_types", get_types_ts); + + const rosidl_service_type_support_t * set_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + rcl_interfaces, + srv, + SetParameters); + ret |= rclc_parameter_server_init_service( + ¶meter_server->set_service, node, "/set_parameters", + set_ts); + + const rosidl_service_type_support_t * list_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + rcl_interfaces, + srv, + ListParameters); + ret |= rclc_parameter_server_init_service( + ¶meter_server->list_service, node, + "/list_parameters", list_ts); + + const rosidl_service_type_support_t * describe_ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + rcl_interfaces, + srv, + DescribeParameters); + ret |= rclc_parameter_server_init_service( + ¶meter_server->describe_service, node, + "/describe_parameters", describe_ts); + + parameter_server->on_callback = false; + parameter_server->notify_changed_over_dds = options->notify_changed_over_dds; + parameter_server->allow_undeclared_parameters = options->allow_undeclared_parameters; + parameter_server->low_mem_mode = options->low_mem_mode; if (parameter_server->notify_changed_over_dds) { - ret |= rcl_publisher_fini(¶meter_server->event_publisher, node); + const rosidl_message_type_support_t * event_ts = ROSIDL_GET_MSG_TYPE_SUPPORT( + rcl_interfaces, + msg, + ParameterEvent); + ret |= rclc_publisher_init( + ¶meter_server->event_publisher, node, event_ts, + "/parameter_events", + &rmw_qos_profile_parameter_events); + } + + if (parameter_server->low_mem_mode) { + ret |= init_parameter_server_memory_low(parameter_server, node, options); + } else { + ret |= init_parameter_server_memory(parameter_server, node, options); } - rosidl_runtime_c__String__fini(¶meter_server->event_list.node); + return ret; +} + +void +rclc_parameter_server_fini_memory_low( + rclc_parameter_server_t * parameter_server) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return ); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + // Get request + allocator.deallocate(parameter_server->get_request.names.data[0].data, allocator.state); + allocator.deallocate(parameter_server->get_request.names.data, allocator.state); + parameter_server->get_request.names.capacity = 0; + parameter_server->get_request.names.size = 0; + + // Get response + allocator.deallocate( + parameter_server->get_response.values.data[0].string_value.data, + allocator.state); + allocator.deallocate(parameter_server->get_response.values.data, allocator.state); + parameter_server->get_response.values.capacity = 0; + parameter_server->get_response.values.size = 0; - for (size_t i = 0; i < parameter_server->describe_request.names.capacity; i++) { + // Set request + allocator.deallocate(parameter_server->set_request.parameters.data[0].name.data, allocator.state); + allocator.deallocate(parameter_server->set_request.parameters.data, allocator.state); + parameter_server->set_request.parameters.capacity = 0; + parameter_server->set_request.parameters.size = 0; + + // Set response + allocator.deallocate(parameter_server->set_response.results.data[0].reason.data, allocator.state); + allocator.deallocate(parameter_server->set_response.results.data, allocator.state); + parameter_server->set_response.results.capacity = 0; + parameter_server->set_response.results.size = 0; + + // List response + for (size_t i = 0; i < parameter_server->list_response.result.names.capacity; ++i) { + parameter_server->list_response.result.names.data[i].data = NULL; + parameter_server->list_response.result.names.data[i].capacity = 0; + parameter_server->list_response.result.names.data[i].size = 0; + } + allocator.deallocate(parameter_server->list_response.result.names.data, allocator.state); + + // Get types request + allocator.deallocate(parameter_server->get_types_request.names.data[0].data, allocator.state); + allocator.deallocate(parameter_server->get_types_request.names.data, allocator.state); + parameter_server->get_types_request.names.capacity = 0; + parameter_server->get_types_request.names.size = 0; + + // Get types response + allocator.deallocate(parameter_server->get_types_response.types.data, allocator.state); + parameter_server->get_types_response.types.capacity = 0; + parameter_server->get_types_response.types.size = 0; + + // Describe parameters request + allocator.deallocate(parameter_server->describe_request.names.data[0].data, allocator.state); + allocator.deallocate(parameter_server->describe_request.names.data, allocator.state); + parameter_server->describe_request.names.size = 0; + parameter_server->describe_request.names.capacity = 0; + + // Describe parameters response + allocator.deallocate( + parameter_server->describe_response.descriptors.data[0].floating_point_range.data, + allocator.state); + allocator.deallocate( + parameter_server->describe_response.descriptors.data[0].integer_range.data, + allocator.state); + allocator.deallocate( + parameter_server->describe_response.descriptors.data[0].description.data, + allocator.state); + allocator.deallocate( + parameter_server->describe_response.descriptors.data[0].additional_constraints.data, + allocator.state); + allocator.deallocate(parameter_server->describe_response.descriptors.data, allocator.state); + parameter_server->describe_response.descriptors.size = 0; + parameter_server->describe_response.descriptors.capacity = 0; + + // Parameter list and parameter descriptors + for (size_t i = 0; i < parameter_server->parameter_list.capacity; ++i) { + allocator.deallocate(parameter_server->parameter_list.data[i].name.data, allocator.state); + allocator.deallocate( + parameter_server->parameter_list.data[i].value.string_value.data, + allocator.state); + allocator.deallocate( + parameter_server->parameter_descriptors.data[i].description.data, + allocator.state); + allocator.deallocate( + parameter_server->parameter_descriptors.data[i].additional_constraints.data, allocator.state); + allocator.deallocate( + parameter_server->parameter_descriptors.data[i].floating_point_range.data, + allocator.state); + allocator.deallocate( + parameter_server->parameter_descriptors.data[i].integer_range.data, + allocator.state); + } + + allocator.deallocate(parameter_server->parameter_list.data, allocator.state); + parameter_server->parameter_list.capacity = 0; + parameter_server->parameter_list.size = 0; + + allocator.deallocate(parameter_server->parameter_descriptors.data, allocator.state); + parameter_server->parameter_descriptors.size = 0; + parameter_server->parameter_descriptors.capacity = 0; + + + if (parameter_server->notify_changed_over_dds) { + parameter_server->event_list.node.data = NULL; + parameter_server->event_list.node.capacity = 0; + parameter_server->event_list.node.size = 0; + } +} + +void +rclc_parameter_server_fini_memory( + rclc_parameter_server_t * parameter_server) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return ); + + // Fini describe msgs + for (size_t i = 0; i < parameter_server->describe_request.names.capacity; ++i) { rosidl_runtime_c__String__fini(¶meter_server->describe_request.names.data[i]); rosidl_runtime_c__String__fini(¶meter_server->describe_response.descriptors.data[i].name); + rosidl_runtime_c__String__fini( + ¶meter_server->describe_response.descriptors.data[i].description); + rosidl_runtime_c__String__fini( + ¶meter_server->describe_response.descriptors.data[i].additional_constraints); + rcl_interfaces__msg__IntegerRange__Sequence__fini( + ¶meter_server->describe_response.descriptors.data[i].integer_range); + rcl_interfaces__msg__FloatingPointRange__Sequence__fini( + ¶meter_server->describe_response.descriptors.data[i].floating_point_range); } rcl_interfaces__msg__ParameterDescriptor__Sequence__fini( @@ -465,7 +944,8 @@ rclc_parameter_server_fini( rcl_interfaces__srv__DescribeParameters_Response__fini(¶meter_server->describe_response); rcl_interfaces__srv__DescribeParameters_Request__fini(¶meter_server->describe_request); - for (size_t i = 0; i < parameter_server->get_types_request.names.capacity; i++) { + // Fini get types msgs + for (size_t i = 0; i < parameter_server->get_types_request.names.capacity; ++i) { rosidl_runtime_c__String__fini(¶meter_server->get_types_request.names.data[i]); } @@ -474,7 +954,8 @@ rclc_parameter_server_fini( rcl_interfaces__srv__GetParameterTypes_Response__fini(¶meter_server->get_types_response); rcl_interfaces__srv__GetParameterTypes_Request__fini(¶meter_server->get_types_request); - for (size_t i = 0; i < parameter_server->set_request.parameters.capacity; i++) { + // Finish set msgs + for (size_t i = 0; i < parameter_server->set_request.parameters.capacity; ++i) { rosidl_runtime_c__String__fini(¶meter_server->set_request.parameters.data[i].name); rosidl_runtime_c__String__fini(¶meter_server->set_response.results.data[i].reason); } @@ -484,7 +965,8 @@ rclc_parameter_server_fini( rcl_interfaces__srv__SetParameters_Response__fini(¶meter_server->set_response); rcl_interfaces__srv__SetParameters_Request__fini(¶meter_server->set_request); - for (size_t i = 0; i < parameter_server->get_request.names.capacity; i++) { + // Finish get msgs + for (size_t i = 0; i < parameter_server->get_request.names.capacity; ++i) { rosidl_runtime_c__String__fini(¶meter_server->get_request.names.data[i]); } @@ -493,7 +975,8 @@ rclc_parameter_server_fini( rcl_interfaces__srv__GetParameters_Response__fini(¶meter_server->get_response); rcl_interfaces__srv__GetParameters_Request__fini(¶meter_server->get_request); - for (size_t i = 0; i < parameter_server->list_response.result.names.capacity; i++) { + // Finish list msgs + for (size_t i = 0; i < parameter_server->list_response.result.names.capacity; ++i) { rosidl_runtime_c__String__fini(¶meter_server->list_response.result.names.data[i]); } @@ -501,12 +984,60 @@ rclc_parameter_server_fini( rcl_interfaces__srv__ListParameters_Response__fini(¶meter_server->list_response); rcl_interfaces__srv__ListParameters_Request__fini(¶meter_server->list_request); - for (size_t i = 0; i < parameter_server->parameter_list.capacity; i++) { + // Free parameter list + for (size_t i = 0; i < parameter_server->parameter_list.capacity; ++i) { rosidl_runtime_c__String__fini(¶meter_server->parameter_list.data[i].name); } rcl_interfaces__msg__Parameter__Sequence__fini(¶meter_server->parameter_list); + // Free parameter descriptor list + for (size_t i = 0; i < parameter_server->parameter_descriptors.capacity; ++i) { + rosidl_runtime_c__String__fini(¶meter_server->parameter_descriptors.data[i].name); + rosidl_runtime_c__String__fini(¶meter_server->parameter_descriptors.data[i].description); + rosidl_runtime_c__String__fini( + ¶meter_server->parameter_descriptors.data[i].additional_constraints); + rcl_interfaces__msg__IntegerRange__Sequence__fini( + ¶meter_server->parameter_descriptors.data[i].integer_range); + rcl_interfaces__msg__FloatingPointRange__Sequence__fini( + ¶meter_server->parameter_descriptors.data[i].floating_point_range); + } + + if (parameter_server->notify_changed_over_dds) { + rosidl_runtime_c__String__fini(¶meter_server->event_list.node); + } + + rcl_interfaces__msg__ParameterDescriptor__Sequence__fini( + ¶meter_server->parameter_descriptors); +} + +rcl_ret_t +rclc_parameter_server_fini( + rclc_parameter_server_t * parameter_server, + rcl_node_t * node) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = RCL_RET_OK; + + ret |= rcl_service_fini(¶meter_server->list_service, node); + ret |= rcl_service_fini(¶meter_server->set_service, node); + ret |= rcl_service_fini(¶meter_server->get_service, node); + ret |= rcl_service_fini(¶meter_server->get_types_service, node); + ret |= rcl_service_fini(¶meter_server->describe_service, node); + + if (parameter_server->notify_changed_over_dds) { + ret |= rcl_publisher_fini(¶meter_server->event_publisher, node); + } + + if (parameter_server->low_mem_mode) { + rclc_parameter_server_fini_memory_low(parameter_server); + } else { + rclc_parameter_server_fini_memory(parameter_server); + } return ret; } @@ -514,7 +1045,19 @@ rcl_ret_t rclc_executor_add_parameter_server( rclc_executor_t * executor, rclc_parameter_server_t * parameter_server, - ModifiedParameter_Callback on_modification) + rclc_parameter_callback_t on_modification) +{ + return rclc_executor_add_parameter_server_with_context( + executor, parameter_server, + on_modification, NULL); +} + +rcl_ret_t +rclc_executor_add_parameter_server_with_context( + rclc_executor_t * executor, + rclc_parameter_server_t * parameter_server, + rclc_parameter_callback_t on_modification, + void * context) { RCL_CHECK_FOR_NULL_WITH_MSG( executor, "executor is a null pointer", return RCL_RET_INVALID_ARGUMENT); @@ -524,6 +1067,7 @@ rclc_executor_add_parameter_server( rcl_ret_t ret; parameter_server->on_modification = on_modification; + parameter_server->context = context; ret = rclc_executor_add_service_with_context( executor, ¶meter_server->list_service, @@ -567,6 +1111,10 @@ rclc_add_parameter( RCL_CHECK_FOR_NULL_WITH_MSG( parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + size_t index = parameter_server->parameter_list.size; if (index >= parameter_server->parameter_list.capacity || @@ -583,19 +1131,140 @@ rclc_add_parameter( } parameter_server->parameter_list.data[index].value.type = type; - parameter_server->parameter_list.size++; + ++parameter_server->parameter_list.size; + + // Add to parameter descriptors + if (!parameter_server->low_mem_mode && !rclc_parameter_set_string( + ¶meter_server->parameter_descriptors.data[index].name, + parameter_name)) + { + return RCL_RET_ERROR; + } + + parameter_server->parameter_descriptors.data[index].type = type; + ++parameter_server->parameter_descriptors.size; if (parameter_server->notify_changed_over_dds) { - rclc_parameter_prepare_parameter_event( + rclc_parameter_prepare_new_event( ¶meter_server->event_list, + ¶meter_server->parameter_list.data[index]); + return rclc_parameter_service_publish_event(parameter_server); + } + + return RCL_RET_OK; +} + +rcl_ret_t +rclc_add_parameter_undeclared( + rclc_parameter_server_t * parameter_server, + Parameter * parameter) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter, "parameter is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + size_t index = parameter_server->parameter_list.size; + + if (index >= parameter_server->parameter_list.capacity || + rclc_parameter_search(¶meter_server->parameter_list, parameter->name.data) != NULL) + { + return RCL_RET_ERROR; + } + + if (RCL_RET_OK != rclc_parameter_copy( ¶meter_server->parameter_list.data[index], - true); + parameter)) + { + return RCL_RET_ERROR; + } + + ++parameter_server->parameter_list.size; + + // Add to parameter descriptors + if (!parameter_server->low_mem_mode && !rclc_parameter_set_string( + ¶meter_server->parameter_descriptors.data[index].name, + parameter->name.data)) + { + return RCL_RET_ERROR; + } + + parameter_server->parameter_descriptors.data[index].type = + parameter->value.type; + ++parameter_server->parameter_descriptors.size; + + if (parameter_server->notify_changed_over_dds) { + rclc_parameter_prepare_new_event( + ¶meter_server->event_list, + ¶meter_server->parameter_list.data[index]); return rclc_parameter_service_publish_event(parameter_server); } return RCL_RET_OK; } +rcl_ret_t +rclc_delete_parameter( + rclc_parameter_server_t * parameter_server, + const char * parameter_name) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + + // Find parameter + size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); + + if (index >= parameter_server->parameter_list.size) { + return RCL_RET_ERROR; + } + + + Parameter * param = ¶meter_server->parameter_list.data[index]; + ParameterDescriptor * param_description = ¶meter_server->parameter_descriptors.data[index]; + + if (parameter_server->notify_changed_over_dds) { + rclc_parameter_prepare_deleted_event(¶meter_server->event_list, param); + rclc_parameter_service_publish_event(parameter_server); + } + + // Reset parameter + rclc_parameter_set_string(¶m->name, ""); + param->value.type = RCLC_PARAMETER_NOT_SET; + + // Reset parameter description + param_description->type = RCLC_PARAMETER_NOT_SET; + param_description->floating_point_range.size = 0; + param_description->integer_range.size = 0; + if (!parameter_server->low_mem_mode) { + rclc_parameter_set_string(¶m_description->description, ""); + rclc_parameter_set_string(¶m_description->additional_constraints, ""); + } + + for (size_t i = index; i < (parameter_server->parameter_list.size - 1); ++i) { + // Move parameter list + rclc_parameter_copy( + ¶meter_server->parameter_list.data[i], + ¶meter_server->parameter_list.data[i + 1]); + + // Move descriptors + rclc_parameter_descriptor_copy( + ¶meter_server->parameter_descriptors.data[i], + ¶meter_server->parameter_descriptors.data[i + 1], + parameter_server->low_mem_mode); + } + + parameter_server->parameter_descriptors.size--; + parameter_server->parameter_list.size--; + + return RCL_RET_OK; +} + rcl_ret_t rclc_parameter_set_bool( rclc_parameter_server_t * parameter_server, @@ -607,6 +1276,10 @@ rclc_parameter_set_bool( RCL_CHECK_FOR_NULL_WITH_MSG( parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + Parameter * parameter = rclc_parameter_search(¶meter_server->parameter_list, parameter_name); @@ -615,19 +1288,24 @@ rclc_parameter_set_bool( } if (parameter->value.type != RCLC_PARAMETER_BOOL) { - return RCL_RET_INVALID_ARGUMENT; + return RCLC_PARAMETER_TYPE_MISMATCH; } - parameter->value.bool_value = value; + Parameter new_parameter = *parameter; + new_parameter.value.bool_value = value; + + if (RCL_RET_OK != + rclc_parameter_execute_callback(parameter_server, parameter, &new_parameter)) + { + return RCLC_PARAMETER_MODIFICATION_REJECTED; + } if (parameter_server->notify_changed_over_dds) { - rclc_parameter_prepare_parameter_event(¶meter_server->event_list, parameter, false); + rclc_parameter_prepare_changed_event(¶meter_server->event_list, parameter); rclc_parameter_service_publish_event(parameter_server); } - if (parameter_server->on_modification) { - parameter_server->on_modification(parameter); - } + parameter->value.bool_value = value; return RCL_RET_OK; } @@ -643,6 +1321,10 @@ rclc_parameter_set_int( RCL_CHECK_FOR_NULL_WITH_MSG( parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + Parameter * parameter = rclc_parameter_search(¶meter_server->parameter_list, parameter_name); @@ -651,19 +1333,24 @@ rclc_parameter_set_int( } if (parameter->value.type != RCLC_PARAMETER_INT) { - return RCL_RET_INVALID_ARGUMENT; + return RCLC_PARAMETER_TYPE_MISMATCH; } - parameter->value.integer_value = value; + Parameter new_parameter = *parameter; + new_parameter.value.integer_value = value; + + if (RCL_RET_OK != + rclc_parameter_execute_callback(parameter_server, parameter, &new_parameter)) + { + return RCLC_PARAMETER_MODIFICATION_REJECTED; + } if (parameter_server->notify_changed_over_dds) { - rclc_parameter_prepare_parameter_event(¶meter_server->event_list, parameter, false); + rclc_parameter_prepare_changed_event(¶meter_server->event_list, parameter); rclc_parameter_service_publish_event(parameter_server); } - if (parameter_server->on_modification) { - parameter_server->on_modification(parameter); - } + parameter->value.integer_value = value; return RCL_RET_OK; } @@ -679,6 +1366,10 @@ rclc_parameter_set_double( RCL_CHECK_FOR_NULL_WITH_MSG( parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + Parameter * parameter = rclc_parameter_search(¶meter_server->parameter_list, parameter_name); @@ -687,19 +1378,24 @@ rclc_parameter_set_double( } if (parameter->value.type != RCLC_PARAMETER_DOUBLE) { - return RCL_RET_INVALID_ARGUMENT; + return RCLC_PARAMETER_TYPE_MISMATCH; } - parameter->value.double_value = value; + Parameter new_parameter = *parameter; + new_parameter.value.double_value = value; + + if (RCL_RET_OK != + rclc_parameter_execute_callback(parameter_server, parameter, &new_parameter)) + { + return RCLC_PARAMETER_MODIFICATION_REJECTED; + } if (parameter_server->notify_changed_over_dds) { - rclc_parameter_prepare_parameter_event(¶meter_server->event_list, parameter, false); + rclc_parameter_prepare_changed_event(¶meter_server->event_list, parameter); rclc_parameter_service_publish_event(parameter_server); } - if (parameter_server->on_modification) { - parameter_server->on_modification(parameter); - } + parameter->value.double_value = value; return RCL_RET_OK; } @@ -795,6 +1491,9 @@ rcl_ret_t rclc_parameter_service_publish_event( rclc_parameter_server_t * parameter_server) { + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + rcl_ret_t ret = RCL_RET_OK; rcutils_time_point_value_t now; @@ -818,6 +1517,15 @@ rclc_parameter_server_init_service( char * service_name, const rosidl_service_type_support_t * srv_type) { + RCL_CHECK_FOR_NULL_WITH_MSG( + service, "service is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + node, "node is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_name, "service_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + srv_type, "srv_type is a null pointer", return RCL_RET_INVALID_ARGUMENT); + const char * node_name = rcl_node_get_name(node); static char get_service_name[RCLC_PARAMETER_MAX_STRING_LENGTH]; @@ -827,6 +1535,168 @@ rclc_parameter_server_init_service( return rclc_service_init(service, node, srv_type, get_service_name, &rmw_qos_profile_parameters); } +rcl_ret_t rclc_add_parameter_description( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, + const char * parameter_description, + const char * additional_constraints) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_description, "parameter_description is a null pointer", + return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + additional_constraints, "additional_constraints is a null pointer", + return RCL_RET_INVALID_ARGUMENT); + + if (parameter_server->low_mem_mode) { + return RCLC_PARAMETER_UNSUPORTED_ON_LOW_MEM; + } + + size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); + + if (index >= parameter_server->parameter_list.size) { + return RCL_RET_ERROR; + } + + ParameterDescriptor * parameter_descriptor = ¶meter_server->parameter_descriptors.data[index]; + + // Set parameter description + if (!rclc_parameter_set_string(¶meter_descriptor->description, parameter_description)) { + return RCL_RET_ERROR; + } + + // Set constraint description + if (!rclc_parameter_set_string( + ¶meter_descriptor->additional_constraints, + additional_constraints)) + { + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t rclc_set_parameter_read_only( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, + bool read_only) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + + size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); + + if (index >= parameter_server->parameter_list.size) { + return RCL_RET_ERROR; + } + + ParameterDescriptor * parameter_descriptor = ¶meter_server->parameter_descriptors.data[index]; + + // Set flag + parameter_descriptor->read_only = read_only; + + return RCL_RET_OK; +} + +rcl_ret_t rclc_add_parameter_constraint_double( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, + double from_value, + double to_value, + double step) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + + size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); + + if (index >= parameter_server->parameter_list.size) { + return RCL_RET_ERROR; + } + + ParameterDescriptor * parameter_descriptor = ¶meter_server->parameter_descriptors.data[index]; + + if (parameter_descriptor->type != RCLC_PARAMETER_DOUBLE) { + return RCL_RET_ERROR; + } + + parameter_descriptor->floating_point_range.data->from_value = from_value; + parameter_descriptor->floating_point_range.data->to_value = to_value; + parameter_descriptor->floating_point_range.data->step = step; + parameter_descriptor->floating_point_range.size = 1; + + return RCL_RET_OK; +} + +rcl_ret_t rclc_add_parameter_constraint_integer( + rclc_parameter_server_t * parameter_server, + const char * parameter_name, int64_t from_value, + int64_t to_value, uint64_t step) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + if (parameter_server->on_callback) { + return RCLC_PARAMETER_DISABLED_ON_CALLBACK; + } + + size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); + + if (index >= parameter_server->parameter_list.size) { + return RCL_RET_ERROR; + } + + ParameterDescriptor * parameter_descriptor = ¶meter_server->parameter_descriptors.data[index]; + + if (parameter_descriptor->type != RCLC_PARAMETER_INT) { + return RCL_RET_ERROR; + } + + parameter_descriptor->integer_range.data->from_value = from_value; + parameter_descriptor->integer_range.data->to_value = to_value; + parameter_descriptor->integer_range.data->step = step; + parameter_descriptor->integer_range.size = 1; + + return RCL_RET_OK; +} + +rcl_ret_t rclc_parameter_execute_callback( + rclc_parameter_server_t * parameter_server, + const Parameter * old_param, + const Parameter * new_param) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter_server, "parameter_server is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + bool ret = true; + + if (parameter_server->on_modification) { + parameter_server->on_callback = true; + ret = parameter_server->on_modification(old_param, new_param, parameter_server->context); + parameter_server->on_callback = false; + } + + return ret ? RCL_RET_OK : RCLC_PARAMETER_MODIFICATION_REJECTED; +} + #if __cplusplus } #endif /* if __cplusplus */ diff --git a/rclc_parameter/src/rclc_parameter/parameter_utils.c b/rclc_parameter/src/rclc_parameter/parameter_utils.c index e3dc8c23..725ec1ab 100644 --- a/rclc_parameter/src/rclc_parameter/parameter_utils.c +++ b/rclc_parameter/src/rclc_parameter/parameter_utils.c @@ -62,12 +62,57 @@ rclc_parameter_copy( return rclc_parameter_value_copy(&dst->value, &src->value); } +rcl_ret_t +rclc_parameter_descriptor_copy( + ParameterDescriptor * dst, + const ParameterDescriptor * src, + bool low_mem) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(dst, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(src, RCL_RET_INVALID_ARGUMENT); + + if (!low_mem) { + if (!rclc_parameter_set_string(&dst->name, src->name.data)) { + return RCL_RET_ERROR; + } + + if (!rclc_parameter_set_string(&dst->description, src->description.data)) { + return RCL_RET_ERROR; + } + + if (!rclc_parameter_set_string( + &dst->additional_constraints, + src->additional_constraints.data)) + { + return RCL_RET_ERROR; + } + } + + dst->type = src->type; + dst->read_only = src->read_only; + + dst->floating_point_range.data[0].from_value = src->floating_point_range.data[0].from_value; + dst->floating_point_range.data[0].to_value = src->floating_point_range.data[0].to_value; + dst->floating_point_range.data[0].step = src->floating_point_range.data[0].step; + dst->floating_point_range.size = src->floating_point_range.size; + + dst->integer_range.data[0].from_value = src->integer_range.data[0].from_value; + dst->integer_range.data[0].to_value = src->integer_range.data[0].to_value; + dst->integer_range.data[0].step = src->integer_range.data[0].step; + dst->integer_range.size = src->integer_range.size; + + return RCL_RET_OK; +} + Parameter * rclc_parameter_search( Parameter__Sequence * parameter_list, const char * param_name) { - for (size_t i = 0; i < parameter_list->size; i++) { + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_list, NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(param_name, NULL); + + for (size_t i = 0; i < parameter_list->size; ++i) { if (!strcmp(param_name, parameter_list->data[i].name.data)) { return ¶meter_list->data[i]; } @@ -76,11 +121,33 @@ rclc_parameter_search( return NULL; } +size_t +rclc_parameter_search_index( + Parameter__Sequence * parameter_list, + const char * param_name) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_list, SIZE_MAX); + RCL_CHECK_ARGUMENT_FOR_NULL(param_name, SIZE_MAX); + + size_t index = SIZE_MAX; + for (size_t i = 0; i < parameter_list->size; ++i) { + if (!strcmp(param_name, parameter_list->data[i].name.data)) { + index = i; + break; + } + } + + return index; +} + bool rclc_parameter_set_string( rosidl_runtime_c__String * str, const char * value) { + RCL_CHECK_ARGUMENT_FOR_NULL(str, false); + RCL_CHECK_ARGUMENT_FOR_NULL(value, false); + if (str->capacity >= (strlen(value) + 1)) { memcpy(str->data, value, strlen(value) + 1); str->size = strlen(str->data); @@ -90,21 +157,104 @@ rclc_parameter_set_string( return false; } -void rclc_parameter_prepare_parameter_event( +rcl_ret_t rclc_parameter_prepare_new_event( ParameterEvent * event, - Parameter * parameter, - bool new) + Parameter * parameter) { + RCL_CHECK_FOR_NULL_WITH_MSG( + event, "event is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter, "parameter is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + rclc_parameter_reset_parameter_event(event); + event->new_parameters.data = parameter; + event->new_parameters.capacity = 1; + event->new_parameters.size = 1; + return RCL_RET_OK; +} + +rcl_ret_t rclc_parameter_prepare_changed_event( + ParameterEvent * event, + Parameter * parameter) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + event, "event is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter, "parameter is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + rclc_parameter_reset_parameter_event(event); + event->changed_parameters.data = parameter; + event->changed_parameters.capacity = 1; + event->changed_parameters.size = 1; + return RCL_RET_OK; +} + +rcl_ret_t rclc_parameter_prepare_deleted_event( + ParameterEvent * event, + Parameter * parameter) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + event, "event is a null pointer", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + parameter, "parameter is a null pointer", return RCL_RET_INVALID_ARGUMENT); + + rclc_parameter_reset_parameter_event(event); + event->deleted_parameters.data = parameter; + event->deleted_parameters.capacity = 1; + event->deleted_parameters.size = 1; + return RCL_RET_OK; +} + +rcl_ret_t rclc_parameter_reset_parameter_event( + ParameterEvent * event) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(event, RCL_RET_INVALID_ARGUMENT); + memset(&event->new_parameters, 0, sizeof(Parameter__Sequence)); memset(&event->changed_parameters, 0, sizeof(Parameter__Sequence)); + memset(&event->deleted_parameters, 0, sizeof(Parameter__Sequence)); + return RCL_RET_OK; +} + +rcl_ret_t rclc_parameter_initialize_empty_string( + rosidl_runtime_c__String * str, + size_t capacity) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(str, RCL_RET_INVALID_ARGUMENT); + + if (capacity < 1) { + return RCL_RET_INVALID_ARGUMENT; + } + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + str->data = allocator.allocate(sizeof(char) * capacity, allocator.state); + + if (str->data == NULL) { + return RCL_RET_ERROR; + } + + str->data[0] = '\0'; + str->capacity = capacity; + str->size = 0; + + return RCL_RET_OK; +} + +bool rclc_parameter_descriptor_initialize_string(rosidl_runtime_c__String * str) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(str, false); + + static char empty_string[RCLC_PARAMETER_MAX_STRING_LENGTH] = ""; + size_t string_capacity = RCLC_PARAMETER_MAX_STRING_LENGTH - 1; - Parameter__Sequence * seq = (new) ? - &event->new_parameters : - &event->changed_parameters; + bool ret = rosidl_runtime_c__String__assignn( + str, + (const char *) empty_string, + string_capacity); - seq->data = parameter; - seq->capacity = 1; - seq->size = 1; + str->size = 0; + return ret; } #if __cplusplus diff --git a/rclc_parameter/src/rclc_parameter/parameter_utils.h b/rclc_parameter/src/rclc_parameter/parameter_utils.h index 3f00adb6..df141f6c 100644 --- a/rclc_parameter/src/rclc_parameter/parameter_utils.h +++ b/rclc_parameter/src/rclc_parameter/parameter_utils.h @@ -36,19 +36,47 @@ rclc_parameter_copy( Parameter * dst, const Parameter * src); +rcl_ret_t +rclc_parameter_descriptor_copy( + ParameterDescriptor * dst, + const ParameterDescriptor * src, + bool low_mem); + Parameter * rclc_parameter_search( Parameter__Sequence * parameter_list, const char * param_name); +size_t +rclc_parameter_search_index( + Parameter__Sequence * parameter_list, + const char * param_name); + bool rclc_parameter_set_string( rosidl_runtime_c__String * str, const char * value); -void rclc_parameter_prepare_parameter_event( +rcl_ret_t rclc_parameter_prepare_new_event( + ParameterEvent * event, + Parameter * parameter); + +rcl_ret_t rclc_parameter_prepare_changed_event( ParameterEvent * event, - Parameter * parameter, - bool new); + Parameter * parameter); + +rcl_ret_t rclc_parameter_prepare_deleted_event( + ParameterEvent * event, + Parameter * parameter); + +rcl_ret_t rclc_parameter_reset_parameter_event( + ParameterEvent * event); + +rcl_ret_t rclc_parameter_initialize_empty_string( + rosidl_runtime_c__String * str, + size_t capacity); + +bool rclc_parameter_descriptor_initialize_string( + rosidl_runtime_c__String * str); #if __cplusplus } diff --git a/rclc_parameter/test/rclc_parameter/test_parameter.cpp b/rclc_parameter/test/rclc_parameter/test_parameter.cpp index 38f708b0..ebe30f5b 100644 --- a/rclc_parameter/test/rclc_parameter/test_parameter.cpp +++ b/rclc_parameter/test/rclc_parameter/test_parameter.cpp @@ -16,7 +16,9 @@ extern "C" { -#include +#include +#include +#include #include } @@ -26,234 +28,1008 @@ extern "C" #include -using namespace std::chrono_literals; - -// #include "parameter_client.hpp" - -static int callcack_calls = 0; -static rclc_parameter_type_t expected_type; -static union { - bool bool_value; - int64_t integer_value; - double double_value; -} expected_value; - -void on_parameter_changed(Parameter * param) -{ - callcack_calls++; - ASSERT_EQ(expected_type, param->value.type); - switch (param->value.type) { - case RCLC_PARAMETER_BOOL: - ASSERT_EQ(param->value.bool_value, expected_value.bool_value); - break; - case RCLC_PARAMETER_INT: - ASSERT_EQ(param->value.integer_value, expected_value.integer_value); - break; - case RCLC_PARAMETER_DOUBLE: - ASSERT_EQ(param->value.double_value, expected_value.double_value); - break; - default: - break; - } -} +#include "rclc_parameter/parameter_utils.h" -TEST(Test, rclc_node_init_default) { - std::string node_name("test_node"); - - // Create auxiliar RCLCPP node - rclcpp::init(0, NULL); - auto param_client_node = std::make_shared("param_aux_client"); - auto parameters_client = std::make_shared( - param_client_node, - node_name); +using namespace std::chrono_literals; +TEST(ParameterTestUnitary, rclc_parameter_server_init_default) { // Init RCLC support - rcl_ret_t rc; rclc_support_t support; rcl_allocator_t allocator = rcl_get_default_allocator(); - rc = rclc_support_init(&support, 0, nullptr, &allocator); - EXPECT_EQ(RCL_RET_OK, rc); + ASSERT_EQ(rclc_support_init(&support, 0, nullptr, &allocator), RCL_RET_OK); // Init node rcl_node_t node; - rc = rclc_node_init_default(&node, node_name.c_str(), "", &support); - EXPECT_EQ(RCL_RET_OK, rc); + ASSERT_EQ(rclc_node_init_default(&node, "test_node", "", &support), RCL_RET_OK); // Init parameter server rclc_parameter_server_t param_server; - rc = rclc_parameter_server_init_default(¶m_server, &node); - EXPECT_EQ(RCL_RET_OK, rc); + ASSERT_EQ(rclc_parameter_server_init_default(¶m_server, &node), RCL_RET_OK); // Test with wrong arguments - rc = rclc_parameter_server_init_default(NULL, &node); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rc); + ASSERT_EQ(rclc_parameter_server_init_default(NULL, &node), RCL_RET_INVALID_ARGUMENT); + rcutils_reset_error(); - rc = rclc_parameter_server_init_default(¶m_server, NULL); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rc); + ASSERT_EQ(rclc_parameter_server_init_default(¶m_server, NULL), RCL_RET_INVALID_ARGUMENT); + rcutils_reset_error(); // Add parameter to executor rclc_executor_t executor; rclc_executor_init( - &executor, &support.context, RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER, + &executor, &support.context, RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES, &allocator); - rc = rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed); - EXPECT_EQ(RCL_RET_OK, rc); + ASSERT_EQ(rclc_executor_add_parameter_server(&executor, ¶m_server, nullptr), RCL_RET_OK); + + // Destroy parameter server + ASSERT_EQ(rclc_parameter_server_fini(¶m_server, &node), RCL_RET_OK); + ASSERT_EQ(rclc_executor_fini(&executor), RCL_RET_OK); + ASSERT_EQ(rcl_node_fini(&node), RCL_RET_OK); +} + +TEST(ParameterTestUnitary, rclc_add_parameter) { + // Init RCLC support + rclc_support_t support; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ASSERT_EQ(rclc_support_init(&support, 0, nullptr, &allocator), RCL_RET_OK); + + // Init node + rcl_node_t node; + ASSERT_EQ(rclc_node_init_default(&node, "test_node", "", &support), RCL_RET_OK); + + // Init parameter server + rclc_parameter_server_t param_server; + ASSERT_EQ(rclc_parameter_server_init_default(¶m_server, &node), RCL_RET_OK); // Test add parameter - std::vector param_names; - rc = rclc_add_parameter(¶m_server, "param1", RCLC_PARAMETER_BOOL); - param_names.push_back("param1"); - EXPECT_EQ(RCL_RET_OK, rc); + ASSERT_EQ(rclc_add_parameter(¶m_server, "param1", RCLC_PARAMETER_BOOL), RCL_RET_OK); // Fail on duplicated name - rc = rclc_add_parameter(¶m_server, "param1", RCLC_PARAMETER_DOUBLE); - EXPECT_EQ(RCL_RET_ERROR, rc); + ASSERT_EQ(rclc_add_parameter(¶m_server, "param1", RCLC_PARAMETER_DOUBLE), RCL_RET_ERROR); // Fail on name length char overflow_name[RCLC_PARAMETER_MAX_STRING_LENGTH + 1]; memset(overflow_name, ' ', RCLC_PARAMETER_MAX_STRING_LENGTH + 1); overflow_name[RCLC_PARAMETER_MAX_STRING_LENGTH] = '\0'; - rc = rclc_add_parameter(¶m_server, overflow_name, RCLC_PARAMETER_BOOL); - EXPECT_EQ(RCL_RET_ERROR, rc); + ASSERT_EQ(rclc_add_parameter(¶m_server, overflow_name, RCLC_PARAMETER_BOOL), RCL_RET_ERROR); // Add more parameters - rc = rclc_add_parameter(¶m_server, "param2", RCLC_PARAMETER_INT); - param_names.push_back("param2"); - EXPECT_EQ(RCL_RET_OK, rc); + ASSERT_EQ(rclc_add_parameter(¶m_server, "param2", RCLC_PARAMETER_INT), RCL_RET_OK); + ASSERT_EQ(rclc_add_parameter(¶m_server, "param3", RCLC_PARAMETER_DOUBLE), RCL_RET_OK); + ASSERT_EQ(rclc_add_parameter(¶m_server, "param4", RCLC_PARAMETER_DOUBLE), RCL_RET_OK); - rc = rclc_add_parameter(¶m_server, "param3", RCLC_PARAMETER_DOUBLE); - param_names.push_back("param3"); - EXPECT_EQ(RCL_RET_OK, rc); + // Fail on too much params + ASSERT_EQ(rclc_add_parameter(¶m_server, "param5", RCLC_PARAMETER_DOUBLE), RCL_RET_ERROR); - rc = rclc_add_parameter(¶m_server, "param4", RCLC_PARAMETER_DOUBLE); - param_names.push_back("param4"); - EXPECT_EQ(RCL_RET_OK, rc); + // Destroy parameter server + ASSERT_EQ(rclc_parameter_server_fini(¶m_server, &node), RCL_RET_OK); + ASSERT_EQ(rcl_node_fini(&node), RCL_RET_OK); +} - // Fail on too much params - rc = rclc_add_parameter(¶m_server, "param5", RCLC_PARAMETER_DOUBLE); - EXPECT_EQ(RCL_RET_ERROR, rc); +class ParameterTestBase : public ::testing::TestWithParam +{ +public: + ParameterTestBase() + : callback_calls(0), + default_spin_timeout(std::chrono::duration(10000)), + options(GetParam()) + {} - // Set parameters - expected_type = RCLC_PARAMETER_BOOL; - expected_value.bool_value = true; - rclc_parameter_set_bool(¶m_server, "param1", static_cast(expected_value.bool_value)); - - expected_type = RCLC_PARAMETER_INT; - expected_value.integer_value = 10; - rclc_parameter_set_int(¶m_server, "param2", static_cast(expected_value.integer_value)); - - expected_type = RCLC_PARAMETER_DOUBLE; - expected_value.double_value = 0.01; - rclc_parameter_set_double( - ¶m_server, "param3", - static_cast(expected_value.double_value)); - - // Get parameters - bool param1; - int64_t param2; - double param3; - - rclc_parameter_get_bool(¶m_server, "param1", ¶m1); - ASSERT_EQ(param1, true); - rclc_parameter_get_int(¶m_server, "param2", ¶m2); - ASSERT_EQ(param2, 10); - rclc_parameter_get_double(¶m_server, "param3", ¶m3); - ASSERT_EQ(param3, 0.01); - - ASSERT_EQ(callcack_calls, 3); - - // Spin RCLC parameter server in a thread - bool spin = true; - std::thread rclc_parameter_server_thread( - [&]() -> void { - while (spin) { - ASSERT_EQ(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)), RCL_RET_OK); + ~ParameterTestBase() {} + + void SetUp() override + { + std::string node_name("test_node"); + + // Init RCLC support + allocator = rcl_get_default_allocator(); + EXPECT_EQ(rclc_support_init(&support, 0, nullptr, &allocator), RCL_RET_OK); + + // Init node + EXPECT_EQ(rclc_node_init_default(&node, node_name.c_str(), "", &support), RCL_RET_OK); + + // Init parameter server + EXPECT_EQ(rclc_parameter_server_init_with_option(¶m_server, &node, &options), RCL_RET_OK); + + // Add parameter to executor + rclc_executor_init( + &executor, &support.context, RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES, + &allocator); + EXPECT_EQ( + rclc_executor_add_parameter_server_with_context( + &executor, ¶m_server, + ParameterTestBase::callback_dispatcher, + this), RCL_RET_OK); + + // Add initial parameters + EXPECT_EQ(rclc_add_parameter(¶m_server, "param1", RCLC_PARAMETER_BOOL), RCL_RET_OK); + EXPECT_EQ(rclc_add_parameter(¶m_server, "param2", RCLC_PARAMETER_INT), RCL_RET_OK); + EXPECT_EQ(rclc_add_parameter(¶m_server, "param3", RCLC_PARAMETER_DOUBLE), RCL_RET_OK); + + // Spin RCLC parameter server in a thread + spin = true; + rclc_parameter_server_thread = std::thread( + [&]() -> void { + while (spin) { + ASSERT_EQ(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)), RCL_RET_OK); + } } - } - ); + ); - // Use auxiliar RCLCPP node for check - auto list_params = parameters_client->list_parameters({}, 10); - ASSERT_EQ(list_params.names.size(), 4u); + // Init rclcpp node + rclcpp::init(0, NULL); + param_client_node = std::make_shared("param_aux_client"); + parameters_client = std::make_shared( + param_client_node, + node_name); + + ASSERT_TRUE(parameters_client->wait_for_service(default_spin_timeout)); + } + + void TearDown() override + { + spin = false; + rclc_parameter_server_thread.join(); + + parameters_client.reset(); + param_client_node.reset(); + rclcpp::shutdown(); + + // Destroy parameter server + ASSERT_EQ(rclc_parameter_server_fini(¶m_server, &node), RCL_RET_OK); + ASSERT_EQ(rclc_executor_fini(&executor), RCL_RET_OK); + ASSERT_EQ(rcl_node_fini(&node), RCL_RET_OK); + } + + static bool callback_dispatcher( + const Parameter * old_param, const Parameter * new_param, + void * context) + { + ParameterTestBase * obj = reinterpret_cast(context); + ++obj->callback_calls; + return obj->on_parameter_changed(old_param, new_param); + } + +protected: + // Callback + size_t callback_calls; + std::function on_parameter_changed; + + // Rclcpp + std::shared_ptr param_client_node; + std::shared_ptr parameters_client; + std::chrono::duration default_spin_timeout; + + // Rclc + rcl_allocator_t allocator; + rclc_support_t support; + rcl_node_t node; + rclc_executor_t executor; + rclc_parameter_options_t options; + rclc_parameter_server_t param_server; + std::thread rclc_parameter_server_thread; + bool spin; +}; + +TEST_P(ParameterTestBase, rclc_add_parameter_constraint) { + // Test with NULL arguments + ASSERT_EQ( + rclc_add_parameter_constraint_integer(NULL, "param2", 0, 0, 0), + RCL_RET_INVALID_ARGUMENT); + ASSERT_EQ( + rclc_add_parameter_constraint_integer( + ¶m_server, NULL, 0, 0, + 0), RCL_RET_INVALID_ARGUMENT); + ASSERT_EQ( + rclc_add_parameter_constraint_double( + NULL, "param3", 0.0, 0.0, + 0.0), RCL_RET_INVALID_ARGUMENT); + ASSERT_EQ( + rclc_add_parameter_constraint_double( + ¶m_server, NULL, 0.0, 0.0, + 0.0), RCL_RET_INVALID_ARGUMENT); + + // Test with invalid parameter name + ASSERT_EQ( + rclc_add_parameter_constraint_integer( + ¶m_server, "invalid_param", 0, 0, + 0), RCL_RET_ERROR); + ASSERT_EQ( + rclc_add_parameter_constraint_double( + ¶m_server, "invalid_param", 0.0, 0.0, + 0.0), RCL_RET_ERROR); + + // Test with invalid parameter type + ASSERT_EQ(rclc_add_parameter_constraint_integer(¶m_server, "param3", 0, 0, 0), RCL_RET_ERROR); + ASSERT_EQ( + rclc_add_parameter_constraint_double( + ¶m_server, "param2", 0.0, 0.0, + 0.0), RCL_RET_ERROR); + + // Test valid values + ASSERT_EQ(rclc_add_parameter_constraint_integer(¶m_server, "param2", 0, 10, 1), RCL_RET_OK); + ASSERT_EQ( + rclc_add_parameter_constraint_double( + ¶m_server, "param3", 0.0, 0.1, + 0.01), RCL_RET_OK); +} + +TEST_P(ParameterTestBase, rclc_add_parameter_description) { + if (options.low_mem_mode) { + ASSERT_EQ( + rclc_add_parameter_description( + ¶m_server, "param1", "parameter_description", + "additional_constraints"), RCLC_PARAMETER_UNSUPORTED_ON_LOW_MEM); + return; + } + + // Test with NULL arguments + ASSERT_EQ(rclc_add_parameter_description(NULL, "", "", ""), RCL_RET_INVALID_ARGUMENT); + ASSERT_EQ(rclc_add_parameter_description(¶m_server, NULL, "", ""), RCL_RET_INVALID_ARGUMENT); + ASSERT_EQ(rclc_add_parameter_description(¶m_server, "", NULL, ""), RCL_RET_INVALID_ARGUMENT); + ASSERT_EQ(rclc_add_parameter_description(¶m_server, "", "", NULL), RCL_RET_INVALID_ARGUMENT); + + // Test with invalid parameter name + ASSERT_EQ(rclc_add_parameter_description(¶m_server, "invalid_param", "", ""), RCL_RET_ERROR); + + // Fail on string length + char overflow_string[RCLC_PARAMETER_MAX_STRING_LENGTH + 1]; + memset(overflow_string, ' ', RCLC_PARAMETER_MAX_STRING_LENGTH + 1); + overflow_string[RCLC_PARAMETER_MAX_STRING_LENGTH] = '\0'; + + ASSERT_EQ( + rclc_add_parameter_description( + ¶m_server, "param1", overflow_string, + ""), RCL_RET_ERROR); + ASSERT_EQ( + rclc_add_parameter_description( + ¶m_server, "param1", "", + overflow_string), RCL_RET_ERROR); + + // Test valid values + ASSERT_EQ( + rclc_add_parameter_description( + ¶m_server, "param1", "parameter_description", + "additional_constraints"), RCL_RET_OK); +} + +TEST_P(ParameterTestBase, rclc_set_parameter_read_only) { + // Test with NULL arguments + ASSERT_EQ(rclc_set_parameter_read_only(NULL, "param1", true), RCL_RET_INVALID_ARGUMENT); + ASSERT_EQ(rclc_set_parameter_read_only(¶m_server, NULL, true), RCL_RET_INVALID_ARGUMENT); + + // Test with invalid parameter name + ASSERT_EQ(rclc_set_parameter_read_only(¶m_server, "invalid_param", true), RCL_RET_ERROR); + + // Test valid values + ASSERT_EQ(rclc_set_parameter_read_only(¶m_server, "param1", true), RCL_RET_OK); +} + +TEST_P(ParameterTestBase, rclc_set_get_parameter) { + size_t expected_callback_calls = 1; + + // Set parameters + { + const char * param_name = "param1"; + bool set_value = true; + bool get_value; + + // Get initial value + ASSERT_EQ(rclc_parameter_get_bool(¶m_server, param_name, &get_value), RCL_RET_OK); + ASSERT_EQ(get_value, false); + + // Set value + on_parameter_changed = [&](const Parameter * old_param, const Parameter * new_param) -> bool { + EXPECT_EQ(strcmp(old_param->name.data, param_name), 0); + EXPECT_EQ(strcmp(new_param->name.data, param_name), 0); + EXPECT_EQ(old_param->value.type, RCLC_PARAMETER_BOOL); + EXPECT_EQ(new_param->value.type, RCLC_PARAMETER_BOOL); + EXPECT_EQ(old_param->value.bool_value, get_value); + EXPECT_EQ(new_param->value.bool_value, set_value); + return true; + }; + + ASSERT_EQ(rclc_parameter_set_bool(¶m_server, param_name, set_value), RCL_RET_OK); + EXPECT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; + + // Get new value + ASSERT_EQ(rclc_parameter_get_bool(¶m_server, param_name, &get_value), RCL_RET_OK); + ASSERT_EQ(set_value, get_value); + } + + { + const char * param_name = "param2"; + int64_t set_value = 10; + int64_t get_value; + + // Get initial value + ASSERT_EQ(rclc_parameter_get_int(¶m_server, param_name, &get_value), RCL_RET_OK); + ASSERT_EQ(get_value, 0); + + // Set value + on_parameter_changed = [&](const Parameter * old_param, const Parameter * new_param) -> bool { + EXPECT_EQ(strcmp(old_param->name.data, param_name), 0); + EXPECT_EQ(strcmp(new_param->name.data, param_name), 0); + EXPECT_EQ(old_param->value.type, RCLC_PARAMETER_INT); + EXPECT_EQ(new_param->value.type, RCLC_PARAMETER_INT); + EXPECT_EQ(old_param->value.integer_value, get_value); + EXPECT_EQ(new_param->value.integer_value, set_value); + return true; + }; + + ASSERT_EQ(rclc_parameter_set_int(¶m_server, param_name, set_value), RCL_RET_OK); + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; + + // Get new value + ASSERT_EQ(rclc_parameter_get_int(¶m_server, param_name, &get_value), RCL_RET_OK); + ASSERT_EQ(set_value, get_value); + } + + { + const char * param_name = "param3"; + double set_value = 0.01; + double get_value; + + // Get initial value + ASSERT_EQ(rclc_parameter_get_double(¶m_server, param_name, &get_value), RCL_RET_OK); + ASSERT_EQ(get_value, 0.0); + + // Set value + on_parameter_changed = [&](const Parameter * old_param, const Parameter * new_param) -> bool { + EXPECT_EQ(strcmp(old_param->name.data, param_name), 0); + EXPECT_EQ(strcmp(new_param->name.data, param_name), 0); + EXPECT_EQ(old_param->value.type, RCLC_PARAMETER_DOUBLE); + EXPECT_EQ(new_param->value.type, RCLC_PARAMETER_DOUBLE); + EXPECT_EQ(old_param->value.double_value, get_value); + EXPECT_EQ(new_param->value.double_value, set_value); + return true; + }; + + ASSERT_EQ( + rclc_parameter_set_double( + ¶m_server, param_name, set_value), RCL_RET_OK); + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; + + // Get new value + ASSERT_EQ(rclc_parameter_get_double(¶m_server, param_name, &get_value), RCL_RET_OK); + ASSERT_EQ(set_value, get_value); + } + + // Fail with user reject + { + const char * param_name = "param3"; + double set_value = 0.05; + double first_get_value; + double second_get_value; + + // Get initial value + ASSERT_EQ(rclc_parameter_get_double(¶m_server, param_name, &first_get_value), RCL_RET_OK); + + // Set value + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + return false; + }; + + ASSERT_EQ( + rclc_parameter_set_double( + ¶m_server, param_name, + set_value), RCLC_PARAMETER_MODIFICATION_REJECTED); + ASSERT_EQ(callback_calls, expected_callback_calls); + + // Get value + ASSERT_EQ(rclc_parameter_get_double(¶m_server, param_name, &second_get_value), RCL_RET_OK); + ASSERT_EQ(first_get_value, second_get_value); + } +} + +TEST_P(ParameterTestBase, rclcpp_set_get_parameter) { + size_t expected_callback_calls = 1; + + // List parameters check + std::vector param_names = {"param1", "param2", "param3"}; + auto list_params = parameters_client->list_parameters({}, 4, default_spin_timeout); + ASSERT_EQ(list_params.names.size(), param_names.size()); for (auto & name : list_params.names) { std::vector::iterator it; it = std::find(param_names.begin(), param_names.end(), name); ASSERT_NE(it, param_names.end()); } - const std::string name("param1"); - const bool defaultvalue = 0; - bool param_value = parameters_client->get_parameter(name, defaultvalue); - ASSERT_EQ(param_value, true); - - // External set bool - expected_type = RCLC_PARAMETER_BOOL; - expected_value.bool_value = false; - std::vector new_params = - {rclcpp::Parameter("param1", expected_value.bool_value)}; - std::vector result = parameters_client->set_parameters( - new_params); - ASSERT_TRUE(result[0].successful); + // Set parameters + { + std::vector param = {rclcpp::Parameter(param_names[0], true)}; + const std::string param_name = param[0].get_name(); + + // Get initial value + bool get_value = parameters_client->get_parameter(param_name); + ASSERT_EQ(get_value, false); + + // Prepare RCLC callback + on_parameter_changed = [&](const Parameter *, const Parameter * new_param) -> bool { + EXPECT_EQ(new_param->value.type, RCLC_PARAMETER_BOOL); + EXPECT_EQ(strcmp(new_param->name.data, param_name.c_str()), 0); + EXPECT_EQ(new_param->value.bool_value, param[0].as_bool()); + return true; + }; + + // Set value + auto result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_TRUE(result[0].successful); + ASSERT_EQ(result[0].reason, ""); + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; - // External fail type - new_params.clear(); - new_params.push_back(rclcpp::Parameter("param1", static_cast(12.2))); - result = parameters_client->set_parameters(new_params); + // Get new value + get_value = parameters_client->get_parameter(param_name); + ASSERT_EQ(param[0].as_bool(), get_value); + } + + { + std::vector param = {rclcpp::Parameter(param_names[1], 10)}; + const std::string param_name = param[0].get_name(); + + // Get initial value + int get_value = parameters_client->get_parameter(param_name); + ASSERT_EQ(get_value, false); + + // Prepare RCLC callback + on_parameter_changed = [&](const Parameter *, const Parameter * new_param) -> bool { + EXPECT_EQ(new_param->value.type, RCLC_PARAMETER_INT); + EXPECT_EQ(strcmp(new_param->name.data, param_name.c_str()), 0); + EXPECT_EQ(new_param->value.integer_value, param[0].as_int()); + return true; + }; + + // Set value + auto result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_TRUE(result[0].successful); + ASSERT_EQ(result[0].reason, ""); + + // Check callback values + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; + + // Get new value + get_value = parameters_client->get_parameter(param_name); + ASSERT_EQ(param[0].as_int(), get_value); + } + + { + std::vector param = {rclcpp::Parameter(param_names[2], -0.01)}; + const std::string param_name = param[0].get_name(); + + // Get initial value + double get_value = parameters_client->get_parameter(param_name); + ASSERT_EQ(get_value, false); + + // Prepare RCLC callback + on_parameter_changed = [&](const Parameter *, const Parameter * new_param) -> bool { + EXPECT_EQ(new_param->value.type, RCLC_PARAMETER_DOUBLE); + EXPECT_EQ(strcmp(new_param->name.data, param_name.c_str()), 0); + EXPECT_EQ(new_param->value.double_value, param[0].as_double()); + return true; + }; + + // Set value + auto result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_TRUE(result[0].successful); + ASSERT_EQ(result[0].reason, ""); + + // Check callback values + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; + + // Get new value + get_value = parameters_client->get_parameter(param_name); + ASSERT_EQ(param[0].as_double(), get_value); + } + + // Fail with user reject + { + std::vector param = {rclcpp::Parameter(param_names[2], -0.05)}; + const std::string param_name = param[0].get_name(); + + // Get initial value + double first_get_value = parameters_client->get_parameter(param_name); + + // Prepare RCLC callback + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + return false; + }; + + // Set value + auto result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_FALSE(result[0].successful); + ASSERT_EQ(result[0].reason, "Rejected by server"); + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; + + // Get value + double second_get_value = parameters_client->get_parameter(param_name); + ASSERT_EQ(first_get_value, second_get_value); + } +} + +TEST_P(ParameterTestBase, rclc_delete_parameter) { + // Get parameter + const char * param_name = "param1"; + bool param_value; + + // Fail if callback is call + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + EXPECT_TRUE(false); // Callback should not be called + return false; + }; + + ASSERT_EQ(rclc_parameter_get_bool(¶m_server, param_name, ¶m_value), RCL_RET_OK); + + // Delete parameter + EXPECT_EQ(rclc_delete_parameter(¶m_server, param_name), RCL_RET_OK); + + // Fail on get deleted parameter + ASSERT_EQ(rclc_parameter_get_bool(¶m_server, param_name, ¶m_value), RCL_RET_ERROR); + + // Fail on deleted parameter + EXPECT_EQ(rclc_delete_parameter(¶m_server, param_name), RCL_RET_ERROR); + + // No callback calls + ASSERT_EQ(callback_calls, 0U); +} + +TEST_P(ParameterTestBase, rclcpp_delete_parameter) { + size_t expected_callback_calls = 1; + + // Use RCLCPP to delete and check parameters + const std::vector parameters = {"param1"}; + + on_parameter_changed = [&](const Parameter * old_param, const Parameter * new_param) -> bool { + EXPECT_NE(old_param, nullptr); + EXPECT_EQ(new_param, nullptr); + return false; + }; + + auto result = parameters_client->delete_parameters(parameters, default_spin_timeout); + ASSERT_FALSE(result.empty()); ASSERT_FALSE(result[0].successful); + ASSERT_EQ(result[0].reason, "Rejected by server"); + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; - // External set int - expected_type = RCLC_PARAMETER_INT; - expected_value.integer_value = 12; - new_params.clear(); - new_params.push_back(rclcpp::Parameter("param2", expected_value.integer_value)); - result = parameters_client->set_parameters(new_params); - ASSERT_TRUE(result[0].successful); + auto list_params = parameters_client->list_parameters({}, 4, default_spin_timeout); + ASSERT_EQ(list_params.names.size(), 3u); + ASSERT_EQ(list_params.names[0], parameters[0]); + + on_parameter_changed = [&](const Parameter * old_param, const Parameter * new_param) -> bool { + EXPECT_NE(old_param, nullptr); + EXPECT_EQ(new_param, nullptr); + return true; + }; - // External set double - expected_type = RCLC_PARAMETER_DOUBLE; - expected_value.double_value = 12.12; - new_params.clear(); - new_params.push_back(rclcpp::Parameter("param3", expected_value.double_value)); - result = parameters_client->set_parameters(new_params); + result = parameters_client->delete_parameters(parameters, default_spin_timeout); + ASSERT_FALSE(result.empty()); ASSERT_TRUE(result[0].successful); + ASSERT_EQ(result[0].reason, ""); + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; - // External get types - const std::vector types_query = { - "param1", - "param2", - "param3" + // Use auxiliar RCLCPP node for check + list_params = parameters_client->list_parameters({}, 4, default_spin_timeout); + ASSERT_EQ(list_params.names.size(), 2U); + ASSERT_EQ( + std::find( + list_params.names.begin(), + list_params.names.end(), + parameters[0]), + list_params.names.end()); +} + +TEST_P(ParameterTestBase, rclcpp_add_parameter) { + std::vector param = {rclcpp::Parameter("param4", 10.5)}; + + if (options.allow_undeclared_parameters) { + size_t expected_callback_calls = 1; + + // Reject add parameter + on_parameter_changed = [&](const Parameter * old_param, const Parameter * new_param) -> bool { + EXPECT_EQ(old_param, nullptr); + EXPECT_NE(new_param, nullptr); + return false; + }; + + auto result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_FALSE(result[0].successful); + ASSERT_EQ(result[0].reason, "New parameter rejected"); + ASSERT_EQ(callback_calls, expected_callback_calls); + ++expected_callback_calls; + + auto list_params = parameters_client->list_parameters({}, 4, default_spin_timeout); + ASSERT_EQ(list_params.names.size(), 3u); + ASSERT_EQ( + std::find( + list_params.names.begin(), list_params.names.end(), + param[0].get_name()), list_params.names.end()); + + // Accept add parameter + on_parameter_changed = [&](const Parameter * old_param, const Parameter * new_param) -> bool { + EXPECT_EQ(old_param, nullptr); + EXPECT_NE(new_param, nullptr); + return true; + }; + + result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_TRUE(result[0].successful); + ASSERT_EQ(result[0].reason, "New parameter added"); + ASSERT_EQ(callback_calls, expected_callback_calls); + + list_params = parameters_client->list_parameters({}, 4, default_spin_timeout); + ASSERT_EQ(list_params.names.size(), 4u); + ASSERT_EQ(list_params.names[3], param[0].get_name()); + + // Check value and type + double param_value = parameters_client->get_parameter(param[0].get_name()); + ASSERT_EQ(param_value, 10.5); + + // Reject parameter on full server + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + EXPECT_TRUE(false); // Callback should not be called + return false; + }; + + param.clear(); + param.push_back(rclcpp::Parameter("param5", 12.2)); + result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_FALSE(result[0].successful); + ASSERT_EQ(result[0].reason, "Parameter server is full"); + ASSERT_EQ(callback_calls, expected_callback_calls); + } else { + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + EXPECT_TRUE(false); // Callback should not be called + return false; + }; + + // Reject add parameter + auto result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_FALSE(result[0].successful); + ASSERT_EQ(result[0].reason, "Parameter not found"); + ASSERT_EQ(callback_calls, 0U); + + auto list_params = parameters_client->list_parameters({}, 4, default_spin_timeout); + ASSERT_EQ(list_params.names.size(), 3u); + ASSERT_EQ( + std::find( + list_params.names.begin(), list_params.names.end(), + param[0].get_name()), list_params.names.end()); + } +} + +TEST_P(ParameterTestBase, rclcpp_read_only_parameter) { + ASSERT_EQ(rclc_set_parameter_read_only(¶m_server, "param2", true), RCL_RET_OK); + + std::vector param = {rclcpp::Parameter("param2", 50)}; + + // Reject set parameter on read only parameter + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + EXPECT_TRUE(false); // Callback should not be called + return false; + }; + + auto result = parameters_client->set_parameters(param, default_spin_timeout); + ASSERT_FALSE(result.empty()); + ASSERT_FALSE(result[0].successful); + ASSERT_EQ(result[0].reason, "Read only parameter"); + ASSERT_EQ(callback_calls, 0U); + + // Check read only flag on descriptor + std::vector params = {param[0].get_name()}; + auto description = parameters_client->describe_parameters(params); + ASSERT_FALSE(description.empty()); + ASSERT_TRUE(description[0].read_only); +} + +TEST_P(ParameterTestBase, rclcpp_parameter_description) { + if (options.low_mem_mode) { + GTEST_SKIP(); + } + + std::vector params = {"param2", "param3"}; + std::string parameter_description = "Parameter 2"; + std::string additional_constraints = "Constrains applied"; + int64_t int_from = -10; + int64_t int_to = 20; + uint64_t int_step = 2; + double double_from = -0.5; + double double_to = 0.5; + double double_step = 0.01; + + // Set parameter description and constrains + ASSERT_EQ( + rclc_add_parameter_description( + ¶m_server, "param2", parameter_description.c_str(), + additional_constraints.c_str()), RCL_RET_OK); + ASSERT_EQ( + rclc_add_parameter_constraint_integer( + ¶m_server, "param2", int_from, int_to, + int_step), RCL_RET_OK); + ASSERT_EQ( + rclc_add_parameter_constraint_double( + ¶m_server, "param3", double_from, double_to, + double_step), RCL_RET_OK); + + // Check parameter descriptions + auto description = parameters_client->describe_parameters(params); + ASSERT_EQ(description.size(), params.size()); + ASSERT_EQ(description[0].name, params[0]); + ASSERT_EQ(description[0].type, RCLC_PARAMETER_INT); + ASSERT_EQ(description[0].description, parameter_description); + ASSERT_EQ(description[0].additional_constraints, additional_constraints); + ASSERT_EQ(description[0].floating_point_range.size(), 0U); + ASSERT_EQ(description[0].integer_range.size(), 1U); + ASSERT_EQ(description[0].integer_range[0].from_value, int_from); + ASSERT_EQ(description[0].integer_range[0].to_value, int_to); + ASSERT_EQ(description[0].integer_range[0].step, int_step); + + ASSERT_EQ(description[1].name, params[1]); + ASSERT_EQ(description[1].type, RCLC_PARAMETER_DOUBLE); + ASSERT_TRUE(description[1].description.empty()); + ASSERT_TRUE(description[1].additional_constraints.empty()); + ASSERT_EQ(description[1].integer_range.size(), 0U); + ASSERT_EQ(description[1].floating_point_range.size(), 1U); + ASSERT_EQ(description[1].floating_point_range[0].from_value, double_from); + ASSERT_EQ(description[1].floating_point_range[0].to_value, double_to); + ASSERT_EQ(description[1].floating_point_range[0].step, double_step); + + // TODO(acuadros95): Test again after new/delete parameter +} + +TEST_P(ParameterTestBase, rclcpp_parameter_description_low) { + if (!options.low_mem_mode) { + GTEST_SKIP(); + } + + std::vector param = { + rclcpp::Parameter("param2", 0), + rclcpp::Parameter("param3", 0.1) }; - std::vector types = parameters_client->get_parameter_types(types_query); - ASSERT_EQ(types.size(), 3u); - ASSERT_EQ(types[0], rclcpp::ParameterType::PARAMETER_BOOL); - ASSERT_EQ(types[1], rclcpp::ParameterType::PARAMETER_INTEGER); - ASSERT_EQ(types[2], rclcpp::ParameterType::PARAMETER_DOUBLE); - // Test callback + int64_t int_from = -10; + int64_t int_to = 20; + uint64_t int_step = 2; + double double_from = -0.5; + double double_to = 0.5; + double double_step = 0.01; + + // Set parameter constrains + ASSERT_EQ( + rclc_add_parameter_description( + ¶m_server, "param2", "", + ""), RCLC_PARAMETER_UNSUPORTED_ON_LOW_MEM); + ASSERT_EQ( + rclc_add_parameter_constraint_integer( + ¶m_server, "param2", int_from, int_to, + int_step), RCL_RET_OK); + ASSERT_EQ( + rclc_add_parameter_constraint_double( + ¶m_server, "param3", double_from, double_to, + double_step), RCL_RET_OK); + + for (auto & parameter : param) { + std::vector request = {parameter.get_name()}; + + // Check parameter descriptions + auto description = parameters_client->describe_parameters(request); + ASSERT_EQ(description.size(), request.size()); + ASSERT_EQ(description[0].name, request[0]); + + if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { + ASSERT_EQ(description[0].type, RCLC_PARAMETER_INT); + ASSERT_EQ(description[0].floating_point_range.size(), 0U); + ASSERT_EQ(description[0].integer_range.size(), 1U); + ASSERT_EQ(description[0].integer_range[0].from_value, int_from); + ASSERT_EQ(description[0].integer_range[0].to_value, int_to); + ASSERT_EQ(description[0].integer_range[0].step, int_step); + } else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { + ASSERT_EQ(description[0].type, RCLC_PARAMETER_DOUBLE); + ASSERT_EQ(description[0].integer_range.size(), 0U); + ASSERT_EQ(description[0].floating_point_range.size(), 1U); + ASSERT_EQ(description[0].floating_point_range[0].from_value, double_from); + ASSERT_EQ(description[0].floating_point_range[0].to_value, double_to); + ASSERT_EQ(description[0].floating_point_range[0].step, double_step); + } + + // TODO(acuadros95): Test again after new/delete parameter + } +} + +TEST_P(ParameterTestBase, notify_changed_over_dds) { + // Subscribe to on_parameter_event auto promise = std::make_shared>(); - auto future = promise->get_future(); - size_t on_parameter_calls = 0; + auto future = promise->get_future().share(); + bool parameter_change = false; + bool parameter_added = false; + bool parameter_deleted = false; + std::string event_parameter = ""; + auto sub = parameters_client->on_parameter_event( - [&](const rcl_interfaces::msg::ParameterEvent::SharedPtr /* event */) -> void + [&](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void { - on_parameter_calls++; + parameter_change = event->changed_parameters.size() == 1; + parameter_added = event->new_parameters.size() == 1; + parameter_deleted = event->deleted_parameters.size() == 1; + event_parameter = ""; + + if (parameter_change) { + event_parameter = event->changed_parameters[0].name; + } + + if (parameter_added) { + event_parameter = event->new_parameters[0].name; + } + + if (parameter_deleted) { + event_parameter = event->deleted_parameters[0].name; + } + promise->set_value(); }); - expected_type = RCLC_PARAMETER_BOOL; - expected_value.double_value = false; - rclc_parameter_set_bool(¶m_server, "param1", false); + // Sleep for pub/sub match + std::this_thread::sleep_for(500ms); - rclcpp::spin_until_future_complete(param_client_node, future.share()); + // Parameter change event + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + return true; + }; - ASSERT_EQ(on_parameter_calls, 1u); + ASSERT_EQ(rclc_parameter_set_bool(¶m_server, "param1", false), RCL_RET_OK); + ASSERT_EQ( + rclcpp::spin_until_future_complete( + param_client_node, future, + default_spin_timeout), + rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(event_parameter, "param1"); + ASSERT_TRUE(parameter_change); + ASSERT_FALSE(parameter_added); + ASSERT_FALSE(parameter_deleted); - rclcpp::shutdown(); + // Parameter added event + promise = std::make_shared>(); + future = promise->get_future().share(); + ASSERT_EQ(rclc_add_parameter(¶m_server, "new_param", RCLC_PARAMETER_BOOL), RCL_RET_OK); + ASSERT_EQ( + rclcpp::spin_until_future_complete( + param_client_node, future, + default_spin_timeout), + rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(event_parameter, "new_param"); + ASSERT_TRUE(parameter_added); + ASSERT_FALSE(parameter_change); + ASSERT_FALSE(parameter_deleted); - spin = false; - rclc_parameter_server_thread.join(); + // Parameter deleted event + promise = std::make_shared>(); + future = promise->get_future().share(); + ASSERT_EQ(rclc_delete_parameter(¶m_server, "new_param"), RCL_RET_OK); + ASSERT_EQ( + rclcpp::spin_until_future_complete( + param_client_node, future, + default_spin_timeout), + rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(event_parameter, "new_param"); + ASSERT_TRUE(parameter_deleted); + ASSERT_FALSE(parameter_change); + ASSERT_FALSE(parameter_added); +} - // Destroy parameter server - ASSERT_EQ(rclc_parameter_server_fini(¶m_server, &node), RCL_RET_OK); +TEST_P(ParameterTestBase, rclcpp_get_parameter_types) { + // External get types + std::vector param = { + rclcpp::Parameter("param1", true), + rclcpp::Parameter("param2", 10), + rclcpp::Parameter("param3", 0.1) + }; + + if (options.low_mem_mode) { + // Request types one by one + for (auto & parameter : param) { + const std::vector param_name = { + parameter.get_name() + }; + auto types = parameters_client->get_parameter_types( + param_name, + default_spin_timeout); + + ASSERT_EQ(types.size(), 1U); + ASSERT_EQ(types[0], parameter.get_type()); + } + } else { + // Request all parameters at once + const std::vector types_query = { + param[0].get_name(), + param[1].get_name(), + param[2].get_name() + }; + std::vector types = parameters_client->get_parameter_types( + types_query, + default_spin_timeout); + ASSERT_EQ(types.size(), types_query.size()); + ASSERT_EQ(types[0], param[0].get_type()); + ASSERT_EQ(types[1], param[1].get_type()); + ASSERT_EQ(types[2], param[2].get_type()); + } } + +TEST_P(ParameterTestBase, rclc_disabled_on_callback) { + // Test disabled methods on callback + on_parameter_changed = [&](const Parameter *, const Parameter *) -> bool { + EXPECT_EQ( + rclc_add_parameter( + ¶m_server, "param_new", + RCLC_PARAMETER_BOOL), RCLC_PARAMETER_DISABLED_ON_CALLBACK); + EXPECT_EQ( + rclc_delete_parameter(¶m_server, "param1"), + RCLC_PARAMETER_DISABLED_ON_CALLBACK); + EXPECT_EQ( + rclc_parameter_set_bool( + ¶m_server, "param1", + true), RCLC_PARAMETER_DISABLED_ON_CALLBACK); + EXPECT_EQ( + rclc_parameter_set_int( + ¶m_server, "param2", + 10), RCLC_PARAMETER_DISABLED_ON_CALLBACK); + EXPECT_EQ( + rclc_parameter_set_double( + ¶m_server, "param3", + 0.1), RCLC_PARAMETER_DISABLED_ON_CALLBACK); + EXPECT_EQ( + rclc_set_parameter_read_only( + ¶m_server, "param1", + true), RCLC_PARAMETER_DISABLED_ON_CALLBACK); + EXPECT_EQ( + rclc_add_parameter_constraint_integer( + ¶m_server, "param2", 0, 0, + 0), RCLC_PARAMETER_DISABLED_ON_CALLBACK); + EXPECT_EQ( + rclc_add_parameter_constraint_double( + ¶m_server, "param3", 0.0, 0.0, + 0.0), RCLC_PARAMETER_DISABLED_ON_CALLBACK); + return true; + }; + + // Trigger callback + ASSERT_EQ(rclc_parameter_set_bool(¶m_server, "param1", true), RCL_RET_OK); + ASSERT_EQ(callback_calls, 1U); +} + +// Init parameter server with allow_undeclared_parameters flag +rclc_parameter_options_t options_low_mem = { + true, // notify_changed_over_dds + 4, // max_params + true, // allow_undeclared_parameters + true // low_mem_mode +}; + +rclc_parameter_options_t default_options = { + true, // notify_changed_over_dds + 4, // max_params + false, // allow_undeclared_parameters + false // low_mem_mode +}; + +INSTANTIATE_TEST_SUITE_P( + ParametersRclcpp, + ParameterTestBase, + ::testing::Values( + default_options, + options_low_mem));