Go to the documentation of this file.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 #include <cstring>
00031 #include <cstdlib>
00032
00033 #include "ros/ros.h"
00034 #include "ros/file_log.h"
00035
00036 #ifdef WIN32
00037 #ifdef ERROR
00038
00039
00040 #undef ERROR
00041 #endif
00042 #endif
00043 #include "rosgraph_msgs/Log.h"
00044
00057 class Rosout
00058 {
00059 public:
00060 std::string log_file_name_;
00061 FILE* handle_;
00062 size_t max_file_size_;
00063 size_t current_file_size_;
00064 size_t max_backup_index_;
00065 size_t current_backup_index_;
00066 ros::NodeHandle node_;
00067 ros::Subscriber rosout_sub_;
00068 ros::Publisher agg_pub_;
00069
00070 Rosout()
00071 {
00072 init();
00073 }
00074
00075 void init()
00076 {
00077 max_file_size_ = 100*1024*1024;
00078 current_file_size_ = 0;
00079 max_backup_index_ = 10;
00080 current_backup_index_ = 0;
00081
00082 log_file_name_ = ros::file_log::getLogDirectory() + "/rosout.log";
00083 handle_ = fopen(log_file_name_.c_str(), "w");
00084
00085 if (handle_ == 0)
00086 {
00087 std::cerr << "Error opening rosout log file '" << log_file_name_.c_str() << "': " << strerror(errno);
00088 }
00089 else
00090 {
00091 std::cout << "logging to " << log_file_name_.c_str() << std::endl;
00092
00093 std::stringstream ss;
00094 ss << "\n\n" << ros::Time::now() << " Node Startup\n";
00095 int written = fprintf(handle_, "%s", ss.str().c_str());
00096 if (written < 0)
00097 {
00098 std::cerr << "Error writting to rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
00099 }
00100 else if (written > 0)
00101 {
00102 current_file_size_ += written;
00103 if (fflush(handle_))
00104 {
00105 std::cerr << "Error flushing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_));
00106 }
00107 }
00108 }
00109
00110 agg_pub_ = node_.advertise<rosgraph_msgs::Log>("/rosout_agg", 0);
00111 std::cout << "re-publishing aggregated messages to /rosout_agg" << std::endl;
00112
00113 rosout_sub_ = node_.subscribe("/rosout", 0, &Rosout::rosoutCallback, this);
00114 std::cout << "subscribed to /rosout" << std::endl;
00115 }
00116
00117 void rosoutCallback(const rosgraph_msgs::Log::ConstPtr& msg)
00118 {
00119 agg_pub_.publish(msg);
00120
00121 std::stringstream ss;
00122 ss << msg->header.stamp << " ";
00123 switch (msg->level)
00124 {
00125 case rosgraph_msgs::Log::FATAL:
00126 ss << "FATAL ";
00127 break;
00128 case rosgraph_msgs::Log::ERROR:
00129 ss << "ERROR ";
00130 break;
00131 case rosgraph_msgs::Log::WARN:
00132 ss << "WARN ";
00133 break;
00134 case rosgraph_msgs::Log::DEBUG:
00135 ss << "DEBUG ";
00136 break;
00137 case rosgraph_msgs::Log::INFO:
00138 ss << "INFO ";
00139 break;
00140 default:
00141 ss << msg->level << " ";
00142 }
00143
00144 ss << "[" << msg->file << ":" << msg->line << "(" << msg->function << ") ";
00145
00146 ss << "[topics: ";
00147 std::vector<std::string>::const_iterator it = msg->topics.begin();
00148 std::vector<std::string>::const_iterator end = msg->topics.end();
00149 for ( ; it != end; ++it )
00150 {
00151 const std::string& topic = *it;
00152
00153 if ( it != msg->topics.begin() )
00154 {
00155 ss << ", ";
00156 }
00157
00158 ss << topic;
00159 }
00160 ss << "] ";
00161
00162 ss << msg->msg;
00163 ss << "\n";
00164 int written = fprintf(handle_, "%s", ss.str().c_str());
00165 if (written < 0)
00166 {
00167 std::cerr << "Error writting to rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
00168 }
00169 else if (written > 0)
00170 {
00171 current_file_size_ += written;
00172 if (fflush(handle_))
00173 {
00174 std::cerr << "Error flushing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_));
00175 }
00176
00177
00178 if (current_file_size_ > max_file_size_)
00179 {
00180 if (fclose(handle_))
00181 {
00182 std::cerr << "Error closing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
00183 }
00184 current_backup_index_++;
00185 if (current_backup_index_ <= max_backup_index_)
00186 {
00187 std::stringstream backup_file_name;
00188 backup_file_name << log_file_name_ << "." << current_backup_index_;
00189 int rc = rename(log_file_name_.c_str(), backup_file_name.str().c_str());
00190 if (rc != 0)
00191 {
00192 rc = remove(backup_file_name.str().c_str());
00193 if (rc == 0)
00194 {
00195 rc = rename(log_file_name_.c_str(), backup_file_name.str().c_str());
00196 if (rc)
00197 {
00198 std::cerr << "Error rotating rosout log file '" << log_file_name_.c_str() << "' to '" << backup_file_name.str().c_str() << "': " << strerror(errno);
00199 }
00200 }
00201 }
00202 if (rc)
00203 {
00204 std::cerr << "Error rotating rosout log file '" << log_file_name_.c_str() << "'' to '" << backup_file_name.str().c_str() << "'" << std::endl;
00205 }
00206 else
00207 {
00208 std::cout << "rosout log file " << log_file_name_.c_str() << " reached max size, back up data to " << backup_file_name.str().c_str() << std::endl;
00209 }
00210 if (current_backup_index_ == max_backup_index_)
00211 {
00212 current_backup_index_ = 0;
00213 }
00214 }
00215 handle_ = fopen(log_file_name_.c_str(), "w");
00216 if (handle_ == 0)
00217 {
00218 std::cerr << "Error opening rosout log file '" << log_file_name_.c_str() << "': " << strerror(errno);
00219 }
00220 current_file_size_ = 0;
00221 }
00222 }
00223 }
00224 };
00225
00226 int main(int argc, char **argv)
00227 {
00228 ros::init(argc, argv, "rosout", ros::init_options::NoRosout);
00229 ros::NodeHandle n;
00230 Rosout r;
00231
00232 ros::spin();
00233
00234 return 0;
00235 }
00236