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

enhanced barrier with foreach

This commit is contained in:
vivek-shankar 2018-07-10 14:00:01 -04:00
parent 1cb390f595
commit 87083c5658
2 changed files with 38 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -55,13 +55,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
var allgood = 0
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
var bi = LOWEST_ROBOT_ID
allgood = 1
while (bi<LOWEST_ROBOT_ID+threshold) {
if(barrier.get(bi) != bc)
allgood = 0
bi = bi + 1
}
allgood = barrier_allgood(barrier,bc)
}
if(allgood) {
@ -78,3 +72,19 @@ function barrier_wait(threshold, transf, resumef, bc) {
timeW = timeW+1
}
barriergood = 1
# Barrer check all entries
function barrier_allgood(barrier, bc) {
barriergood = 1
barrier.foreach(
function(key, value, robot){
#log("VS entry : ", key, " ", value, " ", robot)
if(value != bc and key != "d"){
barriergood = 0
}
}
)
return barriergood
}

22
src/roscontroller.cpp
View File

@ -59,6 +59,18 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
{
robot_id = strtol(robot_name.c_str() + 5, NULL, 10);
}
// Create log dir and open 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);
}
roscontroller::~roscontroller()
@ -68,6 +80,8 @@ roscontroller::~roscontroller()
{
// Destroy the BVM
buzz_utility::buzz_script_destroy();
// Close the data logging file
log.close();
}
void roscontroller::GetRobotId()
@ -137,6 +151,13 @@ void roscontroller::RosControllerRun()
}
if (debug)
ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss);
// log data
log<<ros::Time::now()<<",";
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots<<",";
log <<(int)buzz_utility::get_inmsg_size()<<",";
log <<buzz_utility::getuavstate()<<std::endl;
// Call Step from buzz script
buzz_utility::buzz_script_step();
// Prepare messages and publish them
@ -153,7 +174,6 @@ void roscontroller::RosControllerRun()
buzz_utility::set_robot_var(no_of_robots);
if(update) buzz_update::updates_set_robots(no_of_robots);
// get_xbee_status(); // commented out because it may slow down the node too much, to be tested
ros::spinOnce();
loop_rate.sleep();
// make sure to sleep for the rest of the loop time