Skip to content

Commit 688bd28

Browse files
Acuadros95Acuadros95
authored
Add empty set_atomically parameter service (#354)
* Add empty set_atomically parameter service Signed-off-by: acuadros95 <[email protected]> * Fix cpplint Signed-off-by: acuadros95 <[email protected]> --------- Signed-off-by: acuadros95 <[email protected]>
1 parent 2648503 commit 688bd28

2 files changed

Lines changed: 107 additions & 1 deletion

File tree

‎rclc_parameter/include/rclc_parameter/rclc_parameter.h‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ extern "C"
3434
#include <rcl_interfaces/msg/set_parameters_result.h>
3535
#include <rcl_interfaces/srv/list_parameters.h>
3636
#include <rcl_interfaces/srv/set_parameters.h>
37+
#include <rcl_interfaces/srv/set_parameters_atomically.h>
3738
#include <rcl_interfaces/srv/describe_parameters.h>
3839
#include <rcl_interfaces/msg/parameter_descriptor.h>
3940
#include <rosidl_runtime_c/string_functions.h>
@@ -53,6 +54,10 @@ typedef struct rcl_interfaces__srv__SetParameters_Request SetParameters_Request;
5354
typedef struct rcl_interfaces__srv__SetParameters_Response SetParameters_Response;
5455
typedef struct rcl_interfaces__msg__SetParametersResult SetParameters_Result;
5556

57+
typedef struct rcl_interfaces__srv__SetParametersAtomically_Request SetParametersAtomically_Request;
58+
typedef struct rcl_interfaces__srv__SetParametersAtomically_Response
59+
SetParametersAtomically_Response;
60+
5661
typedef struct rcl_interfaces__srv__DescribeParameters_Request DescribeParameters_Request;
5762
typedef struct rcl_interfaces__srv__DescribeParameters_Response DescribeParameters_Response;
5863

@@ -67,7 +72,7 @@ typedef struct rcl_interfaces__msg__ParameterDescriptor__Sequence ParameterDescr
6772
typedef struct rcl_interfaces__msg__ParameterEvent ParameterEvent;
6873

6974
// Number of RCLC executor handles required for a parameter server
70-
#define RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES 5
75+
#define RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES 6
7176
#define RCLC_PARAMETER_MODIFICATION_REJECTED 4001
7277
#define RCLC_PARAMETER_TYPE_MISMATCH 4002
7378
#define RCLC_PARAMETER_UNSUPORTED_ON_LOW_MEM 4003
@@ -124,6 +129,7 @@ typedef struct rclc_parameter_server_t
124129
rcl_service_t get_service;
125130
rcl_service_t get_types_service;
126131
rcl_service_t set_service;
132+
rcl_service_t set_atomically_service;
127133
rcl_service_t list_service;
128134
rcl_service_t describe_service;
129135
rcl_publisher_t event_publisher;
@@ -137,6 +143,9 @@ typedef struct rclc_parameter_server_t
137143
SetParameters_Request set_request;
138144
SetParameters_Response set_response;
139145

146+
SetParametersAtomically_Request set_atomically_request;
147+
SetParametersAtomically_Response set_atomically_response;
148+
140149
ListParameters_Request list_request;
141150
ListParameters_Response list_response;
142151

‎rclc_parameter/src/rclc_parameter/parameter_server.c‎

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -327,6 +327,19 @@ rclc_parameter_server_set_service_callback(
327327
}
328328
}
329329

330+
void
331+
rclc_parameter_server_set_atomically_service_callback(
332+
const void * req,
333+
void * res,
334+
void * parameter_server)
335+
{
336+
(void) req;
337+
(void) res;
338+
(void) parameter_server;
339+
340+
return;
341+
}
342+
330343
const rclc_parameter_options_t DEFAULT_PARAMETER_SERVER_OPTIONS = {
331344
.notify_changed_over_dds = true,
332345
.max_params = 4,
@@ -430,6 +443,26 @@ init_parameter_server_memory(
430443
&parameter_server->set_response.results.data[i].reason);
431444
}
432445

446+
// Init SetAtomically service msgs
447+
mem_allocs_ok &=
448+
rcl_interfaces__srv__SetParametersAtomically_Request__init(
449+
&parameter_server->set_atomically_request);
450+
mem_allocs_ok &=
451+
rcl_interfaces__srv__SetParametersAtomically_Response__init(
452+
&parameter_server->set_atomically_response);
453+
mem_allocs_ok &= rcl_interfaces__msg__Parameter__Sequence__init(
454+
&parameter_server->set_atomically_request.parameters,
455+
options->max_params);
456+
parameter_server->set_atomically_request.parameters.size = 0;
457+
mem_allocs_ok &= rclc_parameter_descriptor_initialize_string(
458+
&parameter_server->set_atomically_response.result.reason);
459+
460+
// Set response result to unimplemented
461+
rclc_parameter_set_string(
462+
&parameter_server->set_atomically_response.result.reason,
463+
"Unimplemented service");
464+
parameter_server->set_atomically_response.result.successful = false;
465+
433466
// Init Get types service msgs
434467
mem_allocs_ok &= rcl_interfaces__srv__GetParameterTypes_Request__init(
435468
&parameter_server->get_types_request);
@@ -651,6 +684,28 @@ rcl_ret_t init_parameter_server_memory_low(
651684
&parameter_server->set_response.results.data[0].reason,
652685
RCLC_SET_ERROR_MAX_STRING_LENGTH);
653686

