大纲
- enable_logger_service的作用
- 创建获取日志等级的Service
- 创建设置日志等级的Service
- 不同等级日志的底层实现
- 总结
在《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.cppset_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, ©_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 levelRCUTILS_LOG_SEVERITY_DEBUG = 10, ///< The debug log levelRCUTILS_LOG_SEVERITY_INFO = 20, ///< The info log levelRCUTILS_LOG_SEVERITY_WARN = 30, ///< The warn log levelRCUTILS_LOG_SEVERITY_ERROR = 40, ///< The error log levelRCUTILS_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的日志等级。