diff --git a/rcl_yaml_param_parser/CMakeLists.txt b/rcl_yaml_param_parser/CMakeLists.txt new file mode 100644 index 0000000000..0e7b863598 --- /dev/null +++ b/rcl_yaml_param_parser/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.5) + +project(rcl_yaml_param_parser) + +find_package(ament_cmake_ros REQUIRED) +find_package(rcutils REQUIRED) +find_package(rcl REQUIRED) +find_package(libyaml_vendor REQUIRED) +find_package(yaml REQUIRED) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories(include ${yaml_INCLUDE_DIRS} ${rcutils_INCLUDE_DIRS} ${rcl_INCLUDE_DIRS}) + +set(rcl_yaml_parser_sources + src/parser.c +) + +add_library( + ${PROJECT_NAME} + ${rcl_yaml_parser_sources}) +ament_target_dependencies(${PROJECT_NAME} "yaml" "rcutils" "rcl") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "RCL_YAML_PARAM_PARSER_BUILDING_DLL") + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Gtests + ament_add_gtest(test_parse_yaml + test/test_parse_yaml.cpp + ) + if(TARGET test_parse_yaml) + target_link_libraries(test_parse_yaml ${PROJECT_NAME}) + endif() + +endif() + +ament_export_dependencies(ament_cmake) +ament_export_include_directories(include) +install( + DIRECTORY include/ + DESTINATION include +) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/rcl_yaml_param_parser/README.md b/rcl_yaml_param_parser/README.md new file mode 100644 index 0000000000..d86f03dc03 --- /dev/null +++ b/rcl_yaml_param_parser/README.md @@ -0,0 +1,27 @@ +**ROS2 rcl YAML paramter parser** + +Parse a YAML parameter file and populate the C data structure + +The data structure params_st will then be used during node initialization + +YAML parameter file should follow the yaml syntax shown below + +NOTE: It only supports canonical int and float types + +``` +: # optional + : + params: + : + : # optional + : + : + : + params: + : + : # optional + : + : +``` + +This package depends on C libyaml diff --git a/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser.h b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser.h new file mode 100644 index 0000000000..c578947b3a --- /dev/null +++ b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser.h @@ -0,0 +1,60 @@ +// 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 RCL_YAML_PARAM_PARSER__PARSER_H_ +#define RCL_YAML_PARAM_PARSER__PARSER_H_ + +#include + +#include "rcl_yaml_param_parser/types.h" +#include "rcl_yaml_param_parser/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// \brief Init param structure +/// \param[in] allocator memory allocator to be used +/// \return a pointer to param structure on success or NULL on failure +RCL_YAML_PARAM_PARSER_PUBLIC +rcl_params_t * rcl_yaml_node_struct_init( + const rcutils_allocator_t allocator); + +/// \brief Free param structure +/// \param[in] params_st points to the populated paramter struct +RCL_YAML_PARAM_PARSER_PUBLIC +void rcl_yaml_node_struct_fini( + rcl_params_t * params_st); + +/// \brief Parse the YAML file, initialize and populate params_st +/// \param[in] file_path is the path to the YAML file +/// \param[in/out] params_st points to the populated paramter struct +/// \return true on success and false on failure +RCL_YAML_PARAM_PARSER_PUBLIC +bool rcl_parse_yaml_file( + const char * file_path, + rcl_params_t * params_st); + +/// \brief Print the parameter structure to stdout +/// \param[in] params_st points to the populated parameter struct +RCL_YAML_PARAM_PARSER_PUBLIC +void rcl_yaml_node_struct_print( + const rcl_params_t * const params_st); + +#ifdef __cplusplus +} +#endif + +#endif // RCL_YAML_PARAM_PARSER__PARSER_H_ 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 new file mode 100644 index 0000000000..f83be33574 --- /dev/null +++ b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/types.h @@ -0,0 +1,87 @@ +// 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 RCL_YAML_PARAM_PARSER__TYPES_H_ +#define RCL_YAML_PARAM_PARSER__TYPES_H_ + +#include "rcl/allocator.h" +#include "rcutils/types/string_array.h" + +/// \typedef rcl_bool_array_t +/// \brief Array of bool values +typedef struct rcl_bool_array_s +{ + bool * values; + size_t size; +} rcl_bool_array_t; + +/// \typedef rcl_int64_array_t +/// \brief Array of int64_t values +typedef struct rcl_int64_array_s +{ + int64_t * values; + size_t size; +} rcl_int64_array_t; + +/// \typedef rcl_double_array_t +/// \brief Array of double values +typedef struct rcl_double_array_s +{ + double * values; + size_t size; +} rcl_double_array_t; + +/// \typedef rcl_byte_array_t +/// \brief Array of byte values +typedef struct rcl_byte_array_s +{ + uint8_t * values; + size_t size; +} rcl_byte_array_t; + +/// \typedef rcl_variant_t +/// \brief variant_t stores the value of a parameter +/// Only one pointer in this struct will store the value +typedef struct rcl_variant_s +{ + bool * bool_value; ///< If bool, gets stored here + int64_t * integer_value; ///< If integer, gets stored here + double * double_value; ///< If double, gets stored here + char * string_value; ///< If string, gets stored here + rcl_byte_array_t * byte_array_value; ///< If array of bytes + rcl_bool_array_t * bool_array_value; ///< If array of bool's + rcl_int64_array_t * integer_array_value; ///< If array of integers + rcl_double_array_t * double_array_value; ///< If array of doubles + rcutils_string_array_t * string_array_value; ///< If array of strings +} rcl_variant_t; + +/// \typedef rcl_node_params_t +/// \brief node_params_t stores all the parameters(key:value) of a single node +typedef struct rcl_node_params_s +{ + char ** parameter_names; ///< Array of parameter names (keys) + rcl_variant_t * parameter_values; ///< Array of coressponding parameter values + size_t num_params; ///< Number of parameters in the node +} rcl_node_params_t; + +/// \typedef rcl_params_t +/// \brief params_t stores all the parameters of all nodes of a process +typedef struct rcl_params_s +{ + char ** node_names; ///< List of names of the node + rcl_node_params_t * params; ///< Array of parameters + size_t num_nodes; ///< Number of nodes + rcl_allocator_t allocator; ///< Allocator used +} rcl_params_t; + +#endif // RCL_YAML_PARAM_PARSER__TYPES_H_ diff --git a/rcl_yaml_param_parser/include/rcl_yaml_param_parser/visibility_control.h b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/visibility_control.h new file mode 100644 index 0000000000..5a8b8c0e12 --- /dev/null +++ b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/visibility_control.h @@ -0,0 +1,58 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL_YAML_PARAM_PARSER__VISIBILITY_CONTROL_H_ +#define RCL_YAML_PARAM_PARSER__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCL_YAML_PARAM_PARSER_EXPORT __attribute__ ((dllexport)) + #define RCL_YAML_PARAM_PARSER_IMPORT __attribute__ ((dllimport)) + #else + #define RCL_YAML_PARAM_PARSER_EXPORT __declspec(dllexport) + #define RCL_YAML_PARAM_PARSER_IMPORT __declspec(dllimport) + #endif + #ifdef RCL_YAML_PARAM_PARSER_BUILDING_DLL + #define RCL_YAML_PARAM_PARSER_PUBLIC RCL_YAML_PARAM_PARSER_EXPORT + #else + #define RCL_YAML_PARAM_PARSER_PUBLIC RCL_YAML_PARAM_PARSER_IMPORT + #endif + #define RCL_YAML_PARAM_PARSER_PUBLIC_TYPE RCL_YAML_PARAM_PARSER_PUBLIC + #define RCL_YAML_PARAM_PARSER_LOCAL +#else + #define RCL_YAML_PARAM_PARSER_EXPORT __attribute__ ((visibility("default"))) + #define RCL_YAML_PARAM_PARSER_IMPORT + #if __GNUC__ >= 4 + #define RCL_YAML_PARAM_PARSER_PUBLIC __attribute__ ((visibility("default"))) + #define RCL_YAML_PARAM_PARSER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCL_YAML_PARAM_PARSER_PUBLIC + #define RCL_YAML_PARAM_PARSER_LOCAL + #endif + #define RCL_YAML_PARAM_PARSER_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // RCL_YAML_PARAM_PARSER__VISIBILITY_CONTROL_H_ diff --git a/rcl_yaml_param_parser/package.xml b/rcl_yaml_param_parser/package.xml new file mode 100644 index 0000000000..4a86e30d58 --- /dev/null +++ b/rcl_yaml_param_parser/package.xml @@ -0,0 +1,25 @@ + + + + rcl_yaml_param_parser + 0.1.0 + Package containing various utility types and functions for C + Anup Pemmaiah + Apache License 2.0 + + ament_cmake_ros + + libyaml_vendor + yaml + rcutils + rcl + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + launch_testing + + + ament_cmake + + diff --git a/rcl_yaml_param_parser/src/parser.c b/rcl_yaml_param_parser/src/parser.c new file mode 100644 index 0000000000..a8b2db5a67 --- /dev/null +++ b/rcl_yaml_param_parser/src/parser.c @@ -0,0 +1,1384 @@ +// 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 +#include +#include +#include +#include +#include + +#include "rcl_yaml_param_parser/parser.h" +#include "rcl_yaml_param_parser/types.h" + +#include "rcl/error_handling.h" +#include "rcl/types.h" +#include "rcutils/strdup.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 MAX_STRING_SIZE 128U +#define PARAMS_KEY "ros__parameters" +#define NODE_NS_SEPERATOR "/" +#define PARAMETER_NS_SEPERATOR "." + +#define MAX_NUM_NODE_ENTRIES 256U +#define MAX_NUM_PARAMS_PER_NODE 512U + +static rcl_ret_t node_params_init( + rcl_node_params_t * node_params, + const rcl_allocator_t allocator); + +static rcl_ret_t add_val_to_bool_arr( + rcl_bool_array_t * const val_array, + bool * value, + const rcl_allocator_t allocator); + +static rcl_ret_t add_val_to_int_arr( + rcl_int64_array_t * const val_array, + int64_t * value, + const rcl_allocator_t allocator); + +static rcl_ret_t add_val_to_double_arr( + rcl_double_array_t * const val_array, + double * value, + const rcl_allocator_t allocator); + +static rcl_ret_t add_val_to_string_arr( + rcutils_string_array_t * const val_array, + char * value, + const rcl_allocator_t allocator); + +/// +/// TODO (anup.pemmaiah): Support byte array +/// + +static rcl_ret_t add_name_to_ns( + namespace_tracker_t * ns_tracker, + const char * name, + const namespace_type_t namespace_type, + rcl_allocator_t allocator); + +static rcl_ret_t rem_name_from_ns( + namespace_tracker_t * ns_tracker, + const namespace_type_t namespace_type, + rcl_allocator_t allocator); + +static rcl_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, + rcl_allocator_t allocator); + +static void * get_value( + const char * const value, + data_types_t * val_type, + const rcl_allocator_t allocator); + +static rcl_ret_t parse_value( + const yaml_event_t event, + const bool is_seq, + data_types_t * seq_data_type, + rcl_params_t * params_st); + +static rcl_ret_t parse_key( + const yaml_event_t event, + uint32_t * map_level, + bool * is_new_map, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st); + +static rcl_ret_t parse_events( + yaml_parser_t * parser, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st); + +/// +/// Add name to namespace tracker +/// +static rcl_ret_t add_name_to_ns( + namespace_tracker_t * ns_tracker, + const char * name, + const namespace_type_t namespace_type, + rcl_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; + rcl_ret_t res = RCL_RET_OK; + + 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: + res = RCL_RET_ERROR; + break; + } + + if (RCL_RET_OK == res) { + /// Add a name to ns + if (NULL == name) { + return RCL_RET_INVALID_ARGUMENT; + } + if (0U == *cur_count) { + cur_ns = rcutils_strdup(name, allocator); + if (NULL == cur_ns) { + return RCL_RET_BAD_ALLOC; + } + } else { + ns_len = strlen(cur_ns); + name_len = strlen(name); + sep_len = strlen(sep_str); + tot_len = ns_len + name_len + sep_len + 1U; + + if (tot_len > MAX_STRING_SIZE) { + RCL_SET_ERROR_MSG("New namespace string is exceeding max string size", + allocator); + return RCL_RET_ERROR; + } + cur_ns = allocator.reallocate(cur_ns, tot_len, allocator.state); + if (NULL == cur_ns) { + return RCL_RET_BAD_ALLOC; + } + 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 res; +} + +/// +/// Remove name from namespace tracker +/// +static rcl_ret_t rem_name_from_ns( + namespace_tracker_t * ns_tracker, + const namespace_type_t namespace_type, + rcl_allocator_t allocator) +{ + char * cur_ns; + uint32_t * cur_count; + char * sep_str; + size_t ns_len; + size_t tot_len; + rcl_ret_t res = RCL_RET_OK; + + 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: + res = RCL_RET_ERROR; + break; + } + + if (RCL_RET_OK == res) { + /// 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) { + RCL_SET_ERROR_MSG("Internal error. Crossing arrau boundary", allocator); + return RCL_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); + cur_ns = allocator.reallocate(cur_ns, tot_len, allocator.state); + if (NULL == cur_ns) { + return RCL_RET_BAD_ALLOC; + } + 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 res; +} + +/// +/// Replace namespace in namespace tracker +/// +static rcl_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, + rcl_allocator_t allocator) +{ + rcl_ret_t res = RCL_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 = new_ns; + 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 = new_ns; + ns_tracker->num_parameter_ns = new_ns_count; + break; + default: + res = RCL_RET_ERROR; + break; + } + return res; +} + +/// +/// Create rcl_node_params_t structure +/// +static rcl_ret_t node_params_init( + rcl_node_params_t * node_params, + const rcl_allocator_t allocator) +{ + if (NULL == node_params) { + return RCL_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 RCL_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); + return RCL_RET_BAD_ALLOC; + } + + return RCL_RET_OK; +} + +/// +/// Create the rcl_params_t parameter structure +/// +rcl_params_t * rcl_yaml_node_struct_init( + const rcl_allocator_t allocator) +{ + rcl_params_t * params_st; + + params_st = allocator.zero_allocate(1, sizeof(rcl_params_t), allocator.state); + if (NULL == params_st) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return NULL; + } + + params_st->node_names = allocator.zero_allocate(MAX_NUM_NODE_ENTRIES, + sizeof(char *), allocator.state); + if (NULL == params_st->node_names) { + rcl_yaml_node_struct_fini(params_st); + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return NULL; + } + + params_st->params = allocator.zero_allocate(MAX_NUM_NODE_ENTRIES, sizeof(rcl_node_params_t), + allocator.state); + if (NULL == params_st->params) { + rcl_yaml_node_struct_fini(params_st); + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return NULL; + } + + params_st->num_nodes = 0U; + params_st->allocator = allocator; + + return params_st; +} + +/// +/// Free param structure +/// NOTE: If there is an error, would recommend just to safely exit the process instead +/// of calling this free function and continuing +/// +void rcl_yaml_node_struct_fini( + rcl_params_t * params_st) +{ + uint32_t node_idx; + size_t parameter_idx = 0U; + rcl_allocator_t allocator; + + if (NULL == params_st) { + return; + } + allocator = params_st->allocator; + + for (node_idx = 0; node_idx < params_st->num_nodes; node_idx++) { + char * node_name = params_st->node_names[node_idx]; + if (NULL != node_name) { + allocator.deallocate(node_name, allocator.state); + } + + if (NULL != params_st->params) { + char * param_name; + rcl_variant_t * param_var; + for (parameter_idx = 0; parameter_idx < params_st->params[node_idx].num_params; + parameter_idx++) + { + if ((NULL != params_st->params[node_idx].parameter_names) && + (NULL != params_st->params[node_idx].parameter_values)) + { + param_name = params_st->params[node_idx].parameter_names[parameter_idx]; + param_var = &(params_st->params[node_idx].parameter_values[parameter_idx]); + if (NULL != param_name) { + allocator.deallocate(param_name, allocator.state); + } + + if (NULL != param_var) { + if (NULL != param_var->bool_value) { + allocator.deallocate(param_var->bool_value, allocator.state); + } else if (NULL != param_var->integer_value) { + allocator.deallocate(param_var->integer_value, allocator.state); + } else if (NULL != param_var->double_value) { + allocator.deallocate(param_var->double_value, allocator.state); + } else if (NULL != param_var->string_value) { + allocator.deallocate(param_var->string_value, allocator.state); + } 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); + } 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); + } 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); + } else if (NULL != param_var->string_array_value) { + if (RCL_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); + } else { + /// Nothing to do to keep pclint happy + } + } // if (param_var) + } + } // for (parameter_idx) + if (NULL != params_st->params[node_idx].parameter_values) { + allocator.deallocate(params_st->params[node_idx].parameter_values, allocator.state); + } + if (NULL != params_st->params[node_idx].parameter_names) { + allocator.deallocate(params_st->params[node_idx].parameter_names, allocator.state); + } + } // if (params) + } // for (node_idx) + if (NULL != params_st->node_names) { + allocator.deallocate(params_st->node_names, allocator.state); + params_st->node_names = NULL; + } + if (NULL != params_st->params) { + allocator.deallocate(params_st->params, allocator.state); + params_st->params = NULL; + } + params_st->num_nodes = 0U; + allocator.deallocate(params_st, allocator.state); +} + +/// +/// Dump the param structure +/// +void rcl_yaml_node_struct_print( + const rcl_params_t * const params_st) +{ + size_t node_idx; + size_t parameter_idx = 0U; + + if (NULL == params_st) { + return; + } + + printf("\n Node Name\t\t\t\tParameters\n"); + for (node_idx = 0; 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) { + char * param_name; + rcl_variant_t * param_var; + for (parameter_idx = 0; parameter_idx < params_st->params[node_idx].num_params; + parameter_idx++) + { + if ((NULL != params_st->params[node_idx].parameter_names) && + (NULL != params_st->params[node_idx].parameter_values)) + { + param_name = params_st->params[node_idx].parameter_names[parameter_idx]; + param_var = &(params_st->params[node_idx].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 rcl_ret_t add_val_to_bool_arr( + rcl_bool_array_t * const val_array, + bool * value, + const rcl_allocator_t allocator) +{ + if ((NULL == value) || (NULL == val_array)) { + return RCL_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 + 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) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return RCL_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 RCL_RET_OK; +} + +/// +/// Add a value to an integer array. Create the array if it does not exist +/// +static rcl_ret_t add_val_to_int_arr( + rcl_int64_array_t * const val_array, + int64_t * value, + const rcl_allocator_t allocator) +{ + if ((NULL == value) || (NULL == val_array)) { + return RCL_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) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return RCL_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 RCL_RET_OK; +} + +/// +/// Add a value to a double array. Create the array if it does not exist +/// +static rcl_ret_t add_val_to_double_arr( + rcl_double_array_t * const val_array, + double * value, + const rcl_allocator_t allocator) +{ + if ((NULL == value) || (NULL == val_array)) { + return RCL_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) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return RCL_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 RCL_RET_OK; +} + +/// +/// Add a value to a string array. Create the array if it does not exist +/// +static rcl_ret_t add_val_to_string_arr( + rcutils_string_array_t * const val_array, + char * value, + const rcl_allocator_t allocator) +{ + if ((NULL == value) || (NULL == val_array)) { + return RCL_RET_INVALID_ARGUMENT; + } + + if (NULL == val_array->data) { + rcl_ret_t res; + + res = rcutils_string_array_init(val_array, 1, &allocator); + if (RCL_RET_OK != res) { + return res; + } + 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"); + return RCL_RET_BAD_ALLOC; + } + val_array->data[val_array->size] = value; + val_array->size++; + } + return RCL_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, + data_types_t * val_type, + const rcl_allocator_t allocator) +{ + void * ret_val; + int64_t ival; + double dval; + char * endptr = NULL; + + if ((NULL == value) || (NULL == val_type)) { + RCL_SET_ERROR_MSG("Invalid arguments", allocator); + return NULL; + } + + /// Check if it is bool + if ((0 == strncmp(value, "Y", strlen(value))) || + (0 == strncmp(value, "y", strlen(value))) || + (0 == strncmp(value, "yes", strlen(value))) || + (0 == strncmp(value, "Yes", strlen(value))) || + (0 == strncmp(value, "YES", strlen(value))) || + (0 == strncmp(value, "true", strlen(value))) || + (0 == strncmp(value, "True", strlen(value))) || + (0 == strncmp(value, "TRUE", strlen(value))) || + (0 == strncmp(value, "on", strlen(value))) || + (0 == strncmp(value, "On", strlen(value))) || + (0 == strncmp(value, "ON", strlen(value)))) + { + *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 == strncmp(value, "N", strlen(value))) || + (0 == strncmp(value, "n", strlen(value))) || + (0 == strncmp(value, "no", strlen(value))) || + (0 == strncmp(value, "No", strlen(value))) || + (0 == strncmp(value, "NO", strlen(value))) || + (0 == strncmp(value, "false", strlen(value))) || + (0 == strncmp(value, "False", strlen(value))) || + (0 == strncmp(value, "FALSE", strlen(value))) || + (0 == strncmp(value, "off", strlen(value))) || + (0 == strncmp(value, "Off", strlen(value))) || + (0 == strncmp(value, "OFF", strlen(value)))) + { + *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 + 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 + 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 rcl_ret_t parse_value( + const yaml_event_t event, + const bool is_seq, + data_types_t * seq_data_type, + rcl_params_t * params_st) +{ + void * ret_val; + data_types_t val_type; + int res = RCL_RET_OK; + rcl_allocator_t allocator; + + if ((NULL == params_st) || (0U == params_st->num_nodes) || (NULL == seq_data_type)) { + return RCL_RET_INVALID_ARGUMENT; + } + allocator = params_st->allocator; + + const size_t node_idx = (params_st->num_nodes - 1U); + if (0U == params_st->params[node_idx].num_params) { + return RCL_RET_INVALID_ARGUMENT; + } + + const size_t parameter_idx = ((params_st->params[node_idx].num_params) - 1U); + 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); + rcl_variant_t * param_value; + + if (val_size > MAX_STRING_SIZE) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Scalar value at line %d" + " is bigger than %d bytes", line_num, MAX_STRING_SIZE); + return RCL_RET_ERROR; + } else { + if (0U == val_size) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "No value at line %d", line_num); + return RCL_RET_ERROR; + } + } + + if (NULL == value) { + return RCL_RET_INVALID_ARGUMENT; + } + + if (NULL == params_st->params[node_idx].parameter_values) { + RCL_SET_ERROR_MSG("Internal error: Invalid mem", allocator); + return RCL_RET_BAD_ALLOC; + } + + param_value = &(params_st->params[node_idx].parameter_values[parameter_idx]); + + // param_value->string_value = rcutils_strdup(value, allocator); + ret_val = get_value(value, &val_type, allocator); + if (NULL == ret_val) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Error parsing value %s at" + " line %d", value, line_num); + return RCL_RET_ERROR; + } + + switch (val_type) { + case DATA_TYPE_UNKNOWN: + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Unknown data type of" + " value %s at line %d\n", value, line_num); + res = RCL_RET_ERROR; + break; + case DATA_TYPE_BOOL: + if (false == is_seq) { + param_value->bool_value = (bool *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + *seq_data_type = val_type; + param_value->bool_array_value = + allocator.zero_allocate(1U, sizeof(rcl_bool_array_t), allocator.state); + if (NULL == param_value->bool_array_value) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return RCL_RET_BAD_ALLOC; + } + } else { + if (*seq_data_type != val_type) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Sequence should be of same" + " type. Value type 'bool' do not belong at line_num %d", line_num); + allocator.deallocate(ret_val, allocator.state); + return RCL_RET_ERROR; + } + } + res = add_val_to_bool_arr(param_value->bool_array_value, ret_val, allocator); + if (RCL_RET_OK != res) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + return res; + } + } + break; + case DATA_TYPE_INT64: + if (false == is_seq) { + param_value->integer_value = (int64_t *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + *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) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return RCL_RET_BAD_ALLOC; + } + } else { + if (*seq_data_type != val_type) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Sequence should be of same" + " type. Value type 'integer' do not belong at line_num %d", line_num); + allocator.deallocate(ret_val, allocator.state); + return RCL_RET_ERROR; + } + } + res = add_val_to_int_arr(param_value->integer_array_value, ret_val, allocator); + if (RCL_RET_OK != res) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + return res; + } + } + break; + case DATA_TYPE_DOUBLE: + if (false == is_seq) { + param_value->double_value = (double *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + *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) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return RCL_RET_BAD_ALLOC; + } + } else { + if (*seq_data_type != val_type) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Sequence should be of same" + " type. Value type 'double' do not belong at line_num %d", line_num); + allocator.deallocate(ret_val, allocator.state); + return RCL_RET_ERROR; + } + } + res = add_val_to_double_arr(param_value->double_array_value, ret_val, allocator); + if (RCL_RET_OK != res) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + return res; + } + } + break; + case DATA_TYPE_STRING: + if (false == is_seq) { + param_value->string_value = (char *)ret_val; + } else { + if (DATA_TYPE_UNKNOWN == *seq_data_type) { + *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) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem"); + return RCL_RET_BAD_ALLOC; + } + } else { + if (*seq_data_type != val_type) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Sequence should be of same" + " type. Value type 'string' do not belong at line_num %d", line_num); + allocator.deallocate(ret_val, allocator.state); + return RCL_RET_ERROR; + } + } + res = add_val_to_string_arr(param_value->string_array_value, ret_val, allocator); + if (RCL_RET_OK != res) { + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + return res; + } + } + break; + default: + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Unknown data type of value" + " %s at line %d", value, line_num); + res = RCL_RET_ERROR; + break; + } + return res; +} + +/// +/// Parse the key part of the pair +/// +static rcl_ret_t parse_key( + const yaml_event_t event, + uint32_t * map_level, + bool * is_new_map, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st) +{ + int32_t res = RCL_RET_OK; + 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); + size_t num_nodes; + size_t node_idx = 0U; + rcl_allocator_t allocator; + + if ((NULL == map_level) || (NULL == params_st)) { + return RCL_RET_INVALID_ARGUMENT; + } + allocator = params_st->allocator; + + if (val_size > MAX_STRING_SIZE) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Scalar value at line %d" + " is bigger than %d bytes", line_num, MAX_STRING_SIZE); + return RCL_RET_ERROR; + } else { + if (0U == val_size) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "No key at line %d", line_num); + return RCL_RET_ERROR; + } + } + + if (NULL == value) { + return RCL_RET_INVALID_ARGUMENT; + } + num_nodes = params_st->num_nodes; // New node index + if (num_nodes > 0U) { + node_idx = (num_nodes - 1U); // Current node index + } + + switch (*map_level) { + case MAP_UNINIT_LVL: + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Unintialized map level" + " at line %d", line_num); + res = RCL_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))) { + res = add_name_to_ns(ns_tracker, value, NS_TYPE_NODE, allocator); + if (RCL_RET_OK != res) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Internal error" + " adding node namespace at line %d", line_num); + return res; + } + } else { + if (0U == ns_tracker->num_node_ns) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "There are no node names" + " before %s at line %d", PARAMS_KEY, line_num); + return RCL_RET_ERROR; + } + /// 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) { + return RCL_RET_BAD_ALLOC; + } + params_st->node_names[num_nodes] = node_name_ns; + + res = rem_name_from_ns(ns_tracker, NS_TYPE_NODE, allocator); + if (RCL_RET_OK != res) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Internal error" + " adding node namespace at line %d", line_num); + return res; + } + res = node_params_init(&(params_st->params[num_nodes]), allocator); + if (RCL_RET_OK != res) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Error creating node" + " parameter at line %d", line_num); + return RCL_RET_ERROR; + } + params_st->num_nodes++; + /// Bump the map level to PARAMS + (*map_level)++; + } + } + break; + case MAP_PARAMS_LVL: + { + char * parameter_ns; + size_t parameter_idx; + char * param_name; + + /// If it is a new map, the previous key is param namespace + if (true == *is_new_map) { + params_st->params[node_idx].num_params--; + parameter_idx = params_st->params[node_idx].num_params; + parameter_ns = params_st->params[node_idx].parameter_names[parameter_idx]; + if (NULL == parameter_ns) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Internal error" + " creating param namespace at line %d", line_num); + return RCL_RET_ERROR; + } + res = replace_ns(ns_tracker, parameter_ns, (ns_tracker->num_parameter_ns + 1U), + NS_TYPE_PARAM, allocator); + if (RCL_RET_OK != res) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Internal error replacing" + " namespace at line %d", line_num); + return RCL_RET_ERROR; + } + *is_new_map = false; + } + /// Add a parameter name into the node parameters + parameter_idx = params_st->params[node_idx].num_params; + parameter_ns = ns_tracker->parameter_ns; + if (NULL == parameter_ns) { + param_name = rcutils_strdup(value, allocator); + if (NULL == param_name) { + return RCL_RET_BAD_ALLOC; + } + } else { + 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); + + if (tot_len > MAX_STRING_SIZE) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "The name length" + " exceeds the MAX size %d at line %d", MAX_STRING_SIZE, line_num); + return RCL_RET_OK; + } + + param_name = allocator.zero_allocate(1U, tot_len, allocator.state); + if (NULL == param_name) { + return RCL_RET_BAD_ALLOC; + } + + 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'; + } + params_st->params[node_idx].parameter_names[parameter_idx] = param_name; + params_st->params[node_idx].num_params++; + } + break; + default: + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Unknown map level at" + " line %d", line_num); + res = RCL_RET_ERROR; + break; + } + return res; +} + +/// +/// Get events from the parser and process the events +/// +static rcl_ret_t parse_events( + yaml_parser_t * parser, + namespace_tracker_t * ns_tracker, + rcl_params_t * params_st) +{ + int32_t done_parsing = 0; + yaml_event_t event; + rcl_ret_t res = RCL_RET_OK; + bool is_key = true; + bool is_seq = false; + uint32_t line_num = 0; + data_types_t seq_data_type; + uint32_t map_level = 1U; + uint32_t map_depth = 0U; + bool is_new_map = false; + rcl_allocator_t allocator; + + if ((NULL == parser) || (NULL == params_st)) { + return RCL_RET_INVALID_ARGUMENT; + } + allocator = params_st->allocator; + + while (0 == done_parsing) { + if (RCL_RET_OK != res) { + return res; + } + res = yaml_parser_parse(parser, &event); + if (0 == res) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Error parsing a" + " event near line %d", line_num); + return RCL_RET_ERROR; + } else { + res = RCL_RET_OK; + } + 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) { + res = parse_key(event, &map_level, &is_new_map, ns_tracker, + params_st); + if (RCL_RET_OK != res) { + yaml_event_delete(&event); + return res; + } + is_key = false; + } else { + /// It is a value + if (map_level < (uint32_t)(MAP_PARAMS_LVL)) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Cannot have a value" + " before %s at line %d", PARAMS_KEY, line_num); + yaml_event_delete(&event); + return RCL_RET_ERROR; + } + res = parse_value(event, is_seq, &seq_data_type, params_st); + if (RCL_RET_OK != res) { + yaml_event_delete(&event); + return res; + } + if (false == is_seq) { + is_key = true; + } + } + } + yaml_event_delete(&event); + break; + case YAML_SEQUENCE_START_EVENT: + if (true == is_key) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Sequences cannot be key" + " at line %d", line_num); + yaml_event_delete(&event); + return RCL_RET_ERROR; + } + if (map_level < (uint32_t)(MAP_PARAMS_LVL)) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Sequences can only be" + " values and not keys in params. Error at line %d\n", line_num); + yaml_event_delete(&event); + return RCL_RET_ERROR; + } + is_seq = true; + seq_data_type = DATA_TYPE_UNKNOWN; + yaml_event_delete(&event); + break; + case YAML_SEQUENCE_END_EVENT: + is_seq = false; + is_key = true; + yaml_event_delete(&event); + 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; + } + yaml_event_delete(&event); + break; + case YAML_MAPPING_END_EVENT: + if (MAP_PARAMS_LVL == map_level) { + if (ns_tracker->num_parameter_ns > 0U) { + /// Remove param namesapce + res = rem_name_from_ns(ns_tracker, NS_TYPE_PARAM, allocator); + if (RCL_RET_OK != res) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Internal error" + " removing parameter namespace at line %d", line_num); + yaml_event_delete(&event); + return res; + } + } else { + map_level--; + } + } else { + if ((MAP_NODE_NAME_LVL == map_level) && + (map_depth == (ns_tracker->num_node_ns + 1U))) + { + /// Remove node namespace + res = rem_name_from_ns(ns_tracker, NS_TYPE_NODE, allocator); + if (RCL_RET_OK != res) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Internal error" + " removing node namespace at line %d", line_num); + yaml_event_delete(&event); + return res; + } + } + } + map_depth--; + yaml_event_delete(&event); + break; + case YAML_ALIAS_EVENT: + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Will not support aliasing" + " at line %d\n", line_num); + res = RCL_RET_ERROR; + yaml_event_delete(&event); + break; + case YAML_STREAM_START_EVENT: + yaml_event_delete(&event); + break; + case YAML_DOCUMENT_START_EVENT: + yaml_event_delete(&event); + break; + case YAML_DOCUMENT_END_EVENT: + yaml_event_delete(&event); + break; + case YAML_NO_EVENT: + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Received an empty event at" + " line %d", line_num); + res = RCL_RET_ERROR; + yaml_event_delete(&event); + break; + default: + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(allocator, "Unknown YAML event at line" + " %d", line_num); + res = RCL_RET_ERROR; + yaml_event_delete(&event); + break; + } + } + return res; +} + +/// +/// TODO (anup.pemmaiah): Support string yaml similar to yaml file +/// TODO (anup.pemmaiah): Support Mutiple yaml files +/// +/// +/// Parse the YAML file and populate params_st +/// +bool rcl_parse_yaml_file( + const char * file_path, + rcl_params_t * params_st) +{ + int32_t res; + FILE * yaml_file; + yaml_parser_t parser; + namespace_tracker_t ns_tracker; + rcutils_allocator_t allocator; + + if (NULL == params_st) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Pass a initialized paramter structure"); + return false; + } + allocator = params_st->allocator; + + if (NULL == file_path) { + RCL_SET_ERROR_MSG("YAML file path is NULL", allocator); + return false; + } + + res = yaml_parser_initialize(&parser); + if (0 == res) { + RCL_SET_ERROR_MSG("Could not initialize the parser", allocator); + return false; + } + + yaml_file = fopen(file_path, "r"); + if (NULL == yaml_file) { + yaml_parser_delete(&parser); + RCL_SET_ERROR_MSG("Error opening YAML file", allocator); + return false; + } + + yaml_parser_set_input_file(&parser, yaml_file); + + memset(&ns_tracker, 0, sizeof(namespace_tracker_t)); + res = parse_events(&parser, &ns_tracker, params_st); + + yaml_parser_delete(&parser); + fclose(yaml_file); + + if (RCL_RET_OK != res) { + if (NULL != ns_tracker.node_ns) { + allocator.deallocate(ns_tracker.node_ns, allocator.state); + } + if (NULL != ns_tracker.parameter_ns) { + allocator.deallocate(ns_tracker.parameter_ns, allocator.state); + } + rcl_yaml_node_struct_fini(params_st); + return false; + } + + return true; +} diff --git a/rcl_yaml_param_parser/test/correct_config.yaml b/rcl_yaml_param_parser/test/correct_config.yaml new file mode 100644 index 0000000000..51d1bb6183 --- /dev/null +++ b/rcl_yaml_param_parser/test/correct_config.yaml @@ -0,0 +1,51 @@ +# config/test_yaml +--- + +lidar_ns: + lidar_1: + ros__parameters: + id: 10 + name: front_lidar + ports: [2438, 2439, 2440] + driver1: + dx: 4.56 + dy: 2.30 + fr_sensor_specs: [12, 3, 0, 7] + bk_sensor_specs: [12.1, -2.3, 5.2, 9.0] + is_front: true + driver2: + dx: 1.23 + dy: 0.45 + lidar_2: + ros__parameters: + id: 11 + name: back_lidar + dy1: 0.003 + is_back: false + driver: + dz: 7.89 +camera: + ros__parameters: + name: camera1 + loc: front + cam_spec: + angle: 2.34 + supported_brands: ["Bosch", "Novatek", "Mobius"] +new_camera_ns: + new_camera1: + ros__parameters: + is_cam_on: [true, true, false, true, false, false] + brand: Bosch + new_camera2: + ros__parameters: + camera_dr: + dr_name: default + brand: Mobius +drive_px: + ros__parameters: + num_cores: 6 + arch: ARM +intel: + ros__parameters: + num_cores: 8 + arch: x86_64 diff --git a/rcl_yaml_param_parser/test/indented_name_space.yaml b/rcl_yaml_param_parser/test/indented_name_space.yaml new file mode 100644 index 0000000000..6c53f81dd4 --- /dev/null +++ b/rcl_yaml_param_parser/test/indented_name_space.yaml @@ -0,0 +1,17 @@ +# config/test_yaml +--- + lidar_ns: + lidar_1: + ros__parameters: + id: 10 + name: front_lidar + ports: [2438, 2439, 2440] + driver1: + dx: 4.56 + dy: 2.30 + fr_sensor_specs: [12, 3, 0, 7] + bk_sensor_specs: [12.1, -2.3, 5.2, 9.0] + is_front: true + driver2: + dx: 1.23 + dy: 0.45 diff --git a/rcl_yaml_param_parser/test/max_string_sz.yaml b/rcl_yaml_param_parser/test/max_string_sz.yaml new file mode 100644 index 0000000000..1b279f6145 --- /dev/null +++ b/rcl_yaml_param_parser/test/max_string_sz.yaml @@ -0,0 +1,9 @@ +# config/test_yaml +--- + +lidar_ns: + lidar_1: + ros__parameters: + id: 10 + name: "A very long name that will not be supported by the yaml parser. The max supported string size is 128 + characters. Anything over the max size will be rejected" diff --git a/rcl_yaml_param_parser/test/multi_ns_correct.yaml b/rcl_yaml_param_parser/test/multi_ns_correct.yaml new file mode 100644 index 0000000000..503fb52fab --- /dev/null +++ b/rcl_yaml_param_parser/test/multi_ns_correct.yaml @@ -0,0 +1,64 @@ +# config/test_yaml +--- + +lidar_ns: + lidar_1: + lidar11: + lidar111: + ros__parameters: + id: 10 + name: front_lidar + ports: [2438, 2439, 2440] + driver1: + driver11: + driver111: + dx: 4.56 + dy: 2.30 + fr_sensor_specs: [12, 3, 0, 7] + bk_sensor_specs: [12.1, -2.3, 5.2, 9.0] + is_front: true + driver2: + driver21: + dx: 1.23 + dy: 0.45 + lidar_2: + lidar21: + ros__parameters: + driver1: + driver11: + id: 11 + name: back_lidar + back_lidar1: + back_lidar12: + pos: center + ratio: 2:1 + dy1: 0.003 + is_back: false + tmp_driver: + dz: 7.89 +camera: + camera1: + ros__parameters: + name: camera1 + loc: front + cam_spec: + angle: 2.34 + supported_brands: ["Bosch", "Novatek", "Mobius"] +new_camera_ns: + new_camera1: + ros__parameters: + is_cam_on: [true, true, false, true, false, false] + brand: Bosch + new_camera2: + ros__parameters: + camera_dr: + dr_name: default + brand: Mobius +drive_px: + ros__parameters: + num_cores: 6 + arch: ARM +intel: + ros__parameters: + num_cores: 8 + arch: x86_64 diff --git a/rcl_yaml_param_parser/test/no_alias_support.yaml b/rcl_yaml_param_parser/test/no_alias_support.yaml new file mode 100644 index 0000000000..acec7a127d --- /dev/null +++ b/rcl_yaml_param_parser/test/no_alias_support.yaml @@ -0,0 +1,9 @@ +# config/test_yaml +--- + +lidar_ns: + lidar_1: + ros__parameters: + id: 10 + name: &FL "front_lidar" + spec: *FL diff --git a/rcl_yaml_param_parser/test/no_value1.yaml b/rcl_yaml_param_parser/test/no_value1.yaml new file mode 100644 index 0000000000..a4542096f6 --- /dev/null +++ b/rcl_yaml_param_parser/test/no_value1.yaml @@ -0,0 +1,8 @@ +# config/test_yaml +--- + +lidar_ns: + lidar_1: + ros__parameters: + id: + name: "front lidar" diff --git a/rcl_yaml_param_parser/test/params_with_no_node.yaml b/rcl_yaml_param_parser/test/params_with_no_node.yaml new file mode 100644 index 0000000000..3ae2855484 --- /dev/null +++ b/rcl_yaml_param_parser/test/params_with_no_node.yaml @@ -0,0 +1,18 @@ +# config/test_yaml +--- +node1: + ros__parameters: + name: test +ros__parameters: + id: 10 + name: front_lidar + ports: [2438, 2439, 2440] + driver1: + dx: 4.56 + dy: 2.30 + fr_sensor_specs: [12, 3, 0, 7] + bk_sensor_specs: [12.1, -2.3, 5.2, 9.0] + is_front: true + driver2: + dx: 1.23 + dy: 0.45 diff --git a/rcl_yaml_param_parser/test/seq_map1.yaml b/rcl_yaml_param_parser/test/seq_map1.yaml new file mode 100644 index 0000000000..6ebdd4f686 --- /dev/null +++ b/rcl_yaml_param_parser/test/seq_map1.yaml @@ -0,0 +1,14 @@ +# config/test_yaml +--- + +lidar_ns: + lidar_1: + ros__parameters: + - driver: front_driver + dx: 4.56 + dy: 2.30 + sensor_specs: [12, 3, 0, 7] + - driver: back_driver + dx: 1.23 + dy: 0.45 + sensor_specs: [12.1, -2.3, 5.2, 9.0] diff --git a/rcl_yaml_param_parser/test/seq_map2.yaml b/rcl_yaml_param_parser/test/seq_map2.yaml new file mode 100644 index 0000000000..61927bba5a --- /dev/null +++ b/rcl_yaml_param_parser/test/seq_map2.yaml @@ -0,0 +1,16 @@ +# config/test_yaml +--- + +lidar_ns: + lidar_1: + ros__parameters: + id: 10 + name: front_lidar + - driver: front_driver + dx: 4.56 + dy: 2.30 + sensor_specs: [12, 3, 0, 7] + - driver: back_driver + dx: 1.23 + dy: 0.45 + sensor_specs: [12.1, -2.3, 5.2, 9.0] diff --git a/rcl_yaml_param_parser/test/test_parse_yaml.cpp b/rcl_yaml_param_parser/test/test_parse_yaml.cpp new file mode 100644 index 0000000000..194726fbf9 --- /dev/null +++ b/rcl_yaml_param_parser/test/test_parse_yaml.cpp @@ -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 +#include + +#include "rcl_yaml_param_parser/parser.h" + +#include "rcutils/error_handling.h" +#include "rcutils/filesystem.h" + +static char cur_dir[1024]; +rcutils_allocator_t allocator = rcutils_get_default_allocator(); + +TEST(test_file_parser, correct_syntax) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "correct_config.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_TRUE(res); + rcl_yaml_node_struct_print(params_hdl); + rcl_yaml_node_struct_fini(params_hdl); + free(test_path); + free(path); +} + +TEST(test_file_parser, multi_ns_correct_syntax) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "multi_ns_correct.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_TRUE(res); + rcl_yaml_node_struct_print(params_hdl); + rcl_yaml_node_struct_fini(params_hdl); + free(test_path); + free(path); +} + +TEST(test_file_parser, seq_map1) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "seq_map1.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_FALSE(res); + free(test_path); + free(path); +} + +TEST(test_file_parser, seq_map2) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "seq_map2.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_FALSE(res); + free(test_path); + free(path); +} + +TEST(test_file_parser, params_with_no_node) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "params_with_no_node.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_FALSE(res); + free(test_path); + free(path); +} + +TEST(test_file_parser, no_alias_support) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "no_alias_support.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_FALSE(res); + free(test_path); + free(path); +} + +TEST(test_file_parser, max_string_sz) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "max_string_sz.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_FALSE(res); + free(test_path); + free(path); +} + +TEST(test_file_parser, no_value1) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "no_value1.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_FALSE(res); + free(test_path); + free(path); +} + +TEST(test_file_parser, indented_ns) { + rcutils_reset_error(); + EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); + char * test_path = rcutils_join_path(cur_dir, "test"); + char * path = rcutils_join_path(test_path, "indented_name_space.yaml"); + fprintf(stderr, "cur_path: %s\n", path); + EXPECT_TRUE(rcutils_exists(path)); + rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator); + EXPECT_FALSE(NULL == params_hdl); + bool res = rcl_parse_yaml_file(path, params_hdl); + fprintf(stderr, "%s\n", rcutils_get_error_string_safe()); + EXPECT_FALSE(res); + free(test_path); + free(path); +} + +int32_t main(int32_t argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}