Add support for logging service.

Signed-off-by: Lei Liu <Lei.Liu.AP@sony.com>

rclcpp/include/rclcpp/node.hpp

@@ -1579,10 +1579,10 @@ class Node : public std::enable_shared_from_this<Node>
 
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
   rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -24,6 +24,7 @@
 #include "rcl/node.h"
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/executors.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -98,6 +99,18 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this<N
     rclcpp::CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true) override;
 
+  RCLCPP_PUBLIC
+  rclcpp::CallbackGroup::SharedPtr
+  get_builtin_callback_group() override;
+
+  RCLCPP_PUBLIC
+  void
+  start_builtin_executor_thread() override;
+
+  RCLCPP_PUBLIC
+  void
+  stop_builtin_executor_thread() override;
+
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
   get_default_callback_group() override;
@@ -155,6 +168,11 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this<N
   mutable std::recursive_mutex notify_guard_condition_mutex_;
   rclcpp::GuardCondition notify_guard_condition_;
   bool notify_guard_condition_is_valid_;
+
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  rclcpp::CallbackGroup::SharedPtr executor_callback_group_ = nullptr;
+  std::promise<void> executor_promise_;
+  std::thread thread_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -112,6 +112,22 @@ class NodeBaseInterface
     rclcpp::CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true) = 0;
 
+  /// Return a callback group which is executed in internal executor.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::CallbackGroup::SharedPtr
+  get_builtin_callback_group() = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  start_builtin_executor_thread() = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  stop_builtin_executor_thread() = 0;
+
   /// Return the default callback group.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -19,9 +19,13 @@
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_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
 {
@@ -35,7 +39,10 @@ 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::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    bool enable_log_service);
 
   RCLCPP_PUBLIC
   virtual
@@ -52,10 +59,18 @@ class NodeLogging : public NodeLoggingInterface
 private:
   RCLCPP_DISABLE_COPY(NodeLogging)
 
+  RCLCPP_LOCAL
+  void
+  add_log_services();
+
   /// Handle to the NodeBaseInterface given in the constructor.
-  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
 
   rclcpp::Logger logger_;
+
+  rclcpp::Service<rcl_interfaces::srv::GetLoggerLevels>::SharedPtr get_loggers_service_;
+  rclcpp::Service<rcl_interfaces::srv::SetLoggerLevels>::SharedPtr set_loggers_service_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_options.hpp

@@ -232,6 +232,24 @@ class NodeOptions
   NodeOptions &
   start_parameter_services(bool start_parameter_services);
 
+  /// Return the enable_log_service flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_log_service() const;
+
+  /// Set the enable_log_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 work locally, but will not be accessible
+   * remotely.
+   *
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_log_service(bool enable_log_service);
+
   /// Return the start_parameter_event_publisher flag.
   RCLCPP_PUBLIC
   bool
@@ -413,6 +431,8 @@ class NodeOptions
 
   bool start_parameter_services_ {true};
 
+  bool enable_log_service_ {false};
+
   bool start_parameter_event_publisher_ {true};
 
   rcl_clock_type_t clock_type_ {RCL_ROS_TIME};

rclcpp/include/rclcpp/time_source.hpp

@@ -25,7 +25,7 @@
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
 #include "rclcpp/node.hpp"
-#include "rclcpp/executors.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 
 
@@ -138,10 +138,6 @@ class TimeSource
   RCLCPP_PUBLIC
   void set_use_clock_thread(bool use_clock_thread);
 
-  /// Check if the clock thread is joinable
-  RCLCPP_PUBLIC
-  bool clock_thread_is_joinable();
-
   /// TimeSource Destructor
   RCLCPP_PUBLIC
   ~TimeSource();

rclcpp/src/rclcpp/node.cpp

@@ -167,10 +167,14 @@ 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_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())),
+  node_logging_(new rclcpp::node_interfaces::NodeLogging(
+      node_base_,
+      node_services_,
+      options.enable_log_service()
+    )),
   node_clock_(new rclcpp::node_interfaces::NodeClock(
       node_base_,
       node_topics_,
@@ -225,17 +229,19 @@ Node::Node(
     node_topics_->resolve_topic_name("/parameter_events"),
     options.parameter_event_qos(),
     rclcpp::detail::PublisherQosParametersTraits{});
+
+  node_base_->start_builtin_executor_thread();
 }
 
 Node::Node(
   const Node & other,
   const std::string & sub_namespace)
 : node_base_(other.node_base_),
   node_graph_(other.node_graph_),
-  node_logging_(other.node_logging_),
   node_timers_(other.node_timers_),
   node_topics_(other.node_topics_),
   node_services_(other.node_services_),
+  node_logging_(other.node_logging_),
   node_clock_(other.node_clock_),
   node_parameters_(other.node_parameters_),
   node_time_source_(other.node_time_source_),
@@ -263,10 +269,13 @@ Node::Node(
             rmw_namespace_validation_result_string(validation_result),
             invalid_index);
   }
