diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt
index c9779c41b..fffaa237e 100644
--- a/rcl/CMakeLists.txt
+++ b/rcl/CMakeLists.txt
@@ -5,6 +5,7 @@ project(rcl)
find_package(ament_cmake_ros REQUIRED)
find_package(rcl_interfaces REQUIRED)
+find_package(rcl_yaml_param_parser REQUIRED)
find_package(rcutils REQUIRED)
find_package(rmw REQUIRED)
find_package(rmw_implementation REQUIRED)
@@ -63,6 +64,7 @@ add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources})
# specific order: dependents before dependencies
ament_target_dependencies(${PROJECT_NAME}
"rcl_interfaces"
+ "rcl_yaml_param_parser"
"rmw_implementation"
"rmw"
"rcutils"
@@ -94,6 +96,7 @@ ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(ament_cmake)
ament_export_dependencies(rcl_interfaces)
+ament_export_dependencies(rcl_yaml_param_parser)
ament_export_dependencies(rmw_implementation)
ament_export_dependencies(rmw)
ament_export_dependencies(rcutils)
diff --git a/rcl/include/rcl/arguments.h b/rcl/include/rcl/arguments.h
index 5905c5558..91a4be8ed 100644
--- a/rcl/include/rcl/arguments.h
+++ b/rcl/include/rcl/arguments.h
@@ -19,6 +19,7 @@
#include "rcl/macros.h"
#include "rcl/types.h"
#include "rcl/visibility_control.h"
+#include "rcl_yaml_param_parser/types.h"
#ifdef __cplusplus
extern "C"
@@ -36,6 +37,10 @@ typedef struct rcl_arguments_t
#define RCL_ROS_ARGS_FLAG "--ros-args"
#define RCL_ROS_ARGS_EXPLICIT_END_TOKEN "--"
+#define RCL_PARAM_FLAG "--param"
+#define RCL_SHORT_PARAM_FLAG "-p"
+#define RCL_REMAP_FLAG "--remap"
+#define RCL_SHORT_REMAP_FLAG "-r"
#define RCL_LOG_LEVEL_ARG_RULE "__log_level:="
#define RCL_EXTERNAL_LOG_CONFIG_ARG_RULE "__log_config_file:="
@@ -254,6 +259,31 @@ rcl_arguments_get_param_files(
rcl_allocator_t allocator,
char *** parameter_files);
+/// Return all parameter overrides specified on the command line.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] arguments An arguments structure that has been parsed.
+ * \param[out] parameter_overrides Zero or more parameter overrides.
+ * This structure must be finalized by the caller.
+ * \return `RCL_RET_OK` if everything goes correctly, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any function arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_arguments_get_param_overrides(
+ const rcl_arguments_t * arguments,
+ rcl_params_t ** parameter_overrides);
+
/// Return a list of arguments with ROS-specific arguments removed.
/**
* Some arguments may not have been intended as ROS arguments.
diff --git a/rcl/package.xml b/rcl/package.xml
index 041bde4fd..36f36eab2 100644
--- a/rcl/package.xml
+++ b/rcl/package.xml
@@ -13,23 +13,13 @@
rmw
- rcl_interfaces
- rcutils
- rosidl_generator_c
- tinydir_vendor
-
- rcl_interfaces
- rcutils
- rosidl_generator_c
- tinydir_vendor
-
- rcl_interfaces
- ament_cmake
- rcutils
- rosidl_default_runtime
-
+ rcl_interfaces
rcl_logging_noop
+ rcl_yaml_param_parser
+ rcutils
rmw_implementation
+ rosidl_generator_c
+ tinydir_vendor
ament_cmake_gtest
ament_cmake_pytest
diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c
index df6083a32..b66870892 100644
--- a/rcl/src/rcl/arguments.c
+++ b/rcl/src/rcl/arguments.c
@@ -16,6 +16,7 @@
#include "rcl/arguments.h"
+#include
#include
#include "./arguments_impl.h"
@@ -23,6 +24,8 @@
#include "rcl/error_handling.h"
#include "rcl/lexer_lookahead.h"
#include "rcl/validate_topic_name.h"
+#include "rcl_yaml_param_parser/parser.h"
+#include "rcl_yaml_param_parser/types.h"
#include "rcutils/allocator.h"
#include "rcutils/error_handling.h"
#include "rcutils/format_string.h"
@@ -72,6 +75,11 @@ _rcl_parse_param_file_rule(
rcl_allocator_t allocator,
char ** param_file);
+rcl_ret_t
+_rcl_parse_param_rule(
+ const char * arg,
+ rcl_params_t * params);
+
rcl_ret_t
rcl_arguments_get_param_files(
const rcl_arguments_t * arguments,
@@ -115,6 +123,25 @@ rcl_arguments_get_param_files_count(
return args->impl->num_param_files_args;
}
+rcl_ret_t
+rcl_arguments_get_param_overrides(
+ const rcl_arguments_t * arguments,
+ rcl_params_t ** parameter_overrides)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(arguments, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(arguments->impl, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(parameter_overrides, RCL_RET_INVALID_ARGUMENT);
+ if (NULL != *parameter_overrides) {
+ RCL_SET_ERROR_MSG("Output parameter override pointer is not null. May leak memory.");
+ return RCL_RET_INVALID_ARGUMENT;
+ }
+ *parameter_overrides = rcl_yaml_node_struct_copy(arguments->impl->parameter_overrides);
+ if (NULL == *parameter_overrides) {
+ return RCL_RET_BAD_ALLOC;
+ }
+ return RCL_RET_OK;
+}
+
/// Parse an argument that may or may not be a log level rule.
/**
* \param[in] arg the argument to parse
@@ -219,6 +246,7 @@ rcl_parse_arguments(
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;
@@ -237,6 +265,13 @@ rcl_parse_arguments(
ret = RCL_RET_BAD_ALLOC;
goto fail;
}
+
+ args_impl->parameter_overrides = rcl_yaml_node_struct_init(allocator);
+ if (NULL == args_impl->parameter_overrides) {
+ ret = RCL_RET_BAD_ALLOC;
+ goto fail;
+ }
+
args_impl->parameter_files = allocator.allocate(sizeof(char *) * argc, allocator.state);
if (NULL == args_impl->parameter_files) {
ret = RCL_RET_BAD_ALLOC;
@@ -267,6 +302,50 @@ rcl_parse_arguments(
continue;
}
+ // Attempt to parse argument as parameter override flag
+ if (strcmp(RCL_PARAM_FLAG, argv[i]) == 0 || strcmp(RCL_SHORT_PARAM_FLAG, argv[i]) == 0) {
+ if (i + 1 < argc) {
+ // Attempt to parse next argument as parameter override rule
+ if (RCL_RET_OK == _rcl_parse_param_rule(argv[i + 1], args_impl->parameter_overrides)) {
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "param override rule : %s\n", argv[i + 1]);
+ i += 1; // Skip flag here, for loop will skip rule.
+ continue;
+ } else {
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME,
+ "Couldn't parse arg %d (%s) as parameter override rule. Error: %s", i + 1,
+ argv[i + 1], rcl_get_error_string().str);
+ }
+ } else {
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME,
+ "Couldn't parse arg %d (%s) as parameter override flag. No rule found.\n",
+ i, argv[i]);
+ }
+ rcl_reset_error();
+ }
+
+ // Attempt to parse argument as remap rule flag
+ if (strcmp(RCL_REMAP_FLAG, argv[i]) == 0 || strcmp(RCL_SHORT_REMAP_FLAG, argv[i]) == 0) {
+ if (i + 1 < argc) {
+ // Attempt to parse next argument as remap rule
+ rcl_remap_t * rule = &(args_impl->remap_rules[args_impl->num_remap_rules]);
+ *rule = rcl_get_zero_initialized_remap();
+ if (RCL_RET_OK == _rcl_parse_remap_rule(argv[i + 1], allocator, rule)) {
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "remap rule : %s\n", argv[i + 1]);
+ ++(args_impl->num_remap_rules);
+ i += 1; // Skip flag here, for loop will skip rule.
+ continue;
+ }
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME,
+ "Couldn't parse arg %d (%s) as remap rule. Error: %s", i + 1, argv[i + 1],
+ rcl_get_error_string().str);
+ rcl_reset_error();
+ } else {
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME,
+ "Couldn't parse arg %d (%s) as remap rule flag. No rule found.\n",
+ i, argv[i]);
+ }
+ }
+
// Attempt to parse argument as parameter file rule
args_impl->parameter_files[args_impl->num_param_files_args] = NULL;
if (
@@ -585,6 +664,7 @@ rcl_arguments_copy(
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;
@@ -644,6 +724,12 @@ rcl_arguments_copy(
}
}
+ // Copy parameter rules
+ if (args->impl->parameter_overrides) {
+ args_out->impl->parameter_overrides =
+ rcl_yaml_node_struct_copy(args->impl->parameter_overrides);
+ }
+
// Copy parameter files
if (args->impl->num_param_files_args) {
args_out->impl->parameter_files = allocator.allocate(
@@ -699,6 +785,11 @@ rcl_arguments_fini(
args->impl->num_unparsed_ros_args = 0;
args->impl->unparsed_ros_args = NULL;
+ if (args->impl->parameter_overrides) {
+ rcl_yaml_node_struct_fini(args->impl->parameter_overrides);
+ args->impl->parameter_overrides = NULL;
+ }
+
if (args->impl->parameter_files) {
for (int p = 0; p < args->impl->num_param_files_args; ++p) {
args->impl->allocator.deallocate(
@@ -728,6 +819,9 @@ _rcl_parse_remap_fully_qualified_namespace(
{
rcl_ret_t ret;
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+
// Must have at least one Forward slash /
ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_FORWARD_SLASH, NULL, NULL);
if (RCL_RET_WRONG_LEXEME == ret) {
@@ -761,6 +855,9 @@ _rcl_parse_remap_replacement_token(rcl_lexer_lookahead2_t * lex_lookahead)
rcl_ret_t ret;
rcl_lexeme_t lexeme;
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+
ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme);
if (RCL_RET_OK != ret) {
return ret;
@@ -795,6 +892,10 @@ _rcl_parse_remap_replacement_name(
rcl_ret_t ret;
rcl_lexeme_t lexeme;
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(NULL != rule);
+
const char * replacement_start = rcl_lexer_lookahead2_get_text(lex_lookahead);
if (NULL == replacement_start) {
RCL_SET_ERROR_MSG("failed to get start of replacement");
@@ -850,17 +951,20 @@ _rcl_parse_remap_replacement_name(
return RCL_RET_OK;
}
-/// Parse either a token or a wildcard (ex: `foobar`, or `*`, or `**`).
+/// Parse either a resource name token or a wildcard (ex: `foobar`, or `*`, or `**`).
/**
- * \sa _rcl_parse_remap_begin_remap_rule()
+ * \sa _rcl_parse_resource_match()
*/
RCL_LOCAL
rcl_ret_t
-_rcl_parse_remap_match_token(rcl_lexer_lookahead2_t * lex_lookahead)
+_rcl_parse_resource_match_token(rcl_lexer_lookahead2_t * lex_lookahead)
{
rcl_ret_t ret;
rcl_lexeme_t lexeme;
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+
ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme);
if (RCL_RET_OK != ret) {
return ret;
@@ -876,41 +980,32 @@ _rcl_parse_remap_match_token(rcl_lexer_lookahead2_t * lex_lookahead)
return RCL_RET_ERROR;
} else {
RCL_SET_ERROR_MSG("Expecting token or wildcard");
- ret = RCL_RET_INVALID_REMAP_RULE;
+ ret = RCL_RET_WRONG_LEXEME;
}
return ret;
}
-/// Parse the match side of a name remapping rule (ex: `rostopic://foo`)
+/// Parse a resource name match side of a rule (ex: `rostopic://foo`)
/**
- * \sa _rcl_parse_remap_begin_remap_rule()
+ * \sa _rcl_parse_param_rule()
+ * \sa _rcl_parse_remap_match_name()
*/
RCL_LOCAL
rcl_ret_t
-_rcl_parse_remap_match_name(
+_rcl_parse_resource_match(
rcl_lexer_lookahead2_t * lex_lookahead,
- rcl_remap_t * rule)
+ rcl_allocator_t allocator,
+ char ** resource_match)
{
rcl_ret_t ret;
rcl_lexeme_t lexeme;
- // rostopic:// rosservice://
- ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme);
- if (RCL_RET_OK != ret) {
- return ret;
- }
- if (RCL_LEXEME_URL_SERVICE == lexeme) {
- rule->impl->type = RCL_SERVICE_REMAP;
- ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL);
- } else if (RCL_LEXEME_URL_TOPIC == lexeme) {
- rule->impl->type = RCL_TOPIC_REMAP;
- ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL);
- } else {
- rule->impl->type = (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP);
- }
- if (RCL_RET_OK != ret) {
- return ret;
- }
+
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(rcutils_allocator_is_valid(&allocator));
+ assert(NULL != resource_match);
+ assert(NULL == *resource_match);
const char * match_start = rcl_lexer_lookahead2_get_text(lex_lookahead);
if (NULL == match_start) {
@@ -925,13 +1020,13 @@ _rcl_parse_remap_match_name(
}
if (RCL_LEXEME_TILDE_SLASH == lexeme || RCL_LEXEME_FORWARD_SLASH == lexeme) {
ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL);
- }
- if (RCL_RET_OK != ret) {
- return ret;
+ if (RCL_RET_OK != ret) {
+ return ret;
+ }
}
// token ( '/' token )*
- ret = _rcl_parse_remap_match_token(lex_lookahead);
+ ret = _rcl_parse_resource_match_token(lex_lookahead);
if (RCL_RET_OK != ret) {
return ret;
}
@@ -944,7 +1039,7 @@ _rcl_parse_remap_match_name(
if (RCL_RET_WRONG_LEXEME == ret) {
return RCL_RET_INVALID_REMAP_RULE;
}
- ret = _rcl_parse_remap_match_token(lex_lookahead);
+ ret = _rcl_parse_resource_match_token(lex_lookahead);
if (RCL_RET_OK != ret) {
return ret;
}
@@ -956,9 +1051,9 @@ _rcl_parse_remap_match_name(
// Copy match into rule
const char * match_end = rcl_lexer_lookahead2_get_text(lex_lookahead);
- size_t length = (size_t)(match_end - match_start);
- rule->impl->match = rcutils_strndup(match_start, length, rule->impl->allocator);
- if (NULL == rule->impl->match) {
+ const size_t length = (size_t)(match_end - match_start);
+ *resource_match = rcutils_strndup(match_start, length, allocator);
+ if (NULL == *resource_match) {
RCL_SET_ERROR_MSG("failed to copy match");
return RCL_RET_BAD_ALLOC;
}
@@ -966,6 +1061,50 @@ _rcl_parse_remap_match_name(
return RCL_RET_OK;
}
+
+/// Parse the match side of a name remapping rule (ex: `rostopic://foo`)
+/**
+ * \sa _rcl_parse_remap_begin_remap_rule()
+ */
+RCL_LOCAL
+rcl_ret_t
+_rcl_parse_remap_match_name(
+ rcl_lexer_lookahead2_t * lex_lookahead,
+ rcl_remap_t * rule)
+{
+ rcl_ret_t ret;
+ rcl_lexeme_t lexeme;
+
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(NULL != rule);
+
+ // rostopic:// rosservice://
+ ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme);
+ if (RCL_RET_OK != ret) {
+ return ret;
+ }
+ if (RCL_LEXEME_URL_SERVICE == lexeme) {
+ rule->impl->type = RCL_SERVICE_REMAP;
+ ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL);
+ } else if (RCL_LEXEME_URL_TOPIC == lexeme) {
+ rule->impl->type = RCL_TOPIC_REMAP;
+ ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL);
+ } else {
+ rule->impl->type = (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP);
+ }
+ if (RCL_RET_OK != ret) {
+ return ret;
+ }
+
+ ret = _rcl_parse_resource_match(
+ lex_lookahead, rule->impl->allocator, &rule->impl->match);
+ if (RCL_RET_WRONG_LEXEME == ret) {
+ ret = RCL_RET_INVALID_REMAP_RULE;
+ }
+ return ret;
+}
+
/// Parse a name remapping rule (ex: `rostopic:///foo:=bar`).
/**
* \sa _rcl_parse_remap_begin_remap_rule()
@@ -977,6 +1116,11 @@ _rcl_parse_remap_name_remap(
rcl_remap_t * rule)
{
rcl_ret_t ret;
+
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(NULL != rule);
+
// match
ret = _rcl_parse_remap_match_name(lex_lookahead, rule);
if (RCL_RET_OK != ret) {
@@ -1007,6 +1151,11 @@ _rcl_parse_remap_namespace_replacement(
rcl_remap_t * rule)
{
rcl_ret_t ret;
+
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(NULL != rule);
+
// __ns
ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_NS, NULL, NULL);
if (RCL_RET_WRONG_LEXEME == ret) {
@@ -1068,6 +1217,10 @@ _rcl_parse_remap_nodename_replacement(
const char * node_name;
size_t length;
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(NULL != rule);
+
// __node
ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_NODE, NULL, NULL);
if (RCL_RET_WRONG_LEXEME == ret) {
@@ -1101,32 +1254,36 @@ _rcl_parse_remap_nodename_replacement(
}
/// Parse a nodename prefix including trailing colon (ex: `node_name:`).
-/**
- * \sa _rcl_parse_remap_begin_remap_rule()
- */
RCL_LOCAL
rcl_ret_t
-_rcl_parse_remap_nodename_prefix(
+_rcl_parse_nodename_prefix(
rcl_lexer_lookahead2_t * lex_lookahead,
- rcl_remap_t * rule)
+ rcl_allocator_t allocator,
+ char ** node_name)
{
- rcl_ret_t ret;
- const char * node_name;
- size_t length;
+ size_t length = 0;
+ const char * token = NULL;
+
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(rcutils_allocator_is_valid(&allocator));
+ assert(NULL != node_name);
+ assert(NULL == *node_name);
// Expect a token and a colon
- ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_TOKEN, &node_name, &length);
- if (RCL_RET_WRONG_LEXEME == ret) {
- return RCL_RET_INVALID_REMAP_RULE;
+ rcl_ret_t ret =
+ rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_TOKEN, &token, &length);
+ if (RCL_RET_OK != ret) {
+ return ret;
}
ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_COLON, NULL, NULL);
- if (RCL_RET_WRONG_LEXEME == ret) {
- return RCL_RET_INVALID_REMAP_RULE;
+ if (RCL_RET_OK != ret) {
+ return ret;
}
- // copy the node name into the rule
- rule->impl->node_name = rcutils_strndup(node_name, length, rule->impl->allocator);
- if (NULL == rule->impl->node_name) {
+ // Copy the node name
+ *node_name = rcutils_strndup(token, length, allocator);
+ if (NULL == *node_name) {
RCL_SET_ERROR_MSG("failed to allocate node name");
return RCL_RET_BAD_ALLOC;
}
@@ -1134,6 +1291,29 @@ _rcl_parse_remap_nodename_prefix(
return RCL_RET_OK;
}
+/// Parse a nodename prefix for a remap rule.
+/**
+ * \sa _rcl_parse_nodename_prefix()
+ * \sa _rcl_parse_remap_begin_remap_rule()
+ */
+RCL_LOCAL
+rcl_ret_t
+_rcl_parse_remap_nodename_prefix(
+ rcl_lexer_lookahead2_t * lex_lookahead,
+ rcl_remap_t * rule)
+{
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(NULL != rule);
+
+ rcl_ret_t ret = _rcl_parse_nodename_prefix(
+ lex_lookahead, rule->impl->allocator, &rule->impl->node_name);
+ if (RCL_RET_WRONG_LEXEME == ret) {
+ ret = RCL_RET_INVALID_REMAP_RULE;
+ }
+ return ret;
+}
+
/// Start recursive descent parsing of a remap rule.
/**
* \param[in] lex_lookahead a lookahead(2) buffer for the parser to use.
@@ -1153,6 +1333,10 @@ _rcl_parse_remap_begin_remap_rule(
rcl_lexeme_t lexeme1;
rcl_lexeme_t lexeme2;
+ // Check arguments sanity
+ assert(NULL != lex_lookahead);
+ assert(NULL != rule);
+
// Check for optional nodename prefix
ret = rcl_lexer_lookahead2_peek2(lex_lookahead, &lexeme1, &lexeme2);
if (RCL_RET_OK != ret) {
@@ -1218,7 +1402,6 @@ _rcl_parse_log_level_rule(
return RCL_RET_INVALID_LOG_LEVEL_RULE;
}
-
rcl_ret_t
_rcl_parse_remap_rule(
const char * arg,
@@ -1263,6 +1446,81 @@ _rcl_parse_remap_rule(
return ret;
}
+rcl_ret_t
+_rcl_parse_param_rule(
+ const char * arg,
+ rcl_params_t * params)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(params, RCL_RET_INVALID_ARGUMENT);
+
+ rcl_lexer_lookahead2_t lex_lookahead = rcl_get_zero_initialized_lexer_lookahead2();
+
+ rcl_ret_t ret = rcl_lexer_lookahead2_init(&lex_lookahead, arg, params->allocator);
+ if (RCL_RET_OK != ret) {
+ return ret;
+ }
+
+ rcl_lexeme_t lexeme1;
+ rcl_lexeme_t lexeme2;
+ char * node_name = NULL;
+ char * param_name = NULL;
+
+ // Check for optional nodename prefix
+ ret = rcl_lexer_lookahead2_peek2(&lex_lookahead, &lexeme1, &lexeme2);
+ if (RCL_RET_OK != ret) {
+ goto cleanup;
+ }
+
+ if (RCL_LEXEME_TOKEN == lexeme1 && RCL_LEXEME_COLON == lexeme2) {
+ ret = _rcl_parse_nodename_prefix(&lex_lookahead, params->allocator, &node_name);
+ if (RCL_RET_OK != ret) {
+ if (RCL_RET_WRONG_LEXEME == ret) {
+ ret = RCL_RET_INVALID_PARAM_RULE;
+ }
+ goto cleanup;
+ }
+ } else {
+ node_name = rcutils_strdup("/**", params->allocator);
+ if (NULL == node_name) {
+ ret = RCL_RET_BAD_ALLOC;
+ goto cleanup;
+ }
+ }
+
+ ret = _rcl_parse_resource_match(&lex_lookahead, params->allocator, ¶m_name);
+ if (RCL_RET_OK != ret) {
+ if (RCL_RET_WRONG_LEXEME == ret) {
+ ret = RCL_RET_INVALID_PARAM_RULE;
+ }
+ goto cleanup;
+ }
+
+ ret = rcl_lexer_lookahead2_expect(&lex_lookahead, RCL_LEXEME_SEPARATOR, NULL, NULL);
+ if (RCL_RET_WRONG_LEXEME == ret) {
+ ret = RCL_RET_INVALID_PARAM_RULE;
+ goto cleanup;
+ }
+
+ const char * yaml_value = rcl_lexer_lookahead2_get_text(&lex_lookahead);
+ if (!rcl_parse_yaml_value(node_name, param_name, yaml_value, params)) {
+ ret = RCL_RET_INVALID_PARAM_RULE;
+ goto cleanup;
+ }
+
+cleanup:
+ params->allocator.deallocate(param_name, params->allocator.state);
+ params->allocator.deallocate(node_name, params->allocator.state);
+ if (RCL_RET_OK != ret) {
+ if (RCL_RET_OK != rcl_lexer_lookahead2_fini(&lex_lookahead)) {
+ RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini lookahead2 after error occurred");
+ }
+ } else {
+ ret = rcl_lexer_lookahead2_fini(&lex_lookahead);
+ }
+ return ret;
+}
+
rcl_ret_t
_rcl_parse_param_file_rule(
const char * arg,
diff --git a/rcl/src/rcl/arguments_impl.h b/rcl/src/rcl/arguments_impl.h
index a1493ef21..1aa804d29 100644
--- a/rcl/src/rcl/arguments_impl.h
+++ b/rcl/src/rcl/arguments_impl.h
@@ -16,6 +16,7 @@
#define RCL__ARGUMENTS_IMPL_H_
#include "rcl/arguments.h"
+#include "rcl_yaml_param_parser/types.h"
#include "./remap_impl.h"
#ifdef __cplusplus
@@ -36,7 +37,8 @@ typedef struct rcl_arguments_impl_t
/// Length of unparsed_args.
int num_unparsed_args;
- // TODO(mikaelarguedas) consider storing CLI parameter rules here
+ /// Parameter override rules parsed from arguments.
+ rcl_params_t * parameter_overrides;
/// Array of yaml parameter file paths
char ** parameter_files;
/// Length of parameter_files.
diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt
index 9aa5883b7..58468e60e 100644
--- a/rcl/test/CMakeLists.txt
+++ b/rcl/test/CMakeLists.txt
@@ -147,7 +147,8 @@ function(test_target_function)
SRCS rcl/test_arguments.cpp
ENV ${rmw_implementation_env_var}
APPEND_LIBRARY_DIRS ${extra_lib_dirs}
- LIBRARIES ${PROJECT_NAME}
+ LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools
+ AMENT_DEPENDENCIES "osrf_testing_tools_cpp"
)
rcl_add_custom_gtest(test_remap${target_suffix}
diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp
index c36843b23..51236d029 100644
--- a/rcl/test/rcl/test_arguments.cpp
+++ b/rcl/test/rcl/test_arguments.cpp
@@ -14,12 +14,16 @@
#include
#include
+#include
+
+#include "osrf_testing_tools_cpp/scope_exit.hpp"
#include "rcl/rcl.h"
#include "rcl/arguments.h"
-
#include "rcl/error_handling.h"
+#include "rcl_yaml_param_parser/parser.h"
+
#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
@@ -100,11 +104,11 @@ class CLASSNAME (TestArgumentsFixture, RMW_IMPLEMENTATION) : public ::testing::T
} while (0)
bool
-is_valid_ros_arg(const char * arg)
+are_valid_ros_args(int argc, std::vector argv)
{
- const char * argv[] = {"--ros-args", arg};
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
- rcl_ret_t ret = rcl_parse_arguments(2, argv, rcl_get_default_allocator(), &parsed_args);
+ rcl_ret_t ret = rcl_parse_arguments(
+ argc, argv.data(), rcl_get_default_allocator(), &parsed_args);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
bool is_valid = (
0 == rcl_arguments_get_count_unparsed(&parsed_args) &&
@@ -114,59 +118,73 @@ is_valid_ros_arg(const char * arg)
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_invalid_args) {
- EXPECT_TRUE(is_valid_ros_arg("__node:=node_name"));
- EXPECT_TRUE(is_valid_ros_arg("old_name:__node:=node_name"));
- EXPECT_TRUE(is_valid_ros_arg("old_name:__node:=nodename123"));
- EXPECT_TRUE(is_valid_ros_arg("__node:=nodename123"));
- EXPECT_TRUE(is_valid_ros_arg("__ns:=/foo/bar"));
- EXPECT_TRUE(is_valid_ros_arg("__ns:=/"));
- EXPECT_TRUE(is_valid_ros_arg("_:=kq"));
- EXPECT_TRUE(is_valid_ros_arg("nodename:__ns:=/foobar"));
- EXPECT_TRUE(is_valid_ros_arg("foo:=bar"));
- EXPECT_TRUE(is_valid_ros_arg("~/foo:=~/bar"));
- EXPECT_TRUE(is_valid_ros_arg("/foo/bar:=bar"));
- EXPECT_TRUE(is_valid_ros_arg("foo:=/bar"));
- EXPECT_TRUE(is_valid_ros_arg("/foo123:=/bar123"));
- EXPECT_TRUE(is_valid_ros_arg("node:/foo123:=/bar123"));
- EXPECT_TRUE(is_valid_ros_arg("rostopic:=/foo/bar"));
- EXPECT_TRUE(is_valid_ros_arg("rosservice:=baz"));
- EXPECT_TRUE(is_valid_ros_arg("rostopic://rostopic:=rosservice"));
- EXPECT_TRUE(is_valid_ros_arg("rostopic:///rosservice:=rostopic"));
- EXPECT_TRUE(is_valid_ros_arg("rostopic:///foo/bar:=baz"));
- EXPECT_TRUE(is_valid_ros_arg("__params:=file_name.yaml"));
-
- EXPECT_FALSE(is_valid_ros_arg(":="));
- EXPECT_FALSE(is_valid_ros_arg("foo:="));
- EXPECT_FALSE(is_valid_ros_arg(":=bar"));
- EXPECT_FALSE(is_valid_ros_arg("__ns:="));
- EXPECT_FALSE(is_valid_ros_arg("__node:="));
- EXPECT_FALSE(is_valid_ros_arg("__node:=/foo/bar"));
- EXPECT_FALSE(is_valid_ros_arg("__ns:=foo"));
- EXPECT_FALSE(is_valid_ros_arg(":__node:=nodename"));
- EXPECT_FALSE(is_valid_ros_arg("~:__node:=nodename"));
- EXPECT_FALSE(is_valid_ros_arg("}foo:=/bar"));
- EXPECT_FALSE(is_valid_ros_arg("f oo:=/bar"));
- EXPECT_FALSE(is_valid_ros_arg("foo:=/b ar"));
- EXPECT_FALSE(is_valid_ros_arg("f{oo:=/bar"));
- EXPECT_FALSE(is_valid_ros_arg("foo:=/b}ar"));
- EXPECT_FALSE(is_valid_ros_arg("rostopic://:=rosservice"));
- EXPECT_FALSE(is_valid_ros_arg("rostopic::=rosservice"));
- EXPECT_FALSE(is_valid_ros_arg("__param:=file_name.yaml"));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "__node:=node_name"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "old_name:__node:=node_name"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "old_name:__node:=nodename123"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "__node:=nodename123"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "__ns:=/foo/bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "__ns:=/"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "_:=kq"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "nodename:__ns:=/foobar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "foo:=bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "~/foo:=~/bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "/foo/bar:=bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "foo:=/bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "/foo123:=/bar123"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "node:/foo123:=/bar123"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "rostopic:=/foo/bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "rosservice:=baz"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "rostopic://rostopic:=rosservice"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "rostopic:///rosservice:=rostopic"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-r", "rostopic:///foo/bar:=baz"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-p", "foo:=bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-p", "~/foo:=~/bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-p", "/foo/bar:=bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-p", "foo:=/bar"}));
+ EXPECT_TRUE(are_valid_ros_args(3, {"--ros-args", "-p", "/foo123:=/bar123"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__params:=file_name.yaml"}));
+
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", ":="}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "foo:="}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", ":=bar"}));
+
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-p", ":="}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-p", "foo:="}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-p", ":=bar"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "__ns:="}));
+
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "__node:="}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "__node:=/foo/bar"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "__ns:=foo"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", ":__node:=nodename"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "~:__node:=nodename"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "}foo:=/bar"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "f oo:=/bar"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "foo:=/b ar"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "f{oo:=/bar"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "foo:=/b}ar"}));
+
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-p", "}foo:=/bar"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-p", "f oo:=/bar"}));
+
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "rostopic://:=rosservice"}));
+ EXPECT_FALSE(are_valid_ros_args(3, {"--ros-args", "-r", "rostopic::=rosservice"}));
+ EXPECT_FALSE(are_valid_ros_args(2, {"--ros-args", "__param:=file_name.yaml"}));
// Setting logger level
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=UNSET"));
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=DEBUG"));
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=INFO"));
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=WARN"));
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=ERROR"));
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=FATAL"));
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=debug"));
- EXPECT_TRUE(is_valid_ros_arg("__log_level:=Info"));
-
- EXPECT_FALSE(is_valid_ros_arg("__log:=foo"));
- EXPECT_FALSE(is_valid_ros_arg("__loglevel:=foo"));
- EXPECT_FALSE(is_valid_ros_arg("__log_level:="));
- EXPECT_FALSE(is_valid_ros_arg("__log_level:=foo"));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=UNSET"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=DEBUG"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=INFO"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=WARN"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=ERROR"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=FATAL"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=debug"}));
+ EXPECT_TRUE(are_valid_ros_args(2, {"--ros-args", "__log_level:=Info"}));
+
+ EXPECT_FALSE(are_valid_ros_args(2, {"--ros-args", "__log:=foo"}));
+ EXPECT_FALSE(are_valid_ros_args(2, {"--ros-args", "__loglevel:=foo"}));
+ EXPECT_FALSE(are_valid_ros_args(2, {"--ros-args", "__log_level:="}));
+ EXPECT_FALSE(are_valid_ros_args(2, {"--ros-args", "__log_level:=foo"}));
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_args) {
@@ -227,8 +245,10 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_zero_ros_args_w
EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
}
-TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap) {
- const char * argv[] = {"process_name", "--ros-args", "/foo/bar:=/fiz/buz"};
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remap) {
+ const char * argv[] = {
+ "process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--remap", "foo:=/baz"
+ };
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
rcl_ret_t ret;
@@ -240,7 +260,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap) {
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_two_ros_args) {
- const char * argv[] = {"process_name", "--ros-args", "--ros-args", "/foo/bar:=/fiz/buz"};
+ const char * argv[] = {"process_name", "--ros-args", "--ros-args", "-r", "/foo/bar:=/fiz/buz"};
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
rcl_ret_t ret;
@@ -252,7 +272,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_two_r
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_trailing_dashes) {
- const char * argv[] = {"process_name", "--ros-args", "/foo/bar:=/fiz/buz", "--"};
+ const char * argv[] = {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--"};
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
rcl_ret_t ret;
@@ -264,34 +284,35 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_tra
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_two_trailing_dashes) {
- const char * argv[] = {"process_name", "--ros-args", "/foo/bar:=/fiz/buz", "--", "--"};
+ const char * argv[] = {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--", "--"};
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
rcl_ret_t ret;
ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- EXPECT_UNPARSED(parsed_args, 0, 4);
+ EXPECT_UNPARSED(parsed_args, 0, 5);
EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args));
EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_mix_valid_invalid_rules) {
const char * argv[] = {
- "process_name", "--ros-args", "/foo/bar:=", "bar:=/fiz/buz", "}bar:=fiz", "--", "arg"
+ "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "}bar:=fiz", "--", "arg"
};
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
rcl_ret_t ret;
ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- EXPECT_UNPARSED(parsed_args, 0, 6);
- EXPECT_UNPARSED_ROS(parsed_args, 2, 4);
+ EXPECT_UNPARSED(parsed_args, 0, 7);
+ EXPECT_UNPARSED_ROS(parsed_args, 2, 5);
EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy) {
const char * argv[] = {
- "process_name", "--ros-args", "/foo/bar:=", "bar:=/fiz/buz", "__ns:=/foo", "--", "arg"
+ "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "-r", "__ns:=/foo", "--",
+ "arg"
};
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
@@ -304,11 +325,11 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy) {
ret = rcl_arguments_copy(&parsed_args, &copied_args);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- EXPECT_UNPARSED(parsed_args, 0, 6);
+ EXPECT_UNPARSED(parsed_args, 0, 8);
EXPECT_UNPARSED_ROS(parsed_args, 2);
EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
- EXPECT_UNPARSED(copied_args, 0, 6);
+ EXPECT_UNPARSED(copied_args, 0, 8);
EXPECT_UNPARSED_ROS(copied_args, 2);
EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_args));
}
@@ -368,7 +389,9 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_no_args) {
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_two_namespace) {
- const char * argv[] = {"process_name", "--ros-args", "__ns:=/foo/bar", "__ns:=/fiz/buz"};
+ const char * argv[] = {
+ "process_name", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz"
+ };
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
rcl_ret_t ret;
@@ -391,7 +414,9 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_uninitialized_p
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_double_parse) {
- const char * argv[] = {"process_name", "--ros-args", "__ns:=/foo/bar", "__ns:=/fiz/buz"};
+ const char * argv[] = {
+ "process_name", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz"
+ };
int argc = sizeof(argv) / sizeof(const char *);
rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
ASSERT_EQ(RCL_RET_OK,
@@ -426,8 +451,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_twice) {
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args) {
const char * argv[] = {
- "process_name", "-d", "--ros-args", "__ns:=/foo/bar", "__ns:=/fiz/buz", "--",
- "--foo=bar", "--baz", "--ros-args", "--ros-args", "bar:=baz", "--", "--", "arg"
+ "process_name", "-d", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz", "--",
+ "--foo=bar", "--baz", "--ros-args", "--ros-args", "-p", "bar:=baz", "--", "--", "arg"
};
int argc = sizeof(argv) / sizeof(const char *);
@@ -487,7 +512,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args
}
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_zero) {
- const char * argv[] = {"process_name", "--ros-args", "__ns:=/namespace", "random:=arg"};
+ const char * argv[] = {"process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg"};
int argc = sizeof(argv) / sizeof(const char *);
rcl_ret_t ret;
@@ -504,7 +529,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_single) {
const char * argv[] = {
- "process_name", "--ros-args", "__ns:=/namespace", "random:=arg", "__params:=parameter_filepath"
+ "process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg",
+ "__params:=parameter_filepath"
};
int argc = sizeof(argv) / sizeof(const char *);
rcl_ret_t ret;
@@ -531,7 +557,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_
TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_multiple) {
const char * argv[] = {
- "process_name", "--ros-args", "__params:=parameter_filepath1", "__ns:=/namespace",
+ "process_name", "--ros-args", "__params:=parameter_filepath1", "-r", "__ns:=/namespace",
"random:=arg", "__params:=parameter_filepath2"
};
int argc = sizeof(argv) / sizeof(const char *);
@@ -556,3 +582,74 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_
alloc.deallocate(parameter_files, alloc.state);
EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_param_overrides) {
+ const char * argv[] = {"process_name"};
+ int argc = sizeof(argv) / sizeof(const char *);
+
+ rcl_allocator_t alloc = rcl_get_default_allocator();
+ rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+
+ rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ ret = rcl_arguments_get_param_overrides(&parsed_args, NULL);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ rcl_params_t * params = NULL;
+ ret = rcl_arguments_get_param_overrides(NULL, ¶ms);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ rcl_params_t preallocated_params;
+ params = &preallocated_params;
+ ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ params = NULL;
+ ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ EXPECT_EQ(0U, params->num_nodes);
+ rcl_yaml_node_struct_fini(params);
+
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_two_param_overrides) {
+ const char * argv[] = {
+ "process_name", "--ros-args",
+ "--param", "string_param:=test_string",
+ "-p", "some_node:int_param:=4"
+ };
+ int argc = sizeof(argv) / sizeof(const char *);
+
+ rcl_allocator_t alloc = rcl_get_default_allocator();
+ rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+
+ rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+ });
+
+ rcl_params_t * params = NULL;
+ ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+ rcl_yaml_node_struct_fini(params);
+ });
+ EXPECT_EQ(2U, params->num_nodes);
+
+ rcl_variant_t * param_value =
+ rcl_yaml_node_struct_get("/**", "string_param", params);
+ ASSERT_TRUE(NULL != param_value);
+ ASSERT_TRUE(NULL != param_value->string_value);
+ EXPECT_STREQ("test_string", param_value->string_value);
+
+ param_value = rcl_yaml_node_struct_get("some_node", "int_param", params);
+ ASSERT_TRUE(NULL != param_value);
+ ASSERT_TRUE(NULL != param_value->integer_value);
+ EXPECT_EQ(4, *(param_value->integer_value));
+}
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 628d16123..24e331e0d 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
@@ -32,6 +32,13 @@ RCL_YAML_PARAM_PARSER_PUBLIC
rcl_params_t * rcl_yaml_node_struct_init(
const rcutils_allocator_t allocator);
+/// \brief Copy parameter structure
+/// \param[in] params_st points to the parameter struct to be copied
+/// \return a pointer to the copied param structure on success or NULL on failure
+RCL_YAML_PARAM_PARSER_PUBLIC
+rcl_params_t * rcl_yaml_node_struct_copy(
+ const rcl_params_t * params_st);
+
/// \brief Free parameter structure
/// \param[in] params_st points to the populated parameter struct
RCL_YAML_PARAM_PARSER_PUBLIC
diff --git a/rcl_yaml_param_parser/src/parser.c b/rcl_yaml_param_parser/src/parser.c
index 54674a5d6..4fdbe7110 100644
--- a/rcl_yaml_param_parser/src/parser.c
+++ b/rcl_yaml_param_parser/src/parser.c
@@ -393,6 +393,186 @@ rcl_params_t * rcl_yaml_node_struct_init(
return params_st;
}
+///
+/// Copy the rcl_params_t parameter structure
+///
+rcl_params_t * rcl_yaml_node_struct_copy(
+ const rcl_params_t * params_st)
+{
+ RCUTILS_CHECK_ARGUMENT_FOR_NULL(params_st, NULL);
+
+ rcutils_allocator_t allocator = params_st->allocator;
+ rcl_params_t * out_params_st = rcl_yaml_node_struct_init(allocator);
+
+ if (NULL == out_params_st) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem");
+ return NULL;
+ }
+
+ rcutils_ret_t ret;
+ for (size_t node_idx = 0U; node_idx < params_st->num_nodes; ++node_idx) {
+ out_params_st->node_names[node_idx] =
+ rcutils_strdup(params_st->node_names[node_idx], allocator);
+ if (NULL == out_params_st->node_names[node_idx]) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem");
+ goto fail;
+ }
+ out_params_st->num_nodes++;
+
+ rcl_node_params_t * node_params_st = &(params_st->params[node_idx]);
+ rcl_node_params_t * out_node_params_st = &(out_params_st->params[node_idx]);
+ ret = node_params_init(out_node_params_st, allocator);
+ if (RCUTILS_RET_OK != ret) {
+ if (RCUTILS_RET_BAD_ALLOC == ret) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem");
+ }
+ goto fail;
+ }
+ for (size_t parameter_idx = 0U; parameter_idx < node_params_st->num_params; ++parameter_idx) {
+ out_node_params_st->parameter_names[parameter_idx] =
+ rcutils_strdup(node_params_st->parameter_names[parameter_idx], allocator);
+ if (NULL == out_node_params_st->parameter_names[parameter_idx]) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem");
+ goto fail;
+ }
+ out_node_params_st->num_params++;
+
+ 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");
+ 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");
+ 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");
+ 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");
+ 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");
+ 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");
+ 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");
+ 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");
+ 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");
+ 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");
+ 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 == param_var->string_array_value) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Error allocating mem");
+ 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");
+ }
+ 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");
+ goto fail;
+ }
+ }
+ } else {
+ /// Nothing to do to keep pclint happy
+ }
+ }
+ }
+ return out_params_st;
+
+fail:
+ rcl_yaml_node_struct_fini(out_params_st);
+ return NULL;
+}
+
+
///
/// Free param structure
/// NOTE: If there is an error, would recommend just to safely exit the process instead
@@ -1574,6 +1754,10 @@ bool rcl_parse_yaml_value(
RCUTILS_CHECK_ARGUMENT_FOR_NULL(param_name, false);
RCUTILS_CHECK_ARGUMENT_FOR_NULL(yaml_value, false);
+ if (0U == strlen(node_name) || 0U == strlen(param_name) || 0U == strlen(yaml_value)) {
+ return false;
+ }
+
if (NULL == params_st) {
RCUTILS_SAFE_FWRITE_TO_STDERR("Pass an initialized parameter structure");
return false;
diff --git a/rcl_yaml_param_parser/test/test_parse_yaml.cpp b/rcl_yaml_param_parser/test/test_parse_yaml.cpp
index cee2f388b..25706a75c 100644
--- a/rcl_yaml_param_parser/test/test_parse_yaml.cpp
+++ b/rcl_yaml_param_parser/test/test_parse_yaml.cpp
@@ -48,47 +48,112 @@ TEST(test_parser, correct_syntax) {
bool res = rcl_parse_yaml_file(path, params_hdl);
ASSERT_TRUE(res) << rcutils_get_error_string().str;
- rcl_variant_t * param_value = rcl_yaml_node_struct_get("lidar_ns/lidar_1", "ports", params_hdl);
- ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->integer_array_value);
- ASSERT_EQ(3U, param_value->integer_array_value->size);
- EXPECT_EQ(2438, param_value->integer_array_value->values[0]);
- EXPECT_EQ(2439, param_value->integer_array_value->values[1]);
- EXPECT_EQ(2440, param_value->integer_array_value->values[2]);
- res = rcl_parse_yaml_value("lidar_ns/lidar_1", "ports", "[8080]", params_hdl);
- EXPECT_TRUE(res) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->integer_array_value);
- ASSERT_EQ(1U, param_value->integer_array_value->size);
- EXPECT_EQ(8080, param_value->integer_array_value->values[0]);
+ rcl_params_t * copy_of_params_hdl = rcl_yaml_node_struct_copy(params_hdl);
+ ASSERT_TRUE(NULL != copy_of_params_hdl) << rcutils_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+ rcl_yaml_node_struct_fini(copy_of_params_hdl);
+ });
- param_value = rcl_yaml_node_struct_get("lidar_ns/lidar_2", "is_back", params_hdl);
- ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->bool_value);
- EXPECT_FALSE(*param_value->bool_value);
- res = rcl_parse_yaml_value("lidar_ns/lidar_2", "is_back", "true", params_hdl);
- EXPECT_TRUE(res) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->bool_value);
- EXPECT_TRUE(*param_value->bool_value);
+ rcl_params_t * params_hdl_set[] = {params_hdl, copy_of_params_hdl};
+ for (rcl_params_t * params : params_hdl_set) {
+ rcl_variant_t * param_value = rcl_yaml_node_struct_get("lidar_ns/lidar_2", "is_back", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->bool_value);
+ EXPECT_FALSE(*param_value->bool_value);
+ res = rcl_parse_yaml_value("lidar_ns/lidar_2", "is_back", "true", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->bool_value);
+ EXPECT_TRUE(*param_value->bool_value);
- param_value = rcl_yaml_node_struct_get("camera", "cam_spec.angle", params_hdl);
- ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->double_value);
- EXPECT_DOUBLE_EQ(2.34, *param_value->double_value);
- res = rcl_parse_yaml_value("camera", "cam_spec.angle", "2.2", params_hdl);
- EXPECT_TRUE(res) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->double_value);
- EXPECT_DOUBLE_EQ(2.2, *param_value->double_value);
+ param_value = rcl_yaml_node_struct_get("lidar_ns/lidar_2", "id", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->integer_value);
+ EXPECT_EQ(11, *param_value->integer_value);
+ res = rcl_parse_yaml_value("lidar_ns/lidar_2", "id", "12", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->integer_value);
+ EXPECT_EQ(12, *param_value->integer_value);
- param_value = rcl_yaml_node_struct_get("intel", "arch", params_hdl);
- ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->string_value);
- EXPECT_STREQ("x86_64", param_value->string_value);
- res = rcl_parse_yaml_value("intel", "arch", "x86", params_hdl);
- EXPECT_TRUE(res) << rcutils_get_error_string().str;
- ASSERT_TRUE(NULL != param_value->string_value);
- EXPECT_STREQ("x86", param_value->string_value);
+ param_value = rcl_yaml_node_struct_get("camera", "cam_spec.angle", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->double_value);
+ EXPECT_DOUBLE_EQ(2.34, *param_value->double_value);
+ res = rcl_parse_yaml_value("camera", "cam_spec.angle", "2.2", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->double_value);
+ EXPECT_DOUBLE_EQ(2.2, *param_value->double_value);
- rcl_yaml_node_struct_print(params_hdl);
+ param_value = rcl_yaml_node_struct_get("intel", "arch", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->string_value);
+ EXPECT_STREQ("x86_64", param_value->string_value);
+ res = rcl_parse_yaml_value("intel", "arch", "x86", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->string_value);
+ EXPECT_STREQ("x86", param_value->string_value);
+
+ param_value = rcl_yaml_node_struct_get("new_camera_ns/new_camera1", "is_cam_on", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->bool_array_value);
+ ASSERT_EQ(6U, param_value->bool_array_value->size);
+ EXPECT_TRUE(param_value->bool_array_value->values[0]);
+ EXPECT_TRUE(param_value->bool_array_value->values[1]);
+ EXPECT_FALSE(param_value->bool_array_value->values[2]);
+ EXPECT_TRUE(param_value->bool_array_value->values[3]);
+ EXPECT_FALSE(param_value->bool_array_value->values[4]);
+ EXPECT_FALSE(param_value->bool_array_value->values[5]);
+ res = rcl_parse_yaml_value("new_camera_ns/new_camera1", "is_cam_on", "[false, true]", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->bool_array_value);
+ ASSERT_EQ(2U, param_value->bool_array_value->size);
+ EXPECT_FALSE(param_value->bool_array_value->values[0]);
+ EXPECT_TRUE(param_value->bool_array_value->values[1]);
+
+ param_value = rcl_yaml_node_struct_get("lidar_ns/lidar_1", "ports", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->integer_array_value);
+ ASSERT_EQ(3U, param_value->integer_array_value->size);
+ EXPECT_EQ(2438, param_value->integer_array_value->values[0]);
+ EXPECT_EQ(2439, param_value->integer_array_value->values[1]);
+ EXPECT_EQ(2440, param_value->integer_array_value->values[2]);
+ res = rcl_parse_yaml_value("lidar_ns/lidar_1", "ports", "[8080]", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->integer_array_value);
+ ASSERT_EQ(1U, param_value->integer_array_value->size);
+ EXPECT_EQ(8080, param_value->integer_array_value->values[0]);
+
+ param_value = rcl_yaml_node_struct_get(
+ "lidar_ns/lidar_1", "driver1.bk_sensor_specs", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->double_array_value);
+ ASSERT_EQ(4U, param_value->double_array_value->size);
+ EXPECT_DOUBLE_EQ(12.1, param_value->double_array_value->values[0]);
+ EXPECT_DOUBLE_EQ(-2.3, param_value->double_array_value->values[1]);
+ EXPECT_DOUBLE_EQ(5.2, param_value->double_array_value->values[2]);
+ EXPECT_DOUBLE_EQ(9.0, param_value->double_array_value->values[3]);
+ res = rcl_parse_yaml_value("lidar_ns/lidar_1", "driver1.bk_sensor_specs", "[1.0]", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->double_array_value);
+ ASSERT_EQ(1U, param_value->double_array_value->size);
+ EXPECT_DOUBLE_EQ(1.0, param_value->double_array_value->values[0]);
+
+ param_value = rcl_yaml_node_struct_get("camera", "cam_spec.supported_brands", params);
+ ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value->string_array_value);
+ ASSERT_EQ(3U, param_value->string_array_value->size);
+ EXPECT_STREQ("Bosch", param_value->string_array_value->data[0]);
+ EXPECT_STREQ("Novatek", param_value->string_array_value->data[1]);
+ EXPECT_STREQ("Mobius", param_value->string_array_value->data[2]);
+ res = rcl_parse_yaml_value(
+ "camera", "cam_spec.supported_brands", "[Mobius]", params);
+ EXPECT_TRUE(res) << rcutils_get_error_string().str;
+ ASSERT_TRUE(NULL != param_value);
+ ASSERT_TRUE(NULL != param_value->string_array_value);
+ ASSERT_EQ(1U, param_value->string_array_value->size);
+ EXPECT_STREQ("Mobius", param_value->string_array_value->data[0]);
+
+ rcl_yaml_node_struct_print(params);
+ }
}
TEST(test_file_parser, string_array_with_quoted_number) {