Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add empty set_atomically parameter service #354

Merged
merged 2 commits into from
Jun 12, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 10 additions & 1 deletion rclc_parameter/include/rclc_parameter/rclc_parameter.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ extern "C"
#include <rcl_interfaces/msg/set_parameters_result.h>
#include <rcl_interfaces/srv/list_parameters.h>
#include <rcl_interfaces/srv/set_parameters.h>
#include <rcl_interfaces/srv/set_parameters_atomically.h>
#include <rcl_interfaces/srv/describe_parameters.h>
#include <rcl_interfaces/msg/parameter_descriptor.h>
#include <rosidl_runtime_c/string_functions.h>
Expand All @@ -53,6 +54,10 @@ typedef struct rcl_interfaces__srv__SetParameters_Request SetParameters_Request;
typedef struct rcl_interfaces__srv__SetParameters_Response SetParameters_Response;
typedef struct rcl_interfaces__msg__SetParametersResult SetParameters_Result;

typedef struct rcl_interfaces__srv__SetParametersAtomically_Request SetParametersAtomically_Request;
typedef struct rcl_interfaces__srv__SetParametersAtomically_Response
SetParametersAtomically_Response;

typedef struct rcl_interfaces__srv__DescribeParameters_Request DescribeParameters_Request;
typedef struct rcl_interfaces__srv__DescribeParameters_Response DescribeParameters_Response;

Expand All @@ -67,7 +72,7 @@ typedef struct rcl_interfaces__msg__ParameterDescriptor__Sequence ParameterDescr
typedef struct rcl_interfaces__msg__ParameterEvent ParameterEvent;

// Number of RCLC executor handles required for a parameter server
#define RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES 5
#define RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES 6
#define RCLC_PARAMETER_MODIFICATION_REJECTED 4001
#define RCLC_PARAMETER_TYPE_MISMATCH 4002
#define RCLC_PARAMETER_UNSUPORTED_ON_LOW_MEM 4003
Expand Down Expand Up @@ -124,6 +129,7 @@ typedef struct rclc_parameter_server_t
rcl_service_t get_service;
rcl_service_t get_types_service;
rcl_service_t set_service;
rcl_service_t set_atomically_service;
rcl_service_t list_service;
rcl_service_t describe_service;
rcl_publisher_t event_publisher;
Expand All @@ -137,6 +143,9 @@ typedef struct rclc_parameter_server_t
SetParameters_Request set_request;
SetParameters_Response set_response;

SetParametersAtomically_Request set_atomically_request;
SetParametersAtomically_Response set_atomically_response;

ListParameters_Request list_request;
ListParameters_Response list_response;

Expand Down
97 changes: 97 additions & 0 deletions rclc_parameter/src/rclc_parameter/parameter_server.c
Original file line number Diff line number Diff line change
Expand Up @@ -327,6 +327,19 @@ rclc_parameter_server_set_service_callback(
}
}

void
rclc_parameter_server_set_atomically_service_callback(
const void * req,
void * res,
void * parameter_server)
{
(void) req;
(void) res;
(void) parameter_server;

return;
}

const rclc_parameter_options_t DEFAULT_PARAMETER_SERVER_OPTIONS = {
.notify_changed_over_dds = true,
.max_params = 4,
Expand Down Expand Up @@ -430,6 +443,26 @@ init_parameter_server_memory(
&parameter_server->set_response.results.data[i].reason);
}

// Init SetAtomically service msgs
mem_allocs_ok &=
rcl_interfaces__srv__SetParametersAtomically_Request__init(
&parameter_server->set_atomically_request);
mem_allocs_ok &=
rcl_interfaces__srv__SetParametersAtomically_Response__init(
&parameter_server->set_atomically_response);
mem_allocs_ok &= rcl_interfaces__msg__Parameter__Sequence__init(
&parameter_server->set_atomically_request.parameters,
options->max_params);
parameter_server->set_atomically_request.parameters.size = 0;
mem_allocs_ok &= rclc_parameter_descriptor_initialize_string(
&parameter_server->set_atomically_response.result.reason);

// Set response result to unimplemented
rclc_parameter_set_string(
&parameter_server->set_atomically_response.result.reason,
"Unimplemented service");
parameter_server->set_atomically_response.result.successful = false;

// Init Get types service msgs
mem_allocs_ok &= rcl_interfaces__srv__GetParameterTypes_Request__init(
&parameter_server->get_types_request);
Expand Down Expand Up @@ -651,6 +684,28 @@ rcl_ret_t init_parameter_server_memory_low(
&parameter_server->set_response.results.data[0].reason,
RCLC_SET_ERROR_MAX_STRING_LENGTH);

