diff --git a/rclc_parameter/include/rclc_parameter/rclc_parameter.h b/rclc_parameter/include/rclc_parameter/rclc_parameter.h index 71825e9c..c187e659 100644 --- a/rclc_parameter/include/rclc_parameter/rclc_parameter.h +++ b/rclc_parameter/include/rclc_parameter/rclc_parameter.h @@ -70,13 +70,14 @@ typedef struct rcl_interfaces__msg__ParameterEvent ParameterEvent; #define RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER 5 #define PARAMETER_MODIFICATION_REJECTED 4001 #define PARAMETER_TYPE_MISMATCH 4002 +#define UNSUPORTED_ON_LOW_MEM 4003 /** * Parameter event callback * This callback will allow the user to allow or reject the following events: - * - Parameter value change: Set operation on existing parameter. - * - Parameter creation: Set operation on unexisting parameter if `allow_undeclared_parameters` is set. - * - Parameter delete: Delete operation on existing parameter. + * - 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. * *
* Attribute | Adherence @@ -107,6 +108,7 @@ 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 @@ -143,6 +145,7 @@ typedef struct rclc_parameter_server_t bool notify_changed_over_dds; bool allow_undeclared_parameters; + bool low_mem_mode; } rclc_parameter_server_t; /** @@ -413,7 +416,7 @@ rclc_parameter_get_double( /** - * Add a description to a parameter. + * Add a description to a parameter (Unsuported on low memory mode). * This description will be returned on describe parameters requests. * *
@@ -427,14 +430,39 @@ rclc_parameter_get_double( * \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, bool read_only); + 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. + * + *
+ * 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); /** * Add constraint description for an integer parameter. @@ -450,7 +478,6 @@ rclc_add_parameter_description( * * \param[in] parameter_server preallocated rclc_parameter_server_t * \param[in] parameter_name name of the parameter - * \param[in] additional_constraints constraints description * \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. @@ -460,8 +487,7 @@ RCLC_PARAMETER_PUBLIC rcl_ret_t rclc_add_parameter_constraints_integer( rclc_parameter_server_t * parameter_server, - const char * parameter_name, const char * additional_constraints, - int64_t from_value, int64_t to_value, uint64_t step); + const char * parameter_name, int64_t from_value, int64_t to_value, uint64_t step); /** * Add constraint description for an double parameter. @@ -477,7 +503,6 @@ rclc_add_parameter_constraints_integer( * * \param[in] parameter_server preallocated rclc_parameter_server_t * \param[in] parameter_name name of the parameter - * \param[in] additional_constraints constraints description * \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. @@ -487,8 +512,7 @@ RCLC_PARAMETER_PUBLIC rcl_ret_t rclc_add_parameter_constraints_double( rclc_parameter_server_t * parameter_server, - const char * parameter_name, const char * additional_constraints, - double from_value, double to_value, double step); + const char * parameter_name, double from_value, double to_value, double step); #if __cplusplus } diff --git a/rclc_parameter/src/rclc_parameter/parameter_server.c b/rclc_parameter/src/rclc_parameter/parameter_server.c index 7bb50505..d034e4b4 100644 --- a/rclc_parameter/src/rclc_parameter/parameter_server.c +++ b/rclc_parameter/src/rclc_parameter/parameter_server.c @@ -52,7 +52,13 @@ rclc_parameter_server_describe_service_callback( DescribeParameters_Request * request = (DescribeParameters_Request *) req; DescribeParameters_Response * response = (DescribeParameters_Response *) res; + if (request->names.size > response->descriptors.capacity) { + response->descriptors.size = 0; + return; + } + response->descriptors.size = request->names.size; + for (size_t i = 0; i < request->names.size; i++) { size_t index = rclc_parameter_search_index( ¶m_server->parameter_list, @@ -60,17 +66,28 @@ rclc_parameter_server_describe_service_callback( 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); - } else { - rclc_parameter_set_string(&response_descriptor->name, ""); + 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, ""); - 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; } } } @@ -89,9 +106,13 @@ 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); + 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; + } } } @@ -105,6 +126,11 @@ rclc_parameter_server_get_service_callback( 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; @@ -174,16 +200,24 @@ rclc_parameter_server_set_service_callback( 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) { + 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: if (param_server->on_modification && !param_server->on_modification(parameter, NULL)) { @@ -259,6 +293,7 @@ const rclc_parameter_options_t DEFAULT_PARAMETER_SERVER_OPTIONS = { .notify_changed_over_dds = true, .max_params = 4, .allow_undeclared_parameters = false, + .low_mem_mode = false }; rcl_ret_t rclc_parameter_server_init_default( @@ -275,7 +310,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) @@ -289,56 +324,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; - parameter_server->allow_undeclared_parameters = options->allow_undeclared_parameters; - - 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'; @@ -521,30 +506,405 @@ rclc_parameter_server_init_with_option( return ret; } +bool init_parameter_server_memory_low_memory( + rclc_parameter_server_t * parameter_server, + rcl_node_t * node, + const rclc_parameter_options_t * options) +{ + 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++) { + parameter_server->parameter_list.data[i].name.data = allocator.allocate( + sizeof(char) * RCLC_PARAMETER_MAX_STRING_LENGTH, allocator.state); + parameter_server->parameter_list.data[i].name.data[0] = '\0'; + parameter_server->parameter_list.data[i].name.capacity = RCLC_PARAMETER_MAX_STRING_LENGTH; + parameter_server->parameter_list.data[i].name.size = 0; + + parameter_server->parameter_list.data[i].value.string_value.data = + allocator.allocate(sizeof(char), allocator.state); + parameter_server->parameter_list.data[i].value.string_value.data[0] = '\0'; + parameter_server->parameter_list.data[i].value.string_value.capacity = 1; + parameter_server->parameter_list.data[i].value.string_value.size = 0; + + parameter_server->parameter_descriptors.data[i].description.data = + allocator.allocate(sizeof(char), allocator.state); + parameter_server->parameter_descriptors.data[i].description.data[0] = '\0'; + parameter_server->parameter_descriptors.data[i].description.capacity = 1; + parameter_server->parameter_descriptors.data[i].description.size = 0; + + parameter_server->parameter_descriptors.data[i].additional_constraints.data = + allocator.allocate(sizeof(char), allocator.state); + parameter_server->parameter_descriptors.data[i].additional_constraints.data[0] = '\0'; + parameter_server->parameter_descriptors.data[i].additional_constraints.capacity = 1; + parameter_server->parameter_descriptors.data[i].additional_constraints.size = 0; + + 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; + + parameter_server->get_request.names.data[0].data = allocator.allocate( + sizeof(char) * RCLC_PARAMETER_MAX_STRING_LENGTH, allocator.state); + parameter_server->get_request.names.data[0].capacity = RCLC_PARAMETER_MAX_STRING_LENGTH; + parameter_server->get_request.names.data[0].size = 0; + + 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; + + parameter_server->get_response.values.data[0].string_value.data = allocator.allocate( + sizeof(char), + allocator.state); + parameter_server->get_response.values.data[0].string_value.data[0] = '\0'; + parameter_server->get_response.values.data[0].string_value.size = 0; + parameter_server->get_response.values.data[0].string_value.capacity = 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; + + parameter_server->set_request.parameters.data[0].name.data = allocator.allocate( + sizeof(char) * RCLC_PARAMETER_MAX_STRING_LENGTH, allocator.state); + parameter_server->set_request.parameters.data[0].name.capacity = RCLC_PARAMETER_MAX_STRING_LENGTH; + parameter_server->set_request.parameters.data[0].name.size = 0; + + 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; + + parameter_server->set_response.results.data[0].reason.data = allocator.allocate( + sizeof(char) * RCLC_PARAMETER_MAX_STRING_LENGTH, allocator.state); + parameter_server->set_response.results.data[0].reason.capacity = RCLC_PARAMETER_MAX_STRING_LENGTH; + parameter_server->set_response.results.data[0].reason.size = 0; + + // 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; + + parameter_server->get_types_request.names.data[0].data = allocator.allocate( + sizeof(char) * RCLC_PARAMETER_MAX_STRING_LENGTH, allocator.state); + parameter_server->get_types_request.names.data[0].capacity = RCLC_PARAMETER_MAX_STRING_LENGTH; + parameter_server->get_types_request.names.data[0].size = 0; + + 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; + + parameter_server->describe_request.names.data[0].data = allocator.allocate( + sizeof(char) * RCLC_PARAMETER_MAX_STRING_LENGTH, allocator.state); + parameter_server->describe_request.names.data[0].capacity = RCLC_PARAMETER_MAX_STRING_LENGTH; + parameter_server->describe_request.names.data[0].size = 0; + + 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; + + parameter_server->describe_response.descriptors.data[0].description.data = + allocator.allocate(sizeof(char), allocator.state); + parameter_server->describe_response.descriptors.data[0].description.data[0] = '\0'; + parameter_server->describe_response.descriptors.data[0].description.capacity = 1; + parameter_server->describe_response.descriptors.data[0].description.size = 0; + + parameter_server->describe_response.descriptors.data[0].additional_constraints.data = + allocator.allocate(sizeof(char), allocator.state); + parameter_server->describe_response.descriptors.data[0].additional_constraints.data[0] = '\0'; + parameter_server->describe_response.descriptors.data[0].additional_constraints.capacity = 1; + parameter_server->describe_response.descriptors.data[0].additional_constraints.size = 0; + + 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->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); } - rosidl_runtime_c__String__fini(¶meter_server->event_list.node); + if (parameter_server->low_mem_mode) { + ret = init_parameter_server_memory_low_memory(parameter_server, node, options); + } else { + ret = init_parameter_server_memory(parameter_server, node, options); + } + return ret; +} + +void +rclc_parameter_server_fini_memory_low_memory( + rclc_parameter_server_t * parameter_server) +{ + 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, allocator.state); + parameter_server->get_response.values.capacity = 0; + parameter_server->get_response.values.size = 0; + + // 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_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) +{ // 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]); @@ -625,9 +985,41 @@ rclc_parameter_server_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 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_memory(parameter_server); + } else { + rclc_parameter_server_fini_memory(parameter_server); + } return ret; } @@ -707,7 +1099,7 @@ rclc_add_parameter( parameter_server->parameter_list.size++; // Add to parameter descriptors - if (!rclc_parameter_set_string( + if (!parameter_server->low_mem_mode && !rclc_parameter_set_string( ¶meter_server->parameter_descriptors.data[index].name, parameter_name)) { @@ -746,8 +1138,8 @@ rclc_add_parameter_undeclared( } if (RCL_RET_OK != rclc_parameter_copy( - ¶meter_server->parameter_list.data[index], - parameter)) + ¶meter_server->parameter_list.data[index], + parameter)) { return RCL_RET_ERROR; } @@ -755,7 +1147,7 @@ rclc_add_parameter_undeclared( parameter_server->parameter_list.size++; // Add to parameter descriptors - if (!rclc_parameter_set_string( + if (!parameter_server->low_mem_mode && !rclc_parameter_set_string( ¶meter_server->parameter_descriptors.data[index].name, parameter->name.data)) { @@ -793,6 +1185,7 @@ rclc_delete_parameter( return RCL_RET_ERROR; } + Parameter * param = ¶meter_server->parameter_list.data[index]; ParameterDescriptor * param_description = ¶meter_server->parameter_descriptors.data[index]; @@ -801,11 +1194,13 @@ rclc_delete_parameter( param->value.type = RCLC_PARAMETER_NOT_SET; // Reset parameter description - rclc_parameter_set_string(¶m_description->description, ""); - rclc_parameter_set_string(¶m_description->additional_constraints, ""); 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 @@ -816,7 +1211,8 @@ rclc_delete_parameter( // Move descriptors rclc_parameter_descriptor_copy( ¶meter_server->parameter_descriptors.data[i], - ¶meter_server->parameter_descriptors.data[i + 1]); + ¶meter_server->parameter_descriptors.data[i + 1], + parameter_server->low_mem_mode); } parameter_server->parameter_descriptors.size--; @@ -1098,8 +1494,12 @@ rclc_parameter_server_init_service( rcl_ret_t rclc_add_parameter_description( rclc_parameter_server_t * parameter_server, const char * parameter_name, const char * parameter_description, - bool read_only) + const char * additional_constraints) { + if (parameter_server->low_mem_mode) { + return UNSUPORTED_ON_LOW_MEM; + } + size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); if (index >= parameter_server->parameter_list.size) { @@ -1113,14 +1513,38 @@ rcl_ret_t rclc_add_parameter_description( return RCL_RET_ERROR; } + // Set constrain 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) +{ + 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_constraints_double( rclc_parameter_server_t * parameter_server, - const char * parameter_name, - const char * additional_constraints, double from_value, + const char * parameter_name, double from_value, double to_value, double step) { size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); @@ -1135,14 +1559,6 @@ rcl_ret_t rclc_add_parameter_constraints_double( return RCL_RET_ERROR; } - // Set constrain description - if (!rclc_parameter_set_string( - ¶meter_descriptor->additional_constraints, - additional_constraints)) - { - 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; @@ -1153,8 +1569,7 @@ rcl_ret_t rclc_add_parameter_constraints_double( rcl_ret_t rclc_add_parameter_constraints_integer( rclc_parameter_server_t * parameter_server, - const char * parameter_name, - const char * additional_constraints, int64_t from_value, + const char * parameter_name, int64_t from_value, int64_t to_value, uint64_t step) { size_t index = rclc_parameter_search_index(¶meter_server->parameter_list, parameter_name); @@ -1169,14 +1584,6 @@ rcl_ret_t rclc_add_parameter_constraints_integer( return RCL_RET_ERROR; } - // Set constrain description - if (!rclc_parameter_set_string( - ¶meter_descriptor->additional_constraints, - additional_constraints)) - { - 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; diff --git a/rclc_parameter/src/rclc_parameter/parameter_utils.c b/rclc_parameter/src/rclc_parameter/parameter_utils.c index 23df7e03..b8007526 100644 --- a/rclc_parameter/src/rclc_parameter/parameter_utils.c +++ b/rclc_parameter/src/rclc_parameter/parameter_utils.c @@ -65,21 +65,27 @@ rclc_parameter_copy( rcl_ret_t rclc_parameter_descriptor_copy( ParameterDescriptor * dst, - const ParameterDescriptor * src) + 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 (!rclc_parameter_set_string(&dst->name, src->name.data)) { - return RCL_RET_ERROR; - } + 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->description, src->description.data)) { + return RCL_RET_ERROR; + } - if (!rclc_parameter_set_string(&dst->additional_constraints, src->additional_constraints.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; diff --git a/rclc_parameter/src/rclc_parameter/parameter_utils.h b/rclc_parameter/src/rclc_parameter/parameter_utils.h index e7494078..e25e25ef 100644 --- a/rclc_parameter/src/rclc_parameter/parameter_utils.h +++ b/rclc_parameter/src/rclc_parameter/parameter_utils.h @@ -39,7 +39,8 @@ rclc_parameter_copy( rcl_ret_t rclc_parameter_descriptor_copy( ParameterDescriptor * dst, - const ParameterDescriptor * src); + const ParameterDescriptor * src, + bool low_mem); Parameter * rclc_parameter_search( diff --git a/rclc_parameter/test/rclc_parameter/test_parameter.cpp b/rclc_parameter/test/rclc_parameter/test_parameter.cpp index 55343772..ac2aeaf7 100644 --- a/rclc_parameter/test/rclc_parameter/test_parameter.cpp +++ b/rclc_parameter/test/rclc_parameter/test_parameter.cpp @@ -106,11 +106,12 @@ TEST(ParameterTestUnitary, rclc_add_parameter) { ASSERT_EQ(rcl_node_fini(&node), RCL_RET_OK); } -class ParameterTestBase : public ::testing::Test +class ParameterTestBase : public ::testing::TestWithParam { public: ParameterTestBase() - : default_spin_timeout(std::chrono::duration(10000)) + : default_spin_timeout(std::chrono::duration(10000)), + options(GetParam()) { callcack_calls = 0; user_return = true; @@ -133,12 +134,6 @@ class ParameterTestBase : public ::testing::Test // Init node EXPECT_EQ(rclc_node_init_default(&node, node_name.c_str(), "", &support), RCL_RET_OK); - // Init parameter server with allow_undeclared_parameters flag - rclc_parameter_options_t options; - options.notify_changed_over_dds = true; - options.max_params = 4; - options.allow_undeclared_parameters = true; - // Init parameter server EXPECT_EQ(rclc_parameter_server_init_with_option(¶m_server, &node, &options), RCL_RET_OK); @@ -174,7 +169,6 @@ class ParameterTestBase : public ::testing::Test node_name); ASSERT_TRUE(parameters_client->wait_for_service(default_spin_timeout)); - std::this_thread::sleep_for(500ms); } void TearDown() override @@ -233,6 +227,7 @@ class ParameterTestBase : public ::testing::Test 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; @@ -245,7 +240,7 @@ char ParameterTestBase::new_parameter_name[] = ""; ParameterValue ParameterTestBase::new_parameter_value; ParameterValue ParameterTestBase::old_parameter_value; -TEST_F(ParameterTestBase, rclc_set_get_parameter) { +TEST_P(ParameterTestBase, rclc_set_get_parameter) { int expected_callcack_calls = 1; // Set parameters @@ -350,7 +345,7 @@ TEST_F(ParameterTestBase, rclc_set_get_parameter) { } } -TEST_F(ParameterTestBase, rclcpp_set_get_parameter) { +TEST_P(ParameterTestBase, rclcpp_set_get_parameter) { int expected_callcack_calls = 1; // List parameters check @@ -465,7 +460,7 @@ TEST_F(ParameterTestBase, rclcpp_set_get_parameter) { } } -TEST_F(ParameterTestBase, rclc_delete_parameter) { +TEST_P(ParameterTestBase, rclc_delete_parameter) { // Get parameter const char * param_name = "param1"; bool param_value; @@ -479,7 +474,7 @@ TEST_F(ParameterTestBase, rclc_delete_parameter) { ASSERT_EQ(rclc_parameter_get_bool(¶m_server, param_name, ¶m_value), RCL_RET_ERROR); } -TEST_F(ParameterTestBase, rclcpp_delete_parameter) { +TEST_P(ParameterTestBase, rclcpp_delete_parameter) { int expected_callcack_calls = 1; // Use RCLCPP to delete and check parameters @@ -513,55 +508,203 @@ TEST_F(ParameterTestBase, rclcpp_delete_parameter) { list_params.names.end(), parameters[0]), list_params.names.end()); } -TEST_F(ParameterTestBase, rclcpp_add_parameter) { - int expected_callcack_calls = 1; +TEST_P(ParameterTestBase, rclcpp_add_parameter) { + std::vector param = {rclcpp::Parameter("param4", 10.5)}; - // Reject add parameter - user_return = false; - std::vector param = - {rclcpp::Parameter("param4", 10.5)}; + if (options.allow_undeclared_parameters) { + int expected_callcack_calls = 1; + + // Reject add parameter + user_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(callcack_calls, expected_callcack_calls); + expected_callcack_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 + user_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(callcack_calls, expected_callcack_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 + user_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(callcack_calls, expected_callcack_calls); + } else { + // 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(callcack_calls, 0); + + 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 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(callcack_calls, expected_callcack_calls); - expected_callcack_calls++; + ASSERT_EQ(result[0].reason, "Read only parameter"); + ASSERT_EQ(callcack_calls, 0); + + // 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); +} - auto list_params = parameters_client->list_parameters({}, 4, default_spin_timeout); - ASSERT_EQ(list_params.names.size(), 3u); +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( - std::find( - list_params.names.begin(), list_params.names.end(), - param[0].get_name()), list_params.names.end()); + rclc_add_parameter_description( + ¶m_server, "param2", parameter_description.c_str(), + additional_constraints.c_str()), RCL_RET_OK); + ASSERT_EQ( + rclc_add_parameter_constraints_integer( + ¶m_server, "param2", int_from, int_to, + int_step), RCL_RET_OK); + ASSERT_EQ( + rclc_add_parameter_constraints_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); + // TODO(acuadros95): Check empty string filled with spaces + // 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 +} - // Accept add parameter - user_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(callcack_calls, expected_callcack_calls); +TEST_P(ParameterTestBase, rclcpp_parameter_description_low) { + if (!options.low_mem_mode) { + GTEST_SKIP(); + } - 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()); + std::vector param = { + rclcpp::Parameter("param2", 0), + rclcpp::Parameter("param3", 0.1) + }; - // Check value and type - double param_value = parameters_client->get_parameter(param[0].get_name()); - ASSERT_EQ(param_value, 10.5); + 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; - // Reject parameter on full server - user_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(callcack_calls, expected_callcack_calls); + // Set parameter constrains + ASSERT_EQ( + rclc_add_parameter_description(¶m_server, "param2", "", ""), UNSUPORTED_ON_LOW_MEM); + ASSERT_EQ( + rclc_add_parameter_constraints_integer( + ¶m_server, "param2", int_from, int_to, + int_step), RCL_RET_OK); + ASSERT_EQ( + rclc_add_parameter_constraints_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_F(ParameterTestBase, notify_changed_over_dds) { +TEST_P(ParameterTestBase, notify_changed_over_dds) { // Subscribe to on_parameter_event auto promise = std::make_shared>(); auto future = promise->get_future().share(); @@ -589,18 +732,62 @@ TEST_F(ParameterTestBase, notify_changed_over_dds) { ASSERT_FALSE(parameter_deleted); } -TEST_F(ParameterTestBase, rclcpp_get_parameter_types) { +TEST_P(ParameterTestBase, rclcpp_get_parameter_types) { // External get types - const std::vector types_query = { - "param1", - "param2", - "param3" + std::vector param = { + rclcpp::Parameter("param1", true), + rclcpp::Parameter("param2", 10), + rclcpp::Parameter("param3", 0.1) }; - std::vector types = parameters_client->get_parameter_types( - types_query, - default_spin_timeout); - 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); + + 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()); + } } + +// 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));