+
+  node_base_->start_builtin_executor_thread();
 }
 
 Node::~Node()
 {
+  node_base_->stop_builtin_executor_thread();
   // release sub-interfaces in an order that allows them to consult with node_base during tear-down
   node_waitables_.reset();
   node_time_source_.reset();

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -138,6 +138,11 @@ NodeBase::NodeBase(
 
   // Indicate the notify_guard_condition is now valid.
   notify_guard_condition_is_valid_ = true;
+
+  // Create internal executor thread
+  rclcpp::ExecutorOptions exec_options;
+  exec_options.context = context_;
+  executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(exec_options);
 }
 
 NodeBase::~NodeBase()
@@ -210,6 +215,45 @@ NodeBase::create_callback_group(
   return group;
 }
 
+RCLCPP_PUBLIC
+rclcpp::CallbackGroup::SharedPtr
+NodeBase::get_builtin_callback_group()
+{
+  if (!executor_callback_group_) {
+    executor_callback_group_ = create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    executor_->add_callback_group(executor_callback_group_, shared_from_this());
+  }
+  return executor_callback_group_;
+}
+
+RCLCPP_PUBLIC
+void
+NodeBase::start_builtin_executor_thread()
+{
+  if (!thread_.joinable()) {
+    executor_promise_ = std::promise<void>{};
+    thread_ = std::thread(
+      [this]() {
+        auto future = executor_promise_.get_future();
+        executor_->spin_until_future_complete(future);
+      }
+    );
+  }
+}
+
+RCLCPP_PUBLIC
+void
+NodeBase::stop_builtin_executor_thread()
+{
+  if (thread_.joinable()) {
+    executor_promise_.set_value();
+    executor_->cancel();
+    thread_.join();
+  }
+}
+
 rclcpp::CallbackGroup::SharedPtr
 NodeBase::get_default_callback_group()
 {

rclcpp/src/rclcpp/node_interfaces/node_logging.cpp

@@ -16,10 +16,17 @@
 
 using rclcpp::node_interfaces::NodeLogging;
 
-NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base)
-: node_base_(node_base)
+NodeLogging::NodeLogging(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  bool enable_log_service)
+: node_base_(node_base),
+  node_services_(node_services)
 {
   logger_ = rclcpp::get_logger(NodeLogging::get_logger_name());
+  if (enable_log_service) {
+    add_log_services();
+  }
 }
 
 NodeLogging::~NodeLogging()
@@ -37,3 +44,57 @@ NodeLogging::get_logger_name() const
 {
   return rcl_node_get_logger_name(node_base_->get_rcl_node_handle());
 }
+
+void
+NodeLogging::add_log_services(void)
+{
+  const rclcpp::QoS & qos_profile = rclcpp::ServicesQoS();
+  const std::string node_name = node_base_->get_name();
+  auto callback_group = node_base_->get_builtin_callback_group();
+
+  get_loggers_service_ = rclcpp::create_service<rcl_interfaces::srv::GetLoggerLevels>(
+    node_base_, node_services_,
+    node_name + "/get_logger_levels",
+    [](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::GetLoggerLevels::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::GetLoggerLevels::Response> response)
+    {
+      int ret = 0;
+      for (auto & n : request->names) {
+        rcl_interfaces::msg::LoggerLevel level;
+        level.name = n;
+        ret = rcutils_logging_get_logger_level(n.c_str());
+        if (ret < 0) {
+          level.level = 0;
+        } else {
+          level.level = (uint8_t)ret;
+        }
+        response->levels.push_back(std::move(level));
+      }
+    },
+    qos_profile, callback_group);
+
+  set_loggers_service_ = rclcpp::create_service<rcl_interfaces::srv::SetLoggerLevels>(
+    node_base_, node_services_,
+    node_name + "/set_logger_levels",
+    [](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::SetLoggerLevels::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::SetLoggerLevels::Response> response)
+    {
+      int ret = 0;
+      auto result = rcl_interfaces::msg::SetLoggerLevelsResult();
+      for (auto & l : request->levels) {
+        ret = rcutils_logging_set_logger_level(l.name.c_str(), l.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);
+}

rclcpp/src/rclcpp/node_options.cpp

@@ -248,6 +248,19 @@ NodeOptions::start_parameter_services(bool start_parameter_services)
   return *this;
 }
 
+bool
+NodeOptions::enable_log_service() const
+{
+  return this->enable_log_service_;
+}
+
+NodeOptions &
+NodeOptions::enable_log_service(bool enable_log_service)
+{
+  this->enable_log_service_ = enable_log_service;
+  return *this;
+}
+
 bool
 NodeOptions::start_parameter_event_publisher() const
 {