Skip to content

Commit

Permalink
Aux param review (#275)
Browse files Browse the repository at this point in the history 
* Update

Signed-off-by: Pablo Garrido <[email protected]>

* Fix typos

Co-authored-by: acuadros95 <[email protected]>
  • Loading branch information
@pablogs9 @Acuadros95
pablogs9 and Acuadros95 authored Apr 1, 2022
1 parent a79d5b8 commit bc6987a
Show file tree
Hide file tree
Showing 4 changed files with 263 additions and 165 deletions.
18 changes: 13 additions & 5 deletions rclc_examples/src/example_parameter_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,10 @@ void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
rclc_parameter_set_int(&param_server, "param2", (int64_t) value);
}

bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param)
bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context)
{
(void) context;

if (old_param == NULL) {
printf("Creating new parameter %s\n", new_param->name.data);
} else if (new_param == NULL) {
Expand All @@ -46,13 +48,19 @@ bool on_parameter_changed(const Parameter * old_param, const Parameter * new_par
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);
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);
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);
printf(
" Old value: %f, New value: %f (double)", old_param->value.double_value,
new_param->value.double_value);
break;
default:
break;
Expand Down Expand Up @@ -106,7 +114,7 @@ int main()

// Add parameters constrains
rclc_add_parameter_description(&param_server, "param2", "Second parameter", "Only even numbers");
rclc_add_parameter_constraints_integer(&param_server, "param2", -10, 120, 2);
rclc_add_parameter_constraints_integer(&param_server, "param2", -10, 120, 2);

rclc_add_parameter_description(&param_server, "param3", "Third parameter", "");
rclc_set_parameter_read_only(&param_server, "param3", true);
Expand Down
43 changes: 35 additions & 8 deletions rclc_parameter/include/rclc_parameter/rclc_parameter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,10 +68,10 @@ 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 PARAMETER_MODIFICATION_REJECTED 4001
#define PARAMETER_TYPE_MISMATCH 4002
#define UNSUPORTED_ON_LOW_MEM 4003
#define DISABLE_ON_CALLBACK 40004
#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

/**
* Parameter event callback.
Expand All @@ -92,11 +92,13 @@ typedef struct rcl_interfaces__msg__ParameterEvent ParameterEvent;
*
* \param[in] param Parameter actual value, `NULL` for new parameter request.
* \param[in] new_value Parameter new value, `NULL` for parameter removal request.
* \param[in] context Context of the callback.
* \return `true` to accept the parameter event. The operation will be rejected on `false` return.
*/
typedef bool (* ModifiedParameter_Callback)(
typedef bool (* rclc_parameter_callback_t)(
const Parameter * old_param,
const Parameter * new_param);
const Parameter * new_param,
void * context);

// Allowed RCLC parameter types
typedef enum rclc_parameter_type_t
Expand Down Expand Up @@ -146,7 +148,8 @@ typedef struct rclc_parameter_server_t

ParameterEvent event_list;

ModifiedParameter_Callback on_modification;
rclc_parameter_callback_t on_modification;
void * context;
bool on_callback;

