00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef ROSCONSOLE_ROSCONSOLE_H
00033 #define ROSCONSOLE_ROSCONSOLE_H
00034
00035 #include <cstdio>
00036 #include <sstream>
00037 #include <ros/time.h>
00038
00039
00040
00041 #include <log4cxx/logger.h>
00042
00043 #ifdef __GNUC__
00044 #if __GNUC__ >= 3
00045 #define ROSCONSOLE_PRINTF_ATTRIBUTE(a, b) __attribute__ ((__format__ (__printf__, a, b)));
00046 #endif
00047 #endif
00048
00049 #ifndef ROSCONSOLE_PRINTF_ATTRIBUTE
00050 #define ROSCONSOLE_PRINTF_ATTRIBUTE(a, b)
00051 #endif
00052
00053
00054 namespace log4cxx
00055 {
00056 namespace helpers
00057 {
00058 template<typename T> class ObjectPtrT;
00059 }
00060
00061 class Level;
00062 typedef helpers::ObjectPtrT<Level> LevelPtr;
00063
00064 class Logger;
00065 typedef helpers::ObjectPtrT<Logger> LoggerPtr;
00066 }
00067
00068 namespace boost
00069 {
00070 template<typename T> class shared_array;
00071 }
00072
00073 namespace ros
00074 {
00075 namespace console
00076 {
00077 void shutdown();
00078
00079 namespace levels
00080 {
00081 enum Level
00082 {
00083 Debug,
00084 Info,
00085 Warn,
00086 Error,
00087 Fatal,
00088
00089 Count
00090 };
00091 }
00092 typedef levels::Level Level;
00093
00094 extern log4cxx::LevelPtr g_level_lookup[];
00095
00099 extern bool g_initialized;
00100
00107 void initialize();
00108
00109 class FilterBase;
00117 void print(FilterBase* filter, log4cxx::Logger* logger, Level level, const char* file, int line, const char* function, const char* fmt, ...) ROSCONSOLE_PRINTF_ATTRIBUTE(7, 8);
00118
00119 void print(FilterBase* filter, log4cxx::Logger* logger, Level level, const std::stringstream& str, const char* file, int line, const char* function);
00120
00121 struct LogLocation;
00122
00130 void registerLogLocation(LogLocation* loc);
00131
00140 void notifyLoggerLevelsChanged();
00141
00142 void setFixedFilterToken(const std::string& key, const std::string& val);
00143
00147 struct FilterParams
00148 {
00149
00150 const char* file;
00151 int line;
00152 const char* function;
00153 const char* message;
00154
00155
00156 log4cxx::LoggerPtr logger;
00157 Level level;
00158
00159
00160 std::string out_message;
00161 };
00162
00184 class FilterBase
00185 {
00186 public:
00187 virtual ~FilterBase() {}
00192 inline virtual bool isEnabled() { return true; }
00197 inline virtual bool isEnabled(FilterParams& params) { return true; }
00198 };
00199
00200 struct LogLocation;
00204 void initializeLogLocation(LogLocation* loc, const std::string& name, Level level);
00208 void setLogLocationLevel(LogLocation* loc, Level level);
00212 void checkLogLocationEnabled(LogLocation* loc);
00213
00217 struct LogLocation
00218 {
00219 bool initialized_;
00220 bool logger_enabled_;
00221 ::ros::console::Level level_;
00222 log4cxx::Logger* logger_;
00223 };
00224
00225 void vformatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, va_list args);
00226 void formatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, ...);
00227 std::string formatToString(const char* fmt, ...);
00228
00229 }
00230 }
00231
00232 #ifdef WIN32
00233 #define ROS_LIKELY(x) (x)
00234 #define ROS_UNLIKELY(x) (x)
00235 #else
00236 #define ROS_LIKELY(x) __builtin_expect((x),1)
00237 #define ROS_UNLIKELY(x) __builtin_expect((x),0)
00238 #endif
00239
00240 #if defined(MSVC)
00241 #define __ROSCONSOLE_FUNCTION__ __FUNCSIG__
00242 #elif defined(__GNUC__)
00243 #define __ROSCONSOLE_FUNCTION__ __PRETTY_FUNCTION__
00244 #else
00245 #define __ROSCONSOLE_FUNCTION__ ""
00246 #endif
00247
00248
00249 #ifdef ROS_PACKAGE_NAME
00250 #define ROSCONSOLE_PACKAGE_NAME ROS_PACKAGE_NAME
00251 #else
00252 #define ROSCONSOLE_PACKAGE_NAME "unknown_package"
00253 #endif
00254
00255 #define ROSCONSOLE_ROOT_LOGGER_NAME "ros"
00256 #define ROSCONSOLE_NAME_PREFIX ROSCONSOLE_ROOT_LOGGER_NAME "." ROSCONSOLE_PACKAGE_NAME
00257 #define ROSCONSOLE_DEFAULT_NAME ROSCONSOLE_NAME_PREFIX
00258
00259
00260 #define ROSCONSOLE_SEVERITY_DEBUG 0
00261 #define ROSCONSOLE_SEVERITY_INFO 1
00262 #define ROSCONSOLE_SEVERITY_WARN 2
00263 #define ROSCONSOLE_SEVERITY_ERROR 3
00264 #define ROSCONSOLE_SEVERITY_FATAL 4
00265 #define ROSCONSOLE_SEVERITY_NONE 5
00266
00272 #ifndef ROSCONSOLE_MIN_SEVERITY
00273 #define ROSCONSOLE_MIN_SEVERITY ROSCONSOLE_SEVERITY_DEBUG
00274 #endif
00275
00280 #define ROSCONSOLE_AUTOINIT \
00281 do \
00282 { \
00283 if (ROS_UNLIKELY(!::ros::console::g_initialized)) \
00284 { \
00285 ::ros::console::initialize(); \
00286 } \
00287 } while(0)
00288
00289 #define ROSCONSOLE_DEFINE_LOCATION(cond, level, name) \
00290 ROSCONSOLE_AUTOINIT; \
00291 static ::ros::console::LogLocation loc = {false, false, ::ros::console::levels::Count, 0}; \
00292 if (ROS_UNLIKELY(!loc.initialized_)) \
00293 { \
00294 initializeLogLocation(&loc, name, level); \
00295 } \
00296 if (ROS_UNLIKELY(loc.level_ != level)) \
00297 { \
00298 setLogLocationLevel(&loc, level); \
00299 checkLogLocationEnabled(&loc); \
00300 } \
00301 bool enabled = loc.logger_enabled_ && (cond);
00302
00303 #define ROSCONSOLE_PRINT_AT_LOCATION_WITH_FILTER(filter, ...) \
00304 ::ros::console::print(filter, loc.logger_, loc.level_, __FILE__, __LINE__, __ROSCONSOLE_FUNCTION__, __VA_ARGS__)
00305
00306 #define ROSCONSOLE_PRINT_AT_LOCATION(...) \
00307 ROSCONSOLE_PRINT_AT_LOCATION_WITH_FILTER(0, __VA_ARGS__)
00308
00309 #define ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER(filter, args) \
00310 do \
00311 { \
00312 std::stringstream ss; \
00313 ss << args; \
00314 ::ros::console::print(filter, loc.logger_, loc.level_, ss, __FILE__, __LINE__, __ROSCONSOLE_FUNCTION__); \
00315 } while (0)
00316
00317 #define ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args) \
00318 ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER(0, args)
00319
00329 #define ROS_LOG_COND(cond, level, name, ...) \
00330 do \
00331 { \
00332 ROSCONSOLE_DEFINE_LOCATION(cond, level, name); \
00333 \
00334 if (ROS_UNLIKELY(enabled)) \
00335 { \
00336 ROSCONSOLE_PRINT_AT_LOCATION(__VA_ARGS__); \
00337 } \
00338 } while(0)
00339
00349 #define ROS_LOG_STREAM_COND(cond, level, name, args) \
00350 do \
00351 { \
00352 ROSCONSOLE_DEFINE_LOCATION(cond, level, name); \
00353 if (ROS_UNLIKELY(enabled)) \
00354 { \
00355 ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args); \
00356 } \
00357 } while(0)
00358
00365 #define ROS_LOG_ONCE(level, name, ...) \
00366 do \
00367 { \
00368 ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
00369 static bool hit = false; \
00370 if (ROS_UNLIKELY(enabled) && ROS_UNLIKELY(!hit)) \
00371 { \
00372 hit = true; \
00373 ROSCONSOLE_PRINT_AT_LOCATION(__VA_ARGS__); \
00374 } \
00375 } while(0)
00376
00383 #define ROS_LOG_STREAM_ONCE(level, name, args) \
00384 do \
00385 { \
00386 ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
00387 static bool hit = false; \
00388 if (ROS_UNLIKELY(enabled) && ROS_UNLIKELY(!hit)) \
00389 { \
00390 hit = true; \
00391 ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args); \
00392 } \
00393 } while(0)
00394
00402 #define ROS_LOG_THROTTLE(rate, level, name, ...) \
00403 do \
00404 { \
00405 ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
00406 static double last_hit = 0.0; \
00407 ::ros::Time now = ::ros::Time::now(); \
00408 if (ROS_UNLIKELY(enabled) && ROS_UNLIKELY(last_hit + rate <= now.toSec())) \
00409 { \
00410 last_hit = now.toSec(); \
00411 ROSCONSOLE_PRINT_AT_LOCATION(__VA_ARGS__); \
00412 } \
00413 } while(0)
00414
00422 #define ROS_LOG_STREAM_THROTTLE(rate, level, name, args) \
00423 do \
00424 { \
00425 ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
00426 static double last_hit = 0.0; \
00427 ::ros::Time now = ::ros::Time::now(); \
00428 if (ROS_UNLIKELY(enabled) && ROS_UNLIKELY(last_hit + rate <= now.toSec())) \
00429 { \
00430 last_hit = now.toSec(); \
00431 ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args); \
00432 } \
00433 } while(0)
00434
00442 #define ROS_LOG_FILTER(filter, level, name, ...) \
00443 do \
00444 { \
00445 ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
00446 if (ROS_UNLIKELY(enabled) && (filter)->isEnabled()) \
00447 { \
00448 ROSCONSOLE_PRINT_AT_LOCATION_WITH_FILTER(filter, __VA_ARGS__); \
00449 } \
00450 } while(0)
00451
00459 #define ROS_LOG_STREAM_FILTER(filter, level, name, args) \
00460 do \
00461 { \
00462 ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
00463 if (ROS_UNLIKELY(enabled) && (filter)->isEnabled()) \
00464 { \
00465 ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER(filter, args); \
00466 } \
00467 } while(0)
00468
00475 #define ROS_LOG(level, name, ...) ROS_LOG_COND(true, level, name, __VA_ARGS__)
00476
00482 #define ROS_LOG_STREAM(level, name, args) ROS_LOG_STREAM_COND(true, level, name, args)
00483
00484 #include "rosconsole/macros_generated.h"
00485
00486 #endif // ROSCONSOLE_ROSCONSOLE_H