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

neighbors pos publisher addition with raw data

This commit is contained in:
vivek-shankar 2017-01-11 13:14:05 -05:00
parent 177a0cd20d
commit e0f2ee92ec
2 changed files with 25 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/roscontroller.h
View File

@ -39,7 +39,8 @@ private:
double cur_pos[3]; double cur_pos[3];
uint64_t payload; uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0; int timer_step=0;
int robot_id=0; int robot_id=0;
//int oldcmdID=0; //int oldcmdID=0;
@ -74,8 +75,11 @@ private:
/*Refresh neighbours Position for every ten step*/ /*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step); void maintain_pos(int tim_step);
/*Maintain neighbours position*/ /*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ); void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
/*Set the current position of the robot callback*/ /*Set the current position of the robot callback*/
void set_cur_pos(double latitude, void set_cur_pos(double latitude,

26
src/roscontroller.cpp
View File

@ -37,19 +37,19 @@ namespace rosbzz_node{
{ {
/*Update neighbors position inside Buzz*/ /*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map); buzz_utility::neighbour_pos_callback(neighbours_pos_map);
map< int, buzz_utility::Pos_struct >::iterator it; map< int, buzz_utility::Pos_struct >::iterator it;
rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear();
sensor_msgs::NavSatFix tmp; sensor_msgs::NavSatFix tmp;
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){ for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
tmp.position_covariance_type=it->first; //custom robot id storage tmp.position_covariance_type=it->first; //custom robot id storage
tmp.longitude=(it->second).x; tmp.longitude=(it->second).x;
tmp.latitude=(it->second).y; tmp.latitude=(it->second).y;
tmp.altitude=(it->second).z; tmp.altitude=(it->second).z;
neigh_pos_array.pos_neigh.push_back(tmp);
neigh_pos_array.pos_neigh.push_back(tmp);
} }
neigh_pos_pub.publish(neigh_pos_array); neigh_pos_pub.publish(neigh_pos_array);
/*Check updater state and step code*/ /*Check updater state and step code*/
if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING) if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING)
/*Step buzz script */ /*Step buzz script */
@ -207,18 +207,25 @@ namespace rosbzz_node{
void roscontroller::maintain_pos(int tim_step){ void roscontroller::maintain_pos(int tim_step){
if(timer_step >=10){ if(timer_step >=10){
neighbours_pos_map.clear(); neighbours_pos_map.clear();
raw_neighbours_pos_map.clear();
timer_step=0; timer_step=0;
} }
} }
/*Maintain neighbours position*/ /*Puts neighbours position*/
void roscontroller::neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ){ void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id); map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
if(it!=neighbours_pos_map.end()) if(it!=neighbours_pos_map.end())
neighbours_pos_map.erase(it); neighbours_pos_map.erase(it);
neighbours_pos_map.insert(make_pair(id, pos_arr)); neighbours_pos_map.insert(make_pair(id, pos_arr));
} }
/*Puts raw neighbours position*/
void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id);
if(it!=neighbours_pos_map.end())
raw_neighbours_pos_map.erase(it);
raw_neighbours_pos_map.insert(make_pair(id, pos_arr));
}
/*Set the current position of the robot callback*/ /*Set the current position of the robot callback*/
void roscontroller::set_cur_pos(double latitude, void roscontroller::set_cur_pos(double latitude,
@ -285,6 +292,7 @@ namespace rosbzz_node{
/* Extract neighbours position from payload*/ /* Extract neighbours position from payload*/
double neighbours_pos_payload[3]; double neighbours_pos_payload[3];
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
/*Convert obtained position to relative position*/ /*Convert obtained position to relative position*/
for(i=0;i<3;i++){ for(i=0;i<3;i++){
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i]; neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
@ -294,7 +302,9 @@ namespace rosbzz_node{
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
/*pass neighbour position to local maintaner*/ /*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]); buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
neighbours_pos_maintain((int)out[1],n_pos); /*Put RID and pos*/
raw_neighbours_pos_put((int)out[1],n_pos);
neighbours_pos_put((int)out[1],n_pos);
delete[] out; delete[] out;
buzz_utility::in_msg_process((message_obt+3)); buzz_utility::in_msg_process((message_obt+3));