int main(int argc, char *argv[])
{
int write_fd, log_fd;
char result = 0;
int ppid;
const char *lock_type;
progname = argv[0];
if (argc < 5) {
usage();
exit(1);
}
log_fd = atoi(argv[1]);
close(STDOUT_FILENO);
close(STDERR_FILENO);
dup2(log_fd, STDOUT_FILENO);
dup2(log_fd, STDERR_FILENO);
close(log_fd);
ppid = atoi(argv[2]);
write_fd = atoi(argv[3]);
lock_type = argv[4];
if (strcmp(lock_type, "RECORD") == 0) {
if (argc != 8) {
fprintf(stderr, "%s: Invalid number of arguments (%d)\n",
progname, argc);
usage();
exit(1);
}
result = lock_record(argv[5], argv[6], argv[7]);
} else if (strcmp(lock_type, "DB") == 0) {
int n;
/* If there are no databases specified, no need for lock */
if (argc > 5) {
for (n=5; n+1<argc; n+=2) {
result = lock_db(argv[n], argv[n+1]);
if (result != 0) {
break;
}
}
}
} else {
fprintf(stderr, "%s: Invalid lock-type '%s'\n", progname, lock_type);
usage();
exit(1);
}
send_result(write_fd, result);
ctdb_wait_for_process_to_exit(ppid);
return 0;
}
std::string Driver::minidump(const std::string& prefix)
{
if (!log_enabled_)
{
const std::string& err = "Log is not enabled, please enable logging before calling minidump";
std::cout << BOLDRED << err << std::endl
<< RESETCOLOR << std::endl;
return err;
}
// CHECK SIZE IN FOLDER
long files_size = 0;
boost::filesystem::path folderPath(boost::filesystem::current_path());
helpers::filesystem::getFilesSize(folderPath, files_size);
if (files_size > helpers::filesystem::folderMaximumSize)
{
std::cout << BOLDRED << "No more space on robot. You need to upload the presents bags and remove them to make new ones."
<< std::endl << "To remove all the presents bags, you can run this command:" << std::endl
<< "\t$ qicli call ROS-Driver.removeFiles" << RESETCOLOR << std::endl;
return "No more space on robot. You need to upload the presents bags and remove them to make new ones.";
}
// IF A ROSBAG WAS OPENED, FIRST CLOSE IT
if (record_enabled_)
{
stopRecording();
}
// STOP BUFFERIZING
log_enabled_ = false;
for(EventIter iterator = event_map_.begin(); iterator != event_map_.end(); iterator++)
{
iterator->second.isDumping(true);
}
ros::Time time = ros::Time::now();
// START A NEW ROSBAG
boost::mutex::scoped_lock lock_record( mutex_record_ );
recorder_->startRecord(prefix);
// WRITE ALL BUFFER INTO THE ROSBAG
for(EventIter iterator = event_map_.begin(); iterator != event_map_.end(); iterator++)
{
iterator->second.writeDump(time);
}
for(RecIter iterator = rec_map_.begin(); iterator != rec_map_.end(); iterator++)
{
iterator->second.writeDump(time);
}
// RESTART BUFFERIZING
log_enabled_ = true;
for(EventIter iterator = event_map_.begin(); iterator != event_map_.end(); iterator++)
{
iterator->second.isDumping(false);
}
return recorder_->stopRecord(::naoqi::ros_env::getROSIP("eth0"));
}
std::string Driver::minidumpConverters(const std::string& prefix, const std::vector<std::string>& names)
{
if (!log_enabled_)
{
const std::string& err = "Log is not enabled, please enable logging before calling minidump";
std::cout << BOLDRED << err << std::endl
<< RESETCOLOR << std::endl;
return err;
}
// CHECK SIZE IN FOLDER
long files_size = 0;
boost::filesystem::path folderPath(boost::filesystem::current_path());
helpers::filesystem::getFilesSize(folderPath, files_size);
if (files_size > helpers::filesystem::folderMaximumSize)
{
std::cout << BOLDRED << "No more space on robot. You need to upload the presents bags and remove them to make new ones."
<< std::endl << "To remove all the presents bags, you can run this command:" << std::endl
<< "\t$ qicli call ROS-Driver.removeFiles" << RESETCOLOR << std::endl;
return "No more space on robot. You need to upload the presents bags and remove them to make new ones.";
}
// IF A ROSBAG WAS OPENED, FIRST CLOSE IT
if (record_enabled_)
{
stopRecording();
}
// STOP BUFFERIZING
log_enabled_ = false;
for(EventIter iterator = event_map_.begin(); iterator != event_map_.end(); iterator++)
{
iterator->second.isDumping(true);
}
ros::Time time = ros::Time::now();
// WRITE CHOOSEN BUFFER INTO THE ROSBAG
boost::mutex::scoped_lock lock_record( mutex_record_ );
bool is_started = false;
for_each( const std::string& name, names)
{
RecIter it = rec_map_.find(name);
if ( it != rec_map_.end() )
{
if ( !is_started )
{
recorder_->startRecord(prefix);
is_started = true;
}
it->second.writeDump(time);
}
else
{
EventIter it_event = event_map_.find(name);
if ( it_event != event_map_.end() )
{
if ( !is_started )
{
recorder_->startRecord(prefix);
is_started = true;
}
it_event->second.writeDump(time);
}
}
}
void Driver::rosLoop()
{
static std::vector<message_actions::MessageAction> actions;
// ros::Time::init();
while( keep_looping )
{
// clear the callback triggers
actions.clear();
{
boost::mutex::scoped_lock lock( mutex_conv_queue_ );
if (!conv_queue_.empty())
{
// Wait for the next Publisher to be ready
size_t conv_index = conv_queue_.top().conv_index_;
converter::Converter& conv = converters_[conv_index];
ros::Time schedule = conv_queue_.top().schedule_;
// check the publishing condition
// 1. publishing enabled
// 2. has to be registered
// 3. has to be subscribed
PubConstIter pub_it = pub_map_.find( conv.name() );
if ( publish_enabled_ && pub_it != pub_map_.end() && pub_it->second.isSubscribed() )
{
actions.push_back(message_actions::PUBLISH);
}
// check the recording condition
// 1. recording enabled
// 2. has to be registered
// 3. has to be subscribed (configured to be recorded)
RecConstIter rec_it = rec_map_.find( conv.name() );
{
boost::mutex::scoped_lock lock_record( mutex_record_, boost::try_to_lock );
if ( lock_record && record_enabled_ && rec_it != rec_map_.end() && rec_it->second.isSubscribed() )
{
actions.push_back(message_actions::RECORD);
}
}
// bufferize data in recorder
if ( log_enabled_ && rec_it != rec_map_.end() && conv.frequency() != 0)
{
actions.push_back(message_actions::LOG);
}
// only call when we have at least one action to perform
if (actions.size() >0)
{
conv.callAll( actions );
}
ros::Duration d( schedule - ros::Time::now() );
if ( d > ros::Duration(0))
{
d.sleep();
}
// Schedule for a future time or not
conv_queue_.pop();
if ( conv.frequency() != 0 )
{
conv_queue_.push(ScheduledConverter(schedule + ros::Duration(1.0f / conv.frequency()), conv_index));
}
}
else // conv_queue is empty.
{
// sleep one second
ros::Duration(1).sleep();
}
} // mutex scope
if ( publish_enabled_ )
{
ros::spinOnce();
}
} // while loop
}
