diff --git a/rcl_yaml_param_parser/CMakeLists.txt b/rcl_yaml_param_parser/CMakeLists.txt index e2d09a7..54569eb 100644 --- a/rcl_yaml_param_parser/CMakeLists.txt +++ b/rcl_yaml_param_parser/CMakeLists.txt @@ -16,7 +16,12 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") endif() set(rcl_yaml_parser_sources + src/add_to_arrays.c + src/namespace.c + src/node_params.c + src/parse.c src/parser.c + src/yaml_variant.c ) add_library( diff --git a/rcl_yaml_param_parser/include/rcl_yaml_param_parser/types.h b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/types.h index 594f8d4..a6c6bf8 100644 --- a/rcl_yaml_param_parser/include/rcl_yaml_param_parser/types.h +++ b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/types.h @@ -14,6 +14,8 @@ #ifndef RCL_YAML_PARAM_PARSER__TYPES_H_ #define RCL_YAML_PARAM_PARSER__TYPES_H_ +#include + #include "rcutils/allocator.h" #include "rcutils/types/string_array.h" diff --git a/rcl_yaml_param_parser/src/add_to_arrays.c b/rcl_yaml_param_parser/src/add_to_arrays.c new file mode 100644 index 0000000..11f11d2 --- /dev/null +++ b/rcl_yaml_param_parser/src/add_to_arrays.c @@ -0,0 +1,121 @@ +// Copyright 2018 Apex.AI, 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 "./impl/add_to_arrays.h" + +#include "rcutils/error_handling.h" + +#define ADD_VALUE_TO_SIMPLE_ARRAY(val_array, value, value_type, allocator) \ + do { \ + if (NULL == val_array->values) { \ + val_array->values = value; \ + val_array->size = 1; \ + } else { \ + /* Increase the array size by one and add the new value */ \ + value_type * tmp_arr = val_array->values; \ + val_array->values = allocator.zero_allocate( \ + val_array->size + 1U, sizeof(value_type), allocator.state); \ + if (NULL == val_array->values) { \ + val_array->values = tmp_arr; \ + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); \ + return RCUTILS_RET_BAD_ALLOC; \ + } \ + memcpy(val_array->values, tmp_arr, (val_array->size * sizeof(value_type))); \ + val_array->values[val_array->size] = *value; \ + val_array->size++; \ + allocator.deallocate(value, allocator.state); \ + allocator.deallocate(tmp_arr, allocator.state); \ + } \ + return RCUTILS_RET_OK; \ + } while (0) + +rcutils_ret_t add_val_to_bool_arr( + rcl_bool_array_t * const val_array, + bool * value, + const rcutils_allocator_t allocator) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + ADD_VALUE_TO_SIMPLE_ARRAY(val_array, value, bool, allocator); +} + +/// +/// Add a value to an integer array. Create the array if it does not exist +/// +rcutils_ret_t add_val_to_int_arr( + rcl_int64_array_t * const val_array, + int64_t * value, + const rcutils_allocator_t allocator) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + ADD_VALUE_TO_SIMPLE_ARRAY(val_array, value, int64_t, allocator); +} + +/// +/// Add a value to a double array. Create the array if it does not exist +/// +rcutils_ret_t add_val_to_double_arr( + rcl_double_array_t * const val_array, + double * value, + const rcutils_allocator_t allocator) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + ADD_VALUE_TO_SIMPLE_ARRAY(val_array, value, double, allocator); +} + +/// +/// Add a value to a string array. Create the array if it does not exist +/// +rcutils_ret_t add_val_to_string_arr( + rcutils_string_array_t * const val_array, + char * value, + const rcutils_allocator_t allocator) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + if (NULL == val_array->data) { + rcutils_ret_t ret = rcutils_string_array_init(val_array, 1, &allocator); + if (RCUTILS_RET_OK != ret) { + return ret; + } + val_array->data[0U] = value; + } else { + /// Increase the array size by one and add the new value + char ** new_string_arr_ptr = allocator.reallocate( + val_array->data, + ((val_array->size + 1U) * sizeof(char *)), allocator.state); + if (NULL == new_string_arr_ptr) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); + return RCUTILS_RET_BAD_ALLOC; + } + val_array->data = new_string_arr_ptr; + val_array->data[val_array->size] = value; + val_array->size++; + } + return RCUTILS_RET_OK; +} diff --git a/rcl_yaml_param_parser/src/impl/add_to_arrays.h b/rcl_yaml_param_parser/src/impl/add_to_arrays.h new file mode 100644 index 0000000..12c4361 --- /dev/null +++ b/rcl_yaml_param_parser/src/impl/add_to_arrays.h @@ -0,0 +1,79 @@ +// Copyright 2018 Apex.AI, 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 IMPL__ADD_TO_ARRAYS_H_ +#define IMPL__ADD_TO_ARRAYS_H_ + +#include + +#include "rcutils/types.h" + +#include "./types.h" +#include "rcl_yaml_param_parser/types.h" +#include "rcl_yaml_param_parser/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// +/// Add a value to a bool array. Create the array if it does not exist +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t add_val_to_bool_arr( + rcl_bool_array_t * const val_array, + bool * value, + const rcutils_allocator_t allocator); + +/// +/// Add a value to an integer array. Create the array if it does not exist +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t add_val_to_int_arr( + rcl_int64_array_t * const val_array, + int64_t * value, + const rcutils_allocator_t allocator); + +/// +/// Add a value to a double array. Create the array if it does not exist +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t add_val_to_double_arr( + rcl_double_array_t * const val_array, + double * value, + const rcutils_allocator_t allocator); + +/// +/// Add a value to a string array. Create the array if it does not exist +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t add_val_to_string_arr( + rcutils_string_array_t * const val_array, + char * value, + const rcutils_allocator_t allocator); + +/// +/// TODO (anup.pemmaiah): Support byte array +/// + +#ifdef __cplusplus +} +#endif + +#endif // IMPL__ADD_TO_ARRAYS_H_ diff --git a/rcl_yaml_param_parser/src/impl/namespace.h b/rcl_yaml_param_parser/src/impl/namespace.h new file mode 100644 index 0000000..4ca6a4c --- /dev/null +++ b/rcl_yaml_param_parser/src/impl/namespace.h @@ -0,0 +1,68 @@ +// Copyright 2018 Apex.AI, 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 IMPL__NAMESPACE_H_ +#define IMPL__NAMESPACE_H_ + +#include + +#include "rcutils/types.h" + +#include "./types.h" +#include "rcl_yaml_param_parser/types.h" +#include "rcl_yaml_param_parser/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// +/// Add name to namespace tracker +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t add_name_to_ns( + namespace_tracker_t * ns_tracker, + const char * name, + const namespace_type_t namespace_type, + rcutils_allocator_t allocator); + +/// +/// Remove name from namespace tracker +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t rem_name_from_ns( + namespace_tracker_t * ns_tracker, + const namespace_type_t namespace_type, + rcutils_allocator_t allocator); + +/// +/// Replace namespace in namespace tracker +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t replace_ns( + namespace_tracker_t * ns_tracker, + char * const new_ns, + const uint32_t new_ns_count, + const namespace_type_t namespace_type, + rcutils_allocator_t allocator); + +#ifdef __cplusplus +} +#endif + +#endif // IMPL__NAMESPACE_H_ diff --git a/rcl_yaml_param_parser/src/impl/node_params.h b/rcl_yaml_param_parser/src/impl/node_params.h new file mode 100644 index 0000000..16fabd7 --- /dev/null +++ b/rcl_yaml_param_parser/src/impl/node_params.h @@ -0,0 +1,47 @@ +// Copyright 2018 Apex.AI, 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 IMPL__NODE_PARAMS_H_ +#define IMPL__NODE_PARAMS_H_ + +#include "rcl_yaml_param_parser/types.h" +#include "rcl_yaml_param_parser/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// +/// Create rcl_node_params_t structure +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t node_params_init( + rcl_node_params_t * node_params, + const rcutils_allocator_t allocator); + +/// +/// Finalize rcl_node_params_t structure +/// +RCL_YAML_PARAM_PARSER_PUBLIC +void rcl_yaml_node_params_fini( + rcl_node_params_t * node_params, + const rcutils_allocator_t allocator); + +#ifdef __cplusplus +} +#endif + +#endif // IMPL__NODE_PARAMS_H_ diff --git a/rcl_yaml_param_parser/src/impl/parse.h b/rcl_yaml_param_parser/src/impl/parse.h new file mode 100644 index 0000000..781e8f5 --- /dev/null +++ b/rcl_yaml_param_parser/src/impl/parse.h @@ -0,0 +1,94 @@ +// Copyright 2018 Apex.AI, 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 IMPL__PARSE_H_ +#define IMPL__PARSE_H_ + +#include + +#include "rcutils/types.h" + +#include "./types.h" +#include "rcl_yaml_param_parser/types.h" +#include "rcl_yaml_param_parser/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +void * get_value( + const char * const value, + yaml_scalar_style_t style, + data_types_t * val_type, + const rcutils_allocator_t allocator); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t parse_value( + const yaml_event_t event, + const bool is_seq, + const size_t node_idx, + const size_t parameter_idx, + data_types_t * seq_data_type, + rcl_params_t * params_st); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t parse_key( + const yaml_event_t event, + uint32_t * map_level, + bool * is_new_map, + size_t * node_idx, + size_t * parameter_idx, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t parse_file_events( + yaml_parser_t * parser, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t parse_value_events( + yaml_parser_t * parser, + const size_t node_idx, + const size_t parameter_idx, + rcl_params_t * params_st); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t find_node( + const char * node_name, + rcl_params_t * param_st, + size_t * node_idx); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t find_parameter( + const size_t node_idx, + const char * parameter_name, + rcl_params_t * param_st, + size_t * parameter_idx); + +#ifdef __cplusplus +} +#endif + +#endif // IMPL__PARSE_H_ diff --git a/rcl_yaml_param_parser/src/impl/types.h b/rcl_yaml_param_parser/src/impl/types.h new file mode 100644 index 0000000..822cb4e --- /dev/null +++ b/rcl_yaml_param_parser/src/impl/types.h @@ -0,0 +1,72 @@ +// Copyright 2018 Apex.AI, 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. + +/// NOTE: Will allow a max YAML mapping depth of 5 +/// map level 1 : Node name mapping +/// map level 2 : Params mapping + +#ifndef IMPL__TYPES_H_ +#define IMPL__TYPES_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define PARAMS_KEY "ros__parameters" +#define NODE_NS_SEPERATOR "/" +#define PARAMETER_NS_SEPERATOR "." + +#define MAX_NUM_PARAMS_PER_NODE 512U + +typedef enum yaml_map_lvl_e +{ + MAP_UNINIT_LVL = 0U, + MAP_NODE_NAME_LVL = 1U, + MAP_PARAMS_LVL = 2U, +} yaml_map_lvl_t; + +/// Basic supported data types in the yaml file +typedef enum data_types_e +{ + DATA_TYPE_UNKNOWN = 0U, + DATA_TYPE_BOOL = 1U, + DATA_TYPE_INT64 = 2U, + DATA_TYPE_DOUBLE = 3U, + DATA_TYPE_STRING = 4U +} data_types_t; + +typedef enum namespace_type_e +{ + NS_TYPE_NODE = 1U, + NS_TYPE_PARAM = 2U +} namespace_type_t; + +/// Keep track of node and parameter name spaces +typedef struct namespace_tracker_s +{ + char * node_ns; + uint32_t num_node_ns; + char * parameter_ns; + uint32_t num_parameter_ns; +} namespace_tracker_t; + +#ifdef __cplusplus +} +#endif + +#endif // IMPL__TYPES_H_ diff --git a/rcl_yaml_param_parser/src/impl/yaml_variant.h b/rcl_yaml_param_parser/src/impl/yaml_variant.h new file mode 100644 index 0000000..643d1d2 --- /dev/null +++ b/rcl_yaml_param_parser/src/impl/yaml_variant.h @@ -0,0 +1,49 @@ +// Copyright 2018 Apex.AI, 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 IMPL__YAML_VARIANT_H_ +#define IMPL__YAML_VARIANT_H_ + +#include "rcutils/allocator.h" + +#include "./types.h" +#include "rcl_yaml_param_parser/types.h" +#include "rcl_yaml_param_parser/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// +/// Finalize an rcl_yaml_variant_t +/// +RCL_YAML_PARAM_PARSER_PUBLIC +void rcl_yaml_variant_fini( + rcl_variant_t * param_var, + const rcutils_allocator_t allocator); + +/// +/// Copy a yaml_variant_t from param_var to out_param_var +/// +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +bool rcl_yaml_variant_copy( + rcl_variant_t * out_param_var, const rcl_variant_t * param_var, rcutils_allocator_t allocator); + +#ifdef __cplusplus +} +#endif + +#endif // IMPL__YAML_VARIANT_H_ diff --git a/rcl_yaml_param_parser/src/namespace.c b/rcl_yaml_param_parser/src/namespace.c new file mode 100644 index 0000000..db534c3 --- /dev/null +++ b/rcl_yaml_param_parser/src/namespace.c @@ -0,0 +1,193 @@ +// Copyright 2018 Apex.AI, 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 "./impl/namespace.h" + +#include "rcutils/error_handling.h" +#include "rcutils/strdup.h" + +rcutils_ret_t add_name_to_ns( + namespace_tracker_t * ns_tracker, + const char * name, + const namespace_type_t namespace_type, + rcutils_allocator_t allocator) +{ + char * cur_ns; + uint32_t * cur_count; + char * sep_str; + size_t name_len; + size_t ns_len; + size_t sep_len; + size_t tot_len; + + switch (namespace_type) { + case NS_TYPE_NODE: + cur_ns = ns_tracker->node_ns; + cur_count = &(ns_tracker->num_node_ns); + sep_str = NODE_NS_SEPERATOR; + break; + case NS_TYPE_PARAM: + cur_ns = ns_tracker->parameter_ns; + cur_count = &(ns_tracker->num_parameter_ns); + sep_str = PARAMETER_NS_SEPERATOR; + break; + default: + return RCUTILS_RET_ERROR; + } + + /// Add a name to ns + if (NULL == name) { + return RCUTILS_RET_INVALID_ARGUMENT; + } + if (0U == *cur_count) { + cur_ns = rcutils_strdup(name, allocator); + if (NULL == cur_ns) { + return RCUTILS_RET_BAD_ALLOC; + } + } else { + ns_len = strlen(cur_ns); + name_len = strlen(name); + sep_len = strlen(sep_str); + // Check the last sep_len characters of the current NS against the separator string. + if (strcmp(cur_ns + ns_len - sep_len, sep_str) == 0) { + // Current NS already ends with the separator: don't put another separator in. + sep_len = 0; + sep_str = ""; + } + + tot_len = ns_len + sep_len + name_len + 1U; + + char * tmp_ns_ptr = allocator.reallocate(cur_ns, tot_len, allocator.state); + if (NULL == tmp_ns_ptr) { + return RCUTILS_RET_BAD_ALLOC; + } + cur_ns = tmp_ns_ptr; + memcpy((cur_ns + ns_len), sep_str, sep_len); + memcpy((cur_ns + ns_len + sep_len), name, name_len); + cur_ns[tot_len - 1U] = '\0'; + } + *cur_count = (*cur_count + 1U); + + if (NS_TYPE_NODE == namespace_type) { + ns_tracker->node_ns = cur_ns; + } else { + ns_tracker->parameter_ns = cur_ns; + } + return RCUTILS_RET_OK; +} + +rcutils_ret_t rem_name_from_ns( + namespace_tracker_t * ns_tracker, + const namespace_type_t namespace_type, + rcutils_allocator_t allocator) +{ + char * cur_ns; + uint32_t * cur_count; + char * sep_str; + size_t ns_len; + size_t tot_len; + + switch (namespace_type) { + case NS_TYPE_NODE: + cur_ns = ns_tracker->node_ns; + cur_count = &(ns_tracker->num_node_ns); + sep_str = NODE_NS_SEPERATOR; + break; + case NS_TYPE_PARAM: + cur_ns = ns_tracker->parameter_ns; + cur_count = &(ns_tracker->num_parameter_ns); + sep_str = PARAMETER_NS_SEPERATOR; + break; + default: + return RCUTILS_RET_ERROR; + } + + /// Remove last name from ns + if (*cur_count > 0U) { + if (1U == *cur_count) { + allocator.deallocate(cur_ns, allocator.state); + cur_ns = NULL; + } else { + ns_len = strlen(cur_ns); + char * last_idx = NULL; + char * next_str = NULL; + const char * end_ptr = (cur_ns + ns_len); + + next_str = strstr(cur_ns, sep_str); + while (NULL != next_str) { + if (next_str > end_ptr) { + RCUTILS_SET_ERROR_MSG("Internal error. Crossing array boundary"); + return RCUTILS_RET_ERROR; + } + last_idx = next_str; + next_str = (next_str + strlen(sep_str)); + next_str = strstr(next_str, sep_str); + } + if (NULL != last_idx) { + tot_len = ((size_t)(last_idx - cur_ns) + 1U); + char * tmp_ns_ptr = allocator.reallocate(cur_ns, tot_len, allocator.state); + if (NULL == tmp_ns_ptr) { + return RCUTILS_RET_BAD_ALLOC; + } + cur_ns = tmp_ns_ptr; + cur_ns[tot_len - 1U] = '\0'; + } + } + *cur_count = (*cur_count - 1U); + } + if (NS_TYPE_NODE == namespace_type) { + ns_tracker->node_ns = cur_ns; + } else { + ns_tracker->parameter_ns = cur_ns; + } + return RCUTILS_RET_OK; +} + +rcutils_ret_t replace_ns( + namespace_tracker_t * ns_tracker, + char * const new_ns, + const uint32_t new_ns_count, + const namespace_type_t namespace_type, + rcutils_allocator_t allocator) +{ + rcutils_ret_t res = RCUTILS_RET_OK; + + /// Remove the old namespace and point to the new namespace + switch (namespace_type) { + case NS_TYPE_NODE: + if (NULL != ns_tracker->node_ns) { + allocator.deallocate(ns_tracker->node_ns, allocator.state); + } + ns_tracker->node_ns = rcutils_strdup(new_ns, allocator); + if (NULL == ns_tracker->node_ns) { + return RCUTILS_RET_BAD_ALLOC; + } + ns_tracker->num_node_ns = new_ns_count; + break; + case NS_TYPE_PARAM: + if (NULL != ns_tracker->parameter_ns) { + allocator.deallocate(ns_tracker->parameter_ns, allocator.state); + } + ns_tracker->parameter_ns = rcutils_strdup(new_ns, allocator); + if (NULL == ns_tracker->parameter_ns) { + return RCUTILS_RET_BAD_ALLOC; + } + ns_tracker->num_parameter_ns = new_ns_count; + break; + default: + res = RCUTILS_RET_ERROR; + break; + } + return res; +} diff --git a/rcl_yaml_param_parser/src/node_params.c b/rcl_yaml_param_parser/src/node_params.c new file mode 100644 index 0000000..5131a08 --- /dev/null +++ b/rcl_yaml_param_parser/src/node_params.c @@ -0,0 +1,77 @@ +// Copyright 2018 Apex.AI, 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 "./impl/node_params.h" +#include "./impl/types.h" +#include "./impl/yaml_variant.h" + +#include "rcutils/error_handling.h" + +rcutils_ret_t node_params_init( + rcl_node_params_t * node_params, + const rcutils_allocator_t allocator) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_params, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + node_params->parameter_names = allocator.zero_allocate( + MAX_NUM_PARAMS_PER_NODE, sizeof(char *), allocator.state); + if (NULL == node_params->parameter_names) { + return RCUTILS_RET_BAD_ALLOC; + } + + node_params->parameter_values = allocator.zero_allocate( + MAX_NUM_PARAMS_PER_NODE, sizeof(rcl_variant_t), allocator.state); + if (NULL == node_params->parameter_values) { + allocator.deallocate(node_params->parameter_names, allocator.state); + node_params->parameter_names = NULL; + return RCUTILS_RET_BAD_ALLOC; + } + + return RCUTILS_RET_OK; +} + +void rcl_yaml_node_params_fini( + rcl_node_params_t * node_params_st, + const rcutils_allocator_t allocator) +{ + if (NULL == node_params_st) { + return; + } + + if (NULL != node_params_st->parameter_names) { + for (size_t parameter_idx = 0U; parameter_idx < node_params_st->num_params; + parameter_idx++) + { + char * param_name = node_params_st->parameter_names[parameter_idx]; + if (NULL != param_name) { + allocator.deallocate(param_name, allocator.state); + } + } + allocator.deallocate(node_params_st->parameter_names, allocator.state); + node_params_st->parameter_names = NULL; + } + + if (NULL != node_params_st->parameter_values) { + for (size_t parameter_idx = 0U; parameter_idx < node_params_st->num_params; + parameter_idx++) + { + rcl_yaml_variant_fini(&(node_params_st->parameter_values[parameter_idx]), allocator); + } + + allocator.deallocate(node_params_st->parameter_values, allocator.state); + node_params_st->parameter_values = NULL; + } +} diff --git a/rcl_yaml_param_parser/src/parse.c b/rcl_yaml_param_parser/src/parse.c new file mode 100644 index 0000000..f13e359 --- /dev/null +++ b/rcl_yaml_param_parser/src/parse.c @@ -0,0 +1,832 @@ +// Copyright 2018 Apex.AI, 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 "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/strdup.h" + +#include "./impl/add_to_arrays.h" +#include "./impl/parse.h" +#include "./impl/namespace.h" +#include "./impl/node_params.h" + +/// +/// Determine the type of the value and return the converted value +/// NOTE: Only canonical forms supported as of now +/// +void * get_value( + const char * const value, + yaml_scalar_style_t style, + data_types_t * val_type, + const rcutils_allocator_t allocator) +{ + void * ret_val; + int64_t ival; + double dval; + char * endptr = NULL; + + RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, NULL); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_type, NULL); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "allocator is invalid", return NULL); + + /// Check if it is bool + if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && + style != YAML_DOUBLE_QUOTED_SCALAR_STYLE) + { + if ((0 == strcmp(value, "Y")) || + (0 == strcmp(value, "y")) || + (0 == strcmp(value, "yes")) || + (0 == strcmp(value, "Yes")) || + (0 == strcmp(value, "YES")) || + (0 == strcmp(value, "true")) || + (0 == strcmp(value, "True")) || + (0 == strcmp(value, "TRUE")) || + (0 == strcmp(value, "on")) || + (0 == strcmp(value, "On")) || + (0 == strcmp(value, "ON"))) + { + *val_type = DATA_TYPE_BOOL; + ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); + if (NULL == ret_val) { + return NULL; + } + *((bool *)ret_val) = true; + return ret_val; + } + + if ((0 == strcmp(value, "N")) || + (0 == strcmp(value, "n")) || + (0 == strcmp(value, "no")) || + (0 == strcmp(value, "No")) || + (0 == strcmp(value, "NO")) || + (0 == strcmp(value, "false")) || + (0 == strcmp(value, "False")) || + (0 == strcmp(value, "FALSE")) || + (0 == strcmp(value, "off")) || + (0 == strcmp(value, "Off")) || + (0 == strcmp(value, "OFF"))) + { + *val_type = DATA_TYPE_BOOL; + ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); + if (NULL == ret_val) { + return NULL; + } + *((bool *)ret_val) = false; + return ret_val; + } + } + + /// Check for int + if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && + style != YAML_DOUBLE_QUOTED_SCALAR_STYLE) + { + errno = 0; + ival = strtol(value, &endptr, 0); + if ((0 == errno) && (NULL != endptr)) { + if ((NULL != endptr) && (endptr != value)) { + if (('\0' != *value) && ('\0' == *endptr)) { + *val_type = DATA_TYPE_INT64; + ret_val = allocator.zero_allocate(1U, sizeof(int64_t), allocator.state); + if (NULL == ret_val) { + return NULL; + } + *((int64_t *)ret_val) = ival; + return ret_val; + } + } + } + } + + /// Check for float + if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && + style != YAML_DOUBLE_QUOTED_SCALAR_STYLE) + { + errno = 0; + endptr = NULL; + dval = strtod(value, &endptr); + if ((0 == errno) && (NULL != endptr)) { + if ((NULL != endptr) && (endptr != value)) { + if (('\0' != *value) && ('\0' == *endptr)) { + *val_type = DATA_TYPE_DOUBLE; + ret_val = allocator.zero_allocate(1U, sizeof(double), allocator.state); + if (NULL == ret_val) { + return NULL; + } + *((double *)ret_val) = dval; + return ret_val; + } + } + } + errno = 0; + } + + /// It is a string + *val_type = DATA_TYPE_STRING; + ret_val = rcutils_strdup(value, allocator); + return ret_val; +} + +/// +/// Parse the value part of the pair +/// +rcutils_ret_t parse_value( + const yaml_event_t event, + const bool is_seq, + const size_t node_idx, + const size_t parameter_idx, + data_types_t * seq_data_type, + rcl_params_t * params_st) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(seq_data_type, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, RCUTILS_RET_INVALID_ARGUMENT); + + rcutils_allocator_t allocator = params_st->allocator; + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + if (0U == params_st->num_nodes) { + RCUTILS_SET_ERROR_MSG("No node to update"); + return RCUTILS_RET_INVALID_ARGUMENT; + } + + const size_t val_size = event.data.scalar.length; + const char * value = (char *)event.data.scalar.value; + yaml_scalar_style_t style = event.data.scalar.style; + const uint32_t line_num = ((uint32_t)(event.start_mark.line) + 1U); + + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + value, "event argument has no value", return RCUTILS_RET_INVALID_ARGUMENT); + + if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && + style != YAML_DOUBLE_QUOTED_SCALAR_STYLE && + 0U == val_size) + { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("No value at line %d", line_num); + return RCUTILS_RET_ERROR; + } + + if (NULL == params_st->params[node_idx].parameter_values) { + RCUTILS_SET_ERROR_MSG("Internal error: Invalid mem"); + return RCUTILS_RET_BAD_ALLOC; + } + + rcl_variant_t * param_value = &(params_st->params[node_idx].parameter_values[parameter_idx]); + + data_types_t val_type; + void * ret_val = get_value(value, style, &val_type, allocator); + if (NULL == ret_val) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error parsing value %s at line %d", value, line_num); + return RCUTILS_RET_ERROR; + } + + rcutils_ret_t ret = RCUTILS_RET_OK; + switch (val_type) { + case DATA_TYPE_UNKNOWN: + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Unknown data type of value %s at line %d\n", value, line_num); + ret = RCUTILS_RET_ERROR; + break; + case DATA_TYPE_BOOL: + if (!is_seq) { + if (NULL != param_value->bool_value) { + // Overwriting, deallocate original + allocator.deallocate(param_value->bool_value, allocator.state); + } + param_value->bool_value = (bool *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + *seq_data_type = val_type; + if (NULL != param_value->bool_array_value) { + allocator.deallocate(param_value->bool_array_value->values, allocator.state); + allocator.deallocate(param_value->bool_array_value, allocator.state); + param_value->bool_array_value = NULL; + } + param_value->bool_array_value = + allocator.zero_allocate(1U, sizeof(rcl_bool_array_t), allocator.state); + if (NULL == param_value->bool_array_value) { + allocator.deallocate(ret_val, allocator.state); + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); + ret = RCUTILS_RET_BAD_ALLOC; + break; + } + } else { + if (*seq_data_type != val_type) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Sequence should be of same type. Value type 'bool' do not belong at line_num %d", + line_num); + allocator.deallocate(ret_val, allocator.state); + ret = RCUTILS_RET_ERROR; + break; + } + } + ret = add_val_to_bool_arr(param_value->bool_array_value, ret_val, allocator); + if (RCUTILS_RET_OK != ret) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + } + } + break; + case DATA_TYPE_INT64: + if (!is_seq) { + if (NULL != param_value->integer_value) { + // Overwriting, deallocate original + allocator.deallocate(param_value->integer_value, allocator.state); + } + param_value->integer_value = (int64_t *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + if (NULL != param_value->integer_array_value) { + allocator.deallocate(param_value->integer_array_value->values, allocator.state); + allocator.deallocate(param_value->integer_array_value, allocator.state); + param_value->integer_array_value = NULL; + } + *seq_data_type = val_type; + param_value->integer_array_value = + allocator.zero_allocate(1U, sizeof(rcl_int64_array_t), allocator.state); + if (NULL == param_value->integer_array_value) { + allocator.deallocate(ret_val, allocator.state); + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); + ret = RCUTILS_RET_BAD_ALLOC; + break; + } + } else { + if (*seq_data_type != val_type) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Sequence should be of same type. Value type 'integer' do not belong at line_num %d", + line_num); + allocator.deallocate(ret_val, allocator.state); + ret = RCUTILS_RET_ERROR; + break; + } + } + ret = add_val_to_int_arr(param_value->integer_array_value, ret_val, allocator); + if (RCUTILS_RET_OK != ret) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + } + } + break; + case DATA_TYPE_DOUBLE: + if (!is_seq) { + if (NULL != param_value->double_value) { + // Overwriting, deallocate original + allocator.deallocate(param_value->double_value, allocator.state); + } + param_value->double_value = (double *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + if (NULL != param_value->double_array_value) { + allocator.deallocate(param_value->double_array_value->values, allocator.state); + allocator.deallocate(param_value->double_array_value, allocator.state); + param_value->double_array_value = NULL; + } + *seq_data_type = val_type; + param_value->double_array_value = + allocator.zero_allocate(1U, sizeof(rcl_double_array_t), allocator.state); + if (NULL == param_value->double_array_value) { + allocator.deallocate(ret_val, allocator.state); + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); + ret = RCUTILS_RET_BAD_ALLOC; + break; + } + } else { + if (*seq_data_type != val_type) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Sequence should be of same type. Value type 'double' do not belong at line_num %d", + line_num); + allocator.deallocate(ret_val, allocator.state); + ret = RCUTILS_RET_ERROR; + break; + } + } + ret = add_val_to_double_arr(param_value->double_array_value, ret_val, allocator); + if (RCUTILS_RET_OK != ret) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + } + } + break; + case DATA_TYPE_STRING: + if (!is_seq) { + if (NULL != param_value->string_value) { + // Overwriting, deallocate original + allocator.deallocate(param_value->string_value, allocator.state); + } + param_value->string_value = (char *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + if (NULL != param_value->string_array_value) { + if (RCUTILS_RET_OK != rcutils_string_array_fini(param_value->string_array_value)) { + // Log and continue ... + RCUTILS_SAFE_FWRITE_TO_STDERR("Error deallocating string array"); + } + allocator.deallocate(param_value->string_array_value, allocator.state); + param_value->string_array_value = NULL; + } + *seq_data_type = val_type; + param_value->string_array_value = + allocator.zero_allocate(1U, sizeof(rcutils_string_array_t), allocator.state); + if (NULL == param_value->string_array_value) { + allocator.deallocate(ret_val, allocator.state); + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); + ret = RCUTILS_RET_BAD_ALLOC; + break; + } + } else { + if (*seq_data_type != val_type) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Sequence should be of same type. Value type 'string' do not belong at line_num %d", + line_num); + allocator.deallocate(ret_val, allocator.state); + ret = RCUTILS_RET_ERROR; + break; + } + } + ret = add_val_to_string_arr(param_value->string_array_value, ret_val, allocator); + if (RCUTILS_RET_OK != ret) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + } + } + break; + default: + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Unknown data type of value %s at line %d", value, line_num); + ret = RCUTILS_RET_ERROR; + allocator.deallocate(ret_val, allocator.state); + break; + } + return ret; +} + +/// +/// Parse the key part of the pair +/// +rcutils_ret_t parse_key( + const yaml_event_t event, + uint32_t * map_level, + bool * is_new_map, + size_t * node_idx, + size_t * parameter_idx, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(map_level, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, RCUTILS_RET_INVALID_ARGUMENT); + rcutils_allocator_t allocator = params_st->allocator; + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + const size_t val_size = event.data.scalar.length; + const char * value = (char *)event.data.scalar.value; + const uint32_t line_num = ((uint32_t)(event.start_mark.line) + 1U); + + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + value, "event argument has no value", return RCUTILS_RET_INVALID_ARGUMENT); + + if (0U == val_size) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("No key at line %d", line_num); + return RCUTILS_RET_ERROR; + } + + rcutils_ret_t ret = RCUTILS_RET_OK; + switch (*map_level) { + case MAP_UNINIT_LVL: + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Unintialized map level at line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + case MAP_NODE_NAME_LVL: + { + /// Till we get PARAMS_KEY, keep adding to node namespace + if (0 != strncmp(PARAMS_KEY, value, strlen(PARAMS_KEY))) { + ret = add_name_to_ns(ns_tracker, value, NS_TYPE_NODE, allocator); + if (RCUTILS_RET_OK != ret) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Internal error adding node namespace at line %d", line_num); + break; + } + } else { + if (0U == ns_tracker->num_node_ns) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "There are no node names before %s at line %d", PARAMS_KEY, line_num); + ret = RCUTILS_RET_ERROR; + break; + } + /// The previous key(last name in namespace) was the node name. Remove it + /// from the namespace + char * node_name_ns = rcutils_strdup(ns_tracker->node_ns, allocator); + if (NULL == node_name_ns) { + ret = RCUTILS_RET_BAD_ALLOC; + break; + } + + ret = find_node(node_name_ns, params_st, node_idx); + allocator.deallocate(node_name_ns, allocator.state); + if (RCUTILS_RET_OK != ret) { + break; + } + + ret = rem_name_from_ns(ns_tracker, NS_TYPE_NODE, allocator); + if (RCUTILS_RET_OK != ret) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Internal error adding node namespace at line %d", line_num); + break; + } + /// Bump the map level to PARAMS + (*map_level)++; + } + } + break; + case MAP_PARAMS_LVL: + { + char * parameter_ns = NULL; + char * param_name = NULL; + + /// If it is a new map, the previous key is param namespace + if (*is_new_map) { + parameter_ns = params_st->params[*node_idx].parameter_names[*parameter_idx]; + if (NULL == parameter_ns) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Internal error creating param namespace at line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + } + ret = replace_ns( + ns_tracker, parameter_ns, (ns_tracker->num_parameter_ns + 1U), + NS_TYPE_PARAM, allocator); + if (RCUTILS_RET_OK != ret) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Internal error replacing namespace at line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + } + *is_new_map = false; + } + + // Guard against adding more than the maximum allowed parameters + if (params_st->params[*node_idx].num_params >= MAX_NUM_PARAMS_PER_NODE) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Exceeded maximum allowed number of parameters for a node (%d)", + MAX_NUM_PARAMS_PER_NODE); + ret = RCUTILS_RET_ERROR; + break; + } + + /// Add a parameter name into the node parameters + parameter_ns = ns_tracker->parameter_ns; + if (NULL == parameter_ns) { + ret = find_parameter(*node_idx, value, params_st, parameter_idx); + if (ret != RCUTILS_RET_OK) { + break; + } + } else { + ret = find_parameter(*node_idx, parameter_ns, params_st, parameter_idx); + if (ret != RCUTILS_RET_OK) { + break; + } + + const size_t params_ns_len = strlen(parameter_ns); + const size_t param_name_len = strlen(value); + const size_t tot_len = (params_ns_len + param_name_len + 2U); + + param_name = allocator.zero_allocate(1U, tot_len, allocator.state); + if (NULL == param_name) { + ret = RCUTILS_RET_BAD_ALLOC; + break; + } + + memcpy(param_name, parameter_ns, params_ns_len); + param_name[params_ns_len] = '.'; + memcpy((param_name + params_ns_len + 1U), value, param_name_len); + param_name[tot_len - 1U] = '\0'; + + if (NULL != params_st->params[*node_idx].parameter_names[*parameter_idx]) { + // This memory was allocated in find_parameter(), and its pointer is being overwritten + allocator.deallocate( + params_st->params[*node_idx].parameter_names[*parameter_idx], allocator.state); + } + params_st->params[*node_idx].parameter_names[*parameter_idx] = param_name; + } + } + break; + default: + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("Unknown map level at line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + } + return ret; +} + +/// +/// Get events from parsing a parameter YAML file and process them +/// +rcutils_ret_t parse_file_events( + yaml_parser_t * parser, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st) +{ + int32_t done_parsing = 0; + bool is_key = true; + bool is_seq = false; + uint32_t line_num = 0; + data_types_t seq_data_type = DATA_TYPE_UNKNOWN; + uint32_t map_level = 1U; + uint32_t map_depth = 0U; + bool is_new_map = false; + + RCUTILS_CHECK_ARGUMENT_FOR_NULL(parser, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, RCUTILS_RET_INVALID_ARGUMENT); + rcutils_allocator_t allocator = params_st->allocator; + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + yaml_event_t event; + size_t node_idx = 0; + size_t parameter_idx = 0; + rcutils_ret_t ret = RCUTILS_RET_OK; + while (0 == done_parsing) { + if (RCUTILS_RET_OK != ret) { + break; + } + int success = yaml_parser_parse(parser, &event); + if (0 == success) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error parsing a event near line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + } + line_num = ((uint32_t)(event.start_mark.line) + 1U); + switch (event.type) { + case YAML_STREAM_END_EVENT: + done_parsing = 1; + yaml_event_delete(&event); + break; + case YAML_SCALAR_EVENT: + { + /// Need to toggle between key and value at params level + if (is_key) { + ret = parse_key( + event, &map_level, &is_new_map, &node_idx, ¶meter_idx, ns_tracker, params_st); + if (RCUTILS_RET_OK != ret) { + break; + } + is_key = false; + } else { + /// It is a value + if (map_level < (uint32_t)(MAP_PARAMS_LVL)) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Cannot have a value before %s at line %d", PARAMS_KEY, line_num); + ret = RCUTILS_RET_ERROR; + break; + } + if (0U == params_st->num_nodes) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Cannot have a value before %s at line %d", PARAMS_KEY, line_num); + yaml_event_delete(&event); + return RCUTILS_RET_ERROR; + } + if (0U == params_st->params[node_idx].num_params) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Cannot have a value before %s at line %d", PARAMS_KEY, line_num); + yaml_event_delete(&event); + return RCUTILS_RET_ERROR; + } + ret = parse_value(event, is_seq, node_idx, parameter_idx, &seq_data_type, params_st); + if (RCUTILS_RET_OK != ret) { + break; + } + if (!is_seq) { + is_key = true; + } + } + } + break; + case YAML_SEQUENCE_START_EVENT: + if (is_key) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Sequences cannot be key at line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + } + if (map_level < (uint32_t)(MAP_PARAMS_LVL)) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Sequences can only be values and not keys in params. Error at line %d\n", line_num); + ret = RCUTILS_RET_ERROR; + break; + } + is_seq = true; + seq_data_type = DATA_TYPE_UNKNOWN; + break; + case YAML_SEQUENCE_END_EVENT: + is_seq = false; + is_key = true; + break; + case YAML_MAPPING_START_EVENT: + map_depth++; + is_new_map = true; + is_key = true; + /// Disable new map if it is PARAMS_KEY map + if ((MAP_PARAMS_LVL == map_level) && + ((map_depth - (ns_tracker->num_node_ns + 1U)) == 2U)) + { + is_new_map = false; + } + break; + case YAML_MAPPING_END_EVENT: + if (MAP_PARAMS_LVL == map_level) { + if (ns_tracker->num_parameter_ns > 0U) { + /// Remove param namesapce + ret = rem_name_from_ns(ns_tracker, NS_TYPE_PARAM, allocator); + if (RCUTILS_RET_OK != ret) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Internal error removing parameter namespace at line %d", line_num); + break; + } + } else { + map_level--; + } + } else { + if ((MAP_NODE_NAME_LVL == map_level) && + (map_depth == (ns_tracker->num_node_ns + 1U))) + { + /// Remove node namespace + ret = rem_name_from_ns(ns_tracker, NS_TYPE_NODE, allocator); + if (RCUTILS_RET_OK != ret) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Internal error removing node namespace at line %d", line_num); + break; + } + } + } + map_depth--; + break; + case YAML_ALIAS_EVENT: + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Will not support aliasing at line %d\n", line_num); + ret = RCUTILS_RET_ERROR; + break; + case YAML_STREAM_START_EVENT: + break; + case YAML_DOCUMENT_START_EVENT: + break; + case YAML_DOCUMENT_END_EVENT: + break; + case YAML_NO_EVENT: + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Received an empty event at line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + default: + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("Unknown YAML event at line %d", line_num); + ret = RCUTILS_RET_ERROR; + break; + } + yaml_event_delete(&event); + } + return ret; +} + +/// +/// Get events from parsing a parameter YAML value string and process them +/// +rcutils_ret_t parse_value_events( + yaml_parser_t * parser, + const size_t node_idx, + const size_t parameter_idx, + rcl_params_t * params_st) +{ + bool is_seq = false; + data_types_t seq_data_type = DATA_TYPE_UNKNOWN; + rcutils_ret_t ret = RCUTILS_RET_OK; + bool done_parsing = false; + while (RCUTILS_RET_OK == ret && !done_parsing) { + yaml_event_t event; + int success = yaml_parser_parse(parser, &event); + if (0 == success) { + RCUTILS_SET_ERROR_MSG("Error parsing an event"); + ret = RCUTILS_RET_ERROR; + break; + } + switch (event.type) { + case YAML_STREAM_END_EVENT: + done_parsing = true; + break; + case YAML_SCALAR_EVENT: + ret = parse_value( + event, is_seq, node_idx, parameter_idx, &seq_data_type, params_st); + break; + case YAML_SEQUENCE_START_EVENT: + is_seq = true; + seq_data_type = DATA_TYPE_UNKNOWN; + break; + case YAML_SEQUENCE_END_EVENT: + is_seq = false; + break; + case YAML_STREAM_START_EVENT: + break; + case YAML_DOCUMENT_START_EVENT: + break; + case YAML_DOCUMENT_END_EVENT: + break; + case YAML_NO_EVENT: + RCUTILS_SET_ERROR_MSG("Received an empty event"); + ret = RCUTILS_RET_ERROR; + break; + default: + RCUTILS_SET_ERROR_MSG("Unknown YAML event"); + ret = RCUTILS_RET_ERROR; + break; + } + yaml_event_delete(&event); + } + return ret; +} + +/// +/// Find parameter entry index in node parameters' structure +/// +rcutils_ret_t find_parameter( + const size_t node_idx, + const char * parameter_name, + rcl_params_t * param_st, + size_t * parameter_idx) +{ + assert(NULL != parameter_name); + assert(NULL != param_st); + assert(NULL != parameter_idx); + + assert(node_idx < param_st->num_nodes); + + rcl_node_params_t * node_param_st = &(param_st->params[node_idx]); + for (*parameter_idx = 0U; *parameter_idx < node_param_st->num_params; (*parameter_idx)++) { + if (0 == strcmp(node_param_st->parameter_names[*parameter_idx], parameter_name)) { + // Parameter found. + return RCUTILS_RET_OK; + } + } + // Parameter not found, add it. + rcutils_allocator_t allocator = param_st->allocator; + if (NULL != node_param_st->parameter_names[*parameter_idx]) { + param_st->allocator.deallocate( + node_param_st->parameter_names[*parameter_idx], param_st->allocator.state); + } + node_param_st->parameter_names[*parameter_idx] = rcutils_strdup(parameter_name, allocator); + if (NULL == node_param_st->parameter_names[*parameter_idx]) { + return RCUTILS_RET_BAD_ALLOC; + } + node_param_st->num_params++; + return RCUTILS_RET_OK; +} + +/// +/// Find node entry index in parameters' structure +/// +rcutils_ret_t find_node( + const char * node_name, + rcl_params_t * param_st, + size_t * node_idx) +{ + assert(NULL != node_name); + assert(NULL != param_st); + assert(NULL != node_idx); + + for (*node_idx = 0U; *node_idx < param_st->num_nodes; (*node_idx)++) { + if (0 == strcmp(param_st->node_names[*node_idx], node_name)) { + // Node found. + return RCUTILS_RET_OK; + } + } + // Node not found, add it. + rcutils_allocator_t allocator = param_st->allocator; + param_st->node_names[*node_idx] = rcutils_strdup(node_name, allocator); + if (NULL == param_st->node_names[*node_idx]) { + return RCUTILS_RET_BAD_ALLOC; + } + rcutils_ret_t ret = node_params_init(&(param_st->params[*node_idx]), allocator); + if (RCUTILS_RET_OK != ret) { + allocator.deallocate(param_st->node_names[*node_idx], allocator.state); + return ret; + } + param_st->num_nodes++; + return RCUTILS_RET_OK; +} diff --git a/rcl_yaml_param_parser/src/parser.c b/rcl_yaml_param_parser/src/parser.c index d294b5a..35d2322 100644 --- a/rcl_yaml_param_parser/src/parser.c +++ b/rcl_yaml_param_parser/src/parser.c @@ -30,347 +30,12 @@ #include "rcutils/strdup.h" #include "rcutils/types.h" -/// NOTE: Will allow a max YAML mapping depth of 5 -/// map level 1 : Node name mapping -/// map level 2 : Params mapping -typedef enum yaml_map_lvl_e -{ - MAP_UNINIT_LVL = 0U, - MAP_NODE_NAME_LVL = 1U, - MAP_PARAMS_LVL = 2U, -} yaml_map_lvl_t; - -/// Basic supported data types in the yaml file -typedef enum data_types_e -{ - DATA_TYPE_UNKNOWN = 0U, - DATA_TYPE_BOOL = 1U, - DATA_TYPE_INT64 = 2U, - DATA_TYPE_DOUBLE = 3U, - DATA_TYPE_STRING = 4U -} data_types_t; - -typedef enum namespace_type_e -{ - NS_TYPE_NODE = 1U, - NS_TYPE_PARAM = 2U -} namespace_type_t; - -/// Keep track of node and parameter name spaces -typedef struct namespace_tracker_s -{ - char * node_ns; - uint32_t num_node_ns; - char * parameter_ns; - uint32_t num_parameter_ns; -} namespace_tracker_t; - -#define PARAMS_KEY "ros__parameters" -#define NODE_NS_SEPERATOR "/" -#define PARAMETER_NS_SEPERATOR "." +#include "./impl/types.h" +#include "./impl/parse.h" +#include "./impl/node_params.h" +#include "./impl/yaml_variant.h" #define MAX_NUM_NODE_ENTRIES 256U -#define MAX_NUM_PARAMS_PER_NODE 512U - -static rcutils_ret_t node_params_init( - rcl_node_params_t * node_params, - const rcutils_allocator_t allocator); - -static void rcl_yaml_variant_fini( - rcl_variant_t * param_var, - const rcutils_allocator_t allocator); - -static void rcl_yaml_node_params_fini( - rcl_node_params_t * node_params, - const rcutils_allocator_t allocator); - -static rcutils_ret_t add_val_to_bool_arr( - rcl_bool_array_t * const val_array, - bool * value, - const rcutils_allocator_t allocator); - -static rcutils_ret_t add_val_to_int_arr( - rcl_int64_array_t * const val_array, - int64_t * value, - const rcutils_allocator_t allocator); - -static rcutils_ret_t add_val_to_double_arr( - rcl_double_array_t * const val_array, - double * value, - const rcutils_allocator_t allocator); - -static rcutils_ret_t add_val_to_string_arr( - rcutils_string_array_t * const val_array, - char * value, - const rcutils_allocator_t allocator); - -/// -/// TODO (anup.pemmaiah): Support byte array -/// - -static rcutils_ret_t add_name_to_ns( - namespace_tracker_t * ns_tracker, - const char * name, - const namespace_type_t namespace_type, - rcutils_allocator_t allocator); - -static rcutils_ret_t rem_name_from_ns( - namespace_tracker_t * ns_tracker, - const namespace_type_t namespace_type, - rcutils_allocator_t allocator); - -static rcutils_ret_t replace_ns( - namespace_tracker_t * ns_tracker, - char * const new_ns, - const uint32_t new_ns_count, - const namespace_type_t namespace_type, - rcutils_allocator_t allocator); - -static void * get_value( - const char * const value, - yaml_scalar_style_t style, - data_types_t * val_type, - const rcutils_allocator_t allocator); - -static rcutils_ret_t parse_value( - const yaml_event_t event, - const bool is_seq, - const size_t node_idx, - const size_t parameter_idx, - data_types_t * seq_data_type, - rcl_params_t * params_st); - -static rcutils_ret_t parse_key( - const yaml_event_t event, - uint32_t * map_level, - bool * is_new_map, - size_t * node_idx, - size_t * parameter_idx, - namespace_tracker_t * ns_tracker, - rcl_params_t * params_st); - -static rcutils_ret_t parse_file_events( - yaml_parser_t * parser, - namespace_tracker_t * ns_tracker, - rcl_params_t * params_st); - -static rcutils_ret_t parse_value_events( - yaml_parser_t * parser, - const size_t node_idx, - const size_t parameter_idx, - rcl_params_t * params_st); - -/// -/// Add name to namespace tracker -/// -static rcutils_ret_t add_name_to_ns( - namespace_tracker_t * ns_tracker, - const char * name, - const namespace_type_t namespace_type, - rcutils_allocator_t allocator) -{ - char * cur_ns; - uint32_t * cur_count; - char * sep_str; - size_t name_len; - size_t ns_len; - size_t sep_len; - size_t tot_len; - - switch (namespace_type) { - case NS_TYPE_NODE: - cur_ns = ns_tracker->node_ns; - cur_count = &(ns_tracker->num_node_ns); - sep_str = NODE_NS_SEPERATOR; - break; - case NS_TYPE_PARAM: - cur_ns = ns_tracker->parameter_ns; - cur_count = &(ns_tracker->num_parameter_ns); - sep_str = PARAMETER_NS_SEPERATOR; - break; - default: - return RCUTILS_RET_ERROR; - } - - /// Add a name to ns - if (NULL == name) { - return RCUTILS_RET_INVALID_ARGUMENT; - } - if (0U == *cur_count) { - cur_ns = rcutils_strdup(name, allocator); - if (NULL == cur_ns) { - return RCUTILS_RET_BAD_ALLOC; - } - } else { - ns_len = strlen(cur_ns); - name_len = strlen(name); - sep_len = strlen(sep_str); - // Check the last sep_len characters of the current NS against the separator string. - if (strcmp(cur_ns + ns_len - sep_len, sep_str) == 0) { - // Current NS already ends with the separator: don't put another separator in. - sep_len = 0; - sep_str = ""; - } - - tot_len = ns_len + sep_len + name_len + 1U; - - char * tmp_ns_ptr = allocator.reallocate(cur_ns, tot_len, allocator.state); - if (NULL == tmp_ns_ptr) { - return RCUTILS_RET_BAD_ALLOC; - } - cur_ns = tmp_ns_ptr; - memmove((cur_ns + ns_len), sep_str, sep_len); - memmove((cur_ns + ns_len + sep_len), name, name_len); - cur_ns[tot_len - 1U] = '\0'; - } - *cur_count = (*cur_count + 1U); - - if (NS_TYPE_NODE == namespace_type) { - ns_tracker->node_ns = cur_ns; - } else { - ns_tracker->parameter_ns = cur_ns; - } - return RCUTILS_RET_OK; -} - -/// -/// Remove name from namespace tracker -/// -static rcutils_ret_t rem_name_from_ns( - namespace_tracker_t * ns_tracker, - const namespace_type_t namespace_type, - rcutils_allocator_t allocator) -{ - char * cur_ns; - uint32_t * cur_count; - char * sep_str; - size_t ns_len; - size_t tot_len; - - switch (namespace_type) { - case NS_TYPE_NODE: - cur_ns = ns_tracker->node_ns; - cur_count = &(ns_tracker->num_node_ns); - sep_str = NODE_NS_SEPERATOR; - break; - case NS_TYPE_PARAM: - cur_ns = ns_tracker->parameter_ns; - cur_count = &(ns_tracker->num_parameter_ns); - sep_str = PARAMETER_NS_SEPERATOR; - break; - default: - return RCUTILS_RET_ERROR; - } - - /// Remove last name from ns - if (*cur_count > 0U) { - if (1U == *cur_count) { - allocator.deallocate(cur_ns, allocator.state); - cur_ns = NULL; - } else { - ns_len = strlen(cur_ns); - char * last_idx = NULL; - char * next_str = NULL; - const char * end_ptr = (cur_ns + ns_len); - - next_str = strstr(cur_ns, sep_str); - while (NULL != next_str) { - if (next_str > end_ptr) { - RCUTILS_SET_ERROR_MSG("Internal error. Crossing array boundary"); - return RCUTILS_RET_ERROR; - } - last_idx = next_str; - next_str = (next_str + strlen(sep_str)); - next_str = strstr(next_str, sep_str); - } - if (NULL != last_idx) { - tot_len = ((size_t)(last_idx - cur_ns) + 1U); - char * tmp_ns_ptr = allocator.reallocate(cur_ns, tot_len, allocator.state); - if (NULL == tmp_ns_ptr) { - return RCUTILS_RET_BAD_ALLOC; - } - cur_ns = tmp_ns_ptr; - cur_ns[tot_len - 1U] = '\0'; - } - } - *cur_count = (*cur_count - 1U); - } - if (NS_TYPE_NODE == namespace_type) { - ns_tracker->node_ns = cur_ns; - } else { - ns_tracker->parameter_ns = cur_ns; - } - return RCUTILS_RET_OK; -} - -/// -/// Replace namespace in namespace tracker -/// -static rcutils_ret_t replace_ns( - namespace_tracker_t * ns_tracker, - char * const new_ns, - const uint32_t new_ns_count, - const namespace_type_t namespace_type, - rcutils_allocator_t allocator) -{ - rcutils_ret_t res = RCUTILS_RET_OK; - - /// Remove the old namespace and point to the new namespace - switch (namespace_type) { - case NS_TYPE_NODE: - if (NULL != ns_tracker->node_ns) { - allocator.deallocate(ns_tracker->node_ns, allocator.state); - } - ns_tracker->node_ns = rcutils_strdup(new_ns, allocator); - if (NULL == ns_tracker->node_ns) { - return RCUTILS_RET_BAD_ALLOC; - } - ns_tracker->num_node_ns = new_ns_count; - break; - case NS_TYPE_PARAM: - if (NULL != ns_tracker->parameter_ns) { - allocator.deallocate(ns_tracker->parameter_ns, allocator.state); - } - ns_tracker->parameter_ns = rcutils_strdup(new_ns, allocator); - if (NULL == ns_tracker->parameter_ns) { - return RCUTILS_RET_BAD_ALLOC; - } - ns_tracker->num_parameter_ns = new_ns_count; - break; - default: - res = RCUTILS_RET_ERROR; - break; - } - return res; -} - -/// -/// Create rcl_node_params_t structure -/// -static rcutils_ret_t node_params_init( - rcl_node_params_t * node_params, - const rcutils_allocator_t allocator) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_params, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - node_params->parameter_names = allocator.zero_allocate( - MAX_NUM_PARAMS_PER_NODE, sizeof(char *), allocator.state); - if (NULL == node_params->parameter_names) { - return RCUTILS_RET_BAD_ALLOC; - } - - node_params->parameter_values = allocator.zero_allocate( - MAX_NUM_PARAMS_PER_NODE, sizeof(rcl_variant_t), allocator.state); - if (NULL == node_params->parameter_values) { - allocator.deallocate(node_params->parameter_names, allocator.state); - node_params->parameter_names = NULL; - return RCUTILS_RET_BAD_ALLOC; - } - - return RCUTILS_RET_OK; -} /// /// Create the rcl_params_t parameter structure @@ -450,131 +115,10 @@ rcl_params_t * rcl_yaml_node_struct_copy( } out_node_params_st->num_params++; - rcl_variant_t * param_var = &(node_params_st->parameter_values[parameter_idx]); + const rcl_variant_t * param_var = &(node_params_st->parameter_values[parameter_idx]); rcl_variant_t * out_param_var = &(out_node_params_st->parameter_values[parameter_idx]); - if (NULL != param_var->bool_value) { - out_param_var->bool_value = allocator.allocate(sizeof(bool), allocator.state); - if (NULL == out_param_var->bool_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - *(out_param_var->bool_value) = *(param_var->bool_value); - } else if (NULL != param_var->integer_value) { - out_param_var->integer_value = allocator.allocate(sizeof(int64_t), allocator.state); - if (NULL == out_param_var->integer_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - *(out_param_var->integer_value) = *(param_var->integer_value); - } else if (NULL != param_var->double_value) { - out_param_var->double_value = allocator.allocate(sizeof(double), allocator.state); - if (NULL == out_param_var->double_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - *(out_param_var->double_value) = *(param_var->double_value); - } else if (NULL != param_var->string_value) { - out_param_var->string_value = - rcutils_strdup(param_var->string_value, allocator); - if (NULL == out_param_var->string_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - } else if (NULL != param_var->bool_array_value) { - out_param_var->bool_array_value = - allocator.allocate(sizeof(rcl_bool_array_t), allocator.state); - if (NULL == out_param_var->bool_array_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - if (0U != param_var->bool_array_value->size) { - out_param_var->bool_array_value->values = allocator.allocate( - sizeof(bool) * param_var->bool_array_value->size, allocator.state); - if (NULL == out_param_var->bool_array_value->values) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - memcpy( - out_param_var->bool_array_value->values, - param_var->bool_array_value->values, - sizeof(bool) * param_var->bool_array_value->size); - } else { - out_param_var->bool_array_value->values = NULL; - } - out_param_var->bool_array_value->size = param_var->bool_array_value->size; - } else if (NULL != param_var->integer_array_value) { - out_param_var->integer_array_value = - allocator.allocate(sizeof(rcl_int64_array_t), allocator.state); - if (NULL == out_param_var->integer_array_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - if (0U != param_var->integer_array_value->size) { - out_param_var->integer_array_value->values = allocator.allocate( - sizeof(int64_t) * param_var->integer_array_value->size, allocator.state); - if (NULL == out_param_var->integer_array_value->values) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - memcpy( - out_param_var->integer_array_value->values, - param_var->integer_array_value->values, - sizeof(int64_t) * param_var->integer_array_value->size); - } else { - out_param_var->integer_array_value->values = NULL; - } - out_param_var->integer_array_value->size = param_var->integer_array_value->size; - } else if (NULL != param_var->double_array_value) { - out_param_var->double_array_value = - allocator.allocate(sizeof(rcl_double_array_t), allocator.state); - if (NULL == out_param_var->double_array_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - if (0U != param_var->double_array_value->size) { - out_param_var->double_array_value->values = allocator.allocate( - sizeof(double) * param_var->double_array_value->size, allocator.state); - if (NULL == out_param_var->double_array_value->values) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - memcpy( - out_param_var->double_array_value->values, - param_var->double_array_value->values, - sizeof(double) * param_var->double_array_value->size); - } else { - out_param_var->double_array_value->values = NULL; - } - out_param_var->double_array_value->size = param_var->double_array_value->size; - } else if (NULL != param_var->string_array_value) { - out_param_var->string_array_value = - allocator.allocate(sizeof(rcutils_string_array_t), allocator.state); - if (NULL == out_param_var->string_array_value) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - *(out_param_var->string_array_value) = rcutils_get_zero_initialized_string_array(); - ret = rcutils_string_array_init( - out_param_var->string_array_value, - param_var->string_array_value->size, - &(param_var->string_array_value->allocator)); - if (RCUTILS_RET_OK != ret) { - if (RCUTILS_RET_BAD_ALLOC == ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - } - goto fail; - } - for (size_t str_idx = 0U; str_idx < param_var->string_array_value->size; ++str_idx) { - out_param_var->string_array_value->data[str_idx] = rcutils_strdup( - param_var->string_array_value->data[str_idx], - out_param_var->string_array_value->allocator); - if (NULL == out_param_var->string_array_value->data[str_idx]) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - goto fail; - } - } - } else { - /// Nothing to do to keep pclint happy + if (!rcl_yaml_variant_copy(out_param_var, param_var, allocator)) { + goto fail; } } } @@ -585,89 +129,6 @@ fail: return NULL; } -void rcl_yaml_variant_fini( - rcl_variant_t * param_var, - const rcutils_allocator_t allocator) -{ - if (NULL == param_var) { - return; - } - - if (NULL != param_var->bool_value) { - allocator.deallocate(param_var->bool_value, allocator.state); - param_var->bool_value = NULL; - } else if (NULL != param_var->integer_value) { - allocator.deallocate(param_var->integer_value, allocator.state); - param_var->integer_value = NULL; - } else if (NULL != param_var->double_value) { - allocator.deallocate(param_var->double_value, allocator.state); - param_var->double_value = NULL; - } else if (NULL != param_var->string_value) { - allocator.deallocate(param_var->string_value, allocator.state); - param_var->string_value = NULL; - } else if (NULL != param_var->bool_array_value) { - if (NULL != param_var->bool_array_value->values) { - allocator.deallocate(param_var->bool_array_value->values, allocator.state); - } - allocator.deallocate(param_var->bool_array_value, allocator.state); - param_var->bool_array_value = NULL; - } else if (NULL != param_var->integer_array_value) { - if (NULL != param_var->integer_array_value->values) { - allocator.deallocate(param_var->integer_array_value->values, allocator.state); - } - allocator.deallocate(param_var->integer_array_value, allocator.state); - param_var->integer_array_value = NULL; - } else if (NULL != param_var->double_array_value) { - if (NULL != param_var->double_array_value->values) { - allocator.deallocate(param_var->double_array_value->values, allocator.state); - } - allocator.deallocate(param_var->double_array_value, allocator.state); - param_var->double_array_value = NULL; - } else if (NULL != param_var->string_array_value) { - if (RCUTILS_RET_OK != rcutils_string_array_fini(param_var->string_array_value)) { - // Log and continue ... - RCUTILS_SAFE_FWRITE_TO_STDERR("Error deallocating string array"); - } - allocator.deallocate(param_var->string_array_value, allocator.state); - param_var->string_array_value = NULL; - } else { - /// Nothing to do to keep pclint happy - } -} - -void rcl_yaml_node_params_fini( - rcl_node_params_t * node_params_st, - const rcutils_allocator_t allocator) -{ - if (NULL == node_params_st) { - return; - } - - if (NULL != node_params_st->parameter_names) { - for (size_t parameter_idx = 0U; parameter_idx < node_params_st->num_params; - parameter_idx++) - { - char * param_name = node_params_st->parameter_names[parameter_idx]; - if (NULL != param_name) { - allocator.deallocate(param_name, allocator.state); - } - } - allocator.deallocate(node_params_st->parameter_names, allocator.state); - node_params_st->parameter_names = NULL; - } - - if (NULL != node_params_st->parameter_values) { - for (size_t parameter_idx = 0U; parameter_idx < node_params_st->num_params; - parameter_idx++) - { - rcl_yaml_variant_fini(&(node_params_st->parameter_values[parameter_idx]), allocator); - } - - allocator.deallocate(node_params_st->parameter_values, allocator.state); - node_params_st->parameter_values = NULL; - } -} - /// /// Free param structure /// NOTE: If there is an error, would recommend just to safely exit the process instead @@ -706,1061 +167,6 @@ void rcl_yaml_node_struct_fini( allocator.deallocate(params_st, allocator.state); } - -/// -/// Find node entry index in parameters' structure -/// -static rcutils_ret_t find_node( - const char * node_name, - rcl_params_t * param_st, - size_t * node_idx) -{ - assert(NULL != node_name); - assert(NULL != param_st); - assert(NULL != node_idx); - - for (*node_idx = 0U; *node_idx < param_st->num_nodes; (*node_idx)++) { - if (0 == strcmp(param_st->node_names[*node_idx], node_name)) { - // Node found. - return RCUTILS_RET_OK; - } - } - // Node not found, add it. - rcutils_allocator_t allocator = param_st->allocator; - param_st->node_names[*node_idx] = rcutils_strdup(node_name, allocator); - if (NULL == param_st->node_names[*node_idx]) { - return RCUTILS_RET_BAD_ALLOC; - } - rcutils_ret_t ret = node_params_init(&(param_st->params[*node_idx]), allocator); - if (RCUTILS_RET_OK != ret) { - allocator.deallocate(param_st->node_names[*node_idx], allocator.state); - return ret; - } - param_st->num_nodes++; - return RCUTILS_RET_OK; -} - -/// -/// Find paremeter entry index in node parameters' structure -/// -static rcutils_ret_t find_parameter( - const size_t node_idx, - const char * parameter_name, - rcl_params_t * param_st, - size_t * parameter_idx) -{ - assert(NULL != parameter_name); - assert(NULL != param_st); - assert(NULL != parameter_idx); - - assert(node_idx < param_st->num_nodes); - - rcl_node_params_t * node_param_st = &(param_st->params[node_idx]); - for (*parameter_idx = 0U; *parameter_idx < node_param_st->num_params; (*parameter_idx)++) { - if (0 == strcmp(node_param_st->parameter_names[*parameter_idx], parameter_name)) { - // Parameter found. - return RCUTILS_RET_OK; - } - } - // Parameter not found, add it. - rcutils_allocator_t allocator = param_st->allocator; - if (NULL != node_param_st->parameter_names[*parameter_idx]) { - param_st->allocator.deallocate( - node_param_st->parameter_names[*parameter_idx], param_st->allocator.state); - } - node_param_st->parameter_names[*parameter_idx] = rcutils_strdup(parameter_name, allocator); - if (NULL == node_param_st->parameter_names[*parameter_idx]) { - return RCUTILS_RET_BAD_ALLOC; - } - node_param_st->num_params++; - return RCUTILS_RET_OK; -} - -rcl_variant_t * rcl_yaml_node_struct_get( - const char * node_name, - const char * param_name, - rcl_params_t * params_st) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, NULL); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(param_name, NULL); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, NULL); - - rcl_variant_t * param_value = NULL; - - size_t node_idx = 0U; - rcutils_ret_t ret = find_node(node_name, params_st, &node_idx); - if (RCUTILS_RET_OK == ret) { - size_t parameter_idx = 0U; - ret = find_parameter(node_idx, param_name, params_st, ¶meter_idx); - if (RCUTILS_RET_OK == ret) { - param_value = &(params_st->params[node_idx].parameter_values[parameter_idx]); - } - } - return param_value; -} - - -/// -/// Dump the param structure -/// -void rcl_yaml_node_struct_print( - const rcl_params_t * const params_st) -{ - if (NULL == params_st) { - return; - } - - printf("\n Node Name\t\t\t\tParameters\n"); - for (size_t node_idx = 0U; node_idx < params_st->num_nodes; node_idx++) { - int32_t param_col = 50; - const char * const node_name = params_st->node_names[node_idx]; - if (NULL != node_name) { - printf("%s\n", node_name); - } - - if (NULL != params_st->params) { - rcl_node_params_t * node_params_st = &(params_st->params[node_idx]); - for (size_t parameter_idx = 0U; parameter_idx < node_params_st->num_params; parameter_idx++) { - if ( - (NULL != node_params_st->parameter_names) && - (NULL != node_params_st->parameter_values)) - { - char * param_name = node_params_st->parameter_names[parameter_idx]; - rcl_variant_t * param_var = &(node_params_st->parameter_values[parameter_idx]); - if (NULL != param_name) { - printf("%*s", param_col, param_name); - } - - if (NULL != param_var) { - if (NULL != param_var->bool_value) { - printf(": %s\n", *(param_var->bool_value) ? "true" : "false"); - } else if (NULL != param_var->integer_value) { - printf(": %" PRId64 "\n", *(param_var->integer_value)); - } else if (NULL != param_var->double_value) { - printf(": %lf\n", *(param_var->double_value)); - } else if (NULL != param_var->string_value) { - printf(": %s\n", param_var->string_value); - } else if (NULL != param_var->bool_array_value) { - printf(": "); - for (size_t i = 0; i < param_var->bool_array_value->size; i++) { - if (param_var->bool_array_value->values) { - printf( - "%s, ", - (param_var->bool_array_value->values[i]) ? "true" : "false"); - } - } - printf("\n"); - } else if (NULL != param_var->integer_array_value) { - printf(": "); - for (size_t i = 0; i < param_var->integer_array_value->size; i++) { - if (param_var->integer_array_value->values) { - printf("%" PRId64 ", ", param_var->integer_array_value->values[i]); - } - } - printf("\n"); - } else if (NULL != param_var->double_array_value) { - printf(": "); - for (size_t i = 0; i < param_var->double_array_value->size; i++) { - if (param_var->double_array_value->values) { - printf("%lf, ", param_var->double_array_value->values[i]); - } - } - printf("\n"); - } else if (NULL != param_var->string_array_value) { - printf(": "); - for (size_t i = 0; i < param_var->string_array_value->size; i++) { - if (param_var->string_array_value->data[i]) { - printf("%s, ", param_var->string_array_value->data[i]); - } - } - printf("\n"); - } - } - } - } - } - } -} - -/// -/// Add a value to a bool array. Create the array if it does not exist -/// -static rcutils_ret_t add_val_to_bool_arr( - rcl_bool_array_t * const val_array, - bool * value, - const rcutils_allocator_t allocator) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - if (NULL == val_array->values) { - val_array->values = value; - val_array->size = 1; - } else { - /// Increase the array size by one and add the new value - bool * tmp_arr = val_array->values; - val_array->values = allocator.zero_allocate( - val_array->size + 1U, sizeof(bool), allocator.state); - if (NULL == val_array->values) { - val_array->values = tmp_arr; - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - return RCUTILS_RET_BAD_ALLOC; - } - memmove(val_array->values, tmp_arr, (val_array->size * sizeof(bool))); - val_array->values[val_array->size] = *value; - val_array->size++; - allocator.deallocate(value, allocator.state); - allocator.deallocate(tmp_arr, allocator.state); - } - return RCUTILS_RET_OK; -} - -/// -/// Add a value to an integer array. Create the array if it does not exist -/// -static rcutils_ret_t add_val_to_int_arr( - rcl_int64_array_t * const val_array, - int64_t * value, - const rcutils_allocator_t allocator) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - if (NULL == val_array->values) { - val_array->values = value; - val_array->size++; - } else { - /// Increase the array size by one and add the new value - int64_t * tmp_arr = val_array->values; - val_array->values = allocator.zero_allocate( - val_array->size + 1U, sizeof(int64_t), allocator.state); - if (NULL == val_array->values) { - val_array->values = tmp_arr; - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - return RCUTILS_RET_BAD_ALLOC; - } - memmove(val_array->values, tmp_arr, (val_array->size * sizeof(int64_t))); - val_array->values[val_array->size] = *value; - val_array->size++; - allocator.deallocate(value, allocator.state); - allocator.deallocate(tmp_arr, allocator.state); - } - return RCUTILS_RET_OK; -} - -/// -/// Add a value to a double array. Create the array if it does not exist -/// -static rcutils_ret_t add_val_to_double_arr( - rcl_double_array_t * const val_array, - double * value, - const rcutils_allocator_t allocator) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - if (NULL == val_array->values) { - val_array->values = value; - val_array->size++; - } else { - /// Increase the array size by one and add the new value - double * tmp_arr = val_array->values; - val_array->values = allocator.zero_allocate( - val_array->size + 1U, sizeof(double), allocator.state); - if (NULL == val_array->values) { - val_array->values = tmp_arr; - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - return RCUTILS_RET_BAD_ALLOC; - } - memmove(val_array->values, tmp_arr, (val_array->size * sizeof(double))); - val_array->values[val_array->size] = *value; - val_array->size++; - allocator.deallocate(value, allocator.state); - allocator.deallocate(tmp_arr, allocator.state); - } - return RCUTILS_RET_OK; -} - -/// -/// Add a value to a string array. Create the array if it does not exist -/// -static rcutils_ret_t add_val_to_string_arr( - rcutils_string_array_t * const val_array, - char * value, - const rcutils_allocator_t allocator) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_array, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - if (NULL == val_array->data) { - rcutils_ret_t ret = rcutils_string_array_init(val_array, 1, &allocator); - if (RCUTILS_RET_OK != ret) { - return ret; - } - val_array->data[0U] = value; - } else { - /// Increase the array size by one and add the new value - val_array->data = allocator.reallocate( - val_array->data, - ((val_array->size + 1U) * sizeof(char *)), allocator.state); - if (NULL == val_array->data) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - return RCUTILS_RET_BAD_ALLOC; - } - val_array->data[val_array->size] = value; - val_array->size++; - } - return RCUTILS_RET_OK; -} - -/// -/// Determine the type of the value and return the converted value -/// NOTE: Only canonical forms supported as of now -/// -static void * get_value( - const char * const value, - yaml_scalar_style_t style, - data_types_t * val_type, - const rcutils_allocator_t allocator) -{ - void * ret_val; - int64_t ival; - double dval; - char * endptr = NULL; - - RCUTILS_CHECK_ARGUMENT_FOR_NULL(value, NULL); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(val_type, NULL); - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "allocator is invalid", return NULL); - - /// Check if it is bool - if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && - style != YAML_DOUBLE_QUOTED_SCALAR_STYLE) - { - if ((0 == strcmp(value, "Y")) || - (0 == strcmp(value, "y")) || - (0 == strcmp(value, "yes")) || - (0 == strcmp(value, "Yes")) || - (0 == strcmp(value, "YES")) || - (0 == strcmp(value, "true")) || - (0 == strcmp(value, "True")) || - (0 == strcmp(value, "TRUE")) || - (0 == strcmp(value, "on")) || - (0 == strcmp(value, "On")) || - (0 == strcmp(value, "ON"))) - { - *val_type = DATA_TYPE_BOOL; - ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); - if (NULL == ret_val) { - return NULL; - } - *((bool *)ret_val) = true; - return ret_val; - } - - if ((0 == strcmp(value, "N")) || - (0 == strcmp(value, "n")) || - (0 == strcmp(value, "no")) || - (0 == strcmp(value, "No")) || - (0 == strcmp(value, "NO")) || - (0 == strcmp(value, "false")) || - (0 == strcmp(value, "False")) || - (0 == strcmp(value, "FALSE")) || - (0 == strcmp(value, "off")) || - (0 == strcmp(value, "Off")) || - (0 == strcmp(value, "OFF"))) - { - *val_type = DATA_TYPE_BOOL; - ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); - if (NULL == ret_val) { - return NULL; - } - *((bool *)ret_val) = false; - return ret_val; - } - } - - /// Check for int - if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && - style != YAML_DOUBLE_QUOTED_SCALAR_STYLE) - { - errno = 0; - ival = strtol(value, &endptr, 0); - if ((0 == errno) && (NULL != endptr)) { - if ((NULL != endptr) && (endptr != value)) { - if (('\0' != *value) && ('\0' == *endptr)) { - *val_type = DATA_TYPE_INT64; - ret_val = allocator.zero_allocate(1U, sizeof(int64_t), allocator.state); - if (NULL == ret_val) { - return NULL; - } - *((int64_t *)ret_val) = ival; - return ret_val; - } - } - } - } - - /// Check for float - if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && - style != YAML_DOUBLE_QUOTED_SCALAR_STYLE) - { - errno = 0; - endptr = NULL; - dval = strtod(value, &endptr); - if ((0 == errno) && (NULL != endptr)) { - if ((NULL != endptr) && (endptr != value)) { - if (('\0' != *value) && ('\0' == *endptr)) { - *val_type = DATA_TYPE_DOUBLE; - ret_val = allocator.zero_allocate(1U, sizeof(double), allocator.state); - if (NULL == ret_val) { - return NULL; - } - *((double *)ret_val) = dval; - return ret_val; - } - } - } - errno = 0; - } - - /// It is a string - *val_type = DATA_TYPE_STRING; - ret_val = rcutils_strdup(value, allocator); - return ret_val; -} - -/// -/// Parse the value part of the pair -/// -static rcutils_ret_t parse_value( - const yaml_event_t event, - const bool is_seq, - const size_t node_idx, - const size_t parameter_idx, - data_types_t * seq_data_type, - rcl_params_t * params_st) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(seq_data_type, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, RCUTILS_RET_INVALID_ARGUMENT); - - rcutils_allocator_t allocator = params_st->allocator; - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - if (0U == params_st->num_nodes) { - RCUTILS_SET_ERROR_MSG("No node to update"); - return RCUTILS_RET_INVALID_ARGUMENT; - } - - const size_t val_size = event.data.scalar.length; - const char * value = (char *)event.data.scalar.value; - yaml_scalar_style_t style = event.data.scalar.style; - const uint32_t line_num = ((uint32_t)(event.start_mark.line) + 1U); - - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - value, "event argument has no value", return RCUTILS_RET_INVALID_ARGUMENT); - - if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && - style != YAML_DOUBLE_QUOTED_SCALAR_STYLE && - 0U == val_size) - { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("No value at line %d", line_num); - return RCUTILS_RET_ERROR; - } - - if (NULL == params_st->params[node_idx].parameter_values) { - RCUTILS_SET_ERROR_MSG("Internal error: Invalid mem"); - return RCUTILS_RET_BAD_ALLOC; - } - - rcl_variant_t * param_value = &(params_st->params[node_idx].parameter_values[parameter_idx]); - - // param_value->string_value = rcutils_strdup(value, allocator); - data_types_t val_type; - void * ret_val = get_value(value, style, &val_type, allocator); - if (NULL == ret_val) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Error parsing value %s at line %d", value, line_num); - return RCUTILS_RET_ERROR; - } - - rcutils_ret_t ret = RCUTILS_RET_OK; - switch (val_type) { - case DATA_TYPE_UNKNOWN: - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Unknown data type of value %s at line %d\n", value, line_num); - ret = RCUTILS_RET_ERROR; - break; - case DATA_TYPE_BOOL: - if (false == is_seq) { - if (NULL != param_value->bool_value) { - // Overwriting, deallocate original - allocator.deallocate(param_value->bool_value, allocator.state); - } - param_value->bool_value = (bool *)ret_val; - } else { - if (DATA_TYPE_UNKNOWN == *seq_data_type) { - *seq_data_type = val_type; - if (NULL != param_value->bool_array_value) { - allocator.deallocate(param_value->bool_array_value->values, allocator.state); - allocator.deallocate(param_value->bool_array_value, allocator.state); - param_value->bool_array_value = NULL; - } - param_value->bool_array_value = - allocator.zero_allocate(1U, sizeof(rcl_bool_array_t), allocator.state); - if (NULL == param_value->bool_array_value) { - allocator.deallocate(ret_val, allocator.state); - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - ret = RCUTILS_RET_BAD_ALLOC; - break; - } - } else { - if (*seq_data_type != val_type) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Sequence should be of same type. Value type 'bool' do not belong at line_num %d", - line_num); - allocator.deallocate(ret_val, allocator.state); - ret = RCUTILS_RET_ERROR; - break; - } - } - ret = add_val_to_bool_arr(param_value->bool_array_value, ret_val, allocator); - if (RCUTILS_RET_OK != ret) { - if (NULL != ret_val) { - allocator.deallocate(ret_val, allocator.state); - } - } - } - break; - case DATA_TYPE_INT64: - if (false == is_seq) { - if (NULL != param_value->integer_value) { - // Overwriting, deallocate original - allocator.deallocate(param_value->integer_value, allocator.state); - } - param_value->integer_value = (int64_t *)ret_val; - } else { - if (DATA_TYPE_UNKNOWN == *seq_data_type) { - if (NULL != param_value->integer_array_value) { - allocator.deallocate(param_value->integer_array_value->values, allocator.state); - allocator.deallocate(param_value->integer_array_value, allocator.state); - param_value->integer_array_value = NULL; - } - *seq_data_type = val_type; - param_value->integer_array_value = - allocator.zero_allocate(1U, sizeof(rcl_int64_array_t), allocator.state); - if (NULL == param_value->integer_array_value) { - allocator.deallocate(ret_val, allocator.state); - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - ret = RCUTILS_RET_BAD_ALLOC; - break; - } - } else { - if (*seq_data_type != val_type) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Sequence should be of same type. Value type 'integer' do not belong at line_num %d", - line_num); - allocator.deallocate(ret_val, allocator.state); - ret = RCUTILS_RET_ERROR; - break; - } - } - ret = add_val_to_int_arr(param_value->integer_array_value, ret_val, allocator); - if (RCUTILS_RET_OK != ret) { - if (NULL != ret_val) { - allocator.deallocate(ret_val, allocator.state); - } - } - } - break; - case DATA_TYPE_DOUBLE: - if (false == is_seq) { - if (NULL != param_value->double_value) { - // Overwriting, deallocate original - allocator.deallocate(param_value->double_value, allocator.state); - } - param_value->double_value = (double *)ret_val; - } else { - if (DATA_TYPE_UNKNOWN == *seq_data_type) { - if (NULL != param_value->double_array_value) { - allocator.deallocate(param_value->double_array_value->values, allocator.state); - allocator.deallocate(param_value->double_array_value, allocator.state); - param_value->double_array_value = NULL; - } - *seq_data_type = val_type; - param_value->double_array_value = - allocator.zero_allocate(1U, sizeof(rcl_double_array_t), allocator.state); - if (NULL == param_value->double_array_value) { - allocator.deallocate(ret_val, allocator.state); - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - ret = RCUTILS_RET_BAD_ALLOC; - break; - } - } else { - if (*seq_data_type != val_type) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Sequence should be of same type. Value type 'double' do not belong at line_num %d", - line_num); - allocator.deallocate(ret_val, allocator.state); - ret = RCUTILS_RET_ERROR; - break; - } - } - ret = add_val_to_double_arr(param_value->double_array_value, ret_val, allocator); - if (RCUTILS_RET_OK != ret) { - if (NULL != ret_val) { - allocator.deallocate(ret_val, allocator.state); - } - } - } - break; - case DATA_TYPE_STRING: - if (false == is_seq) { - if (NULL != param_value->string_value) { - // Overwriting, deallocate original - allocator.deallocate(param_value->string_value, allocator.state); - } - param_value->string_value = (char *)ret_val; - } else { - if (DATA_TYPE_UNKNOWN == *seq_data_type) { - if (NULL != param_value->string_array_value) { - if (RCUTILS_RET_OK != rcutils_string_array_fini(param_value->string_array_value)) { - // Log and continue ... - RCUTILS_SAFE_FWRITE_TO_STDERR("Error deallocating string array"); - } - allocator.deallocate(param_value->string_array_value, allocator.state); - param_value->string_array_value = NULL; - } - *seq_data_type = val_type; - param_value->string_array_value = - allocator.zero_allocate(1U, sizeof(rcutils_string_array_t), allocator.state); - if (NULL == param_value->string_array_value) { - allocator.deallocate(ret_val, allocator.state); - RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); - ret = RCUTILS_RET_BAD_ALLOC; - break; - } - } else { - if (*seq_data_type != val_type) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Sequence should be of same type. Value type 'string' do not belong at line_num %d", - line_num); - allocator.deallocate(ret_val, allocator.state); - ret = RCUTILS_RET_ERROR; - break; - } - } - ret = add_val_to_string_arr(param_value->string_array_value, ret_val, allocator); - if (RCUTILS_RET_OK != ret) { - if (NULL != ret_val) { - allocator.deallocate(ret_val, allocator.state); - } - } - } - break; - default: - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Unknown data type of value %s at line %d", value, line_num); - ret = RCUTILS_RET_ERROR; - allocator.deallocate(ret_val, allocator.state); - break; - } - return ret; -} - -/// -/// Parse the key part of the pair -/// -static rcutils_ret_t parse_key( - const yaml_event_t event, - uint32_t * map_level, - bool * is_new_map, - size_t * node_idx, - size_t * parameter_idx, - namespace_tracker_t * ns_tracker, - rcl_params_t * params_st) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL(map_level, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, RCUTILS_RET_INVALID_ARGUMENT); - rcutils_allocator_t allocator = params_st->allocator; - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - const size_t val_size = event.data.scalar.length; - const char * value = (char *)event.data.scalar.value; - const uint32_t line_num = ((uint32_t)(event.start_mark.line) + 1U); - - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - value, "event argument has no value", return RCUTILS_RET_INVALID_ARGUMENT); - - if (0U == val_size) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("No key at line %d", line_num); - return RCUTILS_RET_ERROR; - } - - rcutils_ret_t ret = RCUTILS_RET_OK; - switch (*map_level) { - case MAP_UNINIT_LVL: - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Unintialized map level at line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - case MAP_NODE_NAME_LVL: - { - /// Till we get PARAMS_KEY, keep adding to node namespace - if (0 != strncmp(PARAMS_KEY, value, strlen(PARAMS_KEY))) { - ret = add_name_to_ns(ns_tracker, value, NS_TYPE_NODE, allocator); - if (RCUTILS_RET_OK != ret) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Internal error adding node namespace at line %d", line_num); - break; - } - } else { - if (0U == ns_tracker->num_node_ns) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "There are no node names before %s at line %d", PARAMS_KEY, line_num); - ret = RCUTILS_RET_ERROR; - break; - } - /// The previous key(last name in namespace) was the node name. Remove it - /// from the namespace - char * node_name_ns = rcutils_strdup(ns_tracker->node_ns, allocator); - if (NULL == node_name_ns) { - ret = RCUTILS_RET_BAD_ALLOC; - break; - } - - ret = find_node(node_name_ns, params_st, node_idx); - allocator.deallocate(node_name_ns, allocator.state); - if (RCUTILS_RET_OK != ret) { - break; - } - - ret = rem_name_from_ns(ns_tracker, NS_TYPE_NODE, allocator); - if (RCUTILS_RET_OK != ret) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Internal error adding node namespace at line %d", line_num); - break; - } - /// Bump the map level to PARAMS - (*map_level)++; - } - } - break; - case MAP_PARAMS_LVL: - { - char * parameter_ns = NULL; - char * param_name = NULL; - - /// If it is a new map, the previous key is param namespace - if (true == *is_new_map) { - parameter_ns = params_st->params[*node_idx].parameter_names[*parameter_idx]; - if (NULL == parameter_ns) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Internal error creating param namespace at line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - } - ret = replace_ns( - ns_tracker, parameter_ns, (ns_tracker->num_parameter_ns + 1U), - NS_TYPE_PARAM, allocator); - if (RCUTILS_RET_OK != ret) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Internal error replacing namespace at line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - } - *is_new_map = false; - } - - // Guard against adding more than the maximum allowed parameters - if (params_st->params[*node_idx].num_params >= MAX_NUM_PARAMS_PER_NODE) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Exceeded maximum allowed number of parameters for a node (%d)", - MAX_NUM_PARAMS_PER_NODE); - ret = RCUTILS_RET_ERROR; - break; - } - - /// Add a parameter name into the node parameters - parameter_ns = ns_tracker->parameter_ns; - if (NULL == parameter_ns) { - ret = find_parameter(*node_idx, value, params_st, parameter_idx); - if (ret != RCUTILS_RET_OK) { - break; - } - } else { - ret = find_parameter(*node_idx, parameter_ns, params_st, parameter_idx); - if (ret != RCUTILS_RET_OK) { - break; - } - - const size_t params_ns_len = strlen(parameter_ns); - const size_t param_name_len = strlen(value); - const size_t tot_len = (params_ns_len + param_name_len + 2U); - - param_name = allocator.zero_allocate(1U, tot_len, allocator.state); - if (NULL == param_name) { - ret = RCUTILS_RET_BAD_ALLOC; - break; - } - - memmove(param_name, parameter_ns, params_ns_len); - param_name[params_ns_len] = '.'; - memmove((param_name + params_ns_len + 1U), value, param_name_len); - param_name[tot_len - 1U] = '\0'; - - if (NULL != params_st->params[*node_idx].parameter_names[*parameter_idx]) { - // This memory was allocated in find_parameter(), and its pointer is being overwritten - allocator.deallocate( - params_st->params[*node_idx].parameter_names[*parameter_idx], allocator.state); - } - params_st->params[*node_idx].parameter_names[*parameter_idx] = param_name; - } - } - break; - default: - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("Unknown map level at line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - } - return ret; -} - -/// -/// Get events from parsing a parameter YAML file and process them -/// -static rcutils_ret_t parse_file_events( - yaml_parser_t * parser, - namespace_tracker_t * ns_tracker, - rcl_params_t * params_st) -{ - int32_t done_parsing = 0; - bool is_key = true; - bool is_seq = false; - uint32_t line_num = 0; - data_types_t seq_data_type = DATA_TYPE_UNKNOWN; - uint32_t map_level = 1U; - uint32_t map_depth = 0U; - bool is_new_map = false; - - RCUTILS_CHECK_ARGUMENT_FOR_NULL(parser, RCUTILS_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, RCUTILS_RET_INVALID_ARGUMENT); - rcutils_allocator_t allocator = params_st->allocator; - RCUTILS_CHECK_ALLOCATOR_WITH_MSG( - &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); - - yaml_event_t event; - size_t node_idx = 0; - size_t parameter_idx = 0; - rcutils_ret_t ret = RCUTILS_RET_OK; - while (0 == done_parsing) { - if (RCUTILS_RET_OK != ret) { - break; - } - int success = yaml_parser_parse(parser, &event); - if (0 == success) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Error parsing a event near line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - } - line_num = ((uint32_t)(event.start_mark.line) + 1U); - switch (event.type) { - case YAML_STREAM_END_EVENT: - done_parsing = 1; - yaml_event_delete(&event); - break; - case YAML_SCALAR_EVENT: - { - /// Need to toggle between key and value at params level - if (true == is_key) { - ret = parse_key( - event, &map_level, &is_new_map, &node_idx, ¶meter_idx, ns_tracker, params_st); - if (RCUTILS_RET_OK != ret) { - break; - } - is_key = false; - } else { - /// It is a value - if (map_level < (uint32_t)(MAP_PARAMS_LVL)) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Cannot have a value before %s at line %d", PARAMS_KEY, line_num); - ret = RCUTILS_RET_ERROR; - break; - } - if (0U == params_st->num_nodes) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Cannot have a value before %s at line %d", PARAMS_KEY, line_num); - yaml_event_delete(&event); - return RCUTILS_RET_ERROR; - } - if (0U == params_st->params[node_idx].num_params) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Cannot have a value before %s at line %d", PARAMS_KEY, line_num); - yaml_event_delete(&event); - return RCUTILS_RET_ERROR; - } - ret = parse_value(event, is_seq, node_idx, parameter_idx, &seq_data_type, params_st); - if (RCUTILS_RET_OK != ret) { - break; - } - if (false == is_seq) { - is_key = true; - } - } - } - break; - case YAML_SEQUENCE_START_EVENT: - if (true == is_key) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Sequences cannot be key at line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - } - if (map_level < (uint32_t)(MAP_PARAMS_LVL)) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Sequences can only be values and not keys in params. Error at line %d\n", line_num); - ret = RCUTILS_RET_ERROR; - break; - } - is_seq = true; - seq_data_type = DATA_TYPE_UNKNOWN; - break; - case YAML_SEQUENCE_END_EVENT: - is_seq = false; - is_key = true; - break; - case YAML_MAPPING_START_EVENT: - map_depth++; - is_new_map = true; - is_key = true; - /// Disable new map if it is PARAMS_KEY map - if ((MAP_PARAMS_LVL == map_level) && - ((map_depth - (ns_tracker->num_node_ns + 1U)) == 2U)) - { - is_new_map = false; - } - break; - case YAML_MAPPING_END_EVENT: - if (MAP_PARAMS_LVL == map_level) { - if (ns_tracker->num_parameter_ns > 0U) { - /// Remove param namesapce - ret = rem_name_from_ns(ns_tracker, NS_TYPE_PARAM, allocator); - if (RCUTILS_RET_OK != ret) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Internal error removing parameter namespace at line %d", line_num); - break; - } - } else { - map_level--; - } - } else { - if ((MAP_NODE_NAME_LVL == map_level) && - (map_depth == (ns_tracker->num_node_ns + 1U))) - { - /// Remove node namespace - ret = rem_name_from_ns(ns_tracker, NS_TYPE_NODE, allocator); - if (RCUTILS_RET_OK != ret) { - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Internal error removing node namespace at line %d", line_num); - break; - } - } - } - map_depth--; - break; - case YAML_ALIAS_EVENT: - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Will not support aliasing at line %d\n", line_num); - ret = RCUTILS_RET_ERROR; - break; - case YAML_STREAM_START_EVENT: - break; - case YAML_DOCUMENT_START_EVENT: - break; - case YAML_DOCUMENT_END_EVENT: - break; - case YAML_NO_EVENT: - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Received an empty event at line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - default: - RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("Unknown YAML event at line %d", line_num); - ret = RCUTILS_RET_ERROR; - break; - } - yaml_event_delete(&event); - } - return ret; -} - -/// -/// Get events from parsing a parameter YAML value string and process them -/// -static rcutils_ret_t parse_value_events( - yaml_parser_t * parser, - const size_t node_idx, - const size_t parameter_idx, - rcl_params_t * params_st) -{ - bool is_seq = false; - data_types_t seq_data_type = DATA_TYPE_UNKNOWN; - rcutils_ret_t ret = RCUTILS_RET_OK; - bool done_parsing = false; - while (RCUTILS_RET_OK == ret && !done_parsing) { - yaml_event_t event; - int success = yaml_parser_parse(parser, &event); - if (0 == success) { - RCUTILS_SET_ERROR_MSG("Error parsing an event"); - ret = RCUTILS_RET_ERROR; - break; - } - switch (event.type) { - case YAML_STREAM_END_EVENT: - done_parsing = true; - break; - case YAML_SCALAR_EVENT: - ret = parse_value( - event, is_seq, node_idx, parameter_idx, &seq_data_type, params_st); - break; - case YAML_SEQUENCE_START_EVENT: - is_seq = true; - seq_data_type = DATA_TYPE_UNKNOWN; - break; - case YAML_SEQUENCE_END_EVENT: - is_seq = false; - break; - case YAML_STREAM_START_EVENT: - break; - case YAML_DOCUMENT_START_EVENT: - break; - case YAML_DOCUMENT_END_EVENT: - break; - case YAML_NO_EVENT: - RCUTILS_SET_ERROR_MSG("Received an empty event"); - ret = RCUTILS_RET_ERROR; - break; - default: - RCUTILS_SET_ERROR_MSG("Unknown YAML event"); - ret = RCUTILS_RET_ERROR; - break; - } - yaml_event_delete(&event); - } - return ret; -} - /// /// TODO (anup.pemmaiah): Support Mutiple yaml files /// @@ -1864,3 +270,108 @@ bool rcl_parse_yaml_value( return RCUTILS_RET_OK == ret; } + +rcl_variant_t * rcl_yaml_node_struct_get( + const char * node_name, + const char * param_name, + rcl_params_t * params_st) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, NULL); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(param_name, NULL); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, NULL); + + rcl_variant_t * param_value = NULL; + + size_t node_idx = 0U; + rcutils_ret_t ret = find_node(node_name, params_st, &node_idx); + if (RCUTILS_RET_OK == ret) { + size_t parameter_idx = 0U; + ret = find_parameter(node_idx, param_name, params_st, ¶meter_idx); + if (RCUTILS_RET_OK == ret) { + param_value = &(params_st->params[node_idx].parameter_values[parameter_idx]); + } + } + return param_value; +} + +/// +/// Dump the param structure +/// +void rcl_yaml_node_struct_print( + const rcl_params_t * const params_st) +{ + if (NULL == params_st) { + return; + } + + printf("\n Node Name\t\t\t\tParameters\n"); + for (size_t node_idx = 0U; node_idx < params_st->num_nodes; node_idx++) { + int32_t param_col = 50; + const char * const node_name = params_st->node_names[node_idx]; + if (NULL != node_name) { + printf("%s\n", node_name); + } + + if (NULL != params_st->params) { + rcl_node_params_t * node_params_st = &(params_st->params[node_idx]); + for (size_t parameter_idx = 0U; parameter_idx < node_params_st->num_params; parameter_idx++) { + if ( + (NULL != node_params_st->parameter_names) && + (NULL != node_params_st->parameter_values)) + { + char * param_name = node_params_st->parameter_names[parameter_idx]; + rcl_variant_t * param_var = &(node_params_st->parameter_values[parameter_idx]); + if (NULL != param_name) { + printf("%*s", param_col, param_name); + } + + if (NULL != param_var) { + if (NULL != param_var->bool_value) { + printf(": %s\n", *(param_var->bool_value) ? "true" : "false"); + } else if (NULL != param_var->integer_value) { + printf(": %" PRId64 "\n", *(param_var->integer_value)); + } else if (NULL != param_var->double_value) { + printf(": %lf\n", *(param_var->double_value)); + } else if (NULL != param_var->string_value) { + printf(": %s\n", param_var->string_value); + } else if (NULL != param_var->bool_array_value) { + printf(": "); + for (size_t i = 0; i < param_var->bool_array_value->size; i++) { + if (param_var->bool_array_value->values) { + printf( + "%s, ", + (param_var->bool_array_value->values[i]) ? "true" : "false"); + } + } + printf("\n"); + } else if (NULL != param_var->integer_array_value) { + printf(": "); + for (size_t i = 0; i < param_var->integer_array_value->size; i++) { + if (param_var->integer_array_value->values) { + printf("%" PRId64 ", ", param_var->integer_array_value->values[i]); + } + } + printf("\n"); + } else if (NULL != param_var->double_array_value) { + printf(": "); + for (size_t i = 0; i < param_var->double_array_value->size; i++) { + if (param_var->double_array_value->values) { + printf("%lf, ", param_var->double_array_value->values[i]); + } + } + printf("\n"); + } else if (NULL != param_var->string_array_value) { + printf(": "); + for (size_t i = 0; i < param_var->string_array_value->size; i++) { + if (param_var->string_array_value->data[i]) { + printf("%s, ", param_var->string_array_value->data[i]); + } + } + printf("\n"); + } + } + } + } + } + } +} diff --git a/rcl_yaml_param_parser/src/yaml_variant.c b/rcl_yaml_param_parser/src/yaml_variant.c new file mode 100644 index 0000000..37c0467 --- /dev/null +++ b/rcl_yaml_param_parser/src/yaml_variant.c @@ -0,0 +1,178 @@ +// Copyright 2018 Apex.AI, 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 "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "./impl/types.h" +#include "./impl/yaml_variant.h" +#include "rcl_yaml_param_parser/types.h" + +#define RCL_YAML_VARIANT_COPY_VALUE(dest_ptr, src_ptr, allocator, var_type) \ + do { \ + dest_ptr = allocator.allocate(sizeof(var_type), allocator.state); \ + if (NULL == dest_ptr) { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + "Error allocating variant mem when copying value of type " #var_type "\n"); \ + return false; \ + } \ + *(dest_ptr) = *(src_ptr); \ + } while (0) + +#define RCL_YAML_VARIANT_COPY_ARRAY_VALUE( \ + dest_array, src_array, allocator, var_array_type, var_type) \ + do { \ + dest_array = \ + allocator.allocate(sizeof(var_array_type), allocator.state); \ + if (NULL == dest_array) { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + "Error allocating mem for array of type " #var_array_type "\n"); \ + return false; \ + } \ + if (0U != src_array->size) { \ + dest_array->values = allocator.allocate( \ + sizeof(var_type) * src_array->size, allocator.state); \ + if (NULL == dest_array->values) { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + "Error allocating mem for array values of type " #var_type "\n"); \ + return false; \ + } \ + memcpy( \ + dest_array->values, \ + src_array->values, \ + sizeof(var_type) * src_array->size); \ + } else { \ + dest_array->values = NULL; \ + } \ + dest_array->size = src_array->size; \ + } while (0) + +void rcl_yaml_variant_fini( + rcl_variant_t * param_var, + const rcutils_allocator_t allocator) +{ + if (NULL == param_var) { + return; + } + + if (NULL != param_var->bool_value) { + allocator.deallocate(param_var->bool_value, allocator.state); + param_var->bool_value = NULL; + } else if (NULL != param_var->integer_value) { + allocator.deallocate(param_var->integer_value, allocator.state); + param_var->integer_value = NULL; + } else if (NULL != param_var->double_value) { + allocator.deallocate(param_var->double_value, allocator.state); + param_var->double_value = NULL; + } else if (NULL != param_var->string_value) { + allocator.deallocate(param_var->string_value, allocator.state); + param_var->string_value = NULL; + } else if (NULL != param_var->bool_array_value) { + if (NULL != param_var->bool_array_value->values) { + allocator.deallocate(param_var->bool_array_value->values, allocator.state); + } + allocator.deallocate(param_var->bool_array_value, allocator.state); + param_var->bool_array_value = NULL; + } else if (NULL != param_var->integer_array_value) { + if (NULL != param_var->integer_array_value->values) { + allocator.deallocate(param_var->integer_array_value->values, allocator.state); + } + allocator.deallocate(param_var->integer_array_value, allocator.state); + param_var->integer_array_value = NULL; + } else if (NULL != param_var->double_array_value) { + if (NULL != param_var->double_array_value->values) { + allocator.deallocate(param_var->double_array_value->values, allocator.state); + } + allocator.deallocate(param_var->double_array_value, allocator.state); + param_var->double_array_value = NULL; + } else if (NULL != param_var->string_array_value) { + if (RCUTILS_RET_OK != rcutils_string_array_fini(param_var->string_array_value)) { + // Log and continue ... + RCUTILS_SAFE_FWRITE_TO_STDERR("Error deallocating string array"); + } + allocator.deallocate(param_var->string_array_value, allocator.state); + param_var->string_array_value = NULL; + } else { + /// Nothing to do to keep pclint happy + } +} + +bool rcl_yaml_variant_copy( + rcl_variant_t * out_param_var, const rcl_variant_t * param_var, rcutils_allocator_t allocator) +{ + if (NULL == param_var || NULL == out_param_var) { + return false; + } + if (NULL != param_var->bool_value) { + RCL_YAML_VARIANT_COPY_VALUE( + out_param_var->bool_value, param_var->bool_value, allocator, bool); + } else if (NULL != param_var->integer_value) { + RCL_YAML_VARIANT_COPY_VALUE( + out_param_var->integer_value, param_var->integer_value, allocator, int64_t); + } else if (NULL != param_var->double_value) { + RCL_YAML_VARIANT_COPY_VALUE( + out_param_var->double_value, param_var->double_value, allocator, double); + } else if (NULL != param_var->string_value) { + out_param_var->string_value = + rcutils_strdup(param_var->string_value, allocator); + if (NULL == out_param_var->string_value) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating variant mem when copying string_value\n"); + return false; + } + } else if (NULL != param_var->bool_array_value) { + RCL_YAML_VARIANT_COPY_ARRAY_VALUE( + out_param_var->bool_array_value, param_var->bool_array_value, allocator, + rcl_bool_array_t, bool); + } else if (NULL != param_var->integer_array_value) { + RCL_YAML_VARIANT_COPY_ARRAY_VALUE( + out_param_var->integer_array_value, param_var->integer_array_value, allocator, + rcl_int64_array_t, int64_t); + } else if (NULL != param_var->double_array_value) { + RCL_YAML_VARIANT_COPY_ARRAY_VALUE( + out_param_var->double_array_value, param_var->double_array_value, allocator, + rcl_double_array_t, double); + } else if (NULL != param_var->string_array_value) { + out_param_var->string_array_value = + allocator.allocate(sizeof(rcutils_string_array_t), allocator.state); + if (NULL == out_param_var->string_array_value) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem\n"); + return false; + } + *(out_param_var->string_array_value) = rcutils_get_zero_initialized_string_array(); + rcutils_ret_t ret = rcutils_string_array_init( + out_param_var->string_array_value, + param_var->string_array_value->size, + &(param_var->string_array_value->allocator)); + if (RCUTILS_RET_OK != ret) { + if (RCUTILS_RET_BAD_ALLOC == ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem for string array\n"); + } + return false; + } + for (size_t str_idx = 0U; str_idx < param_var->string_array_value->size; ++str_idx) { + out_param_var->string_array_value->data[str_idx] = rcutils_strdup( + param_var->string_array_value->data[str_idx], + out_param_var->string_array_value->allocator); + if (NULL == out_param_var->string_array_value->data[str_idx]) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem for string array values\n"); + return false; + } + } + } else { + /// Nothing to do to keep pclint happy + } + return true; +}