From 07e34d4325ce125e0626398abab71db19f32e8ef Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 5 Jul 2019 13:12:00 -0300 Subject: [PATCH] Support parameter YAML string value parsing. Signed-off-by: Michel Hidalgo --- .../include/rcl_yaml_param_parser/parser.h | 23 +- rcl_yaml_param_parser/src/parser.c | 247 ++++++++++++++++-- .../test/test_parse_yaml.cpp | 13 +- 3 files changed, 249 insertions(+), 34 deletions(-) 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 index e335b27f59..5d11a8721d 100644 --- a/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser.h +++ b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser.h @@ -25,29 +25,42 @@ extern "C" { #endif -/// \brief Init param structure +/// \brief Initialize parameter 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 +/// \brief Free parameter structure +/// \param[in] params_st points to the populated parameter 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[inout] params_st points to the populated paramter struct +/// \param[inout] params_st points to the populated parameter 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 +/// \brief Parse a parameter value as a YAML string, updating params_st accordingly +/// \param[in] node_name is the name of the node to which the parameter belongs +/// \param[in] param_name is the name of the parameter whose value will be parsed +/// \param[in] yaml_value is the parameter value as a YAML string to be parsed +/// \param[inout] params_st points to the parameter struct +/// \return true on success and false on failure +RCL_YAML_PARAM_PARSER_PUBLIC +bool rcl_parse_yaml_value( + const char * node_name, + const char * param_name, + const char * yaml_value, + 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( diff --git a/rcl_yaml_param_parser/src/parser.c b/rcl_yaml_param_parser/src/parser.c index 0d6b2c3ff2..950dc7d852 100644 --- a/rcl_yaml_param_parser/src/parser.c +++ b/rcl_yaml_param_parser/src/parser.c @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "rcl_yaml_param_parser/parser.h" @@ -25,8 +26,8 @@ #include "rcutils/allocator.h" #include "rcutils/error_handling.h" -#include "rcutils/types.h" #include "rcutils/strdup.h" +#include "rcutils/types.h" /// NOTE: Will allow a max YAML mapping depth of 5 /// map level 1 : Node name mapping @@ -126,6 +127,8 @@ static void * get_value( 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); @@ -136,11 +139,17 @@ static rcutils_ret_t parse_key( namespace_tracker_t * ns_tracker, rcl_params_t * params_st); -static rcutils_ret_t parse_events( +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 /// @@ -817,6 +826,8 @@ static void * get_value( 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) { @@ -832,13 +843,6 @@ static rcutils_ret_t parse_value( return RCUTILS_RET_INVALID_ARGUMENT; } - const size_t node_idx = (params_st->num_nodes - 1U); - if (0U == params_st->params[node_idx].num_params) { - RCUTILS_SET_ERROR_MSG("No parameter to update"); - return RCUTILS_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; yaml_scalar_style_t style = event.data.scalar.style; @@ -1056,11 +1060,10 @@ static rcutils_ret_t parse_key( } size_t node_idx = 0U; - size_t num_nodes = params_st->num_nodes; // New node index + size_t num_nodes = params_st->num_nodes; if (num_nodes > 0U) { - node_idx = (num_nodes - 1U); // Current node index + node_idx = num_nodes - 1U; } - rcutils_ret_t ret = RCUTILS_RET_OK; switch (*map_level) { case MAP_UNINIT_LVL: @@ -1112,8 +1115,8 @@ static rcutils_ret_t parse_key( break; case MAP_PARAMS_LVL: { - char * parameter_ns; size_t parameter_idx; + char * parameter_ns; char * param_name; /// If it is a new map, the previous key is param namespace @@ -1186,9 +1189,9 @@ static rcutils_ret_t parse_key( } /// -/// Get events from the parser and process the events +/// Get events from parsing a parameter YAML file and process them /// -static rcutils_ret_t parse_events( +static rcutils_ret_t parse_file_events( yaml_parser_t * parser, namespace_tracker_t * ns_tracker, rcl_params_t * params_st) @@ -1244,7 +1247,21 @@ static rcutils_ret_t parse_events( yaml_event_delete(&event); return RCUTILS_RET_ERROR; } - ret = parse_value(event, is_seq, &seq_data_type, params_st); + 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; + } + const size_t node_idx = (params_st->num_nodes - 1U); + 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; + } + const size_t parameter_idx = (params_st->params[node_idx].num_params - 1U); + ret = parse_value(event, is_seq, node_idx, parameter_idx, &seq_data_type, params_st); if (RCUTILS_RET_OK != ret) { yaml_event_delete(&event); return ret; @@ -1353,7 +1370,140 @@ static rcutils_ret_t parse_events( } /// -/// TODO (anup.pemmaiah): Support string yaml similar to yaml file +/// 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 (!done_parsing) { + yaml_event_t event; + int success = yaml_parser_parse(parser, &event); + if (0 == success) { + RCUTILS_SET_ERROR_MSG("Error parsing an event"); + return RCUTILS_RET_ERROR; + } + switch (event.type) { + case YAML_STREAM_END_EVENT: + done_parsing = true; + yaml_event_delete(&event); + break; + case YAML_SCALAR_EVENT: + ret = parse_value( + event, is_seq, node_idx, parameter_idx, &seq_data_type, params_st); + if (RCUTILS_RET_OK != ret) { + yaml_event_delete(&event); + return ret; + } + yaml_event_delete(&event); + break; + case YAML_SEQUENCE_START_EVENT: + is_seq = true; + seq_data_type = DATA_TYPE_UNKNOWN; + yaml_event_delete(&event); + break; + case YAML_SEQUENCE_END_EVENT: + is_seq = false; + 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: + RCUTILS_SET_ERROR_MSG("Received an empty event"); + ret = RCUTILS_RET_ERROR; + yaml_event_delete(&event); + break; + default: + RCUTILS_SET_ERROR_MSG("Unknown YAML event"); + ret = RCUTILS_RET_ERROR; + yaml_event_delete(&event); + break; + } + } + return ret; +} + +/// +/// 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) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(param_st, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_idx, RCUTILS_RET_INVALID_ARGUMENT); + + bool found = false; + for (*node_idx = 0U; *node_idx < param_st->num_nodes; (*node_idx)++) { + if (0 == strcmp(param_st->node_names[*node_idx], node_name)) { + found = true; + break; + } + } + if (!found) { + 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(¶m_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) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(parameter_name, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(param_st, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(parameter_idx, RCUTILS_RET_INVALID_ARGUMENT); + + bool found = false; + rcl_node_params_t * node_param_st = ¶m_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)) { + found = true; + break; + } + } + if (!found) { + rcutils_allocator_t allocator = param_st->allocator; + 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; +} + +/// /// TODO (anup.pemmaiah): Support Mutiple yaml files /// /// @@ -1363,14 +1513,10 @@ bool rcl_parse_yaml_file( const char * file_path, rcl_params_t * params_st) { - if (NULL == params_st) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Pass a initialized paramter structure"); - return false; - } - rcutils_allocator_t allocator = params_st->allocator; + RCUTILS_CHECK_ARGUMENT_FOR_NULL(file_path, false); - if (NULL == file_path) { - RCUTILS_SET_ERROR_MSG("YAML file path is NULL"); + if (NULL == params_st) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Pass an initialized parameter structure"); return false; } @@ -1392,12 +1538,14 @@ bool rcl_parse_yaml_file( namespace_tracker_t ns_tracker; memset(&ns_tracker, 0, sizeof(namespace_tracker_t)); - rcutils_ret_t ret = parse_events(&parser, &ns_tracker, params_st); + rcutils_ret_t ret = parse_file_events(&parser, &ns_tracker, params_st); - yaml_parser_delete(&parser); fclose(yaml_file); + yaml_parser_delete(&parser); + if (RCUTILS_RET_OK != ret) { + rcutils_allocator_t allocator = params_st->allocator; if (NULL != ns_tracker.node_ns) { allocator.deallocate(ns_tracker.node_ns, allocator.state); } @@ -1410,3 +1558,50 @@ bool rcl_parse_yaml_file( return true; } + +/// +/// Parse a YAML string and populate params_st +/// +bool rcl_parse_yaml_value( + const char * node_name, + const char * param_name, + const char * yaml_value, + rcl_params_t * params_st) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, false); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(param_name, false); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(yaml_value, false); + + if (NULL == params_st) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Pass an initialized parameter structure"); + return false; + } + + size_t node_idx = 0U; + rcutils_ret_t ret = find_node(node_name, params_st, &node_idx); + if (RCUTILS_RET_OK != ret) { + return false; + } + + size_t parameter_idx = 0U; + ret = find_parameter(node_idx, param_name, params_st, ¶meter_idx); + if (RCUTILS_RET_OK != ret) { + return false; + } + + yaml_parser_t parser; + int success = yaml_parser_initialize(&parser); + if (0 == success) { + RCUTILS_SET_ERROR_MSG("Could not initialize the parser"); + return false; + } + + yaml_parser_set_input_string( + &parser, (const unsigned char *)yaml_value, strlen(yaml_value)); + + ret = parse_value_events(&parser, node_idx, parameter_idx, params_st); + + yaml_parser_delete(&parser); + + return RCUTILS_RET_OK == ret; +} diff --git a/rcl_yaml_param_parser/test/test_parse_yaml.cpp b/rcl_yaml_param_parser/test/test_parse_yaml.cpp index 274e62f79a..76cef6449c 100644 --- a/rcl_yaml_param_parser/test/test_parse_yaml.cpp +++ b/rcl_yaml_param_parser/test/test_parse_yaml.cpp @@ -23,7 +23,7 @@ static char cur_dir[1024]; -TEST(test_file_parser, correct_syntax) { +TEST(test_parser, correct_syntax) { rcutils_reset_error(); EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)); rcutils_allocator_t allocator = rcutils_get_default_allocator(); @@ -34,8 +34,15 @@ TEST(test_file_parser, correct_syntax) { 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().str); - EXPECT_TRUE(res); + EXPECT_TRUE(res) << rcutils_get_error_string().str; + res = rcl_parse_yaml_value("lidar_ns/lidar_1", "ports", "[8080]", params_hdl); + EXPECT_TRUE(res) << rcutils_get_error_string().str; + res = rcl_parse_yaml_value("lidar_ns/lidar_2", "is_back", "true", params_hdl); + EXPECT_TRUE(res) << rcutils_get_error_string().str; + res = rcl_parse_yaml_value("camera", "cam_spec.angle", "2.2", params_hdl); + EXPECT_TRUE(res) << rcutils_get_error_string().str; + res = rcl_parse_yaml_value("intel", "num_cores", "12", params_hdl); + EXPECT_TRUE(res) << rcutils_get_error_string().str; rcl_yaml_node_struct_print(params_hdl); rcl_yaml_node_struct_fini(params_hdl); allocator.deallocate(test_path, allocator.state);