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 #if defined(__APPLE__) && defined(__GNUC__) && defined(__llvm__) && !defined(__clang__) && (__GNUC__ == 4) && (__GNUC_MINOR__ == 2)
00033 #error This code is known to provoke a compiler crash with llvm-gcc 4.2. You will have better luck with clang++. See code.ros.org/trac/ros/ticket/3626
00034 #endif
00035
00036 #include "ros/console.h"
00037 #include "ros/assert.h"
00038 #include <ros/time.h>
00039
00040 #include <boost/thread.hpp>
00041 #include <boost/shared_array.hpp>
00042 #include <boost/regex.hpp>
00043 #include <boost/make_shared.hpp>
00044
00045 #include <cstdarg>
00046 #include <cstdlib>
00047 #include <cstdio>
00048 #include <memory>
00049 #include <cstring>
00050 #include <stdexcept>
00051
00052
00053 namespace ros
00054 {
00055 namespace console
00056 {
00057 namespace impl
00058 {
00059
00060 void initialize();
00061
00062 void shutdown();
00063
00064 void register_appender(LogAppender* appender);
00065
00066 void print(void* handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line);
00067
00068 bool isEnabledFor(void* handle, ::ros::console::Level level);
00069
00070 void* getHandle(const std::string& name);
00071
00072 std::string getName(void* handle);
00073
00074 bool get_loggers(std::map<std::string, levels::Level>& loggers);
00075
00076 bool set_logger_level(const std::string& name, levels::Level level);
00077
00078 }
00079
00080
00081 bool g_initialized = false;
00082 bool g_shutting_down = false;
00083 boost::mutex g_init_mutex;
00084
00085 #ifdef ROSCONSOLE_BACKEND_LOG4CXX
00086 log4cxx::LevelPtr g_level_lookup[levels::Count] =
00087 {
00088 log4cxx::Level::getDebug(),
00089 log4cxx::Level::getInfo(),
00090 log4cxx::Level::getWarn(),
00091 log4cxx::Level::getError(),
00092 log4cxx::Level::getFatal(),
00093 };
00094 #endif
00095 std::string g_last_error_message = "Unknown Error";
00096
00097 #ifdef WIN32
00098 #define COLOR_NORMAL ""
00099 #define COLOR_RED ""
00100 #define COLOR_GREEN ""
00101 #define COLOR_YELLOW ""
00102 #else
00103 #define COLOR_NORMAL "\033[0m"
00104 #define COLOR_RED "\033[31m"
00105 #define COLOR_GREEN "\033[32m"
00106 #define COLOR_YELLOW "\033[33m"
00107 #endif
00108 const char* g_format_string = "[${severity}] [${time}]: ${message}";
00109
00110 typedef std::map<std::string, std::string> M_string;
00111 M_string g_extra_fixed_tokens;
00112
00113 void setFixedFilterToken(const std::string& key, const std::string& val)
00114 {
00115 g_extra_fixed_tokens[key] = val;
00116 }
00117
00118 struct FixedToken : public Token
00119 {
00120 FixedToken(const std::string& str)
00121 : str_(str)
00122 {}
00123
00124 virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
00125 {
00126 return str_.c_str();
00127 }
00128
00129 std::string str_;
00130 };
00131
00132 struct FixedMapToken : public Token
00133 {
00134 FixedMapToken(const std::string& str)
00135 : str_(str)
00136 {}
00137
00138 virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
00139 {
00140 M_string::iterator it = g_extra_fixed_tokens.find(str_);
00141 if (it == g_extra_fixed_tokens.end())
00142 {
00143 return ("${" + str_ + "}").c_str();
00144 }
00145
00146 return it->second.c_str();
00147 }
00148
00149 std::string str_;
00150 };
00151
00152 struct PlaceHolderToken : public Token
00153 {
00154 virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
00155 {
00156 return "PLACEHOLDER";
00157 }
00158 };
00159
00160 struct SeverityToken : public Token
00161 {
00162 virtual std::string getString(void*, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
00163 {
00164 (void)str;
00165 (void)file;
00166 (void)function;
00167 (void)line;
00168 if (level == levels::Fatal)
00169 {
00170 return "FATAL";
00171 }
00172 else if (level == levels::Error)
00173 {
00174 return "ERROR";
00175 }
00176 else if (level == levels::Warn)
00177 {
00178 return " WARN";
00179 }
00180 else if (level == levels::Info)
00181 {
00182 return " INFO";
00183 }
00184 else if (level == levels::Debug)
00185 {
00186 return "DEBUG";
00187 }
00188
00189 return "UNKNO";
00190 }
00191 };
00192
00193 struct MessageToken : public Token
00194 {
00195 virtual std::string getString(void*, ::ros::console::Level, const char* str, const char*, const char*, int)
00196 {
00197 return str;
00198 }
00199 };
00200
00201 struct TimeToken : public Token
00202 {
00203 virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
00204 {
00205 std::stringstream ss;
00206 if (ros::Time::isValid() && ros::Time::isSimTime())
00207 {
00208 ss << ros::WallTime::now() << ", " << ros::Time::now();
00209 }
00210 else
00211 {
00212 ss << ros::WallTime::now();
00213 }
00214 return ss.str();
00215 }
00216 };
00217
00218 struct ThreadToken : public Token
00219 {
00220 virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
00221 {
00222 std::stringstream ss;
00223 ss << boost::this_thread::get_id();
00224 return ss.str();
00225 }
00226 };
00227
00228 struct LoggerToken : public Token
00229 {
00230 virtual std::string getString(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
00231 {
00232 (void)level;
00233 (void)str;
00234 (void)file;
00235 (void)function;
00236 (void)line;
00237 return ::ros::console::impl::getName(logger_handle);
00238 }
00239 };
00240
00241 struct FileToken : public Token
00242 {
00243 virtual std::string getString(void*, ::ros::console::Level, const char*, const char* file, const char*, int)
00244 {
00245 return file;
00246 }
00247 };
00248
00249 struct FunctionToken : public Token
00250 {
00251 virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char* function, int)
00252 {
00253 return function;
00254 }
00255 };
00256
00257 struct LineToken : public Token
00258 {
00259 virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int line)
00260 {
00261 std::stringstream ss;
00262 ss << line;
00263 return ss.str();
00264 }
00265 };
00266
00267 TokenPtr createTokenFromType(const std::string& type)
00268 {
00269 if (type == "severity")
00270 {
00271 return TokenPtr(boost::make_shared<SeverityToken>());
00272 }
00273 else if (type == "message")
00274 {
00275 return TokenPtr(boost::make_shared<MessageToken>());
00276 }
00277 else if (type == "time")
00278 {
00279 return TokenPtr(boost::make_shared<TimeToken>());
00280 }
00281 else if (type == "thread")
00282 {
00283 return TokenPtr(boost::make_shared<ThreadToken>());
00284 }
00285 else if (type == "logger")
00286 {
00287 return TokenPtr(boost::make_shared<LoggerToken>());
00288 }
00289 else if (type == "file")
00290 {
00291 return TokenPtr(boost::make_shared<FileToken>());
00292 }
00293 else if (type == "line")
00294 {
00295 return TokenPtr(boost::make_shared<LineToken>());
00296 }
00297 else if (type == "function")
00298 {
00299 return TokenPtr(boost::make_shared<FunctionToken>());
00300 }
00301
00302 return TokenPtr(boost::make_shared<FixedMapToken>(type));
00303 }
00304
00305 void Formatter::init(const char* fmt)
00306 {
00307 format_ = fmt;
00308
00309 boost::regex e("\\$\\{([a-z|A-Z]+)\\}");
00310 boost::match_results<std::string::const_iterator> results;
00311 std::string::const_iterator start, end;
00312 start = format_.begin();
00313 end = format_.end();
00314 bool matched_once = false;
00315 std::string last_suffix;
00316 while (boost::regex_search(start, end, results, e))
00317 {
00318 #if 0
00319 for (size_t i = 0; i < results.size(); ++i)
00320 {
00321 std::cout << i << "|" << results.prefix() << "|" << results[i] << "|" << results.suffix() << std::endl;
00322 }
00323 #endif
00324
00325 std::string token = results[1];
00326 last_suffix = results.suffix();
00327 tokens_.push_back(TokenPtr(boost::make_shared<FixedToken>(results.prefix())));
00328 tokens_.push_back(createTokenFromType(token));
00329
00330 start = results[0].second;
00331 matched_once = true;
00332 }
00333
00334 if (matched_once)
00335 {
00336 tokens_.push_back(TokenPtr(boost::make_shared<FixedToken>(last_suffix)));
00337 }
00338 else
00339 {
00340 tokens_.push_back(TokenPtr(boost::make_shared<FixedToken>(format_)));
00341 }
00342 }
00343
00344 void Formatter::print(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
00345 {
00346 const char* color = NULL;
00347 FILE* f = stdout;
00348
00349 if (level == levels::Fatal)
00350 {
00351 color = COLOR_RED;
00352 f = stderr;
00353 }
00354 else if (level == levels::Error)
00355 {
00356 color = COLOR_RED;
00357 f = stderr;
00358 }
00359 else if (level == levels::Warn)
00360 {
00361 color = COLOR_YELLOW;
00362 }
00363 else if (level == levels::Info)
00364 {
00365 color = COLOR_NORMAL;
00366 }
00367 else if (level == levels::Debug)
00368 {
00369 color = COLOR_GREEN;
00370 }
00371
00372 ROS_ASSERT(color != NULL);
00373
00374 std::stringstream ss;
00375 ss << color;
00376 V_Token::iterator it = tokens_.begin();
00377 V_Token::iterator end = tokens_.end();
00378 for (; it != end; ++it)
00379 {
00380 ss << (*it)->getString(logger_handle, level, str, file, function, line);
00381 }
00382 ss << COLOR_NORMAL;
00383
00384 fprintf(f, "%s\n", ss.str().c_str());
00385 }
00386
00387 Formatter g_formatter;
00388
00389
00390 void _print(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
00391 {
00392 g_formatter.print(logger_handle, level, str, file, function, line);
00393 }
00394
00395 void initialize()
00396 {
00397 boost::mutex::scoped_lock lock(g_init_mutex);
00398
00399 if (!g_initialized)
00400 {
00401
00402 char* format_string = NULL;
00403 #ifdef _MSC_VER
00404 _dupenv_s(&format_string, NULL, "ROSCONSOLE_FORMAT");
00405 #else
00406 format_string = getenv("ROSCONSOLE_FORMAT");
00407 #endif
00408 if (format_string)
00409 {
00410 g_format_string = format_string;
00411 }
00412
00413 g_formatter.init(g_format_string);
00414 backend::function_notifyLoggerLevelsChanged = notifyLoggerLevelsChanged;
00415 backend::function_print = _print;
00416
00417 ::ros::console::impl::initialize();
00418 g_initialized = true;
00419 }
00420 }
00421
00422 void vformatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, va_list args)
00423 {
00424 #ifdef _MSC_VER
00425 va_list arg_copy = args;
00426 #else
00427 va_list arg_copy;
00428 va_copy(arg_copy, args);
00429 #endif
00430 #ifdef _MSC_VER
00431 size_t total = vsnprintf_s(buffer.get(), buffer_size, buffer_size, fmt, args);
00432 #else
00433 size_t total = vsnprintf(buffer.get(), buffer_size, fmt, args);
00434 #endif
00435 if (total >= buffer_size)
00436 {
00437 buffer_size = total + 1;
00438 buffer.reset(new char[buffer_size]);
00439
00440 #ifdef _MSC_VER
00441 vsnprintf_s(buffer.get(), buffer_size, buffer_size, fmt, arg_copy);
00442 #else
00443 vsnprintf(buffer.get(), buffer_size, fmt, arg_copy);
00444 #endif
00445 }
00446 va_end(arg_copy);
00447 }
00448
00449 void formatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, ...)
00450 {
00451 va_list args;
00452 va_start(args, fmt);
00453
00454 vformatToBuffer(buffer, buffer_size, fmt, args);
00455
00456 va_end(args);
00457 }
00458
00459 std::string formatToString(const char* fmt, ...)
00460 {
00461 boost::shared_array<char> buffer;
00462 size_t size = 0;
00463
00464 va_list args;
00465 va_start(args, fmt);
00466
00467 vformatToBuffer(buffer, size, fmt, args);
00468
00469 va_end(args);
00470
00471 return std::string(buffer.get(), size);
00472 }
00473
00474 #define INITIAL_BUFFER_SIZE 4096
00475 static boost::mutex g_print_mutex;
00476 static boost::shared_array<char> g_print_buffer(new char[INITIAL_BUFFER_SIZE]);
00477 static size_t g_print_buffer_size = INITIAL_BUFFER_SIZE;
00478 static boost::thread::id g_printing_thread_id;
00479 void print(FilterBase* filter, void* logger_handle, Level level,
00480 const char* file, int line, const char* function, const char* fmt, ...)
00481 {
00482 if (g_shutting_down)
00483 return;
00484
00485 if (g_printing_thread_id == boost::this_thread::get_id())
00486 {
00487 fprintf(stderr, "Warning: recursive print statement has occurred. Throwing out recursive print.\n");
00488 return;
00489 }
00490
00491 boost::mutex::scoped_lock lock(g_print_mutex);
00492
00493 g_printing_thread_id = boost::this_thread::get_id();
00494
00495 va_list args;
00496 va_start(args, fmt);
00497
00498 vformatToBuffer(g_print_buffer, g_print_buffer_size, fmt, args);
00499
00500 va_end(args);
00501
00502 bool enabled = true;
00503
00504 if (filter)
00505 {
00506 FilterParams params;
00507 params.file = file;
00508 params.function = function;
00509 params.line = line;
00510 params.level = level;
00511 params.logger = logger_handle;
00512 params.message = g_print_buffer.get();
00513 enabled = filter->isEnabled(params);
00514 level = params.level;
00515
00516 if (!params.out_message.empty())
00517 {
00518 size_t msg_size = params.out_message.size();
00519 if (g_print_buffer_size <= msg_size)
00520 {
00521 g_print_buffer_size = msg_size + 1;
00522 g_print_buffer.reset(new char[g_print_buffer_size]);
00523 }
00524
00525 memcpy(g_print_buffer.get(), params.out_message.c_str(), msg_size + 1);
00526 }
00527 }
00528
00529 if (enabled)
00530 {
00531 if (level == levels::Error)
00532 {
00533 g_last_error_message = g_print_buffer.get();
00534 }
00535 try
00536 {
00537 ::ros::console::impl::print(logger_handle, level, g_print_buffer.get(), file, function, line);
00538 }
00539 catch (std::exception& e)
00540 {
00541 fprintf(stderr, "Caught exception while logging: [%s]\n", e.what());
00542 }
00543 }
00544
00545 g_printing_thread_id = boost::thread::id();
00546 }
00547
00548 void print(FilterBase* filter, void* logger_handle, Level level,
00549 const std::stringstream& ss, const char* file, int line, const char* function)
00550 {
00551 if (g_shutting_down)
00552 return;
00553
00554 if (g_printing_thread_id == boost::this_thread::get_id())
00555 {
00556 fprintf(stderr, "Warning: recursive print statement has occurred. Throwing out recursive print.\n");
00557 return;
00558 }
00559
00560 boost::mutex::scoped_lock lock(g_print_mutex);
00561
00562 g_printing_thread_id = boost::this_thread::get_id();
00563
00564 bool enabled = true;
00565 std::string str = ss.str();
00566
00567 if (filter)
00568 {
00569 FilterParams params;
00570 params.file = file;
00571 params.function = function;
00572 params.line = line;
00573 params.level = level;
00574 params.logger = logger_handle;
00575 params.message = g_print_buffer.get();
00576 enabled = filter->isEnabled(params);
00577 level = params.level;
00578
00579 if (!params.out_message.empty())
00580 {
00581 str = params.out_message;
00582 }
00583 }
00584
00585 if (enabled)
00586 {
00587 if (level == levels::Error)
00588 {
00589 g_last_error_message = str;
00590 }
00591 try
00592 {
00593 ::ros::console::impl::print(logger_handle, level, str.c_str(), file, function, line);
00594 }
00595 catch (std::exception& e)
00596 {
00597 fprintf(stderr, "Caught exception while logging: [%s]\n", e.what());
00598 }
00599 }
00600
00601 g_printing_thread_id = boost::thread::id();
00602 }
00603
00604 typedef std::vector<LogLocation*> V_LogLocation;
00605 V_LogLocation g_log_locations;
00606 boost::mutex g_locations_mutex;
00607 void registerLogLocation(LogLocation* loc)
00608 {
00609 boost::mutex::scoped_lock lock(g_locations_mutex);
00610
00611 g_log_locations.push_back(loc);
00612 }
00613
00614 void checkLogLocationEnabledNoLock(LogLocation* loc)
00615 {
00616 loc->logger_enabled_ = ::ros::console::impl::isEnabledFor(loc->logger_, loc->level_);
00617 }
00618
00619 void initializeLogLocation(LogLocation* loc, const std::string& name, Level level)
00620 {
00621 boost::mutex::scoped_lock lock(g_locations_mutex);
00622
00623 if (loc->initialized_)
00624 {
00625 return;
00626 }
00627
00628 loc->logger_ = ::ros::console::impl::getHandle(name);
00629 loc->level_ = level;
00630
00631 g_log_locations.push_back(loc);
00632
00633 checkLogLocationEnabledNoLock(loc);
00634
00635 loc->initialized_ = true;
00636 }
00637
00638 void setLogLocationLevel(LogLocation* loc, Level level)
00639 {
00640 boost::mutex::scoped_lock lock(g_locations_mutex);
00641 loc->level_ = level;
00642 }
00643
00644 void checkLogLocationEnabled(LogLocation* loc)
00645 {
00646 boost::mutex::scoped_lock lock(g_locations_mutex);
00647 checkLogLocationEnabledNoLock(loc);
00648 }
00649
00650 void notifyLoggerLevelsChanged()
00651 {
00652 boost::mutex::scoped_lock lock(g_locations_mutex);
00653
00654 V_LogLocation::iterator it = g_log_locations.begin();
00655 V_LogLocation::iterator end = g_log_locations.end();
00656 for ( ; it != end; ++it )
00657 {
00658 LogLocation* loc = *it;
00659 checkLogLocationEnabledNoLock(loc);
00660 }
00661 }
00662
00663 class StaticInit
00664 {
00665 public:
00666 StaticInit()
00667 {
00668 ROSCONSOLE_AUTOINIT;
00669 }
00670 };
00671 StaticInit g_static_init;
00672
00673
00674 void register_appender(LogAppender* appender)
00675 {
00676 ros::console::impl::register_appender(appender);
00677 }
00678
00679 void shutdown()
00680 {
00681 g_shutting_down = true;
00682 ros::console::impl::shutdown();
00683 }
00684
00685 bool get_loggers(std::map<std::string, levels::Level>& loggers)
00686 {
00687 return ros::console::impl::get_loggers(loggers);
00688 }
00689
00690 bool set_logger_level(const std::string& name, levels::Level level)
00691 {
00692 return ros::console::impl::set_logger_level(name, level);
00693 }
00694
00695 }
00696 }