diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 5a0ec1513..fb2fe8e58 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -23,6 +23,9 @@ set(${PROJECT_NAME}_sources src/rcl/graph.c src/rcl/guard_condition.c src/rcl/node.c + src/rcl/parameter.c + src/rcl/parameter_client.c + src/rcl/parameter_service.c src/rcl/publisher.c src/rcl/rcl.c src/rcl/rmw_implementation_identifier_check.c diff --git a/rcl/include/rcl/parameter.h b/rcl/include/rcl/parameter.h new file mode 100644 index 000000000..176d735e5 --- /dev/null +++ b/rcl/include/rcl/parameter.h @@ -0,0 +1,161 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__PARAMETER_H_ +#define RCL__PARAMETER_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl_interfaces/msg/parameter.h" +#include "rcl_interfaces/msg/parameter_event.h" + +typedef int rcl_param_action_t; +#define RCL_GET_PARAMETERS 0 +#define RCL_GET_PARAMETER_TYPES 1 +#define RCL_SET_PARAMETERS 2 +#define RCL_SET_PARAMETERS_ATOMICALLY 3 +#define RCL_LIST_PARAMETERS 4 +#define RCL_NUMBER_OF_PARAMETER_ACTIONS 5 +#define RCL_PARAMETER_ACTION_UNKNOWN 6 + +/// rcl_parameter_client_set_ adds the parameter key, value pair to the +/// pending set_parameters_request + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_bool( + rcl_interfaces__msg__Parameter * parameter, const char * parameter_name, bool value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_integer( + rcl_interfaces__msg__Parameter * parameter, const char * parameter_name, int value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_double( + rcl_interfaces__msg__Parameter * parameter, const char * parameter_name, double value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_string( + rcl_interfaces__msg__Parameter * parameter, + const char * parameter_name, + const char * value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_bytes( + rcl_interfaces__msg__Parameter * parameter, + const char * parameter_name, + const uint8_t * value); + +// etc. for all types + +/// Get the boolean value of the parameter, or return error if the parameter is not a boolean. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_get_bool(const rcl_interfaces__msg__Parameter * parameter, bool * output); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_get_int(const rcl_interfaces__msg__Parameter * parameter, int * output); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_get_double(const rcl_interfaces__msg__Parameter * parameter, double * output); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_get_string( + const rcl_interfaces__msg__Parameter * parameter, rosidl_generator_c__String * output); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_get_bytes( + const rcl_interfaces__msg__Parameter * parameter, rosidl_generator_c__byte__Array * output); + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_value_bool( + rcl_interfaces__msg__ParameterValue * parameter_value, bool value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_value_integer( + rcl_interfaces__msg__ParameterValue * parameter_value, int value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_value_double( + rcl_interfaces__msg__ParameterValue * parameter_value, double value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_value_string( + rcl_interfaces__msg__ParameterValue * parameter_value, + const char * value); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_set_value_bytes( + rcl_interfaces__msg__ParameterValue * parameter_value, + const uint8_t * value); + +// etc. for all types + +// Some other convenience function ideas: +// Get type enum from a ParameterValue +// rcl_parameter_get_type(rcl_interfaces__msg__ParameterValue * parameter_value, uint8 type) + +// Get type enum from a Parameter +// rcl_parameter_get_type(rcl_interfaces__msg__Parameter * parameter, uint8 type) + +bool rcl_parameter_value_compare( + const rcl_interfaces__msg__ParameterValue * parameter1, + const rcl_interfaces__msg__ParameterValue * parameter2); + +// diff the new state and the old state of parameters to populate a ParameterEvent message struct +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_convert_changes_to_event( + const rcl_interfaces__msg__Parameter__Array * prior_state, + const rcl_interfaces__msg__Parameter__Array * new_state, + rcl_interfaces__msg__ParameterEvent * parameter_event); + +#if __cplusplus +} +#endif + +#endif // RCL__PARAMETER_H_ diff --git a/rcl/include/rcl/parameter_client.h b/rcl/include/rcl/parameter_client.h new file mode 100644 index 000000000..aedd1ac63 --- /dev/null +++ b/rcl/include/rcl/parameter_client.h @@ -0,0 +1,182 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__PARAMETER_CLIENT_H_ +#define RCL__PARAMETER_CLIENT_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include + +#include "rcl/client.h" +#include "rcl/node.h" +#include "rcl/parameter.h" +#include "rcl/wait.h" + +/// TODO: documentation!!! + +/// Internal rcl implementation struct +struct rcl_parameter_client_impl_t; + +/// There is no sync/async parameter client distinction in rcl. +typedef struct rcl_parameter_client_t +{ + struct rcl_parameter_client_impl_t * impl; +} rcl_parameter_client_t; + +typedef struct rcl_parameter_client_options_t +{ + // quality of service settings for all parameter-related services + rmw_qos_profile_t qos; + rcl_allocator_t allocator; + char * remote_node_name; + rmw_qos_profile_t parameter_event_qos; +} rcl_parameter_client_options_t; + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_parameter_client_options_t +rcl_parameter_client_get_default_options(void); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_parameter_client_t +rcl_get_zero_initialized_parameter_client(void); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_init( + rcl_parameter_client_t * parameter_client, + rcl_node_t * node, + const rcl_parameter_client_options_t * options +); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_fini(rcl_parameter_client_t * parameter_client); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_send_set_request( + const rcl_parameter_client_t * parameter_client, + const rcl_interfaces__msg__Parameter__Array * parameters, + int64_t * sequence_number); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_interfaces__msg__SetParametersResult__Array * +rcl_parameter_client_take_set_response( + const rcl_parameter_client_t * parameter_client, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_send_get_request( + const rcl_parameter_client_t * client, + const rosidl_generator_c__String__Array * names, + int64_t * sequence_number); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_interfaces__msg__ParameterValue__Array * +rcl_parameter_client_take_get_response( + const rcl_parameter_client_t * client, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_send_get_types_request( + const rcl_parameter_client_t * client, + const rosidl_generator_c__String__Array * parameter_names, + int64_t * sequence_number); + +RCL_PUBLIC +RCL_WARN_UNUSED +rosidl_generator_c__uint8__Array * +rcl_parameter_client_take_get_types_response( + const rcl_parameter_client_t * client, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_send_set_atomically_request( + const rcl_parameter_client_t * client, + const rcl_interfaces__msg__Parameter__Array * parameter_values, + int64_t * sequence_number); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_interfaces__msg__SetParametersResult * +rcl_parameter_client_take_set_atomically_response( + const rcl_parameter_client_t * client, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_send_list_request( + const rcl_parameter_client_t * client, + const rosidl_generator_c__String__Array * prefixes, + uint64_t depth, + int64_t * sequence_number); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_interfaces__msg__ListParametersResult * +rcl_parameter_client_take_list_response( + const rcl_parameter_client_t * client, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_take_event( + const rcl_parameter_client_t * client, + rcl_interfaces__msg__ParameterEvent * parameter_event, + rmw_message_info_t * message_info +); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_parameter_client( + rcl_wait_set_t * wait_set, + const rcl_parameter_client_t * parameter_client); + +// To be called after rcl_wait +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_client_get_pending_action( + const rcl_wait_set_t * wait_set, + const rcl_parameter_client_t * parameter_client, + rcl_param_action_t * action); + +#if __cplusplus +} +#endif + +#endif // RCL__PARAMETER_CLIENT_H_ diff --git a/rcl/include/rcl/parameter_service.h b/rcl/include/rcl/parameter_service.h new file mode 100644 index 000000000..eeed9d407 --- /dev/null +++ b/rcl/include/rcl/parameter_service.h @@ -0,0 +1,179 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__PARAMETER_SERVICE_H_ +#define RCL__PARAMETER_SERVICE_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include + +#include "rcl/node.h" +#include "rcl/parameter.h" +#include "rcl/service.h" +#include "rcl/wait.h" + +struct rcl_parameter_service_impl_t; + +typedef struct rcl_parameter_service_t +{ + struct rcl_parameter_service_impl_t * impl; +} rcl_parameter_service_t; + +typedef struct rcl_parameter_service_options_t +{ + // quality of service settings for all services + rmw_qos_profile_t qos; + rmw_qos_profile_t parameter_event_qos; + rcl_allocator_t allocator; + char * remote_node_name; +} rcl_parameter_service_options_t; + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_parameter_service_options_t +rcl_parameter_service_get_default_options(void); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_parameter_service_t +rcl_get_zero_initialized_parameter_service(void); + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_init( + rcl_parameter_service_t * parameter_service, + rcl_node_t * node, + const rcl_parameter_service_options_t * options +); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_fini(rcl_parameter_service_t * parameter_service); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_interfaces__msg__Parameter__Array * +rcl_parameter_service_take_set_request( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_send_set_response( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header, + const rcl_interfaces__msg__SetParametersResult__Array * set_parameter_results); + +RCL_PUBLIC +RCL_WARN_UNUSED +rosidl_generator_c__String__Array * +rcl_parameter_service_take_get_request( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_send_get_response( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header, + const rcl_interfaces__msg__ParameterValue__Array * parameters); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_send_get_types_response( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header, + const rosidl_generator_c__uint8__Array * parameter_types); + +RCL_PUBLIC +RCL_WARN_UNUSED +rosidl_generator_c__String__Array * +rcl_parameter_service_take_get_types_request( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_interfaces__msg__Parameter__Array * +rcl_parameter_service_take_set_atomically_request( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_send_set_atomically_response( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header, + const rcl_interfaces__msg__SetParametersResult * set_parameters_result); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_take_list_request( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header, + rosidl_generator_c__String__Array * prefixes, + uint64_t * depth); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_send_list_response( + const rcl_parameter_service_t * service, + rmw_request_id_t * request_header, + const rcl_interfaces__msg__ListParametersResult * set_parameters_result); + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_publish_event( + const rcl_parameter_service_t * service, + const rcl_interfaces__msg__ParameterEvent * event); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_parameter_service( + rcl_wait_set_t * wait_set, + const rcl_parameter_service_t * parameter_service); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_get_pending_action( + const rcl_wait_set_t * wait_set, + const rcl_parameter_service_t * parameter_service, + rcl_param_action_t * action +); + +#if __cplusplus +} +#endif + +#endif // RCL__PARAMETER_SERVICE_H_ diff --git a/rcl/src/rcl/parameter.c b/rcl/src/rcl/parameter.c new file mode 100644 index 000000000..b556d3783 --- /dev/null +++ b/rcl/src/rcl/parameter.c @@ -0,0 +1,263 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#include +#endif + +#include +#include +#include +#include + +#include +#include "rosidl_generator_c/string_functions.h" +#include "rcl/types.h" +#include "./common.h" + +#define RCL_DEFINE_SET_PARAMETER(TYPE, CTYPE, ENUM_TYPE) \ + rcl_ret_t \ + rcl_parameter_set_ ## TYPE( \ + rcl_interfaces__msg__Parameter * parameter, const char * parameter_name, CTYPE value) \ + { \ + if (!rosidl_generator_c__String__assign(¶meter->name, parameter_name)) { \ + return RCL_RET_ERROR; \ + } \ + parameter->value.type = rcl_interfaces__msg__ParameterType__ ## ENUM_TYPE; \ + parameter->value.TYPE ## _value = value; \ + return RCL_RET_OK; \ + } + +#define RCL_DEFINE_SET_PARAMETER_ARRAY_TYPE(TYPE, ROSIDL_TYPE, CTYPE, ENUM_TYPE) \ + rcl_ret_t \ + rcl_parameter_set_ ## TYPE( \ + rcl_interfaces__msg__Parameter * parameter, const char * parameter_name, const CTYPE * value) \ + { \ + if (!rosidl_generator_c__String__assign(¶meter->name, parameter_name)) { \ + return RCL_RET_ERROR; \ + } \ + parameter->value.type = rcl_interfaces__msg__ParameterType__ ## ENUM_TYPE; \ + ROSIDL_TYPE ## __assign(¶meter->value.TYPE ## _value, value); \ + return RCL_RET_OK; \ + } + +RCL_DEFINE_SET_PARAMETER(bool, bool, PARAMETER_BOOL) +RCL_DEFINE_SET_PARAMETER(integer, int, PARAMETER_INTEGER) +RCL_DEFINE_SET_PARAMETER(double, double, PARAMETER_DOUBLE) + +RCL_DEFINE_SET_PARAMETER_ARRAY_TYPE(string, rosidl_generator_c__String, char, PARAMETER_STRING) + +#define RCL_DEFINE_SET_PARAMETER_VALUE(TYPE, CTYPE, ENUM_TYPE) \ + rcl_ret_t \ + rcl_parameter_set_value_ ## TYPE( \ + rcl_interfaces__msg__ParameterValue * parameter_value, CTYPE value) \ + { \ + parameter_value->type = rcl_interfaces__msg__ParameterType__ ## ENUM_TYPE; \ + parameter_value->TYPE ## _value = value; \ + return RCL_RET_OK; \ + } + +#define RCL_DEFINE_SET_PARAMETER_VALUE_ARRAY_TYPE(TYPE, ROSIDL_TYPE, CTYPE, ENUM_TYPE) \ + rcl_ret_t \ + rcl_parameter_set_value_ ## TYPE( \ + rcl_interfaces__msg__ParameterValue * parameter_value, const CTYPE * value) \ + { \ + parameter_value->type = rcl_interfaces__msg__ParameterType__ ## ENUM_TYPE; \ + ROSIDL_TYPE ## __assign(¶meter_value->TYPE ## _value, value); \ + return RCL_RET_OK; \ + } + +RCL_DEFINE_SET_PARAMETER_VALUE(bool, bool, PARAMETER_BOOL) +RCL_DEFINE_SET_PARAMETER_VALUE(integer, int, PARAMETER_INTEGER) +RCL_DEFINE_SET_PARAMETER_VALUE(double, double, PARAMETER_DOUBLE) + +RCL_DEFINE_SET_PARAMETER_VALUE_ARRAY_TYPE(string, rosidl_generator_c__String, char, + PARAMETER_STRING) + +// Check if two parameters are equal +bool rcl_parameter_value_compare(const rcl_interfaces__msg__ParameterValue * parameter1, + const rcl_interfaces__msg__ParameterValue * parameter2) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter1, false); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter2, false); + if (parameter1->type != parameter2->type) { + return false; + } + switch (parameter1->type) { + case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL: + return parameter1->bool_value == parameter2->bool_value; + case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER: + return parameter1->integer_value == parameter2->integer_value; + case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE: + return parameter1->double_value == parameter2->double_value; + case rcl_interfaces__msg__ParameterType__PARAMETER_STRING: + return strcmp(parameter1->string_value.data, parameter2->string_value.data) != 0; + case rcl_interfaces__msg__ParameterType__PARAMETER_BYTES: + // Not implemented + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_NOT_SET: + default: + return false; + } + + return false; +} + +rcl_ret_t +rcl_parameter_value_copy(rcl_interfaces__msg__ParameterValue * dst, + const rcl_interfaces__msg__ParameterValue * src) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(dst, false); + RCL_CHECK_ARGUMENT_FOR_NULL(src, false); + + dst->type = src->type; + + switch (src->type) { + case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL: + dst->bool_value = src->bool_value; + return RCL_RET_OK; + case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER: + dst->integer_value = src->integer_value; + return RCL_RET_OK; + case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE: + dst->double_value = src->double_value; + return RCL_RET_OK; + case rcl_interfaces__msg__ParameterType__PARAMETER_STRING: + return rosidl_generator_c__String__assign( + &dst->string_value, src->string_value.data) ? RCL_RET_OK : RCL_RET_ERROR; + case rcl_interfaces__msg__ParameterType__PARAMETER_BYTES: + // Not implemented + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_NOT_SET: + default: + return RCL_RET_ERROR; + } + return RCL_RET_ERROR; +} + +rcl_ret_t +rcl_parameter_copy(rcl_interfaces__msg__Parameter * dst, + const rcl_interfaces__msg__Parameter * src) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(dst, false); + RCL_CHECK_ARGUMENT_FOR_NULL(src, false); + if (!rosidl_generator_c__String__assign(&dst->name, src->name.data)) { + return RCL_RET_ERROR; + } + return rcl_parameter_value_copy(&dst->value, &src->value); +} + +rcl_ret_t +rcl_parameter_convert_changes_to_event( + const rcl_interfaces__msg__Parameter__Array * prior_state, + const rcl_interfaces__msg__Parameter__Array * new_state, + rcl_interfaces__msg__ParameterEvent * parameter_event) +{ + // Diff the prior state and the new state and fill the parameter_event struct + RCL_CHECK_ARGUMENT_FOR_NULL(prior_state, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(new_state, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_event, RCL_RET_INVALID_ARGUMENT); + + size_t prior_idx, new_idx; + size_t num_deleted_params = 0; + size_t num_changed_params = 0; + size_t num_new_params = 0; + // go through and count the numbers we're going to need + for (prior_idx = 0; prior_idx < prior_state->size; ++prior_idx) { + rcl_interfaces__msg__Parameter * prior_entry = &prior_state->data[prior_idx]; + for (new_idx = 0; new_idx < new_state->size; ++new_idx) { + rcl_interfaces__msg__Parameter * new_entry = &new_state->data[new_idx]; + // If the parameter has the same name but a different type or value, count it as changed. + if (strcmp(prior_entry->name.data, new_entry->name.data) == 0) { + if (!rcl_parameter_value_compare(&prior_entry->value, &new_entry->value)) { + num_changed_params++; + } + break; + } + } + if (new_idx == new_state->size) { + // No entry in new_state was found with a matching name to prior_entry (deleted) + num_deleted_params++; + } + } + // Could optimize this if we allocated some space to store matches from the previous comparisons + for (new_idx = 0; new_idx < new_state->size; ++new_idx) { + rcl_interfaces__msg__Parameter * new_entry = &new_state->data[new_idx]; + for (prior_idx = 0; prior_idx < prior_state->size; ++prior_idx) { + rcl_interfaces__msg__Parameter * prior_entry = &prior_state->data[prior_idx]; + if (strcmp(prior_entry->name.data, new_entry->name.data) == 0) { + break; + } + } + if (prior_idx == prior_state->size) { + num_new_params++; + } + } + + rcl_interfaces__msg__Parameter__Array__fini(¶meter_event->deleted_parameters); + rcl_interfaces__msg__Parameter__Array__fini(¶meter_event->changed_parameters); + rcl_interfaces__msg__Parameter__Array__fini(¶meter_event->new_parameters); + + rcl_interfaces__msg__Parameter__Array__init(¶meter_event->deleted_parameters, + num_deleted_params); + rcl_interfaces__msg__Parameter__Array__init(¶meter_event->changed_parameters, + num_changed_params); + rcl_interfaces__msg__Parameter__Array__init(¶meter_event->new_parameters, num_new_params); + + size_t new_array_idx = 0; + size_t deleted_idx = 0; + size_t changed_idx = 0; + + for (prior_idx = 0; prior_idx < prior_state->size; ++prior_idx) { + rcl_interfaces__msg__Parameter * prior_entry = &prior_state->data[prior_idx]; + for (new_idx = 0; new_idx < new_state->size; ++new_idx) { + rcl_interfaces__msg__Parameter * new_entry = &new_state->data[new_idx]; + // If the parameter has the same name but a different type or value, count it as changed. + if (strcmp(prior_entry->name.data, new_entry->name.data) == 0) { + if (!rcl_parameter_value_compare(&prior_entry->value, &new_entry->value)) { + rcl_parameter_copy(¶meter_event->changed_parameters.data[changed_idx++], new_entry); + } + break; + } + } + if (new_idx == new_state->size) { + // If no entry in new_state was found with a matching name to prior_entry, count as deleted. + rcl_parameter_copy(¶meter_event->deleted_parameters.data[deleted_idx++], prior_entry); + } + } + // Could optimize this if we allocated some space to store matches from the previous comparisons + for (new_idx = 0; new_idx < new_state->size; ++new_idx) { + rcl_interfaces__msg__Parameter * new_entry = &new_state->data[new_idx]; + for (prior_idx = 0; prior_idx < prior_state->size; ++prior_idx) { + rcl_interfaces__msg__Parameter * prior_entry = &prior_state->data[prior_idx]; + if (strcmp(prior_entry->name.data, new_entry->name.data) == 0) { + break; + } + } + if (prior_idx == prior_state->size) { + rcl_parameter_copy(¶meter_event->new_parameters.data[new_array_idx++], new_entry); + } + } + parameter_event->changed_parameters.size = num_changed_params; + parameter_event->deleted_parameters.size = num_deleted_params; + parameter_event->new_parameters.size = num_new_params; + + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/parameter_client.c b/rcl/src/rcl/parameter_client.c new file mode 100644 index 000000000..2ca25f874 --- /dev/null +++ b/rcl/src/rcl/parameter_client.c @@ -0,0 +1,437 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "rmw/qos_profiles.h" + +#include "rcl/error_handling.h" +#include "rcl/parameter_client.h" +#include "rcl/subscription.h" + +#include "./common.h" + +typedef struct rcl_parameter_client_impl_t +{ + rcl_parameter_client_options_t options; + rcl_node_t * node; + rcl_client_t get_client; + rcl_client_t get_types_client; // Also referred to as describe parameters in docs + rcl_client_t set_client; + rcl_client_t set_atomically_client; + rcl_client_t list_client; + + rcl_subscription_t event_subscription; + + // Storage for requests/responses for each client + rcl_interfaces__srv__GetParameters_Request get_request; + rcl_interfaces__srv__GetParameters_Response get_response; + + rcl_interfaces__srv__GetParameterTypes_Request get_types_request; + rcl_interfaces__srv__GetParameterTypes_Response get_types_response; + + rcl_interfaces__srv__SetParameters_Request set_request; + rcl_interfaces__srv__SetParameters_Response set_response; + + rcl_interfaces__srv__SetParametersAtomically_Request set_atomically_request; + rcl_interfaces__srv__SetParametersAtomically_Response set_atomically_response; + + rcl_interfaces__srv__ListParameters_Request list_request; + rcl_interfaces__srv__ListParameters_Response list_response; + + int64_t get_sequence_number; + int64_t get_types_sequence_number; + int64_t set_sequence_number; + int64_t set_atomically_sequence_number; + int64_t list_sequence_number; +} rcl_parameter_client_impl_t; + +rcl_parameter_client_options_t +rcl_parameter_client_get_default_options(void) +{ + static rcl_parameter_client_options_t default_options; + default_options.qos = rmw_qos_profile_parameters; + default_options.parameter_event_qos = rmw_qos_profile_parameter_events; + default_options.allocator = rcl_get_default_allocator(); + default_options.remote_node_name = NULL; + return default_options; +} + +rcl_parameter_client_t +rcl_get_zero_initialized_parameter_client(void) +{ + static rcl_parameter_client_t null_client = {0}; + return null_client; +} + +#define RCL_PARAMETER_INITIALIZE_CLIENT(VERB, SRV_TYPE_NAME, SRV_SUFFIX) \ + { \ + const rosidl_service_type_support_t * VERB ## _ts = ROSIDL_GET_TYPE_SUPPORT( \ + rcl_interfaces, srv, SRV_TYPE_NAME \ + ); \ + \ + size_t VERB ## len = strlen(node_name) + strlen(SRV_SUFFIX) + 1; \ + char * VERB ## _service_name = (char *)options->allocator.allocate( \ + VERB ## len, options->allocator.state); \ + memset(VERB ## _service_name, 0, VERB ## len); \ + VERB ## _service_name = memcpy( \ + VERB ## _service_name, node_name, strlen(node_name) + 1); \ + memcpy((VERB ## _service_name + strlen(node_name)), \ + SRV_SUFFIX, strlen(SRV_SUFFIX) + 1); \ + \ + ret = rcl_client_init( \ + ¶meter_client->impl->VERB ## _client, \ + node, \ + VERB ## _ts, \ + VERB ## _service_name, \ + &client_options); \ + options->allocator.deallocate(VERB ## _service_name, options->allocator.state); \ + if (ret != RCL_RET_OK) { \ + fail_ret = ret; \ + goto fail_ ## VERB; \ + } \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Request__init( \ + ¶meter_client->impl->VERB ## _request); \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Response__init( \ + ¶meter_client->impl->VERB ## _response); \ + parameter_client->impl->VERB ## _sequence_number = 0; \ + } \ + +#define RCL_PARAMETER_CLIENT_FINI(VERB, SRV_TYPE_NAME) \ + ret = rcl_client_fini(¶meter_client->impl->VERB ## _client, parameter_client->impl->node); \ + if (ret != RCL_RET_OK) { \ + fprintf(stderr, "rcl_client_fini failed for client " #VERB "\n"); \ + fail_ret = ret; \ + } \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Request__fini( \ + ¶meter_client->impl->VERB ## _request); \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Response__fini( \ + ¶meter_client->impl->VERB ## _response); \ + + +rcl_ret_t +rcl_parameter_client_init( + rcl_parameter_client_t * parameter_client, + rcl_node_t * node, + const rcl_parameter_client_options_t * options +) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (!node->impl) { + RCL_SET_ERROR_MSG("invalid node"); + return RCL_RET_NODE_INVALID; + } + if (parameter_client->impl) { + RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + + rcl_ret_t ret; + rcl_ret_t fail_ret = RCL_RET_ERROR; + + parameter_client->impl = (rcl_parameter_client_impl_t *)options->allocator.allocate( + sizeof(rcl_parameter_client_impl_t), options->allocator.state); + if (!parameter_client->impl) { + goto fail_alloc; + } + memset(parameter_client->impl, 0, sizeof(rcl_parameter_client_impl_t)); + + parameter_client->impl->node = node; + rcl_client_options_t client_options = rcl_client_get_default_options(); + client_options.qos = options->qos; + client_options.allocator = options->allocator; + + const char * node_name = options->remote_node_name != + NULL ? node_name = options->remote_node_name : rcl_node_get_name(node); + + // Initialize all clients in impl storage + RCL_PARAMETER_INITIALIZE_CLIENT(get, GetParameters, "__get_parameters"); + RCL_PARAMETER_INITIALIZE_CLIENT(get_types, GetParameterTypes, "__get_parameter_types"); + RCL_PARAMETER_INITIALIZE_CLIENT(set, SetParameters, "__set_parameters"); + RCL_PARAMETER_INITIALIZE_CLIENT(set_atomically, SetParametersAtomically, + "__set_parameters_atomically"); + RCL_PARAMETER_INITIALIZE_CLIENT(list, ListParameters, "__list_parameters"); + + const rosidl_message_type_support_t * event_ts = ROSIDL_GET_TYPE_SUPPORT( + rcl_interfaces, msg, ParameterEvent); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + subscription_options.allocator = options->allocator; + subscription_options.qos = options->parameter_event_qos; + const char * event_topic_name = "parameter_events"; + parameter_client->impl->event_subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init( + ¶meter_client->impl->event_subscription, node, event_ts, event_topic_name, + &subscription_options); + if (ret != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + + parameter_client->impl->options = *options; + return RCL_RET_OK; + +fail: + ret = rcl_subscription_fini(¶meter_client->impl->event_subscription, + parameter_client->impl->node); + if (ret != RCL_RET_OK) { + fail_ret = ret; + fprintf(stderr, "rcl_subscription_fini failed in fail block of rcl_parameter_client_init\n"); + } + +fail_list: + RCL_PARAMETER_CLIENT_FINI(list, ListParameters); + +fail_set_atomically: + RCL_PARAMETER_CLIENT_FINI(set_atomically, SetParametersAtomically); + +fail_set: + RCL_PARAMETER_CLIENT_FINI(set, SetParameters); + +fail_get_types: + RCL_PARAMETER_CLIENT_FINI(get_types, GetParameterTypes); + +fail_get: + RCL_PARAMETER_CLIENT_FINI(get, GetParameters); + +fail_alloc: + + return fail_ret; +} + + +rcl_ret_t +rcl_parameter_client_fini(rcl_parameter_client_t * parameter_client) +{ + rcl_ret_t ret; + rcl_ret_t fail_ret = RCL_RET_OK; + RCL_PARAMETER_CLIENT_FINI(get, GetParameters); + RCL_PARAMETER_CLIENT_FINI(get_types, GetParameterTypes); + RCL_PARAMETER_CLIENT_FINI(set, SetParameters); + RCL_PARAMETER_CLIENT_FINI(set_atomically, SetParametersAtomically); + RCL_PARAMETER_CLIENT_FINI(list, ListParameters); + + ret = rcl_subscription_fini(¶meter_client->impl->event_subscription, + parameter_client->impl->node); + if (ret != RCL_RET_OK) { + fail_ret = ret; + } + + rcl_allocator_t allocator = parameter_client->impl->options.allocator; + allocator.deallocate(parameter_client->impl, allocator.state); + + if (fail_ret != RCL_RET_OK) { + return fail_ret; + } + return ret; +} + + +#define DEFINE_RCL_PARAMETER_CLIENT_SEND_REQUEST(VERB, REQUEST_SUBTYPE, SUBFIELD_NAME) \ + rcl_ret_t \ + rcl_parameter_client_send_ ## VERB ## _request( \ + const rcl_parameter_client_t * parameter_client, \ + const REQUEST_SUBTYPE * SUBFIELD_NAME, \ + int64_t * sequence_number) \ + { \ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_client, RCL_RET_INVALID_ARGUMENT); \ + RCL_CHECK_ARGUMENT_FOR_NULL(SUBFIELD_NAME, RCL_RET_INVALID_ARGUMENT); \ + RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT); \ + \ + parameter_client->impl->VERB ## _request.SUBFIELD_NAME = *SUBFIELD_NAME; \ + \ + rcl_ret_t ret = rcl_send_request( \ + ¶meter_client->impl->VERB ## _client, ¶meter_client->impl->VERB ## _request, \ + ¶meter_client->impl->VERB ## _sequence_number); \ + *sequence_number = parameter_client->impl->VERB ## _sequence_number; \ + return ret; \ + } + +DEFINE_RCL_PARAMETER_CLIENT_SEND_REQUEST(get, rosidl_generator_c__String__Array, names) +DEFINE_RCL_PARAMETER_CLIENT_SEND_REQUEST(get_types, rosidl_generator_c__String__Array, names) +DEFINE_RCL_PARAMETER_CLIENT_SEND_REQUEST(set, rcl_interfaces__msg__Parameter__Array, parameters) +DEFINE_RCL_PARAMETER_CLIENT_SEND_REQUEST(set_atomically, rcl_interfaces__msg__Parameter__Array, + parameters) + +rcl_ret_t +rcl_parameter_client_send_list_request( + const rcl_parameter_client_t * parameter_client, + const rosidl_generator_c__String__Array * prefixes, + uint64_t depth, + int64_t * sequence_number) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(prefixes, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT); + + parameter_client->impl->list_request.prefixes = *prefixes; + parameter_client->impl->list_request.depth = depth; + + rcl_ret_t ret = rcl_send_request( + ¶meter_client->impl->list_client, ¶meter_client->impl->list_request, + ¶meter_client->impl->list_sequence_number); + *sequence_number = parameter_client->impl->list_sequence_number; + return ret; +} + +#define DEFINE_RCL_PARAMETER_CLIENT_TAKE_RESPONSE(VERB, REQUEST_SUBTYPE, SUBFIELD_NAME) \ + REQUEST_SUBTYPE * \ + rcl_parameter_client_take_ ## VERB ## _response( \ + const rcl_parameter_client_t * parameter_client, \ + rmw_request_id_t * request_header) \ + { \ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_client, NULL); \ + \ + rcl_ret_t ret = rcl_take_response( \ + ¶meter_client->impl->VERB ## _client, request_header, \ + ¶meter_client->impl->VERB ## _response); \ + if (ret != RCL_RET_OK) { \ + return NULL; \ + } \ + \ + return ¶meter_client->impl->VERB ## _response.SUBFIELD_NAME; \ + } + +DEFINE_RCL_PARAMETER_CLIENT_TAKE_RESPONSE(get, rcl_interfaces__msg__ParameterValue__Array, values) +DEFINE_RCL_PARAMETER_CLIENT_TAKE_RESPONSE(get_types, rosidl_generator_c__uint8__Array, types) +DEFINE_RCL_PARAMETER_CLIENT_TAKE_RESPONSE(set, rcl_interfaces__msg__SetParametersResult__Array, + results) +DEFINE_RCL_PARAMETER_CLIENT_TAKE_RESPONSE(set_atomically, rcl_interfaces__msg__SetParametersResult, + result) +DEFINE_RCL_PARAMETER_CLIENT_TAKE_RESPONSE(list, rcl_interfaces__msg__ListParametersResult, result) + +rcl_ret_t +rcl_parameter_client_take_event( + const rcl_parameter_client_t * parameter_client, + rcl_interfaces__msg__ParameterEvent * parameter_event, + rmw_message_info_t * message_info +) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_event, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = rcl_take(¶meter_client->impl->event_subscription, parameter_event, + message_info); + + return ret; +} + +rcl_ret_t +rcl_wait_set_add_parameter_client( + rcl_wait_set_t * wait_set, + const rcl_parameter_client_t * parameter_client) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_client, RCL_RET_INVALID_ARGUMENT); + rcl_ret_t ret; + + ret = rcl_wait_set_add_client(wait_set, ¶meter_client->impl->get_client); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add get_parameters client to waitset!"); + return ret; + } + ret = rcl_wait_set_add_client(wait_set, ¶meter_client->impl->get_types_client); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add get_parameter_types client to waitset!"); + return ret; + } + ret = rcl_wait_set_add_client(wait_set, ¶meter_client->impl->set_client); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add set_parameters client to waitset!"); + return ret; + } + ret = rcl_wait_set_add_client(wait_set, ¶meter_client->impl->set_atomically_client); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add set_parameters_atomically client to waitset!"); + return ret; + } + ret = rcl_wait_set_add_client(wait_set, ¶meter_client->impl->list_client); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add list_parameters client to waitset!"); + return ret; + } + + ret = rcl_wait_set_add_subscription(wait_set, ¶meter_client->impl->event_subscription); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add parameter events subscription to waitset!"); + return ret; + } + + return ret; +} + +rcl_ret_t +rcl_parameter_client_get_pending_action( + const rcl_wait_set_t * wait_set, + const rcl_parameter_client_t * parameter_client, + rcl_param_action_t * action) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(action, RCL_RET_INVALID_ARGUMENT); + size_t i = 0; + size_t j = 0; + + for (i = 0; i < wait_set->size_of_clients; ++i) { + for (j = 0; j < RCL_NUMBER_OF_PARAMETER_ACTIONS; ++j) { + rcl_client_t * client_ptr = NULL; + *action = j; + switch (j) { + case RCL_GET_PARAMETERS: + client_ptr = ¶meter_client->impl->get_client; + break; + case RCL_GET_PARAMETER_TYPES: + client_ptr = ¶meter_client->impl->get_types_client; + break; + case RCL_SET_PARAMETERS: + client_ptr = ¶meter_client->impl->set_client; + break; + case RCL_SET_PARAMETERS_ATOMICALLY: + client_ptr = ¶meter_client->impl->set_atomically_client; + break; + case RCL_LIST_PARAMETERS: + client_ptr = ¶meter_client->impl->list_client; + break; + default: + return RCL_RET_ERROR; + } + if (client_ptr == wait_set->clients[i]) { + return RCL_RET_OK; + } + } + *action = RCL_PARAMETER_ACTION_UNKNOWN; + } + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/parameter_service.c b/rcl/src/rcl/parameter_service.c new file mode 100644 index 000000000..26d3b6a1a --- /dev/null +++ b/rcl/src/rcl/parameter_service.c @@ -0,0 +1,415 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "rmw/qos_profiles.h" + +#include "rcl/parameter_service.h" +#include "rcl/publisher.h" +#include "./common.h" + +typedef struct rcl_parameter_service_impl_t +{ + rcl_parameter_service_options_t options; + rcl_node_t * node; + 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_publisher_t event_publisher; + + rcl_interfaces__srv__GetParameters_Request get_request; + rcl_interfaces__srv__GetParameters_Response get_response; + + rcl_interfaces__srv__GetParameterTypes_Request get_types_request; + rcl_interfaces__srv__GetParameterTypes_Response get_types_response; + + rcl_interfaces__srv__SetParameters_Request set_request; + rcl_interfaces__srv__SetParameters_Response set_response; + + rcl_interfaces__srv__SetParametersAtomically_Request set_atomically_request; + rcl_interfaces__srv__SetParametersAtomically_Response set_atomically_response; + + rcl_interfaces__srv__ListParameters_Request list_request; + rcl_interfaces__srv__ListParameters_Response list_response; +} rcl_parameter_service_impl_t; + +rcl_parameter_service_options_t +rcl_parameter_service_get_default_options(void) +{ + static rcl_parameter_service_options_t default_options; + default_options.qos = rmw_qos_profile_parameters; + default_options.parameter_event_qos = rmw_qos_profile_parameter_events; + default_options.allocator = rcl_get_default_allocator(); + default_options.remote_node_name = NULL; + return default_options; +} + +rcl_parameter_service_t +rcl_get_zero_initialized_parameter_service(void) +{ + static rcl_parameter_service_t null_service = {0}; + return null_service; +} + +#define RCL_PARAMETER_INITIALIZE_SERVICE(VERB, SRV_TYPE_NAME, SRV_SUFFIX) \ + { \ + const rosidl_service_type_support_t * VERB ## _ts = ROSIDL_GET_TYPE_SUPPORT( \ + rcl_interfaces, srv, SRV_TYPE_NAME \ + ); \ + \ + size_t VERB ## len = strlen(node_name) + strlen(SRV_SUFFIX) + 1; \ + char * VERB ## _service_name = (char *)options->allocator.allocate( \ + VERB ## len, options->allocator.state); \ + memset(VERB ## _service_name, 0, VERB ## len); \ + VERB ## _service_name = memcpy( \ + VERB ## _service_name, node_name, strlen(node_name) + 1); \ + memcpy((VERB ## _service_name + strlen(node_name)), \ + SRV_SUFFIX, strlen(SRV_SUFFIX) + 1); \ + \ + ret = rcl_service_init( \ + ¶meter_service->impl->VERB ## _service, \ + node, \ + VERB ## _ts, \ + VERB ## _service_name, \ + &service_options); \ + options->allocator.deallocate(VERB ## _service_name, options->allocator.state); \ + if (ret != RCL_RET_OK) { \ + fail_ret = ret; \ + goto fail_ ## VERB; \ + } \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Request__init( \ + ¶meter_service->impl->VERB ## _request); \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Response__init( \ + ¶meter_service->impl->VERB ## _response); \ + } \ + +#define RCL_PARAMETER_SERVICE_FINI(VERB, SRV_TYPE_NAME) \ + ret = rcl_service_fini( \ + ¶meter_service->impl->VERB ## _service, parameter_service->impl->node); \ + if (ret != RCL_RET_OK) { \ + fprintf(stderr, "rcl_service_fini failed for service " #VERB "\n"); \ + fail_ret = ret; \ + } \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Request__fini( \ + ¶meter_service->impl->VERB ## _request); \ + rcl_interfaces__srv__ ## SRV_TYPE_NAME ## _Response__fini( \ + ¶meter_service->impl->VERB ## _response); \ + + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_init( + rcl_parameter_service_t * parameter_service, + rcl_node_t * node, + const rcl_parameter_service_options_t * options +) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (!node->impl) { + RCL_SET_ERROR_MSG("invalid node"); + return RCL_RET_NODE_INVALID; + } + if (parameter_service->impl) { + RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + rcl_ret_t ret; + rcl_ret_t fail_ret = RCL_RET_ERROR; + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = options->qos; + service_options.allocator = options->allocator; + + parameter_service->impl = (rcl_parameter_service_impl_t *)options->allocator.allocate( + sizeof(rcl_parameter_service_impl_t), options->allocator.state); + if (!parameter_service->impl) { + goto fail_alloc; + } + memset(parameter_service->impl, 0, sizeof(rcl_parameter_service_impl_t)); + parameter_service->impl->node = node; + + const char * node_name = options->remote_node_name != + NULL ? node_name = options->remote_node_name : rcl_node_get_name(node); + + // Initialize all services in impl storage + RCL_PARAMETER_INITIALIZE_SERVICE(get, GetParameters, "__get_parameters"); + RCL_PARAMETER_INITIALIZE_SERVICE(get_types, GetParameterTypes, "__get_parameter_types"); + RCL_PARAMETER_INITIALIZE_SERVICE(set, SetParameters, "__set_parameters"); + RCL_PARAMETER_INITIALIZE_SERVICE(set_atomically, SetParametersAtomically, + "__set_parameters_atomically"); + RCL_PARAMETER_INITIALIZE_SERVICE(list, ListParameters, "__list_parameters"); + + const rosidl_message_type_support_t * event_ts = ROSIDL_GET_TYPE_SUPPORT( + rcl_interfaces, msg, ParameterEvent); + // TODO(jacquelinekay) Should the parameter event topic name be namespaced? + // Is this a configuration option? + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.allocator = options->allocator; + publisher_options.qos = options->parameter_event_qos; + const char * event_topic_name = "parameter_events"; + ret = rcl_publisher_init( + ¶meter_service->impl->event_publisher, node, event_ts, event_topic_name, + &publisher_options); + if (ret != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + + parameter_service->impl->options = *options; + return RCL_RET_OK; + +fail: + ret = rcl_publisher_fini( + ¶meter_service->impl->event_publisher, parameter_service->impl->node); + if (ret != RCL_RET_OK) { + fail_ret = ret; + fprintf(stderr, "rcl_publisher_fini failed in fail block of rcl_parameter_service_init\n"); + } + +fail_list: + RCL_PARAMETER_SERVICE_FINI(list, ListParameters); + +fail_set_atomically: + RCL_PARAMETER_SERVICE_FINI(set_atomically, SetParametersAtomically); + +fail_set: + RCL_PARAMETER_SERVICE_FINI(set, SetParameters); + +fail_get_types: + RCL_PARAMETER_SERVICE_FINI(get_types, GetParameterTypes); + +fail_get: + RCL_PARAMETER_SERVICE_FINI(get, GetParameters); + +fail_alloc: + + return fail_ret; +} + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parameter_service_fini(rcl_parameter_service_t * parameter_service) +{ + rcl_ret_t ret; + rcl_ret_t fail_ret = RCL_RET_OK; + RCL_PARAMETER_SERVICE_FINI(get, GetParameters); + RCL_PARAMETER_SERVICE_FINI(get_types, GetParameterTypes); + RCL_PARAMETER_SERVICE_FINI(set, SetParameters); + RCL_PARAMETER_SERVICE_FINI(set_atomically, SetParametersAtomically); + RCL_PARAMETER_SERVICE_FINI(list, ListParameters); + + ret = + rcl_publisher_fini(¶meter_service->impl->event_publisher, parameter_service->impl->node); + if (ret != RCL_RET_OK) { + fail_ret = ret; + } + + rcl_allocator_t allocator = parameter_service->impl->options.allocator; + allocator.deallocate(parameter_service->impl, allocator.state); + + if (fail_ret != RCL_RET_OK) { + return fail_ret; + } + return ret; +} + +#define DEFINE_RCL_PARAMETER_SERVICE_TAKE_REQUEST(VERB, REQUEST_SUBTYPE, SUBFIELD_NAME) \ + REQUEST_SUBTYPE * \ + rcl_parameter_service_take_ ## VERB ## _request( \ + const rcl_parameter_service_t * parameter_service, \ + rmw_request_id_t * request_header) \ + { \ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_service, NULL); \ + \ + rcl_ret_t ret = rcl_take_request( \ + ¶meter_service->impl->VERB ## _service, request_header, \ + ¶meter_service->impl->VERB ## _request); \ + if (ret != RCL_RET_OK) { \ + return NULL; \ + } \ + \ + return ¶meter_service->impl->VERB ## _request.SUBFIELD_NAME; \ + } + +DEFINE_RCL_PARAMETER_SERVICE_TAKE_REQUEST(get, rosidl_generator_c__String__Array, names) +DEFINE_RCL_PARAMETER_SERVICE_TAKE_REQUEST(get_types, rosidl_generator_c__String__Array, names) +DEFINE_RCL_PARAMETER_SERVICE_TAKE_REQUEST(set, rcl_interfaces__msg__Parameter__Array, parameters) +DEFINE_RCL_PARAMETER_SERVICE_TAKE_REQUEST(set_atomically, rcl_interfaces__msg__Parameter__Array, + parameters) + +rcl_ret_t +rcl_parameter_service_take_list_request( + const rcl_parameter_service_t * parameter_service, + rmw_request_id_t * request_header, + rosidl_generator_c__String__Array * prefixes, + uint64_t * depth) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(prefixes, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(depth, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = rcl_take_request( + ¶meter_service->impl->list_service, request_header, ¶meter_service->impl->list_request); + + prefixes = ¶meter_service->impl->list_request.prefixes; + depth = ¶meter_service->impl->list_request.depth; + + return ret; +} + + +#define DEFINE_RCL_PARAMETER_SERVICE_SEND_RESPONSE(VERB, REQUEST_SUBTYPE, SUBFIELD_NAME) \ + rcl_ret_t \ + rcl_parameter_service_send_ ## VERB ## _response( \ + const rcl_parameter_service_t * parameter_service, \ + rmw_request_id_t * request_header, \ + const REQUEST_SUBTYPE * SUBFIELD_NAME) \ + { \ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_service, RCL_RET_INVALID_ARGUMENT); \ + RCL_CHECK_ARGUMENT_FOR_NULL(SUBFIELD_NAME, RCL_RET_INVALID_ARGUMENT); \ + \ + parameter_service->impl->VERB ## _response.SUBFIELD_NAME = *SUBFIELD_NAME; \ + \ + rcl_ret_t ret = rcl_send_response( \ + ¶meter_service->impl->VERB ## _service, request_header, \ + ¶meter_service->impl->VERB ## _response); \ + \ + return ret; \ + } + +DEFINE_RCL_PARAMETER_SERVICE_SEND_RESPONSE(get, rcl_interfaces__msg__ParameterValue__Array, values) +DEFINE_RCL_PARAMETER_SERVICE_SEND_RESPONSE(get_types, rosidl_generator_c__uint8__Array, types) +DEFINE_RCL_PARAMETER_SERVICE_SEND_RESPONSE(set, rcl_interfaces__msg__SetParametersResult__Array, + results) +DEFINE_RCL_PARAMETER_SERVICE_SEND_RESPONSE(set_atomically, rcl_interfaces__msg__SetParametersResult, + result) +DEFINE_RCL_PARAMETER_SERVICE_SEND_RESPONSE(list, rcl_interfaces__msg__ListParametersResult, result) + +rcl_ret_t +rcl_parameter_service_publish_event( + const rcl_parameter_service_t * parameter_service, + const rcl_interfaces__msg__ParameterEvent * event) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(event, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = rcl_publish(¶meter_service->impl->event_publisher, event); + + return ret; +} + +rcl_ret_t +rcl_wait_set_add_parameter_service( + rcl_wait_set_t * wait_set, + const rcl_parameter_service_t * parameter_service) +{ + rcl_ret_t ret; + + ret = rcl_wait_set_add_service(wait_set, ¶meter_service->impl->get_service); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add get_parameters service to waitset!"); + return ret; + } + ret = rcl_wait_set_add_service(wait_set, ¶meter_service->impl->get_types_service); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add get_parameter_types service to waitset!"); + return ret; + } + ret = rcl_wait_set_add_service(wait_set, ¶meter_service->impl->set_service); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add set_parameters service to waitset!"); + return ret; + } + ret = rcl_wait_set_add_service(wait_set, ¶meter_service->impl->set_atomically_service); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add set_parameters_atomically service to waitset!"); + return ret; + } + ret = rcl_wait_set_add_service(wait_set, ¶meter_service->impl->list_service); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Failed to add list_parameters service to waitset!"); + return ret; + } + + return ret; +} + +rcl_ret_t +rcl_parameter_service_get_pending_action( + const rcl_wait_set_t * wait_set, + const rcl_parameter_service_t * parameter_service, + rcl_param_action_t * action) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(action, RCL_RET_INVALID_ARGUMENT); + size_t i = 0; + size_t j = 0; + + for (i = 0; i < wait_set->size_of_services; ++i) { + for (j = 0; j < RCL_NUMBER_OF_PARAMETER_ACTIONS; ++j) { + rcl_service_t * service_ptr = NULL; + *action = j; + switch (j) { + case RCL_GET_PARAMETERS: + service_ptr = ¶meter_service->impl->get_service; + break; + case RCL_GET_PARAMETER_TYPES: + service_ptr = ¶meter_service->impl->get_types_service; + break; + case RCL_SET_PARAMETERS: + service_ptr = ¶meter_service->impl->set_service; + break; + case RCL_SET_PARAMETERS_ATOMICALLY: + service_ptr = ¶meter_service->impl->set_atomically_service; + break; + case RCL_LIST_PARAMETERS: + service_ptr = ¶meter_service->impl->list_service; + break; + default: + *action = RCL_PARAMETER_ACTION_UNKNOWN; + return RCL_RET_ERROR; + } + if (service_ptr == wait_set->services[i]) { + return RCL_RET_OK; + } + } + } + *action = RCL_PARAMETER_ACTION_UNKNOWN; + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 53e585b0a..474c42dae 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -301,6 +301,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } else { \ wait_set->Type ## s = (const rcl_ ## Type ## _t * *)allocator.reallocate( \ (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ + memset(wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * size); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ wait_set->size_of_ ## Type ## s = size; \ @@ -319,13 +320,14 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al /* Also resize the rmw storage. */ \ wait_set->impl->RMWCount = 0; \ wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ - wait_set->impl->RMWStorage, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ + wait_set->impl->RMWStorage, sizeof(rmw_ ## Type ## _t *) * size, allocator.state); \ if (!wait_set->impl->RMWStorage) { \ allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ wait_set->size_of_ ## Type ## s = 0; \ RCL_SET_ERROR_MSG("allocating memory failed"); \ return RCL_RET_BAD_ALLOC; \ - } + } \ + memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * size); \ /* Implementation-specific notes: * diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index a3aeab87c..1d9756b45 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -96,6 +96,19 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} ) + +# if(${rmw_implementation} STREQUAL "rmw_connext_dynamic_cpp") +# # TODO(jacquelinekay) remove once complex data types work for connext dynamic +# message(STATUS "Skipping parameters test for Connext Dynamic for now") +# else() + rcl_add_custom_gtest(test_parameters${target_suffix} + SRCS rcl/test_parameters.cpp + LIBRARIES ${PROJECT_NAME}${target_suffix} + AMENT_DEPENDENCIES ${rmw_implementation} + TIMEOUT 20 + ) +# endif() + rcl_add_custom_gtest(test_publisher${target_suffix} SRCS rcl/test_publisher.cpp ENV ${extra_test_env} @@ -109,7 +122,7 @@ function(test_target_function) ENV ${extra_test_env} APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} - AMENT_DEPENDENCIES ${rmw_implementation} "example_interfaces" + AMENT_DEPENDENCIES ${rmw_implementation} "example_interfaces" "rcl_interfaces" ) rcl_add_custom_gtest(test_subscription${target_suffix} diff --git a/rcl/test/rcl/test_parameters.cpp b/rcl/test/rcl/test_parameters.cpp new file mode 100644 index 000000000..8b80a2bb2 --- /dev/null +++ b/rcl/test/rcl/test_parameters.cpp @@ -0,0 +1,603 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rosidl_generator_c/string_functions.h" +#include "rosidl_generator_c/primitives_array_functions.h" + +#include "rcl_interfaces/msg/list_parameters_result__functions.h" + +#include "rcl_interfaces/msg/parameter__functions.h" +#include "rcl_interfaces/msg/parameter_type__struct.h" +#include "rcl_interfaces/msg/parameter_value__functions.h" +#include "rcl_interfaces/msg/set_parameters_result__functions.h" + +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcl/parameter.h" +#include "rcl/parameter_client.h" +#include "rcl/parameter_service.h" + +#include "rcl/rcl.h" + + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#define WAIT_TIME -1 +// #define WAIT_TIME 1000000000 +#define NUM_PARAMS 4 + + +class CLASSNAME (TestParametersFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_node_t * node_ptr = nullptr; + rcl_wait_set_t * wait_set = nullptr; + rcl_parameter_service_t * parameter_service = nullptr; + rcl_parameter_client_t * parameter_client = nullptr; + + void SetUp() + { + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "parameter_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + this->wait_set = new rcl_wait_set_t; + *this->wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(wait_set, 0, 0, 0, 0, 0, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + this->parameter_client = new rcl_parameter_client_t; + *this->parameter_client = rcl_get_zero_initialized_parameter_client(); + rcl_parameter_client_options_t cs_options = rcl_parameter_client_get_default_options(); + ret = rcl_parameter_client_init(this->parameter_client, this->node_ptr, &cs_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + this->parameter_service = new rcl_parameter_service_t; + *this->parameter_service = rcl_get_zero_initialized_parameter_service(); + rcl_parameter_service_options_t ps_options = rcl_parameter_service_get_default_options(); + ret = rcl_parameter_service_init(this->parameter_service, this->node_ptr, &ps_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + + void TearDown() + { + rcl_ret_t ret; + if (this->wait_set) { + ret = rcl_wait_set_fini(this->wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + delete this->wait_set; + } + + if (this->parameter_service) { + ret = rcl_parameter_service_fini(this->parameter_service); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + delete this->parameter_service; + } + + if (this->parameter_client) { + ret = rcl_parameter_client_fini(this->parameter_client); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + delete this->parameter_client; + } + + if (this->node_ptr) { + ret = rcl_node_fini(this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + delete this->node_ptr; + } + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } +}; + +// Helper function for filling in hardcoded test values +rcl_ret_t fill_parameter_array(rcl_interfaces__msg__Parameter__Array * parameters) +{ + size_t parameters_idx = 0; + rcl_ret_t ret = rcl_parameter_set_bool(¶meters->data[parameters_idx++], "bool_param", true); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_parameter_set_integer(¶meters->data[parameters_idx++], "int_param", 123); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_parameter_set_double(¶meters->data[parameters_idx++], "float_param", 45.67); + if (ret != RCL_RET_OK) { + return ret; + } + + ret = rcl_parameter_set_string( + ¶meters->data[parameters_idx++], "string_param", "hello world"); + // TODO(jacquelinekay) Bytes and other arrays of primitives need more helper functions + /* + uint8_t bytes[5] = "\1\2\3\4"; + ret = rcl_parameter_set_bytes(¶meters->data[parameters_idx++], "bytes_param", &(bytes[0])); + */ + return ret; +} + +void compare_parameter_array(const rcl_interfaces__msg__Parameter__Array * parameters) +{ + ASSERT_EQ(parameters->size, NUM_PARAMS); + size_t parameters_idx = 0; + + EXPECT_EQ(strcmp(parameters->data[parameters_idx].name.data, "bool_param"), 0); + EXPECT_TRUE(parameters->data[parameters_idx++].value.bool_value); + + EXPECT_EQ(strcmp(parameters->data[parameters_idx].name.data, "int_param"), 0); + EXPECT_EQ(parameters->data[parameters_idx++].value.integer_value, 123); + + EXPECT_EQ(strcmp(parameters->data[parameters_idx].name.data, "float_param"), 0); + EXPECT_EQ(parameters->data[parameters_idx++].value.double_value, 45.67); + + EXPECT_EQ(strcmp(parameters->data[parameters_idx].name.data, "string_param"), 0); + EXPECT_EQ(strcmp(parameters->data[parameters_idx].value.string_value.data, "hello world"), 0); +} + +void compare_parameter_array(const rcl_interfaces__msg__ParameterValue__Array * parameters) +{ + ASSERT_EQ(parameters->size, NUM_PARAMS); + size_t parameters_idx = 0; + + EXPECT_TRUE(parameters->data[parameters_idx++].bool_value); + EXPECT_EQ(parameters->data[parameters_idx++].integer_value, 123); + EXPECT_EQ(parameters->data[parameters_idx++].double_value, 45.67); + EXPECT_EQ(strcmp(parameters->data[parameters_idx].string_value.data, "hello world"), 0); +} + +void compare_parameter_array(const rosidl_generator_c__String__Array * parameter_names) +{ + ASSERT_EQ(parameter_names->size, NUM_PARAMS); + size_t parameters_idx = 0; + + EXPECT_EQ(strcmp(parameter_names->data[parameters_idx++].data, "bool_param"), 0); + EXPECT_EQ(strcmp(parameter_names->data[parameters_idx++].data, "int_param"), 0); + EXPECT_EQ(strcmp(parameter_names->data[parameters_idx++].data, "float_param"), 0); + EXPECT_EQ(strcmp(parameter_names->data[parameters_idx++].data, "string_param"), 0); +} + +rcl_ret_t fill_parameter_array(rcl_interfaces__msg__ParameterValue__Array * parameters) +{ + size_t parameters_idx = 0; + rcl_ret_t ret = rcl_parameter_set_value_bool(¶meters->data[parameters_idx++], true); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_parameter_set_value_integer(¶meters->data[parameters_idx++], 123); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_parameter_set_value_double(¶meters->data[parameters_idx++], 45.67); + if (ret != RCL_RET_OK) { + return ret; + } + + ret = rcl_parameter_set_value_string(¶meters->data[parameters_idx++], "hello world"); + /* + uint8_t bytes[5] = "\1\2\3\4"; + ret = rcl_parameter_set_value_bytes(¶meters->data[parameters_idx++], &(bytes[0])); + */ + return ret; +} + +bool fill_parameter_names_array(rosidl_generator_c__String__Array * names) +{ + size_t parameters_idx = 0; + if (!rosidl_generator_c__String__assign(&names->data[parameters_idx++], "bool_param")) { + return false; + } + if (!rosidl_generator_c__String__assign(&names->data[parameters_idx++], "int_param")) { + return false; + } + if (!rosidl_generator_c__String__assign(&names->data[parameters_idx++], "float_param")) { + return false; + } + if (!rosidl_generator_c__String__assign(&names->data[parameters_idx++], "string_param")) { + return false; + } + // rosidl_generator_c__String__assign(&names->data[parameters_idx++], "bytes_param"); + return true; +} + +rcl_ret_t prepare_wait_set( + rcl_wait_set_t * wait_set, rcl_parameter_service_t * parameter_service, + rcl_parameter_client_t * parameter_client) +{ + rcl_ret_t ret = rcl_wait_set_clear_services(wait_set); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_wait_set_clear_clients(wait_set); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_wait_set_clear_subscriptions(wait_set); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_wait_set_resize_services(wait_set, RCL_NUMBER_OF_PARAMETER_ACTIONS); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_wait_set_resize_clients(wait_set, RCL_NUMBER_OF_PARAMETER_ACTIONS); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_wait_set_resize_subscriptions(wait_set, 1); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_wait_set_add_parameter_service(wait_set, parameter_service); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_wait_set_add_parameter_client(wait_set, parameter_client); + return ret; +} + +// TODO(jacquelinekay) Test un-setting parameters using set_parameters +TEST_F(CLASSNAME(TestParametersFixture, RMW_IMPLEMENTATION), test_set_parameters) { + rmw_request_id_t request_header; + rcl_param_action_t action = RCL_PARAMETER_ACTION_UNKNOWN; + rcl_ret_t ret; + + rcl_interfaces__msg__Parameter__Array parameters; + EXPECT_TRUE(rcl_interfaces__msg__Parameter__Array__init(¶meters, NUM_PARAMS)); + + ret = fill_parameter_array(¶meters); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + int64_t seq_num; + ret = rcl_parameter_client_send_set_request(this->parameter_client, ¶meters, &seq_num); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + ret = rcl_parameter_service_get_pending_action(wait_set, parameter_service, &action); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + EXPECT_EQ(action, RCL_SET_PARAMETERS); + + rcl_interfaces__msg__Parameter__Array * parameters_req = rcl_parameter_service_take_set_request( + this->parameter_service, &request_header); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + // Validate the request + compare_parameter_array(parameters_req); + + // For now we'll just set them all to be successful + // Should SetParametersResult have a "name" field for the parameter key it describes? + + rcl_interfaces__msg__SetParametersResult__Array results; + EXPECT_TRUE(rcl_interfaces__msg__SetParametersResult__Array__init(&results, NUM_PARAMS)); + + for (size_t i = 0; i < NUM_PARAMS; ++i) { + results.data[i].successful = true; + rosidl_generator_c__String__assign(&results.data[i].reason, "success"); + } + ret = rcl_parameter_service_send_set_response( + this->parameter_service, &request_header, &results); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_parameter_client_get_pending_action(wait_set, parameter_client, &action); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + EXPECT_EQ(action, RCL_SET_PARAMETERS); + + rcl_interfaces__msg__SetParametersResult__Array * results_response = + rcl_parameter_client_take_set_response(this->parameter_client, &request_header); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + for (size_t i = 0; i < NUM_PARAMS; ++i) { + EXPECT_TRUE(results_response->data[i].successful); + EXPECT_EQ(strcmp(results.data[i].reason.data, "success"), 0); + } + + rcl_interfaces__msg__Parameter__Array prior_state; + EXPECT_TRUE(rcl_interfaces__msg__Parameter__Array__init(&prior_state, 3)); + + // Bogus values for the previous state: one the same, one removed, one changed + ret = rcl_parameter_set_integer(&prior_state.data[0], "int_param", 123); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_parameter_set_integer(&prior_state.data[1], "deleted", 24); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_parameter_set_double(&prior_state.data[2], "float_param", -45.67); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + rcl_interfaces__msg__ParameterEvent event; + EXPECT_TRUE(rcl_interfaces__msg__ParameterEvent__init(&event)); + + ret = rcl_parameter_convert_changes_to_event(&prior_state, parameters_req, &event); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + auto validate_event = [](const rcl_interfaces__msg__ParameterEvent & param_event) { + EXPECT_EQ(strcmp(param_event.changed_parameters.data[0].name.data, "float_param"), 0); + EXPECT_EQ(param_event.changed_parameters.data[0].value.double_value, 45.67); + + EXPECT_EQ(strcmp(param_event.deleted_parameters.data[0].name.data, "deleted"), 0); + + // Ordering of new parameters doesn't matter + // New parameters + EXPECT_EQ(strcmp(param_event.new_parameters.data[0].name.data, "bool_param"), 0); + EXPECT_TRUE(param_event.new_parameters.data[0].value.bool_value); + + EXPECT_EQ(strcmp(param_event.new_parameters.data[1].name.data, "string_param"), 0); + EXPECT_EQ(strcmp(param_event.new_parameters.data[1].value.string_value.data, "hello world"), + 0); + }; + + validate_event(event); + + ret = rcl_parameter_service_publish_event(this->parameter_service, &event); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + rcl_interfaces__msg__ParameterEvent event_response; + EXPECT_TRUE(rcl_interfaces__msg__ParameterEvent__init(&event_response)); + ret = rcl_parameter_client_take_event(this->parameter_client, &event_response, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + validate_event(event_response); +} + +TEST_F(CLASSNAME(TestParametersFixture, RMW_IMPLEMENTATION), test_set_parameters_atomically) { + rmw_request_id_t request_header; + rcl_param_action_t action; + rcl_ret_t ret; + + rcl_interfaces__msg__Parameter__Array parameters; + EXPECT_TRUE(rcl_interfaces__msg__Parameter__Array__init(¶meters, NUM_PARAMS)); + + rcl_interfaces__msg__SetParametersResult result; + EXPECT_TRUE(rcl_interfaces__msg__SetParametersResult__init(&result)); + + ret = fill_parameter_array(¶meters); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + int64_t seq_num; + ret = rcl_parameter_client_send_set_atomically_request( + this->parameter_client, ¶meters, &seq_num); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_parameter_service_get_pending_action(this->wait_set, this->parameter_service, &action); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + EXPECT_EQ(action, RCL_SET_PARAMETERS_ATOMICALLY); + + rcl_interfaces__msg__Parameter__Array * parameters_req = + rcl_parameter_service_take_set_atomically_request(this->parameter_service, &request_header); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + compare_parameter_array(parameters_req); + + // For now we'll just set them all to be successful + // Should SetParametersResult have a "name" field for the parameter key it describes? + result.successful = true; + rosidl_generator_c__String__assign(&result.reason, "Because reasons"); + ret = rcl_parameter_service_send_set_atomically_response( + this->parameter_service, &request_header, &result); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + ret = rcl_parameter_client_get_pending_action(this->wait_set, this->parameter_client, &action); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + EXPECT_EQ(action, RCL_SET_PARAMETERS_ATOMICALLY); + + rcl_interfaces__msg__SetParametersResult * result_response = + rcl_parameter_client_take_set_atomically_response(this->parameter_client, &request_header); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe; + + EXPECT_TRUE(result_response->successful); + EXPECT_EQ(strcmp(result_response->reason.data, "Because reasons"), 0); +} + + +TEST_F(CLASSNAME(TestParametersFixture, RMW_IMPLEMENTATION), test_get_parameters) { + rmw_request_id_t request_header; + rcl_ret_t ret; + rcl_param_action_t action; + (void) ret; + + rosidl_generator_c__String__Array parameter_names; + EXPECT_TRUE(rosidl_generator_c__String__Array__init(¶meter_names, NUM_PARAMS)); + + rcl_interfaces__msg__ParameterValue__Array parameter_values; + EXPECT_TRUE(rcl_interfaces__msg__ParameterValue__Array__init(¶meter_values, NUM_PARAMS)); + + EXPECT_TRUE(fill_parameter_names_array(¶meter_names)) << rcl_get_error_string_safe(); + + int64_t seq_num; + ret = rcl_parameter_client_send_get_request( + this->parameter_client, ¶meter_names, &seq_num); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_parameter_service_get_pending_action(this->wait_set, this->parameter_service, &action); + EXPECT_EQ(action, RCL_GET_PARAMETERS); + + rosidl_generator_c__String__Array * request = rcl_parameter_service_take_get_request( + this->parameter_service, &request_header); + EXPECT_EQ(ret, RCL_RET_OK); + compare_parameter_array(request); + + // Assign some bogus values + // In a real client library, these would be pulled from storage + ret = fill_parameter_array(¶meter_values); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_parameter_service_send_get_response( + this->parameter_service, &request_header, ¶meter_values); + EXPECT_EQ(ret, RCL_RET_OK); + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_parameter_client_get_pending_action(this->wait_set, this->parameter_client, &action); + EXPECT_EQ(action, RCL_GET_PARAMETERS); + + // TODO(jacquelinekay): Should GetParameters_Response have a Parameter__Array subfield + // instead of a ParameterValue__Array? + rcl_interfaces__msg__ParameterValue__Array * response = rcl_parameter_client_take_get_response( + this->parameter_client, &request_header); + EXPECT_EQ(ret, RCL_RET_OK); + + compare_parameter_array(response); +} + + +TEST_F(CLASSNAME(TestParametersFixture, RMW_IMPLEMENTATION), test_get_parameter_types) { + rmw_request_id_t request_header; + rcl_ret_t ret; + rcl_param_action_t action; + (void) ret; + + rosidl_generator_c__String__Array parameter_names; + EXPECT_TRUE(rosidl_generator_c__String__Array__init(¶meter_names, NUM_PARAMS)); + + rosidl_generator_c__uint8__Array parameter_types; + EXPECT_TRUE(rosidl_generator_c__uint8__Array__init(¶meter_types, NUM_PARAMS)); + + EXPECT_TRUE(fill_parameter_names_array(¶meter_names)) << rcl_get_error_string_safe(); + int64_t seq_num; + ret = rcl_parameter_client_send_get_types_request(this->parameter_client, ¶meter_names, + &seq_num); + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_parameter_service_get_pending_action(this->wait_set, this->parameter_service, &action); + EXPECT_EQ(action, RCL_GET_PARAMETER_TYPES); + + rosidl_generator_c__String__Array * request = rcl_parameter_service_take_get_types_request( + this->parameter_service, &request_header); + compare_parameter_array(request); + + { + size_t parameters_idx = 0; + parameter_types.data[parameters_idx++] = rcl_interfaces__msg__ParameterType__PARAMETER_BOOL; + parameter_types.data[parameters_idx++] = rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER; + parameter_types.data[parameters_idx++] = rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE; + parameter_types.data[parameters_idx++] = rcl_interfaces__msg__ParameterType__PARAMETER_STRING; + parameter_types.data[parameters_idx++] = rcl_interfaces__msg__ParameterType__PARAMETER_BYTES; + } + ret = rcl_parameter_service_send_get_types_response(this->parameter_service, &request_header, + ¶meter_types); + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_parameter_client_get_pending_action(this->wait_set, this->parameter_client, &action); + EXPECT_EQ(action, RCL_GET_PARAMETER_TYPES); + + rosidl_generator_c__uint8__Array * response = rcl_parameter_client_take_get_types_response( + this->parameter_client, &request_header); + + { + size_t parameters_idx = 0; + EXPECT_EQ(response->data[parameters_idx++], rcl_interfaces__msg__ParameterType__PARAMETER_BOOL); + EXPECT_EQ(response->data[parameters_idx++], + rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER); + EXPECT_EQ(response->data[parameters_idx++], + rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE); + EXPECT_EQ(response->data[parameters_idx++], + rcl_interfaces__msg__ParameterType__PARAMETER_STRING); + // EXPECT_EQ( + // response.data[parameters_idx++], rcl_interfaces__msg__ParameterType__PARAMETER_BYTES); + } +} + +TEST_F(CLASSNAME(TestParametersFixture, RMW_IMPLEMENTATION), test_list_parameters) { + rmw_request_id_t request_header; + rcl_ret_t ret; + rcl_param_action_t action; + (void) ret; + + rcl_interfaces__msg__ListParametersResult list_result; + EXPECT_TRUE(rcl_interfaces__msg__ListParametersResult__init(&list_result)); + EXPECT_TRUE(rosidl_generator_c__String__Array__init(&list_result.names, NUM_PARAMS)); + EXPECT_TRUE(rosidl_generator_c__String__Array__init(&list_result.prefixes, NUM_PARAMS)); + + rosidl_generator_c__String__Array prefixes; + EXPECT_TRUE(rosidl_generator_c__String__Array__init(&prefixes, 0)); + + uint64_t depth = 0; + int64_t seq_num; + ret = rcl_parameter_client_send_list_request(this->parameter_client, &prefixes, depth, &seq_num); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + ret = rcl_parameter_service_get_pending_action(this->wait_set, this->parameter_service, &action); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_EQ(action, RCL_LIST_PARAMETERS); + + rosidl_generator_c__String__Array prefixes_req; + uint64_t depth_req; + EXPECT_TRUE(rosidl_generator_c__String__Array__init(&prefixes_req, 0)); + ret = rcl_parameter_service_take_list_request(this->parameter_service, &request_header, + &prefixes_req, &depth_req); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + // put some names in + EXPECT_TRUE(fill_parameter_names_array(&list_result.names)) << rcl_get_error_string_safe(); + ret = rcl_parameter_service_send_list_response(this->parameter_service, &request_header, + &list_result); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + ret = prepare_wait_set(this->wait_set, this->parameter_service, this->parameter_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + ret = rcl_wait(this->wait_set, WAIT_TIME); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + ret = rcl_parameter_client_get_pending_action(this->wait_set, this->parameter_client, &action); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_EQ(action, RCL_LIST_PARAMETERS); + + rcl_interfaces__msg__ListParametersResult * result_response = + rcl_parameter_client_take_list_response(this->parameter_client, &request_header); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + + compare_parameter_array(&result_response->names); +} diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index dd7489c85..6d5419d75 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -22,6 +22,9 @@ #include "rcl/rcl.h" +#include "rcl_interfaces/srv/set_parameters.h" +#include "rcl_interfaces/msg/set_parameters_result.h" +#include "rcl_interfaces/msg/parameter.h" #include "example_interfaces/srv/add_two_ints.h" #include "../memory_tools/memory_tools.hpp" @@ -151,7 +154,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) // Initialize a request. example_interfaces__srv__AddTwoInts_Request client_request; - example_interfaces__srv__AddTwoInts_Request__init(&client_request); + EXPECT_TRUE(example_interfaces__srv__AddTwoInts_Request__init(&client_request)); client_request.a = 1; client_request.b = 2; int64_t sequence_number; @@ -169,7 +172,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) { // Initialize a response. example_interfaces__srv__AddTwoInts_Response service_response; - example_interfaces__srv__AddTwoInts_Response__init(&service_response); + EXPECT_TRUE(example_interfaces__srv__AddTwoInts_Response__init(&service_response)); auto msg_exit = make_scope_exit([&service_response]() { stop_memory_checking(); example_interfaces__srv__AddTwoInts_Response__fini(&service_response); @@ -177,7 +180,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) // Initialize a separate instance of the request and take the pending request. example_interfaces__srv__AddTwoInts_Request service_request; - example_interfaces__srv__AddTwoInts_Request__init(&service_request); + EXPECT_TRUE(example_interfaces__srv__AddTwoInts_Request__init(&service_request)); rmw_request_id_t header; ret = rcl_take_request(&service, &header, &service_request); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); @@ -193,7 +196,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) // Initialize the response owned by the client and take the response. example_interfaces__srv__AddTwoInts_Response client_response; - example_interfaces__srv__AddTwoInts_Response__init(&client_response); + EXPECT_TRUE(example_interfaces__srv__AddTwoInts_Response__init(&client_response)); rmw_request_id_t header; ret = rcl_take_response(&client, &header, &client_response); diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 72a7af7a5..b81e0817c 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -213,7 +213,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription ret = rcl_publish(&publisher, &msg); // TODO(wjwwood): re-enable this fini when ownership of the string is resolved. // currently with Connext we will spuriously get a double free here. - // std_msgs__msg__String__fini(&msg); + std_msgs__msg__String__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); } bool success;