rosout.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <cstring>
31 #include <cstdlib>
32 
33 #include "ros/ros.h"
34 #include "ros/file_log.h"
35 
36 #ifdef WIN32
37  #ifdef ERROR
38  // ach, windows.h polluting everything again,
39  // clashes with autogenerated rosgraph_msgs/Log.h
40  #undef ERROR
41  #endif
42 #endif
43 #include "rosgraph_msgs/Log.h"
44 
57 class Rosout
58 {
59 public:
60  std::string log_file_name_;
61  FILE* handle_;
70 
71  Rosout() :
72  log_file_name_(ros::file_log::getLogDirectory() + "/rosout.log"),
73  handle_(NULL),
74  max_file_size_(100*1024*1024),
75  current_file_size_(0),
76  max_backup_index_(10),
77  current_backup_index_(0),
78  omit_topics_(false)
79  {
80  init();
81  }
82 
83  void init()
84  {
85  handle_ = fopen(log_file_name_.c_str(), "w");
86 
87  if (handle_ == 0)
88  {
89  std::cerr << "Error opening rosout log file '" << log_file_name_.c_str() << "': " << strerror(errno);
90  }
91  else
92  {
93  std::cout << "logging to " << log_file_name_.c_str() << std::endl;
94 
95  std::stringstream ss;
96  ss << "\n\n" << ros::Time::now() << " Node Startup\n";
97  int written = fprintf(handle_, "%s", ss.str().c_str());
98  if (written < 0)
99  {
100  std::cerr << "Error writting to rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
101  }
102  else if (written > 0)
103  {
104  current_file_size_ += written;
105  if (fflush(handle_))
106  {
107  std::cerr << "Error flushing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_));
108  }
109  }
110  }
111 
112  agg_pub_ = node_.advertise<rosgraph_msgs::Log>("/rosout_agg", 0);
113  std::cout << "re-publishing aggregated messages to /rosout_agg" << std::endl;
114 
115  rosout_sub_ = node_.subscribe("/rosout", 0, &Rosout::rosoutCallback, this);
116  std::cout << "subscribed to /rosout" << std::endl;
117  }
118 
119  void rosoutCallback(const rosgraph_msgs::Log::ConstPtr& msg)
120  {
121  agg_pub_.publish(msg);
122 
123  std::stringstream ss;
124  ss << msg->header.stamp << " ";
125  switch (msg->level)
126  {
127  case rosgraph_msgs::Log::FATAL:
128  ss << "FATAL ";
129  break;
130  case rosgraph_msgs::Log::ERROR:
131  ss << "ERROR ";
132  break;
133  case rosgraph_msgs::Log::WARN:
134  ss << "WARN ";
135  break;
136  case rosgraph_msgs::Log::DEBUG:
137  ss << "DEBUG ";
138  break;
139  case rosgraph_msgs::Log::INFO:
140  ss << "INFO ";
141  break;
142  default:
143  ss << msg->level << " ";
144  }
145 
146  ss << msg->name << " ";
147  ss << "[" << msg->file << ":" << msg->line << "(" << msg->function << ")] ";
148 
149  // check parameter server for omit_topics flag and set class member
150  node_.getParamCached("/rosout/omit_topics", omit_topics_);
151 
152  if (!omit_topics_)
153  {
154  ss << "[topics: ";
155  std::vector<std::string>::const_iterator it = msg->topics.begin();
156  std::vector<std::string>::const_iterator end = msg->topics.end();
157  for ( ; it != end; ++it )
158  {
159  const std::string& topic = *it;
160 
161  if ( it != msg->topics.begin() )
162  {
163  ss << ", ";
164  }
165 
166  ss << topic;
167  }
168  ss << "] ";
169  }
170 
171  ss << msg->msg;
172  ss << "\n";
173  int written = fprintf(handle_, "%s", ss.str().c_str());
174  if (written < 0)
175  {
176  std::cerr << "Error writting to rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
177  }
178  else if (written > 0)
179  {
180  current_file_size_ += written;
181  if (fflush(handle_))
182  {
183  std::cerr << "Error flushing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_));
184  }
185 
186  // check for rolling
187  if (current_file_size_ > max_file_size_)
188  {
189  std::cout << "rosout log file " << log_file_name_.c_str() << " reached max size, rotating log files" << std::endl;
190  if (fclose(handle_))
191  {
192  std::cerr << "Error closing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
193  }
194  if (current_backup_index_ == max_backup_index_)
195  {
196  std::stringstream backup_file_name;
197  backup_file_name << log_file_name_ << "." << max_backup_index_;
198  int rc = remove(backup_file_name.str().c_str());
199  if (rc != 0)
200  {
201  std::cerr << "Error deleting oldest rosout log file '" << backup_file_name.str().c_str() << "': " << strerror(errno) << std::endl;
202  }
203  }
204  std::size_t i = std::min(max_backup_index_, current_backup_index_ + 1);
205  while (i > 0)
206  {
207  std::stringstream current_file_name;
208  current_file_name << log_file_name_;
209  if (i > 1)
210  {
211  current_file_name << "." << (i - 1);
212  }
213  std::stringstream rotated_file_name;
214  rotated_file_name << log_file_name_ << "." << i;
215  int rc = rename(current_file_name.str().c_str(), rotated_file_name.str().c_str());
216  if (rc != 0)
217  {
218  std::cerr << "Error rotating rosout log file '" << current_file_name.str().c_str() << "' to '" << rotated_file_name.str().c_str() << "': " << strerror(errno) << std::endl;
219  }
220  --i;
221  }
222  if (current_backup_index_ < max_backup_index_)
223  {
225  }
226  handle_ = fopen(log_file_name_.c_str(), "w");
227  if (handle_ == 0)
228  {
229  std::cerr << "Error opening rosout log file '" << log_file_name_.c_str() << "': " << strerror(errno);
230  }
231  current_file_size_ = 0;
232  }
233  }
234  }
235 };
236 
237 int main(int argc, char **argv)
238 {
239  ros::init(argc, argv, "rosout", ros::init_options::NoRosout);
240  ros::NodeHandle n;
241  Rosout r;
242 
243  ros::spin();
244 
245  return 0;
246 }
247 
bool omit_topics_
Definition: rosout.cpp:69
FILE * handle_
Definition: rosout.cpp:61
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())
size_t max_file_size_
Definition: rosout.cpp:62
bool getParamCached(const std::string &key, std::string &s) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void init()
Definition: rosout.cpp:83
std::string log_file_name_
Definition: rosout.cpp:60
size_t current_backup_index_
Definition: rosout.cpp:65
size_t current_file_size_
Definition: rosout.cpp:63
ros::NodeHandle node_
Definition: rosout.cpp:66
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
size_t max_backup_index_
Definition: rosout.cpp:64
ROSCPP_DECL const std::string & getLogDirectory()
Rosout()
Definition: rosout.cpp:71
static Time now()
ros::Publisher agg_pub_
Definition: rosout.cpp:68
void rosoutCallback(const rosgraph_msgs::Log::ConstPtr &msg)
Definition: rosout.cpp:119
int main(int argc, char **argv)
Definition: rosout.cpp:237
ros::Subscriber rosout_sub_
Definition: rosout.cpp:67


rosout
Author(s): Josh Faust
autogenerated on Sun Feb 3 2019 03:30:01