moved log commands to dedicated functions

This commit is contained in:
dave 2018-09-26 22:55:11 -04:00
parent 2150efe5b7
commit c60c1e53ec
2 changed files with 61 additions and 41 deletions

View File

@ -184,6 +184,10 @@ private:
mavros_msgs::SetMode m_cmdSetMode; mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client; ros::ServiceClient mode_client;
// CSV log management functions
void initcsvlog();
void logtocsv();
// Initialize publisher and subscriber, done in the constructor // Initialize publisher and subscriber, done in the constructor
void Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); void Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);

View File

@ -68,17 +68,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
time_sync_jumped = false; time_sync_jumped = false;
out_msg_time=0; out_msg_time=0;
// Create log dir and open log file // Create log dir and open log file
std::string path = initcsvlog();
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
std::string folder_check="mkdir -p "+path;
system(folder_check.c_str());
for(int i=5;i>0;i--){
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
}
path += "logger_"+std::to_string(robot_id)+"_0.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
} }
roscontroller::~roscontroller() roscontroller::~roscontroller()
@ -103,7 +93,7 @@ void roscontroller::GetRobotId()
while (!xbeestatus_srv.call(robot_id_srv_request, robot_id_srv_response)) while (!xbeestatus_srv.call(robot_id_srv_request, robot_id_srv_response))
{ {
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
ROS_WARN("ROSBUZZ is waiting for Xbee device ID"); ROS_WARN("ROSBUZZ is waiting for Network device ID");
} }
robot_id = robot_id_srv_response.value.integer; robot_id = robot_id_srv_response.value.integer;
@ -134,8 +124,6 @@ void roscontroller::RosControllerRun()
// check for BVMSTATE variable // check for BVMSTATE variable
if(buzz_utility::get_bvmstate()=="Not Available") if(buzz_utility::get_bvmstate()=="Not Available")
ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE."); ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
// set log data double precision
log <<std::setprecision(15) << std::fixed;
// DEBUG // DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
@ -166,33 +154,7 @@ void roscontroller::RosControllerRun()
ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss);
// log data // log data
// hardware time now logtocsv();
log << ros::Time::now().toNSec() << ",";
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots << ",";
log << neighbours_pos_map.size() << ",";
log << (int)inmsgdata.size() << "," << message_number << ",";
log << out_msg_time <<",";
log << out_msg_size<<",";
log << buzz_utility::get_bvmstate();
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
for (; it != neighbours_pos_map.end(); ++it)
{
log << "," << it->first << ",";
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z;
}
for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it)
{
log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," <<
it->sent_time << ","<< it->received_time;
}
inmsgdata.clear();
log << std::endl;
// Call Step from buzz script // Call Step from buzz script
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
@ -226,6 +188,60 @@ void roscontroller::RosControllerRun()
} }
} }
void roscontroller::initcsvlog()
/*
/ Create the CSV log file
/-------------------------------------------------------*/
{
std::string path =
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
std::string folder_check="mkdir -p "+path;
system(folder_check.c_str());
for(int i=5;i>0;i--){
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
}
path += "logger_"+std::to_string(robot_id)+"_0.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
// set log data double precision
log <<std::setprecision(15) << std::fixed;
}
void roscontroller::logtocsv()
/*
/ Push a new line of data in the CSV log file
/-------------------------------------------------------*/
{
// hardware time now
log << ros::Time::now().toNSec() << ",";
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots << ",";
log << neighbours_pos_map.size() << ",";
log << (int)inmsgdata.size() << "," << message_number << ",";
log << out_msg_time <<",";
log << out_msg_size<<",";
log << buzz_utility::get_bvmstate();
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
for (; it != neighbours_pos_map.end(); ++it)
{
log << "," << it->first << ",";
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z;
}
for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it)
{
log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," <<
it->sent_time << ","<< it->received_time;
}
inmsgdata.clear();
log << std::endl;
}
void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
/* /*
/ Get all required parameters from the ROS launch file / Get all required parameters from the ROS launch file