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

Merge branch 'test' of https://github.com/MISTLab/ROSBuzz into dev

This commit is contained in:
dave 2017-05-23 23:32:29 -04:00
parent 0810963359 95f80f5e9d
commit 79282cc2ab
6 changed files with 43 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/buzz_update.h
View File

@ -6,7 +6,7 @@
#include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h>
#include <fstream>
#define delete_p(p) do { free(p); p = NULL; } while(0)
@ -133,4 +133,5 @@ int compile_bzz();
void updates_set_robots(int robots);
void collect_data(std::ofstream &logger);
#endif

2
include/roscontroller.h
View File

@ -122,6 +122,8 @@ private:
int setpoint_counter;
double my_x = 0, my_y = 0;
std::ofstream log;
/*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;

2
script/stand_by.bzz
View File

@ -21,7 +21,7 @@ barrier_val = barrier.size()
neighbors.listen(updated,
function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " )
if(value==update_no) barrier_val=ROBOTS
if(value==update_no) barrier.put(rid,1)
}
)

16
src/buzz_update.cpp
View File

@ -15,7 +15,7 @@
static struct timeval t1, t2;
static int timer_steps=0;
//static clock_t end;
void collect_data();
/*Temp end */
static int fd,wd =0;
static int old_update =0;
@ -89,6 +89,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
//neigh = 0;
//updater->outmsg_queue=
// update_table->barrier=nvs;
// open logger
}
/*Check for .bzz file chages*/
@ -178,6 +179,7 @@ void code_message_inqueue_process(){
(char*) dbgf_name,(size_t)update_bcode_size) ) {
*(uint16_t*)(updater->update_no)=update_no;
neigh=1;
//gettimeofday(&t1, NULL);
}
}
}
@ -241,7 +243,7 @@ void update_routine(const char* bcfname,
}
else{
//gettimeofday(&t1, NULL);
if(neigh==0 && (!is_msg_present())){
ROS_INFO("Sending code... \n");
code_message_outqueue_append();
@ -257,8 +259,6 @@ void update_routine(const char* bcfname,
*(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL);
//collect_data();
timer_steps=0;
neigh=0;
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
//buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated=1;
@ -297,6 +297,7 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
gettimeofday(&t1, NULL);
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM);
@ -412,12 +413,15 @@ int compile_bzz(){
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
return system(bzzfile_in_compile.str().c_str());
}
void collect_data(){
void collect_data(std::ofstream &logger){
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
//int bytecodesize=(int);
fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
logger<<(int)no_of_robot<<","<<neigh<<","<<(double)time_spent<<","<<(int)timer_steps<<","<<*(size_t*)updater->bcode_size<<","<<(int)*(uint8_t*)updater->update_no;
timer_steps=0;
neigh=0;
//fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
//FILE *Fileptr;
//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);

2
src/buzz_utility.cpp
View File

@ -708,7 +708,7 @@ int create_stig_tables() {
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
set_robot_var(buzzdict_size(VM->swarmmembers)+1);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
//int status = 1;

30
src/roscontroller.cpp
View File

@ -45,6 +45,9 @@ namespace rosbzz_node{
} else {
robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
}
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path+="Update.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
}
/*---------------------
@ -57,6 +60,7 @@ namespace rosbzz_node{
buzz_utility::buzz_script_destroy();
/* Stop the robot */
uav_done();
log.close();
}
void roscontroller::GetRobotId()
@ -106,15 +110,25 @@ namespace rosbzz_node{
if(get_update_status()){
set_read_update_status();
multi_msg=true;
log<<cur_pos.latitude<<","<<cur_pos.longitude<<","<<cur_pos.altitude<<",";
collect_data(log);
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;
}
log<<std::endl;
}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
//no_of_robots=get_number_of_robots();
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
//buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/
if(multi_msg)
//if(multi_msg)
updates_set_robots(no_of_robots);
ROS_INFO("ROBOTS: %i , acutal : %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
@ -911,21 +925,21 @@ namespace rosbzz_node{
}*/
void roscontroller::get_number_of_robots(){
int cur_robots=(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1;
if(no_of_robots==0){
no_of_robots=neighbours_pos_map.size()+1;
no_of_robots=cur_robots;
}
else{
if(no_of_robots!=neighbours_pos_map.size()+1 && no_cnt==0){
if(no_of_robots!=cur_robots && no_cnt==0){
no_cnt++;
old_val=neighbours_pos_map.size()+1;
old_val=cur_robots;
}
else if(no_cnt!=0 && old_val==neighbours_pos_map.size()+1){
else if(no_cnt!=0 && old_val==cur_robots){
no_cnt++;
if(no_cnt>=4){
no_of_robots=neighbours_pos_map.size()+1;
if(no_cnt>=150 || cur_robots > no_of_robots){
no_of_robots=cur_robots;
no_cnt=0;
}
}