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

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

This commit is contained in:
David St-Onge 2017-04-02 22:49:23 -04:00
parent 4b71014572 df6557ff9c
commit 7499d59834
2 changed files with 49 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
include/roscontroller.h
View File

@ -46,7 +46,15 @@ public:
void RosControllerRun();
private:
struct num_robot_count
{
uint8_t history[10];
uint8_t index=0;
uint8_t current=0;
num_robot_count(){}
}; typedef struct num_robot_count Num_robot_count ;
double cur_pos[3];
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
@ -63,6 +71,7 @@ private:
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
bool rcclient;
bool multi_msg;
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub;
@ -157,6 +166,8 @@ private:
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c);
int get_number_of_robots();
};
}

38
src/roscontroller.cpp
View File

@ -73,7 +73,8 @@ namespace rosbzz_node{
multi_msg=true;
}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
no_of_robots=get_number_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*/
updates_set_robots(no_of_robots);
@ -654,6 +655,41 @@ namespace rosbzz_node{
}*/
int roscontroller::get_number_of_robots(){
if(count_robots.current !=0){
uint8_t current_count,odd_count,odd_val=0;
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=neighbours_pos_map.size()+1;
count_robots.index++;
if(count_robots.index >9) count_robots.index =0;
for(int i=0;i<10;i++){
if(count_robots.history[i]==count_robots.current){
current_count++;
}
else{
if(count_robots.history[i]!=0){
odd_count++;
odd_val=count_robots.history[i];
}
}
}
if(odd_count>count_robots.current){
count_robots.current=odd_val;
}
}
else{
if(neighbours_pos_map.size()!=0){
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=neighbours_pos_map.size()+1;
count_robots.index++;
if(count_robots.index >9){
count_robots.index =0;
count_robots.current=neighbours_pos_map.size()+1;
}
}
}
return count_robots.current;
}
}