Robot Operating System——深度解析日志功能的实现

在《Robot Operating System——远程修改日志等级》中，我们学习了日志相关的功能，但是没有进行深入分析。本文将分析下列几个课题

• enable_logger_service的作用
• 不同等级日志的底层实现

enable_logger_service的作用

当我们需要Node可以被远程修改日志等级时，可以通过在构造Node时，开启enable_logger_service功能。

class LoggerServiceNode : public rclcpp::Node
{
public:
  explicit LoggerServiceNode(const std::string & node_name)
  : Node(node_name, rclcpp::NodeOptions().enable_logger_service(true))
  {

这样Node就会调用create_logger_services创建两个Service，分别用于获取和设置日志等级。

  if (options.enable_logger_service()) {
    node_logging_->create_logger_services(node_services_);
  }

创建获取日志等级的Service

在create_logger_services的底层，首先创建的是获取日志等级的Service。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp
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<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)
    {
      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<uint8_t>(ret);
        }
        response->levels.push_back(std::move(logger_level));
      }
    },
    qos_profile, callback_group);

在回调中，ROS2会通过向rcutils_logging_get_logger_level传递Node的名称来查询它对应的等级。它的底层又会调用get_severity_level方法

// https://github.com/ros2/rcutils/blob/jazzy/src/logging.c
int rcutils_logging_get_logger_leveln(const char * name, size_t name_length)
{
……
  char * short_name = rcutils_strndup(name, name_length, g_rcutils_logging_allocator);
  if (short_name == NULL) {
    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
      "Failed to allocate memory when looking up logger level for '%s'", name);
    return -1;
  }

  int severity;
  rcutils_ret_t ret = get_severity_level(short_name, &severity);

// https://github.com/ros2/rcutils/blob/jazzy/src/logging.c
static rcutils_ret_t get_severity_level(const char * name, int * severity)
{
  rcutils_ret_t ret =
    rcutils_hash_map_get(&g_rcutils_logging_severities_map, &name, severity);

最后我们追踪到，静态变量g_rcutils_logging_severities_map保存了不同Node的日志等级信息。不出意外的话，我们在设置日志等级时也会看到它的身影。

创建设置日志等级的Service

这个Service底层调用的是rcutils_logging_set_logger_level方法来设置Node的日志等级。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp
  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)
    {
      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);
}

