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

added msg size + nei id + nei pos to log in sim

This commit is contained in:
vivek-shankar 2018-07-22 14:51:33 -04:00
parent 7ab97b4fce
commit f6f79e1d1b
6 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
barrier = nil
timeW = 0
BVMSTATE = resumef
} else if(timeW % 50 == 0 and bc > 0)
} else if(timeW % 100 == 0 and bc > 0)
neighbors.broadcast("cmd", bc)
timeW = timeW+1

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

@ -269,7 +269,7 @@ function nei_cmd_listen() {
if(logical_time != nil) reinit_time_sync()
barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777)
barrier_ready(777)
neighbors.broadcast("cmd", 777)
#neighbors.broadcast("cmd", 777)
}else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)

8
buzz_scripts/include/timesync.bzz
View File

@ -6,7 +6,7 @@ logical_time = 0
TIME_JUMP_THR = 5
TIME_TO_FORGET = 20
TIME_TO_SYNC = 200
COM_DELAY = 3
COM_DELAY = 2
# table to store neighbor time data
time_nei_table = {}
# Algo. global parameters
@ -44,6 +44,7 @@ function init_time_sync(){
function step_time_sync(){
logical_time = logical_time + 1
sync_timer = sync_timer + 1
log("Logical time now ", logical_time)
if(sync_timer < TIME_TO_SYNC){
log(" SYNC ALGO ACTIVE time:", sync_timer)
cnt = 0
@ -51,8 +52,7 @@ function step_time_sync(){
if(size(time_nei_table) > 0){
foreach(time_nei_table, function(key, value) {
if(value.time != 0){
#log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max)
var local_offset = value.time - logical_time - value.age
var local_offset = value.time - logical_time + value.age
if(local_offset > 0){
avg_offset = avg_offset + 1 * local_offset
cnt = cnt + 1
@ -80,10 +80,8 @@ function step_time_sync(){
jumped = 0
syncError=0
var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) }
#string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical))
neighbors.broadcast("time_sync",mstr)
}
log("Logical time now ", logical_time)
}
# Function to set sync timer to zero and reinitiate sync. algo

2
include/buzz_utility.h
View File

@ -78,6 +78,8 @@ void set_robot_var(int ROBOTS);
int get_inmsg_size();
std::vector<uint8_t*> get_inmsg_vector();
std::string getuavstate();
int getlogicaltime();

4
src/buzz_utility.cpp
View File

@ -565,6 +565,10 @@ int get_inmsg_size()
return IN_MSG.size();
}
std::vector<uint8_t*> get_inmsg_vector(){
return IN_MSG;
}
string getuavstate()
/*
/ return current BVM state

21
src/roscontroller.cpp
View File

@ -158,7 +158,26 @@ void roscontroller::RosControllerRun()
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots<<",";
log <<(int)buzz_utility::get_inmsg_size()<<",";
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << neighbours_pos_map.size()<< ",";
for (; it != neighbours_pos_map.end(); ++it)
{
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z << ",";
}
std::vector<uint8_t*> in_msg = buzz_utility::get_inmsg_vector();
log <<(int)in_msg.size()<<",";
for (std::vector<uint8_t*>::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){
uint8_t* first_INmsg = (uint8_t*)(*it);
size_t tot = 0;
// Size is at first 2 bytes
uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
tot += sizeof(uint16_t);
// Decode neighbor Id
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
log<<(int)neigh_id<<","<<(int)msg_size<<",";
}
log <<buzz_utility::getuavstate()<<std::endl;
// Call Step from buzz script
buzz_utility::buzz_script_step();