v01
This commit is contained in:
Ivan
236
thirdparty/ros/ros_comm/tools/rosout/rosout.cpp
vendored
Normal file
236
thirdparty/ros/ros_comm/tools/rosout/rosout.cpp
vendored
Normal file
@@ -0,0 +1,236 @@
|
||||
/*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <cstring>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "ros/ros.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#ifdef WIN32
|
||||
#ifdef ERROR
|
||||
// ach, windows.h polluting everything again,
|
||||
// clashes with autogenerated rosgraph_msgs/Log.h
|
||||
#undef ERROR
|
||||
#endif
|
||||
#endif
|
||||
#include "rosgraph_msgs/Log.h"
|
||||
|
||||
/**
|
||||
* @mainpage
|
||||
*
|
||||
* @htmlinclude manifest.html
|
||||
*
|
||||
* @b rosout logs messages sent to the /rosout topic,
|
||||
* which is a system-wide string logging mechanism.
|
||||
*/
|
||||
|
||||
/**
|
||||
* the rosout node subscribes to /rosout, logs the messages to file, and re-broadcasts the messages to /rosout_agg
|
||||
*/
|
||||
class Rosout
|
||||
{
|
||||
public:
|
||||
std::string log_file_name_;
|
||||
FILE* handle_;
|
||||
size_t max_file_size_;
|
||||
size_t current_file_size_;
|
||||
size_t max_backup_index_;
|
||||
size_t current_backup_index_;
|
||||
ros::NodeHandle node_;
|
||||
ros::Subscriber rosout_sub_;
|
||||
ros::Publisher agg_pub_;
|
||||
|
||||
Rosout()
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
void init()
|
||||
{
|
||||
max_file_size_ = 100*1024*1024;
|
||||
current_file_size_ = 0;
|
||||
max_backup_index_ = 10;
|
||||
current_backup_index_ = 0;
|
||||
|
||||
log_file_name_ = ros::file_log::getLogDirectory() + "/rosout.log";
|
||||
handle_ = fopen(log_file_name_.c_str(), "w");
|
||||
|
||||
if (handle_ == 0)
|
||||
{
|
||||
std::cerr << "Error opening rosout log file '" << log_file_name_.c_str() << "': " << strerror(errno);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "logging to " << log_file_name_.c_str() << std::endl;
|
||||
|
||||
std::stringstream ss;
|
||||
ss << "\n\n" << ros::Time::now() << " Node Startup\n";
|
||||
int written = fprintf(handle_, "%s", ss.str().c_str());
|
||||
if (written < 0)
|
||||
{
|
||||
std::cerr << "Error writting to rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
|
||||
}
|
||||
else if (written > 0)
|
||||
{
|
||||
current_file_size_ += written;
|
||||
if (fflush(handle_))
|
||||
{
|
||||
std::cerr << "Error flushing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
agg_pub_ = node_.advertise<rosgraph_msgs::Log>("/rosout_agg", 0);
|
||||
std::cout << "re-publishing aggregated messages to /rosout_agg" << std::endl;
|
||||
|
||||
rosout_sub_ = node_.subscribe("/rosout", 0, &Rosout::rosoutCallback, this);
|
||||
std::cout << "subscribed to /rosout" << std::endl;
|
||||
}
|
||||
|
||||
void rosoutCallback(const rosgraph_msgs::Log::ConstPtr& msg)
|
||||
{
|
||||
agg_pub_.publish(msg);
|
||||
|
||||
std::stringstream ss;
|
||||
ss << msg->header.stamp << " ";
|
||||
switch (msg->level)
|
||||
{
|
||||
case rosgraph_msgs::Log::FATAL:
|
||||
ss << "FATAL ";
|
||||
break;
|
||||
case rosgraph_msgs::Log::ERROR:
|
||||
ss << "ERROR ";
|
||||
break;
|
||||
case rosgraph_msgs::Log::WARN:
|
||||
ss << "WARN ";
|
||||
break;
|
||||
case rosgraph_msgs::Log::DEBUG:
|
||||
ss << "DEBUG ";
|
||||
break;
|
||||
case rosgraph_msgs::Log::INFO:
|
||||
ss << "INFO ";
|
||||
break;
|
||||
default:
|
||||
ss << msg->level << " ";
|
||||
}
|
||||
|
||||
ss << "[" << msg->file << ":" << msg->line << "(" << msg->function << ") ";
|
||||
|
||||
ss << "[topics: ";
|
||||
std::vector<std::string>::const_iterator it = msg->topics.begin();
|
||||
std::vector<std::string>::const_iterator end = msg->topics.end();
|
||||
for ( ; it != end; ++it )
|
||||
{
|
||||
const std::string& topic = *it;
|
||||
|
||||
if ( it != msg->topics.begin() )
|
||||
{
|
||||
ss << ", ";
|
||||
}
|
||||
|
||||
ss << topic;
|
||||
}
|
||||
ss << "] ";
|
||||
|
||||
ss << msg->msg;
|
||||
ss << "\n";
|
||||
int written = fprintf(handle_, "%s", ss.str().c_str());
|
||||
if (written < 0)
|
||||
{
|
||||
std::cerr << "Error writting to rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
|
||||
}
|
||||
else if (written > 0)
|
||||
{
|
||||
current_file_size_ += written;
|
||||
if (fflush(handle_))
|
||||
{
|
||||
std::cerr << "Error flushing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_));
|
||||
}
|
||||
|
||||
// check for rolling
|
||||
if (current_file_size_ > max_file_size_)
|
||||
{
|
||||
if (fclose(handle_))
|
||||
{
|
||||
std::cerr << "Error closing rosout log file '" << log_file_name_.c_str() << "': " << strerror(ferror(handle_)) << std::endl;
|
||||
}
|
||||
current_backup_index_++;
|
||||
if (current_backup_index_ <= max_backup_index_)
|
||||
{
|
||||
std::stringstream backup_file_name;
|
||||
backup_file_name << log_file_name_ << "." << current_backup_index_;
|
||||
int rc = rename(log_file_name_.c_str(), backup_file_name.str().c_str());
|
||||
if (rc != 0)
|
||||
{
|
||||
rc = remove(backup_file_name.str().c_str());
|
||||
if (rc == 0)
|
||||
{
|
||||
rc = rename(log_file_name_.c_str(), backup_file_name.str().c_str());
|
||||
if (rc)
|
||||
{
|
||||
std::cerr << "Error rotating rosout log file '" << log_file_name_.c_str() << "' to '" << backup_file_name.str().c_str() << "': " << strerror(errno);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (rc)
|
||||
{
|
||||
std::cerr << "Error rotating rosout log file '" << log_file_name_.c_str() << "'' to '" << backup_file_name.str().c_str() << "'" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
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;
|
||||
}
|
||||
if (current_backup_index_ == max_backup_index_)
|
||||
{
|
||||
current_backup_index_ = 0;
|
||||
}
|
||||
}
|
||||
handle_ = fopen(log_file_name_.c_str(), "w");
|
||||
if (handle_ == 0)
|
||||
{
|
||||
std::cerr << "Error opening rosout log file '" << log_file_name_.c_str() << "': " << strerror(errno);
|
||||
}
|
||||
current_file_size_ = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosout", ros::init_options::NoRosout);
|
||||
ros::NodeHandle n;
|
||||
Rosout r;
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user