rcutils_ret_t rcutils_logging_set_logger_level(const char * name, int level)
{
……
  rcutils_ret_t add_key_ret = add_key_to_hash_map(name, level, true);
  ……

static rcutils_ret_t add_key_to_hash_map(const char * name, int level, bool set_by_user)
{
……
  rcutils_ret_t hash_map_ret =
    rcutils_hash_map_set(&g_rcutils_logging_severities_map, &copy_name, &level);

不同等级日志的底层实现

我们追踪下RCLCPP_DEBUG、RCLCPP_INFO、RCLCPP_WARN、RCLCPP_ERROR和RCLCPP_FATAL的底层实现。

// /opt/ros/jazzy/include/rclcpp/rclcpp/logging.hpp
#define RCLCPP_DEBUG(logger, ...) \
  do { \
    static_assert( \
      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
      typename ::rclcpp::Logger>::value, \
      "First argument to logging macros must be an rclcpp::Logger"); \
 \
    RCUTILS_LOG_DEBUG_NAMED( \
      (logger).get_name(), \
      __VA_ARGS__); \
  } while (0)

#define RCLCPP_INFO(logger, ...) \
  do { \
    static_assert( \
      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
      typename ::rclcpp::Logger>::value, \
      "First argument to logging macros must be an rclcpp::Logger"); \
 \
    RCUTILS_LOG_INFO_NAMED( \
      (logger).get_name(), \
      __VA_ARGS__); \
  } while (0)

#define RCLCPP_WARN(logger, ...) \
  do { \
    static_assert( \
      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
      typename ::rclcpp::Logger>::value, \
      "First argument to logging macros must be an rclcpp::Logger"); \
 \
    RCUTILS_LOG_WARN_NAMED( \
      (logger).get_name(), \
      __VA_ARGS__); \
  } while (0)

#define RCLCPP_ERROR(logger, ...) \
  do { \
    static_assert( \
      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
      typename ::rclcpp::Logger>::value, \
      "First argument to logging macros must be an rclcpp::Logger"); \
 \
    RCUTILS_LOG_ERROR_NAMED( \
      (logger).get_name(), \
      __VA_ARGS__); \
  } while (0)

#define RCLCPP_FATAL(logger, ...) \
  do { \
    static_assert( \
      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
      typename ::rclcpp::Logger>::value, \
      "First argument to logging macros must be an rclcpp::Logger"); \
 \
    RCUTILS_LOG_FATAL_NAMED( \
      (logger).get_name(), \
      __VA_ARGS__); \
  } while (0)

它们底层调用的都是RCUTILS_LOG_COND_NAMED，只是等级不同。

// /opt/ros/jazzy/include/rcutils/rcutils/logging_macros.h
# define RCUTILS_LOG_DEBUG_NAMED(name, ...) \
  RCUTILS_LOG_COND_NAMED( \
    RCUTILS_LOG_SEVERITY_DEBUG, \
    RCUTILS_LOG_CONDITION_EMPTY, RCUTILS_LOG_CONDITION_EMPTY, name, \
    __VA_ARGS__)
    
# define RCUTILS_LOG_INFO_NAMED(name, ...) \
  RCUTILS_LOG_COND_NAMED( \
    RCUTILS_LOG_SEVERITY_INFO, \
    RCUTILS_LOG_CONDITION_EMPTY, RCUTILS_LOG_CONDITION_EMPTY, name, \
    __VA_ARGS__)

# define RCUTILS_LOG_ERROR(...) \
  RCUTILS_LOG_COND_NAMED( \
    RCUTILS_LOG_SEVERITY_ERROR, \
    RCUTILS_LOG_CONDITION_EMPTY, RCUTILS_LOG_CONDITION_EMPTY, NULL, \
    __VA_ARGS__)
    
# define RCUTILS_LOG_ERROR_NAMED(name, ...) \
  RCUTILS_LOG_COND_NAMED( \
    RCUTILS_LOG_SEVERITY_ERROR, \
    RCUTILS_LOG_CONDITION_EMPTY, RCUTILS_LOG_CONDITION_EMPTY, name, \
    __VA_ARGS__)

# define RCUTILS_LOG_FATAL_NAMED(name, ...) \
  RCUTILS_LOG_COND_NAMED( \
    RCUTILS_LOG_SEVERITY_FATAL, \
    RCUTILS_LOG_CONDITION_EMPTY, RCUTILS_LOG_CONDITION_EMPTY, name, \
    __VA_ARGS__)

这些等级的枚举值是

// /opt/ros/jazzy/include/rcutils/rcutils/logging.h
enum RCUTILS_LOG_SEVERITY
{
  RCUTILS_LOG_SEVERITY_UNSET = 0,  ///< The unset log level
  RCUTILS_LOG_SEVERITY_DEBUG = 10,  ///< The debug log level
  RCUTILS_LOG_SEVERITY_INFO = 20,  ///< The info log level
  RCUTILS_LOG_SEVERITY_WARN = 30,  ///< The warn log level
  RCUTILS_LOG_SEVERITY_ERROR = 40,  ///< The error log level
  RCUTILS_LOG_SEVERITY_FATAL = 50,  ///< The fatal log level
};

RCUTILS_LOG_COND_NAMED的逻辑是：先通过日志级别severity判断该日志是否需要输出，如果要输出则调用rcutils_log_internal进行输出。比如我们设置的日志级别时WARN，那么RCLCPP_DEBUG、RCLCPP_INFO不会输出日志。

// /opt/ros/jazzy/include/rcutils/rcutils/logging_macros.h
#define RCUTILS_LOG_COND_NAMED(severity, condition_before, condition_after, name, ...) \
  do { \
    RCUTILS_LOGGING_AUTOINIT; \
    static rcutils_log_location_t __rcutils_logging_location = {__func__, __FILE__, __LINE__}; \
    if (rcutils_logging_logger_is_enabled_for(name, severity)) { \
      condition_before \
      rcutils_log_internal(&__rcutils_logging_location, severity, name, __VA_ARGS__); \
      condition_after \
    } \
  } while (0)

我们继续追踪rcutils_logging_logger_is_enabled_for的实现

// https://github.com/ros2/rcutils/blob/jazzy/src/logging.c
bool rcutils_logging_logger_is_enabled_for(const char * name, int severity)
{
  RCUTILS_LOGGING_AUTOINIT;
  int logger_level = g_rcutils_logging_default_logger_level;
  if (name) {
    logger_level = rcutils_logging_get_logger_effective_level(name);
    if (-1 == logger_level) {
      RCUTILS_SAFE_FWRITE_TO_STDERR_WITH_FORMAT_STRING(
        "Error determining if logger '%s' is enabled for severity '%d'\n",
        name, severity);
      return false;
    }
  }
  return severity >= logger_level;
}

int rcutils_logging_get_logger_effective_level(const char * name)
{
……
  // Start by trying to find the exact name.
  int severity;
  rcutils_ret_t ret = get_severity_level(name, &severity);

static rcutils_ret_t get_severity_level(const char * name, int * severity)
{
  rcutils_ret_t ret =
    rcutils_hash_map_get(&g_rcutils_logging_severities_map, &name, severity);
  if (ret != RCUTILS_RET_OK) {
    // One possible response is RCUTILS_RET_NOT_FOUND, but the higher layers may be OK with that.
    return ret;
  }

  // See the comment in add_key_to_hash_map() on why we remove the bottom bit.
  (*severity) &= ~(0x1);

  return RCUTILS_RET_OK;
}

可以看到，兜兜转转，我们还是回到静态变量g_rcutils_logging_severities_maps上。

总结

• enable_logger_service会让Node启动两个Service，分别用于获取和设置Node的日志等级。
• 在ROS2中，会通过静态变量g_rcutils_logging_severities_mapb保存不同Node的日志等级。
• RCLCPP_DEBUG、RCLCPP_INFO、RCLCPP_WARN、RCLCPP_ERROR和RCLCPP_FATAL第一个参数要传递Node的rclcpp::Logger的原因是：ROS2需要通过它获取名称，进而判断该Node的日志等级。