rosconsole.cpp
Go to the documentation of this 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 
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 // declare interface for rosconsole implementations
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 } // namespace impl
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     // Check for the format string environment variable
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; // dangerous?
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 } // namespace console
00696 } // namespace ros


rosconsole
Author(s): Josh Faust
autogenerated on Thu Jun 6 2019 21:09:45