From 3610b68348adbc441f130e058f8a8e38032e855a Mon Sep 17 00:00:00 2001 From: Lei Liu <64953129+llapx@users.noreply.github.com> Date: Tue, 18 Apr 2023 23:30:00 +0800 Subject: [PATCH] Add support for logging service. (#2122) * Add support for logging service. * Update to not modify interfaces and not change time_source * Use unique_ptr for NodeBuiltinExecutorImpl * Use user thread to run logger service * Update code for lifecycle_node Signed-off-by: Barry Xu Signed-off-by: Lei Liu --- .../rclcpp/node_interfaces/node_logging.hpp | 15 +- .../node_logging_interface.hpp | 8 + rclcpp/include/rclcpp/node_options.hpp | 21 ++ rclcpp/src/rclcpp/node.cpp | 6 +- .../rclcpp/node_interfaces/node_logging.cpp | 56 ++++- rclcpp/src/rclcpp/node_options.cpp | 13 ++ rclcpp/test/rclcpp/CMakeLists.txt | 7 + rclcpp/test/rclcpp/test_logger_service.cpp | 214 ++++++++++++++++++ rclcpp/test/rclcpp/test_node_options.cpp | 5 + rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- 10 files changed, 342 insertions(+), 5 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_logger_service.cpp diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp index ea91064be7..30e7495368 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp @@ -23,6 +23,9 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" + namespace rclcpp { namespace node_interfaces @@ -35,7 +38,7 @@ class NodeLogging : public NodeLoggingInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) RCLCPP_PUBLIC - explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base); + explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); RCLCPP_PUBLIC virtual @@ -49,13 +52,21 @@ class NodeLogging : public NodeLoggingInterface const char * get_logger_name() const override; + RCLCPP_PUBLIC + void + create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) override; + private: RCLCPP_DISABLE_COPY(NodeLogging) /// Handle to the NodeBaseInterface given in the constructor. - rclcpp::node_interfaces::NodeBaseInterface * node_base_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::Logger logger_; + + rclcpp::Service::SharedPtr get_loggers_service_; + rclcpp::Service::SharedPtr set_loggers_service_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp index 870a81a53b..7f9776cf36 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -19,6 +19,7 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" @@ -54,6 +55,13 @@ class NodeLoggingInterface virtual const char * get_logger_name() const = 0; + + /// create logger services + RCLCPP_PUBLIC + virtual + void + create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 47a97fd3c4..a30ce4cf11 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -50,6 +50,7 @@ class NodeOptions * - clock_type = RCL_ROS_TIME * - clock_qos = rclcpp::ClockQoS() * - use_clock_thread = true + * - enable_logger_service = false * - rosout_qos = rclcpp::RosoutQoS() * - parameter_event_qos = rclcpp::ParameterEventQoS * - with history setting and depth from rmw_qos_profile_parameter_events @@ -232,6 +233,24 @@ class NodeOptions NodeOptions & start_parameter_services(bool start_parameter_services); + /// Return the enable_logger_service flag. + RCLCPP_PUBLIC + bool + enable_logger_service() const; + + /// Set the enable_logger_service flag, return this for logger idiom. + /** + * If true, ROS services are created to allow external nodes to get + * and set logger levels of this node. + * + * If false, loggers will still be configured and set logger levels locally, + * but logger levels cannot be changed remotely . + * + */ + RCLCPP_PUBLIC + NodeOptions & + enable_logger_service(bool enable_log_service); + /// Return the start_parameter_event_publisher flag. RCLCPP_PUBLIC bool @@ -421,6 +440,8 @@ class NodeOptions bool use_clock_thread_ {true}; + bool enable_logger_service_ {false}; + rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS( rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) ); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c1fbdb1f98..53e77dd796 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -167,7 +167,7 @@ Node::Node( options.use_intra_process_comms(), options.enable_topic_statistics())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), - node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_)), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), @@ -225,6 +225,10 @@ Node::Node( node_topics_->resolve_topic_name("/parameter_events"), options.parameter_event_qos(), rclcpp::detail::PublisherQosParametersTraits{}); + + if (options.enable_logger_service()) { + node_logging_->create_logger_services(node_services_); + } } Node::Node( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp index 3c2266d403..4f7476a09e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rclcpp/node_impl.hpp" #include "rclcpp/node_interfaces/node_logging.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" using rclcpp::node_interfaces::NodeLogging; -NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base) +NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_base_(node_base) { logger_ = rclcpp::get_logger(NodeLogging::get_logger_name()); @@ -37,3 +39,55 @@ NodeLogging::get_logger_name() const { return rcl_node_get_logger_name(node_base_->get_rcl_node_handle()); } + +void NodeLogging::create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) +{ + rclcpp::ServicesQoS qos_profile; + const std::string node_name = node_base_->get_name(); + auto callback_group = node_base_->get_default_callback_group(); + + get_loggers_service_ = rclcpp::create_service( + node_base_, node_services, + node_name + "/get_logger_levels", + []( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + for (auto & name : request->names) { + rcl_interfaces::msg::LoggerLevel logger_level; + logger_level.name = name; + auto ret = rcutils_logging_get_logger_level(name.c_str()); + if (ret < 0) { + logger_level.level = 0; + } else { + logger_level.level = static_cast(ret); + } + response->levels.push_back(std::move(logger_level)); + } + }, + qos_profile, callback_group); + + set_loggers_service_ = rclcpp::create_service( + node_base_, node_services, + node_name + "/set_logger_levels", + []( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + rcl_interfaces::msg::SetLoggerLevelsResult result; + for (auto & level : request->levels) { + auto ret = rcutils_logging_set_logger_level(level.name.c_str(), level.level); + if (ret != RCUTILS_RET_OK) { + result.successful = false; + result.reason = rcutils_get_error_string().str; + } else { + result.successful = true; + } + response->results.push_back(std::move(result)); + } + }, + qos_profile, callback_group); +} diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index b90a4b27e7..ada87ce5fe 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -248,6 +248,19 @@ NodeOptions::start_parameter_services(bool start_parameter_services) return *this; } +bool +NodeOptions::enable_logger_service() const +{ + return this->enable_logger_service_; +} + +NodeOptions & +NodeOptions::enable_logger_service(bool enable_logger_service) +{ + this->enable_logger_service_ = enable_logger_service; + return *this; +} + bool NodeOptions::start_parameter_event_publisher() const { diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 449250ed7a..dcb1d36d8e 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -649,6 +649,13 @@ if(TARGET test_wait_for_message) target_link_libraries(test_wait_for_message ${PROJECT_NAME}) endif() +ament_add_gtest(test_logger_service test_logger_service.cpp) +if(TARGET test_logger_service) + ament_target_dependencies(test_logger_service + "rcl_interfaces") + target_link_libraries(test_logger_service ${PROJECT_NAME}) +endif() + ament_add_gtest(test_interface_traits test_interface_traits.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_interface_traits) diff --git a/rclcpp/test/rclcpp/test_logger_service.cpp b/rclcpp/test/rclcpp/test_logger_service.cpp new file mode 100644 index 0000000000..92392f82aa --- /dev/null +++ b/rclcpp/test/rclcpp/test_logger_service.cpp @@ -0,0 +1,214 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" + +using namespace std::chrono_literals; + +class TestLoggerService : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(true); + node_ = std::make_shared("test_logger_service", "/test", options); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node_; + std::thread thread_; +}; + +TEST_F(TestLoggerService, check_connect_get_logger_service) { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(2s)); +} + +TEST_F(TestLoggerService, check_connect_set_logger_service) { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(2s)); +} + +TEST_F(TestLoggerService, test_set_and_get_one_logging_level) { + std::string test_logger_name = "rcl"; + uint8_t test_logger_level = 20; + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = test_logger_name; + level.level = test_logger_level; + request->levels.push_back(level); + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), 1u); + ASSERT_TRUE(result_get->results[0].successful); + ASSERT_STREQ(result_get->results[0].reason.c_str(), ""); + } + + { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + request->names.emplace_back(test_logger_name); + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->levels.size(), 1u); + ASSERT_STREQ(result_get->levels[0].name.c_str(), test_logger_name.c_str()); + ASSERT_EQ(result_get->levels[0].level, test_logger_level); + } +} + +TEST_F(TestLoggerService, test_set_and_get_multi_logging_level) { + std::vector> test_data { + {"rcl", 30}, + {"rclcpp", 40}, + {"/test/test_logger_service", 50} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_TRUE(result_get->results[0].successful); + } + } + + // Get multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + request->names.emplace_back(std::get<0>(set_level)); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->levels.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_STREQ(result_get->levels[i].name.c_str(), std::get<0>(test_data[i]).c_str()); + ASSERT_EQ(result_get->levels[i].level, std::get<1>(test_data[i])); + } + } +} + +TEST_F(TestLoggerService, test_set_logging_level_with_invalid_param) { + std::vector> test_data { + {"rcl", 12}, + {"/test/test_logger_service", 22} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_FALSE(result_get->results[i].successful); + // Check string starts with prefix + ASSERT_EQ( + result_get->results[i].reason.rfind("Unable to determine severity_string for severity", 0), + 0); + } + } +} + +TEST_F(TestLoggerService, test_set_logging_level_with_partial_invalid_param) { + std::vector> test_data { + {"rcl", 20}, + {"rclcpp", 22}, + {"/test/test_logger_service", 30} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + ASSERT_TRUE(result_get->results[0].successful); + ASSERT_FALSE(result_get->results[1].successful); + ASSERT_TRUE(result_get->results[2].successful); + } +} diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 2468923229..49af93aa8a 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -266,6 +266,11 @@ TEST(TestNodeOptions, bool_setters_and_getters) { EXPECT_FALSE(options.automatically_declare_parameters_from_overrides()); options.automatically_declare_parameters_from_overrides(true); EXPECT_TRUE(options.automatically_declare_parameters_from_overrides()); + + options.enable_logger_service(false); + EXPECT_FALSE(options.enable_logger_service()); + options.enable_logger_service(true); + EXPECT_TRUE(options.enable_logger_service()); } TEST(TestNodeOptions, parameter_event_qos) { diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index a57a95e6be..4c0b94cb42 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -76,7 +76,7 @@ LifecycleNode::LifecycleNode( options.use_intra_process_comms(), options.enable_topic_statistics())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), - node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_)), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),