bool notify_changed_over_dds;
Expand Down Expand Up @@ -236,7 +239,31 @@ 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
*
* <hr>
* 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
Expand Down
89 changes: 48 additions & 41 deletions rclc_parameter/src/rclc_parameter/parameter_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -256,9 +256,9 @@ rclc_parameter_server_set_service_callback(

if (ret == RCL_RET_INVALID_ARGUMENT) {
rclc_parameter_set_string(message, "Set parameter error");
} else if (ret == PARAMETER_MODIFICATION_REJECTED) {
} else if (ret == RCLC_PARAMETER_MODIFICATION_REJECTED) {
rclc_parameter_set_string(message, "Rejected by server");
} else if (ret == PARAMETER_TYPE_MISMATCH) {
} else if (ret == RCLC_PARAMETER_TYPE_MISMATCH) {
rclc_parameter_set_string(message, "Type mismatch");
} else {
response->results.data[i].successful = true;
Expand Down Expand Up @@ -1067,7 +1067,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);
Expand All @@ -1077,6 +1089,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, &parameter_server->list_service,
Expand Down Expand Up @@ -1121,7 +1134,7 @@ rclc_add_parameter(
parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT);

if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

size_t index = parameter_server->parameter_list.size;
Expand Down Expand Up @@ -1223,7 +1236,7 @@ rclc_delete_parameter(
parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT);

if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

// Find parameter
Expand Down Expand Up @@ -1286,7 +1299,7 @@ rclc_parameter_set_bool(
parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT);

if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

Parameter * parameter =
Expand All @@ -1297,18 +1310,16 @@ rclc_parameter_set_bool(
}

if (parameter->value.type != RCLC_PARAMETER_BOOL) {
return PARAMETER_TYPE_MISMATCH;
return RCLC_PARAMETER_TYPE_MISMATCH;
}

if (parameter_server->on_modification) {
Parameter new_parameter = *parameter;
new_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 PARAMETER_MODIFICATION_REJECTED;
}
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) {
Expand All @@ -1333,7 +1344,7 @@ rclc_parameter_set_int(
parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT);

if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

Parameter * parameter =
Expand All @@ -1344,18 +1355,16 @@ rclc_parameter_set_int(
}

if (parameter->value.type != RCLC_PARAMETER_INT) {
return PARAMETER_TYPE_MISMATCH;
return RCLC_PARAMETER_TYPE_MISMATCH;
}

if (parameter_server->on_modification) {
Parameter new_parameter = *parameter;
new_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 PARAMETER_MODIFICATION_REJECTED;
}
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) {
Expand All @@ -1380,7 +1389,7 @@ rclc_parameter_set_double(
parameter_name, "parameter_name is a null pointer", return RCL_RET_INVALID_ARGUMENT);

if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

Parameter * parameter =
Expand All @@ -1391,18 +1400,16 @@ rclc_parameter_set_double(
}

if (parameter->value.type != RCLC_PARAMETER_DOUBLE) {
return PARAMETER_TYPE_MISMATCH;
return RCLC_PARAMETER_TYPE_MISMATCH;
}

if (parameter_server->on_modification) {
Parameter new_parameter = *parameter;
new_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 PARAMETER_MODIFICATION_REJECTED;
}
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) {
Expand Down Expand Up @@ -1544,7 +1551,7 @@ rcl_ret_t rclc_add_parameter_description(
const char * additional_constraints)
{
if (parameter_server->low_mem_mode) {
return UNSUPORTED_ON_LOW_MEM;
return RCLC_PARAMETER_UNSUPORTED_ON_LOW_MEM;
}

size_t index = rclc_parameter_search_index(&parameter_server->parameter_list, parameter_name);
Expand Down Expand Up @@ -1576,7 +1583,7 @@ rcl_ret_t rclc_set_parameter_read_only(
const char * parameter_name, bool read_only)
{
if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

size_t index = rclc_parameter_search_index(&parameter_server->parameter_list, parameter_name);
Expand All @@ -1599,7 +1606,7 @@ rcl_ret_t rclc_add_parameter_constraints_double(
double to_value, double step)
{
if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

size_t index = rclc_parameter_search_index(&parameter_server->parameter_list, parameter_name);
Expand Down Expand Up @@ -1628,7 +1635,7 @@ rcl_ret_t rclc_add_parameter_constraints_integer(
int64_t to_value, uint64_t step)
{
if (parameter_server->on_callback) {
return DISABLE_ON_CALLBACK;
return RCLC_PARAMETER_DISABLED_ON_CALLBACK;
}

size_t index = rclc_parameter_search_index(&parameter_server->parameter_list, parameter_name);
Expand Down Expand Up @@ -1659,11 +1666,11 @@ rcl_ret_t rclc_parameter_execute_callback(

if (parameter_server->on_modification) {
parameter_server->on_callback = true;
ret = parameter_server->on_modification(old_param, new_param);
ret = parameter_server->on_modification(old_param, new_param, parameter_server->context);
parameter_server->on_callback = false;
}

return ret ? RCL_RET_OK : PARAMETER_MODIFICATION_REJECTED;
return ret ? RCL_RET_OK : RCLC_PARAMETER_MODIFICATION_REJECTED;
}

#if __cplusplus
Expand Down
Loading

0 comments on commit bc6987a

Please sign in to comment.