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

inertia for no.of robots

Browse Source
This commit is contained in:
vivek-shankar 2017-04-02 22:46:48 -04:00
parent 4dbcecf2da
commit 2647f9dc9a
2 changed files with 49 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
include/roscontroller.h
View File

@ -46,6 +46,14 @@ public:
void RosControllerRun(); void RosControllerRun();
private: 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]; double cur_pos[3];
uint64_t payload; uint64_t payload;
@ -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; std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
bool rcclient; bool rcclient;
bool multi_msg; bool multi_msg;
Num_robot_count count_robots;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv; ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub; ros::Publisher payload_pub;
@ -157,6 +166,8 @@ private:
/*Robot independent subscribers*/ /*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c); 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; multi_msg=true;
} }
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/ /*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); buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates*/ /*Set no of robots for updates*/
updates_set_robots(no_of_robots); 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;
}
} }