687+
// Init SetAtomically service msgs
688+
parameter_server->set_atomically_request.parameters.data = allocator.zero_allocate(
689+
1, sizeof(Parameter),
690+
allocator.state);
691+
parameter_server->set_atomically_request.parameters.size = 0;
692+
parameter_server->set_atomically_request.parameters.capacity = 1;
693+
694+
ret |= rclc_parameter_initialize_empty_string(
695+
&parameter_server->set_atomically_request.parameters.data[0].name,
696+
RCLC_PARAMETER_MAX_STRING_LENGTH);
697+
698+
char * unimplemented_msg = "Unimplemented service";
699+
ret |= rclc_parameter_initialize_empty_string(
700+
&parameter_server->set_atomically_response.result.reason,
701+
strlen(unimplemented_msg) + 1);
702+
703+
// Set response result to unimplemented
704+
rclc_parameter_set_string(
705+
&parameter_server->set_atomically_response.result.reason,
706+
unimplemented_msg);
707+
parameter_server->set_atomically_response.result.successful = false;
708+
654709
// Get parameter types:
655710
// - Only one parameter type can be retrieved per request
656711
parameter_server->get_types_request.names.data =
@@ -764,6 +819,14 @@ rclc_parameter_server_init_with_option(
764819
&parameter_server->set_service, node, "/set_parameters",
765820
set_ts);
766821

822+
const rosidl_service_type_support_t * set_atom_ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
823+
rcl_interfaces,
824+
srv,
825+
SetParametersAtomically);
826+
ret |= rclc_parameter_server_init_service(
827+
&parameter_server->set_atomically_service, node, "/set_parameters_atomically",
828+
set_atom_ts);
829+
767830
const rosidl_service_type_support_t * list_ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
768831
rcl_interfaces,
769832
srv,
@@ -840,6 +903,19 @@ rclc_parameter_server_fini_memory_low(
840903
parameter_server->set_response.results.capacity = 0;
841904
parameter_server->set_response.results.size = 0;
842905

906+
// Set atomically request
907+
allocator.deallocate(
908+
parameter_server->set_atomically_request.parameters.data[0].name.data,
909+
allocator.state);
910+
allocator.deallocate(parameter_server->set_atomically_request.parameters.data, allocator.state);
911+
parameter_server->set_atomically_request.parameters.capacity = 0;
912+
parameter_server->set_atomically_request.parameters.size = 0;
913+
914+
// Set atomically response
915+
allocator.deallocate(
916+
parameter_server->set_atomically_response.result.reason.data,
917+
allocator.state);
918+
843919
// List response
844920
for (size_t i = 0; i < parameter_server->list_response.result.names.capacity; ++i) {
845921
parameter_server->list_response.result.names.data[i].data = NULL;
@@ -965,6 +1041,20 @@ rclc_parameter_server_fini_memory(
9651041
rcl_interfaces__srv__SetParameters_Response__fini(&parameter_server->set_response);
9661042
rcl_interfaces__srv__SetParameters_Request__fini(&parameter_server->set_request);
9671043

1044+
// Finish set atomically msgs
1045+
for (size_t i = 0; i < parameter_server->set_atomically_request.parameters.capacity; ++i) {
1046+
rosidl_runtime_c__String__fini(
1047+
&parameter_server->set_atomically_request.parameters.data[i].name);
1048+
}
1049+
1050+
rosidl_runtime_c__String__fini(&parameter_server->set_atomically_response.result.reason);
1051+
rcl_interfaces__msg__Parameter__Sequence__fini(
1052+
&parameter_server->set_atomically_request.parameters);
1053+
rcl_interfaces__srv__SetParametersAtomically_Response__fini(
1054+
&parameter_server->set_atomically_response);
1055+
rcl_interfaces__srv__SetParametersAtomically_Request__fini(
1056+
&parameter_server->set_atomically_request);
1057+
9681058
// Finish get msgs
9691059
for (size_t i = 0; i < parameter_server->get_request.names.capacity; ++i) {
9701060
rosidl_runtime_c__String__fini(&parameter_server->get_request.names.data[i]);
@@ -1025,6 +1115,7 @@ rclc_parameter_server_fini(
10251115

10261116
ret |= rcl_service_fini(&parameter_server->list_service, node);
10271117
ret |= rcl_service_fini(&parameter_server->set_service, node);
1118+
ret |= rcl_service_fini(&parameter_server->set_atomically_service, node);
10281119
ret |= rcl_service_fini(&parameter_server->get_service, node);
10291120
ret |= rcl_service_fini(&parameter_server->get_types_service, node);
10301121
ret |= rcl_service_fini(&parameter_server->describe_service, node);
@@ -1085,6 +1176,12 @@ rclc_executor_add_parameter_server_with_context(
10851176
rclc_parameter_server_set_service_callback,
10861177
parameter_server);
10871178

1179+
ret |= rclc_executor_add_service_with_context(
1180+
executor, &parameter_server->set_atomically_service,
1181+
&parameter_server->set_atomically_request, &parameter_server->set_atomically_response,
1182+
rclc_parameter_server_set_atomically_service_callback,
1183+
parameter_server);
1184+
10881185
ret |= rclc_executor_add_service_with_context(
10891186
executor, &parameter_server->get_service,
10901187
&parameter_server->get_request, &parameter_server->get_response,

0 commit comments

Comments
 (0)