Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

data logging fixes.

This commit is contained in:
vivek-shankar 2018-08-06 14:26:17 -04:00
parent 79857a3f55
commit bf4f23bc48
3 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buzz_scripts/include/act/states.bzz
View File

@ -10,7 +10,7 @@ include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2.0 # m/steps GOTO_MAXVEL = 0.2 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 1.0 # m. GOTODIST_TOL = 1.0 # m.
GOTOANG_TOL = 0.1 # rad. GOTOANG_TOL = 0.1 # rad.

6
include/roscontroller.h
View File

@ -104,8 +104,8 @@ private:
uint16_t nid; uint16_t nid;
uint16_t size; uint16_t size;
double sent_time; double sent_time;
double received_time; uint64_t received_time;
MsgData(int mi, uint16_t ni, uint16_t s, double st, double rt): MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt):
msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){}; msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){};
MsgData(){}; MsgData(){};
}; };
@ -122,7 +122,7 @@ private:
ros::Time logical_clock; ros::Time logical_clock;
ros::Time previous_step_time; ros::Time previous_step_time;
std::vector<msg_data> inmsgdata; std::vector<msg_data> inmsgdata;
double out_msg_time; uint64_t out_msg_time;
double logical_time_rate; double logical_time_rate;
bool time_sync_jumped; bool time_sync_jumped;
std::string robot_name = ""; std::string robot_name = "";

14
src/roscontroller.cpp
View File

@ -176,25 +176,25 @@ void roscontroller::RosControllerRun()
log<<ros::Time::now().toSec()<<","; log<<ros::Time::now().toSec()<<",";
get_logical_time(); get_logical_time();
// logical time now // logical time now
log<<logical_clock.toSec()<<","; log<<logical_clock.toNSec()<<",";
log<<buzz_utility::get_timesync_itr()<<","; log<<buzz_utility::get_timesync_itr()<<",";
log<<buzz_utility::get_timesync_state()<<","; log<<buzz_utility::get_timesync_state()<<",";
log<<cur_pos.latitude << "," << cur_pos.longitude << "," log<<cur_pos.latitude * 100000 << "," << cur_pos.longitude * 100000 << ","
<< cur_pos.altitude << ","; << cur_pos.altitude * 100000 << ",";
log << (int)no_of_robots<<","; log << (int)no_of_robots<<",";
log << neighbours_pos_map.size()<< ","; log << neighbours_pos_map.size()<< ",";
log<<(int)inmsgdata.size()<<","; log<<(int)inmsgdata.size()<<",";
log<< message_number<<","; log<< message_number<<",";
log<< out_msg_time<<","; log<< out_msg_time<<",";
log <<buzz_utility::getuavstate(); log <<buzz_utility::getuavstate();
if(neighbours_pos_map.size() > 0)log<<","; // if(neighbours_pos_map.size() > 0)log<<",";
map<int, buzz_utility::Pos_struct>::iterator it = map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin(); neighbours_pos_map.begin();
for (; it != neighbours_pos_map.end(); ++it) for (; it != neighbours_pos_map.end(); ++it)
{ {
log<< it->first<<","; log<< it->first<<",";
log << (double)it->second.x << "," << (double)it->second.y log <<","<< (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z; << "," << (double)it->second.z;
} }
for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){
@ -654,7 +654,7 @@ with size......... | /
} }
//Store out msg time //Store out msg time
get_logical_time(); get_logical_time();
out_msg_time = logical_clock.toSec(); out_msg_time = logical_clock.toNSec();
// publish prepared messages in respective topic // publish prepared messages in respective topic
payload_pub.publish(payload_out); payload_pub.publish(payload_out);
delete[] out; delete[] out;
@ -1171,7 +1171,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index));
get_logical_time(); get_logical_time();
// store in msg data for data log // store in msg data for data log
msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toNSec());
inmsgdata.push_back(inm); inmsgdata.push_back(inm);
if (debug) if (debug)