rosconsole: rosconsole: rosconsole.cpp Source File

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Josh Faust
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   // Have to be able to encode wchar LogStrings on windows.
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           // has to handle LogString with wchar types.
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           // has to handle LogString with wchar types.
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   // First load the default config file from ROS_ROOT/config/rosconsole.config
00414   char* ros_root_cstr = NULL;
00415 #ifdef _MSC_VER
00416   _dupenv_s(&ros_root_cstr, NULL, "ROS_ROOT");
00417 #else
00418   ros_root_cstr = getenv("ROS_ROOT");
00419 #endif
00420   if (!ros_root_cstr)
00421   {
00422     log4cxx::LoggerPtr ros_logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
00423     ros_logger->setLevel(log4cxx::Level::getInfo());
00424 
00425     log4cxx::LoggerPtr roscpp_superdebug = log4cxx::Logger::getLogger("ros.roscpp.superdebug");
00426     roscpp_superdebug->setLevel(log4cxx::Level::getWarn());
00427   }
00428   else
00429   {
00430     std::string config_file = std::string(ros_root_cstr) + "/config/rosconsole.config";
00431     log4cxx::PropertyConfigurator::configure(config_file);
00432   }
00433   char* config_file_cstr = NULL;
00434 #ifdef _MSC_VER
00435   _dupenv_s(&config_file_cstr, NULL, "ROSCONSOLE_CONFIG_FILE");
00436 #else
00437   config_file_cstr = getenv("ROSCONSOLE_CONFIG_FILE");
00438 #endif
00439   if ( config_file_cstr )
00440   {
00441     std::string config_file = config_file_cstr;
00442     
00443     log4cxx::PropertyConfigurator::configure(config_file);
00444   }
00445 
00446   // Check for the format string environment variable
00447   char* format_string = NULL;
00448 #ifdef _MSC_VER
00449   _dupenv_s(&format_string, NULL, "ROSCONSOLE_FORMAT");
00450 #else
00451   format_string =  getenv("ROSCONSOLE_FORMAT");
00452 #endif
00453   if (format_string)
00454   {
00455     g_format_string = format_string;
00456   }
00457 
00458   g_formatter.init(g_format_string);
00459 
00460   log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
00461   logger->addAppender(new ROSConsoleStdioAppender);
00462 #ifdef _MSC_VER
00463   if ( ros_root_cstr != NULL ) {
00464           free(ros_root_cstr);
00465   }
00466   if ( config_file_cstr != NULL ) {
00467           free(config_file_cstr);
00468   }
00469   if ( format_string != NULL ) {
00470           free(format_string);
00471   }
00472   // getenv implementations don't need free'ing.
00473 #endif
00474 }
00475 
00476 void initialize()
00477 {
00478   boost::mutex::scoped_lock lock(g_init_mutex);
00479 
00480   if (!g_initialized)
00481   {
00482     do_initialize();
00483     g_initialized = true;
00484   }
00485 }
00486 
00487 void vformatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, va_list args)
00488 {
00489 #ifdef _MSC_VER
00490   va_list arg_copy = args; // dangerous?
00491 #else
00492   va_list arg_copy;
00493   va_copy(arg_copy, args);
00494 #endif
00495 #ifdef _MSC_VER
00496   size_t total = vsnprintf_s(buffer.get(), buffer_size, buffer_size, fmt, args);
00497 #else
00498   size_t total = vsnprintf(buffer.get(), buffer_size, fmt, args);
00499 #endif
00500   if (total >= buffer_size)
00501   {
00502     buffer_size = total + 1;
00503     buffer.reset(new char[buffer_size]);
00504 
00505 #ifdef _MSC_VER
00506     vsnprintf_s(buffer.get(), buffer_size, buffer_size, fmt, arg_copy);
00507 #else
00508     vsnprintf(buffer.get(), buffer_size, fmt, arg_copy);
00509 #endif
00510     va_end(arg_copy);
00511   }
00512 }
00513 
00514 void formatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, ...)
00515 {
00516   va_list args;
00517   va_start(args, fmt);
00518 
00519   vformatToBuffer(buffer, buffer_size, fmt, args);
00520 
00521   va_end(args);
00522 }
00523 
00524 std::string formatToString(const char* fmt, ...)
00525 {
00526   boost::shared_array<char> buffer;
00527   size_t size = 0;
00528 
00529   va_list args;
00530   va_start(args, fmt);
00531 
00532   vformatToBuffer(buffer, size, fmt, args);
00533 
00534   va_end(args);
00535 
00536   return std::string(buffer.get(), size);
00537 }
00538 
00539 #define INITIAL_BUFFER_SIZE 4096
00540 static boost::mutex g_print_mutex;
00541 static boost::shared_array<char> g_print_buffer(new char[INITIAL_BUFFER_SIZE]);
00542 static size_t g_print_buffer_size = INITIAL_BUFFER_SIZE;
00543 static boost::thread::id g_printing_thread_id;
00544 void print(FilterBase* filter, log4cxx::Logger* logger, Level level, 
00545            const char* file, int line, const char* function, const char* fmt, ...)
00546 {
00547   if (g_shutting_down)
00548     return;
00549 
00550   if (g_printing_thread_id == boost::this_thread::get_id())
00551   {
00552     fprintf(stderr, "Warning: recursive print statement has occurred.  Throwing out recursive print.\n");
00553     return;
00554   }
00555 
00556   boost::mutex::scoped_lock lock(g_print_mutex);
00557 
00558   g_printing_thread_id = boost::this_thread::get_id();
00559 
00560   va_list args;
00561   va_start(args, fmt);
00562 
00563   vformatToBuffer(g_print_buffer, g_print_buffer_size, fmt, args);
00564 
00565   va_end(args);
00566 
00567   log4cxx::LoggerPtr logger_ptr(logger);
00568   bool enabled = true;
00569 
00570   if (filter)
00571   {
00572     FilterParams params;
00573     params.file = file;
00574     params.function = function;
00575     params.line = line;
00576     params.level = level;
00577     params.logger = logger_ptr;
00578     params.message = g_print_buffer.get();
00579     enabled = filter->isEnabled(params);
00580     logger_ptr = params.logger;
00581     level = params.level;
00582 
00583     if (!params.out_message.empty())
00584     {
00585       size_t msg_size = params.out_message.size();
00586       if (g_print_buffer_size <= msg_size)
00587       {
00588         g_print_buffer_size = msg_size + 1;
00589         g_print_buffer.reset(new char[g_print_buffer_size]);
00590       }
00591 
00592       memcpy(g_print_buffer.get(), params.out_message.c_str(), msg_size + 1);
00593     }
00594   }
00595 
00596   if (enabled)
00597   {
00598     try
00599     {
00600       logger_ptr->forcedLog(g_level_lookup[level], g_print_buffer.get(), log4cxx::spi::LocationInfo(file, function, line));
00601     }
00602     catch (std::exception& e)
00603     {
00604       fprintf(stderr, "Caught exception while logging: [%s]\n", e.what());
00605     }
00606   }
00607 
00608   g_printing_thread_id = boost::thread::id();
00609 }
00610 
00611 void print(FilterBase* filter, log4cxx::Logger* logger, Level level, 
00612            const std::stringstream& ss, const char* file, int line, const char* function)
00613 {
00614   if (g_shutting_down)
00615     return;
00616 
00617   if (g_printing_thread_id == boost::this_thread::get_id())
00618   {
00619     fprintf(stderr, "Warning: recursive print statement has occurred.  Throwing out recursive print.\n");
00620     return;
00621   }
00622 
00623   boost::mutex::scoped_lock lock(g_print_mutex);
00624 
00625   g_printing_thread_id = boost::this_thread::get_id();
00626 
00627   log4cxx::LoggerPtr logger_ptr(logger);
00628   bool enabled = true;
00629   std::string str = ss.str();
00630 
00631   if (filter)
00632   {
00633     FilterParams params;
00634     params.file = file;
00635     params.function = function;
00636     params.line = line;
00637     params.level = level;
00638     params.logger = logger_ptr;
00639     params.message = g_print_buffer.get();
00640     enabled = filter->isEnabled(params);
00641     logger_ptr = params.logger;
00642     level = params.level;
00643 
00644     if (!params.out_message.empty())
00645     {
00646       str = params.out_message;
00647     }
00648   }
00649 
00650   if (enabled)
00651   {
00652     try
00653     {
00654       logger->forcedLog(g_level_lookup[level], str, log4cxx::spi::LocationInfo(file, function, line));
00655     }
00656     catch (std::exception& e)
00657     {
00658       fprintf(stderr, "Caught exception while logging: [%s]\n", e.what());
00659     }
00660   }
00661 
00662   g_printing_thread_id = boost::thread::id();
00663 }
00664 
00665 typedef std::vector<LogLocation*> V_LogLocation;
00666 V_LogLocation g_log_locations;
00667 boost::mutex g_locations_mutex;
00668 void registerLogLocation(LogLocation* loc)
00669 {
00670   boost::mutex::scoped_lock lock(g_locations_mutex);
00671 
00672   g_log_locations.push_back(loc);
00673 }
00674 
00675 void checkLogLocationEnabledNoLock(LogLocation* loc)
00676 {
00677   loc->logger_enabled_ = loc->logger_->isEnabledFor(g_level_lookup[loc->level_]);
00678 }
00679 
00680 void initializeLogLocation(LogLocation* loc, const std::string& name, Level level)
00681 {
00682   boost::mutex::scoped_lock lock(g_locations_mutex);
00683 
00684   if (loc->initialized_)
00685   {
00686     return;
00687   }
00688 
00689   loc->logger_ = &(*log4cxx::Logger::getLogger(name));
00690   loc->level_ = level;
00691 
00692   g_log_locations.push_back(loc);
00693 
00694   checkLogLocationEnabledNoLock(loc);
00695 
00696   loc->initialized_ = true;
00697 }
00698 
00699 void setLogLocationLevel(LogLocation* loc, Level level)
00700 {
00701   boost::mutex::scoped_lock lock(g_locations_mutex);
00702   loc->level_ = level;
00703 }
00704 
00705 void checkLogLocationEnabled(LogLocation* loc)
00706 {
00707   boost::mutex::scoped_lock lock(g_locations_mutex);
00708   checkLogLocationEnabledNoLock(loc);
00709 }
00710 
00711 void notifyLoggerLevelsChanged()
00712 {
00713   boost::mutex::scoped_lock lock(g_locations_mutex);
00714 
00715   V_LogLocation::iterator it = g_log_locations.begin();
00716   V_LogLocation::iterator end = g_log_locations.end();
00717   for ( ; it != end; ++it )
00718   {
00719     LogLocation* loc = *it;
00720     checkLogLocationEnabledNoLock(loc);
00721   }
00722 }
00723 
00724 class StaticInit
00725 {
00726 public:
00727   StaticInit()
00728   {
00729     ROSCONSOLE_AUTOINIT;
00730   }
00731 };
00732 StaticInit g_static_init;
00733 
00734 
00735 void shutdown() 
00736 {
00737   g_shutting_down = true;
00738 }
00739 
00740 
00741 
00742 } // namespace console
00743 } // namespace ros