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