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]> (cherry picked from commit 688bd28)
ros2 · Jun 12, 2023 · 01c4a73 · 01c4a73
1 parent 2648503
commit 01c4a73
Show file tree

Hide file tree

Showing 2 changed files with 107 additions and 1 deletion.
diff --git a/rclc_parameter/include/rclc_parameter/rclc_parameter.h b/rclc_parameter/include/rclc_parameter/rclc_parameter.h
@@ -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>
@@ -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;
 
@@ -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
@@ -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;
@@ -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;
 

diff --git a/rclc_parameter/src/rclc_parameter/parameter_server.c b/rclc_parameter/src/rclc_parameter/parameter_server.c
@@ -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,
@@ -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);
@@ -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 =
@@ -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,
@@ -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;
@@ -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]);
@@ -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);
@@ -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,