From 8cf33b3ad01774fcc8b197619a4415ae83430036 Mon Sep 17 00:00:00 2001 From: Barry Xu <43591679+Barry-Xu-2018@users.noreply.github.com> Date: Fri, 29 Nov 2019 21:11:47 +0800 Subject: [PATCH] Add flag to enable/disable rosout logging in each node individually. (#532) * Add flag to enable/disable rosout logging in each node individually. Signed-off-by: Barry Xu --- rcl/include/rcl/node_options.h | 3 + rcl/src/rcl/node.c | 9 +- rcl/src/rcl/node_options.c | 2 + rcl/test/CMakeLists.txt | 8 + rcl/test/rcl/test_logging_rosout.cpp | 256 +++++++++++++++++++++++++++ 5 files changed, 275 insertions(+), 3 deletions(-) create mode 100644 rcl/test/rcl/test_logging_rosout.cpp diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index 2512c9f..7f09411 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -57,6 +57,9 @@ typedef struct rcl_node_options_t /// Command line arguments that apply only to this node. rcl_arguments_t arguments; + + /// Flag to enable rosout for this node + bool enable_rosout; } rcl_node_options_t; /// Return the default node options in a rcl_node_options_t. diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 5522c79..aae4460 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -358,7 +358,7 @@ rcl_node_init( } // The initialization for the rosout publisher requires the node to be in initialized to a point // that it can create new topic publishers - if (rcl_logging_rosout_enabled()) { + if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { ret = rcl_logging_rosout_init_publisher_for_node(node); if (ret != RCL_RET_OK) { // error message already set @@ -376,7 +376,10 @@ rcl_node_init( goto cleanup; fail: if (node->impl) { - if (rcl_logging_rosout_enabled() && node->impl->logger_name) { + if (rcl_logging_rosout_enabled() && + node->impl->options.enable_rosout && + node->impl->logger_name) + { ret = rcl_logging_rosout_fini_publisher_for_node(node); RCUTILS_LOG_ERROR_EXPRESSION_NAMED((ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME, "Failed to fini publisher for node: %i", ret); @@ -443,7 +446,7 @@ rcl_node_fini(rcl_node_t * node) rcl_allocator_t allocator = node->impl->options.allocator; rcl_ret_t result = RCL_RET_OK; rcl_ret_t rcl_ret = RCL_RET_OK; - if (rcl_logging_rosout_enabled()) { + if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { rcl_ret = rcl_logging_rosout_fini_publisher_for_node(node); if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) { RCL_SET_ERROR_MSG("Unable to fini publisher for node."); diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index 3d7eddc..fa78808 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -30,6 +30,7 @@ rcl_node_get_default_options() static rcl_node_options_t default_options = { .domain_id = RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, .use_global_arguments = true, + .enable_rosout = true, }; // Must set the allocator after because it is not a compile time constant. default_options.allocator = rcl_get_default_allocator(); @@ -51,6 +52,7 @@ rcl_node_options_copy( options_out->domain_id = options->domain_id; options_out->allocator = options->allocator; options_out->use_global_arguments = options->use_global_arguments; + options_out->enable_rosout = options->enable_rosout; if (NULL != options->arguments.impl) { rcl_ret_t ret = rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); return ret; diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index fe06422..401cc3b 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -206,6 +206,14 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" ) + rcl_add_custom_gtest(test_logging_rosout${target_suffix} + SRCS rcl/test_logging_rosout.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "rcl_interfaces" + ) + rcl_add_custom_gtest(test_namespace${target_suffix} SRCS test_namespace.cpp ENV ${rmw_implementation_env_var} diff --git a/rcl/test/rcl/test_logging_rosout.cpp b/rcl/test/rcl/test_logging_rosout.cpp new file mode 100644 index 0000000..7d64060 --- /dev/null +++ b/rcl/test/rcl/test_logging_rosout.cpp @@ -0,0 +1,256 @@ +// Copyright 2019 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. + +#include + +#include +#include + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/subscription.h" +#include "rcl_interfaces/msg/log.h" +#include "rcutils/logging_macros.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#define EXPAND(x) x +#define TEST_FIXTURE_P_RMW(test_fixture_name) CLASSNAME(test_fixture_name, \ + RMW_IMPLEMENTATION) +#define APPLY(macro, ...) EXPAND(macro(__VA_ARGS__)) +#define TEST_P_RMW(test_case_name, test_name) \ + APPLY(TEST_P, \ + CLASSNAME(test_case_name, RMW_IMPLEMENTATION), test_name) +#define INSTANTIATE_TEST_CASE_P_RMW(instance_name, test_case_name, ...) \ + EXPAND(APPLY(INSTANTIATE_TEST_CASE_P, instance_name, \ + CLASSNAME(test_case_name, RMW_IMPLEMENTATION), __VA_ARGS__)) + +struct TestParameters +{ + int argc; + const char ** argv; + bool enable_node_option_rosout; + bool expected_success; + std::string description; +}; + +// for ::testing::PrintToStringParamName() to show the exact test case name +std::ostream & operator<<( + std::ostream & out, + const TestParameters & params) +{ + out << params.description; + return out; +} + +class TEST_FIXTURE_P_RMW (TestLoggingRosoutFixture) + : public ::testing::TestWithParam +{ +public: + void SetUp() + { + auto param = GetParam(); + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + + ret = rcl_init(param.argc, param.argv, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // create node + rcl_node_options_t node_options = rcl_node_get_default_options(); + if (!param.enable_node_option_rosout) { + node_options.enable_rosout = param.enable_node_option_rosout; + } + const char * name = "test_rcl_node_logging_rosout"; + const char * namespace_ = "/ns"; + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + ret = rcl_node_init(this->node_ptr, name, namespace_, this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // create rosout subscription + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, Log); + const char * topic = "rosout"; + this->subscription_ptr = new rcl_subscription_t; + *this->subscription_ptr = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + this->subscription_ptr, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_subscription_fini(this->subscription_ptr, this->node_ptr); + delete this->subscription_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + +protected: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_subscription_t * subscription_ptr; +}; + +void +wait_for_subscription_to_be_ready( + rcl_subscription_t * subscription, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms, + bool & success) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + size_t iteration = 0; + do { + ++iteration; + ret = rcl_wait_set_clear(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { + success = true; + return; + } + } + } while (iteration < max_tries); + success = false; +} + +/* Testing the subscriber of topic 'rosout' whether to get event from logging or not. + */ +TEST_P_RMW(TestLoggingRosoutFixture, test_logging_rosout) { + // log + RCUTILS_LOG_INFO_NAMED(rcl_node_get_logger_name(this->node_ptr), "SOMETHING"); + + bool success; + wait_for_subscription_to_be_ready(this->subscription_ptr, this->context_ptr, 10, 100, success); + ASSERT_EQ(success, GetParam().expected_success); +} + +// +// create set of input and expected values +// +std::vector +get_parameters() +{ + std::vector parameters; + static int s_argc = 2; + static const char * s_argv_enable_rosout[] = {"--ros-args", "--enable-rosout-logs"}; + static const char * s_argv_disable_rosout[] = {"--ros-args", "--disable-rosout-logs"}; + + /* + * Test with enable(implicit) global rosout logs and enable node option of rosout. + */ + parameters.push_back({ + 0, + nullptr, + true, + true, + "test_enable_implicit_global_rosout_enable_node_option" + }); + /* + * Test with enable(implicit) global rosout logs and disable node option of rosout. + */ + parameters.push_back({ + 0, + nullptr, + false, + false, + "test_enable_implicit_global_rosout_disable_node_option" + }); + /* + * Test with enable(explicit) global rosout logs and enable node option of rosout. + */ + parameters.push_back({ + s_argc, + s_argv_enable_rosout, + true, + true, + "test_enable_explicit_global_rosout_enable_node_option" + }); + /* + * Test with enable(explicit) global rosout logs and disable node option of rosout. + */ + parameters.push_back({ + s_argc, + s_argv_enable_rosout, + false, + false, + "test_enable_explicit_global_rosout_disable_node_option" + }); + /* + * Test with disable global rosout logs and enable node option of rosout. + */ + parameters.push_back({ + s_argc, + s_argv_disable_rosout, + true, + false, + "test_disable_global_rosout_enable_node_option" + }); + /* + * Test with disable global rosout logs and disable node option of rosout. + */ + parameters.push_back({ + s_argc, + s_argv_disable_rosout, + false, + false, + "test_disable_global_rosout_disable_node_option" + }); + + return parameters; +} + +INSTANTIATE_TEST_CASE_P_RMW( + TestLoggingRosoutWithDifferentSettings, + TestLoggingRosoutFixture, + ::testing::ValuesIn(get_parameters()), + ::testing::PrintToStringParamName());