Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rcl issues 469 test arguments #564

Merged
merged 8 commits into from
Mar 12, 2020
86 changes: 51 additions & 35 deletions rcl/src/rcl/arguments.c
Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,16 @@ _atob(
const char * str,
bool * val);

/// Allocate and zero initialize arguments impl and.
/**
* \param[out] args target arguments to set impl
* \param[in] allocator an allocator to use
* \return RCL_RET_OK if a valid rule was parsed, or
* \return RCL_RET_BAD_ALLOC if an allocation failed
*/
rcl_ret_t
_rcl_allocate_initialized_arguments_impl(rcl_arguments_t * args, rcl_allocator_t * allocator);

rcl_ret_t
rcl_parse_arguments(
int argc,
Expand All @@ -323,26 +333,11 @@ rcl_parse_arguments(
rcl_ret_t ret;
rcl_ret_t fail_ret;

args_output->impl = allocator.allocate(sizeof(rcl_arguments_impl_t), allocator.state);
if (NULL == args_output->impl) {
return RCL_RET_BAD_ALLOC;
ret = _rcl_allocate_initialized_arguments_impl(args_output, &allocator);
if (RCL_RET_OK != ret) {
return ret;
}
rcl_arguments_impl_t * args_impl = args_output->impl;
args_impl->num_remap_rules = 0;
args_impl->remap_rules = NULL;
args_impl->log_level = -1;
args_impl->external_log_config_file = NULL;
args_impl->unparsed_args = NULL;
args_impl->num_unparsed_args = 0;
args_impl->unparsed_ros_args = NULL;
args_impl->num_unparsed_ros_args = 0;
args_impl->parameter_overrides = NULL;
args_impl->parameter_files = NULL;
args_impl->num_param_files_args = 0;
args_impl->log_stdout_disabled = false;
args_impl->log_rosout_disabled = false;
args_impl->log_ext_lib_disabled = false;
args_impl->allocator = allocator;

if (argc == 0) {
// there are no arguments to parse
Expand Down Expand Up @@ -967,24 +962,11 @@ rcl_arguments_copy(

rcl_allocator_t allocator = args->impl->allocator;

args_out->impl = allocator.allocate(sizeof(rcl_arguments_impl_t), allocator.state);
if (NULL == args_out->impl) {
return RCL_RET_BAD_ALLOC;
rcl_ret_t ret = _rcl_allocate_initialized_arguments_impl(args_out, &allocator);
if (RCL_RET_OK != ret) {
return ret;
}

args_out->impl->allocator = allocator;

// Zero so it's safe to call rcl_arguments_fini() if an error occurrs while copying.
args_out->impl->num_remap_rules = 0;
args_out->impl->remap_rules = NULL;
args_out->impl->unparsed_args = NULL;
args_out->impl->num_unparsed_args = 0;
args_out->impl->unparsed_ros_args = NULL;
args_out->impl->num_unparsed_ros_args = 0;
args_out->impl->parameter_overrides = NULL;
args_out->impl->parameter_files = NULL;
args_out->impl->num_param_files_args = 0;

if (args->impl->num_unparsed_args) {
// Copy unparsed args
args_out->impl->unparsed_args = allocator.allocate(
Expand Down Expand Up @@ -1030,7 +1012,7 @@ rcl_arguments_copy(
args_out->impl->num_remap_rules = args->impl->num_remap_rules;
for (int i = 0; i < args->impl->num_remap_rules; ++i) {
args_out->impl->remap_rules[i] = rcl_get_zero_initialized_remap();
rcl_ret_t ret = rcl_remap_copy(
ret = rcl_remap_copy(
&(args->impl->remap_rules[i]), &(args_out->impl->remap_rules[i]));
if (RCL_RET_OK != ret) {
if (RCL_RET_OK != rcl_arguments_fini(args_out)) {
Expand Down Expand Up @@ -1117,6 +1099,12 @@ rcl_arguments_fini(
args->impl->parameter_files = NULL;
}

if (NULL != args->impl->external_log_config_file) {
args->impl->allocator.deallocate(
args->impl->external_log_config_file, args->impl->allocator.state);
args->impl->external_log_config_file = NULL;
}

args->impl->allocator.deallocate(args->impl, args->impl->allocator.state);
args->impl = NULL;
return ret;
Expand Down Expand Up @@ -2094,6 +2082,34 @@ _atob(
return RCL_RET_ERROR;
}

rcl_ret_t
_rcl_allocate_initialized_arguments_impl(rcl_arguments_t * args, rcl_allocator_t * allocator)
{
args->impl = allocator->allocate(sizeof(rcl_arguments_impl_t), allocator->state);
if (NULL == args->impl) {
return RCL_RET_BAD_ALLOC;
}

rcl_arguments_impl_t * args_impl = args->impl;
args_impl->num_remap_rules = 0;
args_impl->remap_rules = NULL;
args_impl->log_level = -1;
args_impl->external_log_config_file = NULL;
args_impl->unparsed_args = NULL;
args_impl->num_unparsed_args = 0;
args_impl->unparsed_ros_args = NULL;
args_impl->num_unparsed_ros_args = 0;
args_impl->parameter_overrides = NULL;
args_impl->parameter_files = NULL;
args_impl->num_param_files_args = 0;
args_impl->log_stdout_disabled = false;
args_impl->log_rosout_disabled = false;
args_impl->log_ext_lib_disabled = false;
args_impl->allocator = *allocator;

return RCL_RET_OK;
}

#ifdef __cplusplus
}
#endif
Expand Down
1 change: 1 addition & 0 deletions rcl_yaml_param_parser/src/parser.c
Original file line number Diff line number Diff line change
Expand Up @@ -1343,6 +1343,7 @@ static rcutils_ret_t parse_key(
}

ret = find_node(node_name_ns, params_st, node_idx);
allocator.deallocate(node_name_ns, allocator.state);
if (RCUTILS_RET_OK != ret) {
break;
}
Expand Down