// Init SetAtomically service msgs
parameter_server->set_atomically_request.parameters.data = allocator.zero_allocate(
1, sizeof(Parameter),
allocator.state);
parameter_server->set_atomically_request.parameters.size = 0;
parameter_server->set_atomically_request.parameters.capacity = 1;

ret |= rclc_parameter_initialize_empty_string(
&parameter_server->set_atomically_request.parameters.data[0].name,
RCLC_PARAMETER_MAX_STRING_LENGTH);

char * unimplemented_msg = "Unimplemented service";
ret |= rclc_parameter_initialize_empty_string(
&parameter_server->set_atomically_response.result.reason,
strlen(unimplemented_msg) + 1);

// Set response result to unimplemented
rclc_parameter_set_string(
&parameter_server->set_atomically_response.result.reason,
unimplemented_msg);
parameter_server->set_atomically_response.result.successful = false;

// Get parameter types:
// - Only one parameter type can be retrieved per request
parameter_server->get_types_request.names.data =
Expand Down Expand Up @@ -764,6 +819,14 @@ rclc_parameter_server_init_with_option(
&parameter_server->set_service, node, "/set_parameters",
set_ts);

const rosidl_service_type_support_t * set_atom_ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
rcl_interfaces,
srv,
SetParametersAtomically);
ret |= rclc_parameter_server_init_service(
&parameter_server->set_atomically_service, node, "/set_parameters_atomically",
set_atom_ts);

const rosidl_service_type_support_t * list_ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
rcl_interfaces,
srv,
Expand Down Expand Up @@ -840,6 +903,19 @@ rclc_parameter_server_fini_memory_low(
parameter_server->set_response.results.capacity = 0;
parameter_server->set_response.results.size = 0;

// Set atomically request
allocator.deallocate(
parameter_server->set_atomically_request.parameters.data[0].name.data,
allocator.state);
allocator.deallocate(parameter_server->set_atomically_request.parameters.data, allocator.state);
parameter_server->set_atomically_request.parameters.capacity = 0;
parameter_server->set_atomically_request.parameters.size = 0;

// Set atomically response
allocator.deallocate(
parameter_server->set_atomically_response.result.reason.data,
allocator.state);

// 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;
Expand Down Expand Up @@ -965,6 +1041,20 @@ rclc_parameter_server_fini_memory(
rcl_interfaces__srv__SetParameters_Response__fini(&parameter_server->set_response);
rcl_interfaces__srv__SetParameters_Request__fini(&parameter_server->set_request);

// Finish set atomically msgs
for (size_t i = 0; i < parameter_server->set_atomically_request.parameters.capacity; ++i) {
rosidl_runtime_c__String__fini(
&parameter_server->set_atomically_request.parameters.data[i].name);
}

rosidl_runtime_c__String__fini(&parameter_server->set_atomically_response.result.reason);
rcl_interfaces__msg__Parameter__Sequence__fini(
&parameter_server->set_atomically_request.parameters);
rcl_interfaces__srv__SetParametersAtomically_Response__fini(
&parameter_server->set_atomically_response);
rcl_interfaces__srv__SetParametersAtomically_Request__fini(
&parameter_server->set_atomically_request);

// Finish get msgs
for (size_t i = 0; i < parameter_server->get_request.names.capacity; ++i) {
rosidl_runtime_c__String__fini(&parameter_server->get_request.names.data[i]);
Expand Down Expand Up @@ -1025,6 +1115,7 @@ rclc_parameter_server_fini(

ret |= rcl_service_fini(&parameter_server->list_service, node);
ret |= rcl_service_fini(&parameter_server->set_service, node);
ret |= rcl_service_fini(&parameter_server->set_atomically_service, node);
ret |= rcl_service_fini(&parameter_server->get_service, node);
ret |= rcl_service_fini(&parameter_server->get_types_service, node);
ret |= rcl_service_fini(&parameter_server->describe_service, node);
Expand Down Expand Up @@ -1085,6 +1176,12 @@ rclc_executor_add_parameter_server_with_context(
rclc_parameter_server_set_service_callback,
parameter_server);

ret |= rclc_executor_add_service_with_context(
executor, &parameter_server->set_atomically_service,
&parameter_server->set_atomically_request, &parameter_server->set_atomically_response,
rclc_parameter_server_set_atomically_service_callback,
parameter_server);

ret |= rclc_executor_add_service_with_context(
executor, &parameter_server->get_service,
&parameter_server->get_request, &parameter_server->get_response,
Expand Down