43 #include "rosgraph_msgs/Log.h" 77 max_file_size_ = 100*1024*1024;
78 current_file_size_ = 0;
79 max_backup_index_ = 10;
80 current_backup_index_ = 0;
83 handle_ = fopen(log_file_name_.c_str(),
"w");
87 std::cerr <<
"Error opening rosout log file '" << log_file_name_.c_str() <<
"': " << strerror(errno);
91 std::cout <<
"logging to " << log_file_name_.c_str() << std::endl;
95 int written = fprintf(handle_,
"%s", ss.str().c_str());
98 std::cerr <<
"Error writting to rosout log file '" << log_file_name_.c_str() <<
"': " << strerror(ferror(handle_)) << std::endl;
100 else if (written > 0)
102 current_file_size_ += written;
105 std::cerr <<
"Error flushing rosout log file '" << log_file_name_.c_str() <<
"': " << strerror(ferror(handle_));
110 agg_pub_ = node_.
advertise<rosgraph_msgs::Log>(
"/rosout_agg", 0);
111 std::cout <<
"re-publishing aggregated messages to /rosout_agg" << std::endl;
114 std::cout <<
"subscribed to /rosout" << std::endl;
121 std::stringstream ss;
122 ss << msg->header.stamp <<
" ";
125 case rosgraph_msgs::Log::FATAL:
128 case rosgraph_msgs::Log::ERROR:
131 case rosgraph_msgs::Log::WARN:
134 case rosgraph_msgs::Log::DEBUG:
137 case rosgraph_msgs::Log::INFO:
141 ss << msg->level <<
" ";
144 ss <<
"[" << msg->file <<
":" << msg->line <<
"(" << msg->function <<
") ";
147 std::vector<std::string>::const_iterator it = msg->topics.begin();
148 std::vector<std::string>::const_iterator end = msg->topics.end();
149 for ( ; it != end; ++it )
151 const std::string& topic = *it;
153 if ( it != msg->topics.begin() )
164 int written = fprintf(handle_,
"%s", ss.str().c_str());
167 std::cerr <<
"Error writting to rosout log file '" << log_file_name_.c_str() <<
"': " << strerror(ferror(handle_)) << std::endl;
169 else if (written > 0)
171 current_file_size_ += written;
174 std::cerr <<
"Error flushing rosout log file '" << log_file_name_.c_str() <<
"': " << strerror(errno);
178 if (current_file_size_ > max_file_size_)
182 std::cerr <<
"Error closing rosout log file '" << log_file_name_.c_str() <<
"': " << strerror(errno) << std::endl;
184 current_backup_index_++;
185 if (current_backup_index_ <= max_backup_index_)
187 std::stringstream backup_file_name;
189 int rc = rename(log_file_name_.c_str(), backup_file_name.str().c_str());
192 rc =
remove(backup_file_name.str().c_str());
195 rc = rename(log_file_name_.c_str(), backup_file_name.str().c_str());
198 std::cerr <<
"Error rotating rosout log file '" << log_file_name_.c_str() <<
"' to '" << backup_file_name.str().c_str() <<
"': " << strerror(errno);
204 std::cerr <<
"Error rotating rosout log file '" << log_file_name_.c_str() <<
"'' to '" << backup_file_name.str().c_str() <<
"'" << std::endl;
208 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;
210 if (current_backup_index_ == max_backup_index_)
212 current_backup_index_ = 0;
215 handle_ = fopen(log_file_name_.c_str(),
"w");
218 std::cerr <<
"Error opening rosout log file '" << log_file_name_.c_str() <<
"': " << strerror(errno);
220 current_file_size_ = 0;
226 int main(
int argc,
char **argv)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string log_file_name_
size_t current_backup_index_
size_t current_file_size_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL const std::string & getLogDirectory()
void rosoutCallback(const rosgraph_msgs::Log::ConstPtr &msg)
int main(int argc, char **argv)
ros::Subscriber rosout_sub_