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

changes in handeling robot id

This commit is contained in:
vivek-shankar 2017-01-13 18:23:44 -05:00
parent 6489ee1c60
commit 32d802d5aa
3 changed files with 47 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/buzz_utility.h
View File

@ -51,5 +51,6 @@ int update_step_test();
uint16_t get_robotid(); uint16_t get_robotid();
uint8_t get_rid_uint8compac(int rid_old);
} }

48
src/buzz_utility.cpp
View File

@ -269,14 +269,15 @@ namespace buzz_utility{
//cout<<"bo file name"<<bo_filename; //cout<<"bo file name"<<bo_filename;
/* Get hostname */ /* Get hostname */
char hstnm[30]; char hstnm[30];
//char* hstnm ="M1003";
gethostname(hstnm, 30); gethostname(hstnm, 30);
/* Make numeric id from hostname */ /* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */ /* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 2, NULL, 10); //robot_id; int id = strtol(hstnm+3, NULL, 10);
cout << " Robot ID: " << (uint8_t)id<< endl; cout << " Robot ID: " <<id<< endl;
/* Reset the Buzz VM */ /* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(id); VM = buzzvm_new((int)id);
/* Get rid of debug info */ /* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
@ -339,6 +340,7 @@ namespace buzz_utility{
/* Make numeric id from hostname */ /* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */ /* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 1, NULL, 10); int id = strtol(hstnm + 1, NULL, 10);
/* Reset the Buzz VM */ /* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(id); VM = buzzvm_new(id);
@ -552,7 +554,47 @@ uint16_t get_robotid(){
return (uint16_t)id; return (uint16_t)id;
} }
uint8_t get_rid_uint8compac(int rid_old){
uint8_t new_rid;
switch(rid_old){
case 1:
new_rid=1;
break;
case 2:
new_rid=2;
break;
case 3:
new_rid=3;
break;
case 4:
new_rid=4;
break;
case 5:
new_rid=5;
break;
case 6:
new_rid=6;
break;
case 7:
new_rid=7;
break;
case 8:
new_rid=8;
break;
case 9:
new_rid=9;
break;
case 10:
new_rid=10;
break;
default:
new_rid=0;
}
return new_rid;
}
} }

2
src/roscontroller.cpp
View File

@ -42,7 +42,7 @@ namespace rosbzz_node{
rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear();
for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){ for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
sensor_msgs::NavSatFix neigh_tmp; sensor_msgs::NavSatFix neigh_tmp;
neigh_tmp.position_covariance_type=it->first; //custom robot id storage neigh_tmp.position_covariance_type=buzz_utility::get_rid_uint8compac(it->first); //custom robot id storage
neigh_tmp.longitude=(it->second).x; neigh_tmp.longitude=(it->second).x;
neigh_tmp.latitude=(it->second).y; neigh_tmp.latitude=(it->second).y;
neigh_tmp.altitude=(it->second).z; neigh_tmp.altitude=(it->second).z;