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

changed to old barrier and modified log for easy parsing

This commit is contained in:
vivek-shankar 2018-07-24 15:55:13 -04:00
parent 24db1dc4fc
commit 2378a3881a
3 changed files with 78 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
buzz_scripts/include/act/old_barrier.bzz Normal file
View File

@ -0,0 +1,69 @@
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_TIMEOUT = 1200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
#
# Sets a barrier
#
function barrier_create() {
# reset
timeW = 0
# create barrier vstig
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
BARRIER_VSTIG = BARRIER_VSTIG +1
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
}
function barrier_set(threshold, transf, resumef,bc) {
statef = function() {
barrier_wait(threshold, transf, resumef,bc);
}
BVMSTATE = "BARRIERWAIT"
barrier_create()
}
#
# Make yourself ready
#
function barrier_ready(bc) {
#log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put("d", 0)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf, resumef,bc) {
barrier.put(id, 1)
barrier.get(id)
log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
barrier=nil
timeW = 0
BVMSTATE = resumef
}
timeW = timeW+1
}

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

@ -4,7 +4,7 @@
#
########################################
include "utils/vec2.bzz"
include "act/barrier.bzz"
include "act/old_barrier.bzz"
include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m.
@ -191,8 +191,8 @@ function rc_cmd_listen() {
} else if(flight.rc_cmd==21) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "TURNEDOFF"
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
barrier_ready(21)
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
#barrier_ready(21)
BVMSTATE = "STOP"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==20) {

11
src/roscontroller.cpp
View File

@ -170,16 +170,18 @@ void roscontroller::RosControllerRun()
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots<<",";
log << neighbours_pos_map.size()<< ",";
std::vector<uint8_t*> in_msg = buzz_utility::get_inmsg_vector();
log <<(int)in_msg.size()<<",";
log <<buzz_utility::getuavstate()<<",";
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;
@ -190,8 +192,7 @@ void roscontroller::RosControllerRun()
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
log<<(int)neigh_id<<","<<(int)msg_size<<",";
}
log <<buzz_utility::getuavstate()<<std::endl;
log<<std::endl;
// Call Step from buzz script
buzz_utility::buzz_script_step();
// Prepare messages and publish them