diff --git a/include/roscontroller.h b/include/roscontroller.h index 2db4380..f754be9 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -40,7 +40,7 @@ private: uint64_t payload; 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; + //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; int timer_step=0; int robot_id=0; //int oldcmdID=0; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0e3e2a0..eb59414 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -40,13 +40,14 @@ namespace rosbzz_node{ map< int, buzz_utility::Pos_struct >::iterator it; rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); - sensor_msgs::NavSatFix tmp; - 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.longitude=(it->second).x; - tmp.latitude=(it->second).y; - tmp.altitude=(it->second).z; - neigh_pos_array.pos_neigh.push_back(tmp); + for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){ + sensor_msgs::NavSatFix neigh_tmp; + neigh_tmp.position_covariance_type=it->first; //custom robot id storage + neigh_tmp.longitude=(it->second).x; + neigh_tmp.latitude=(it->second).y; + neigh_tmp.altitude=(it->second).z; + neigh_pos_array.pos_neigh.push_back(neigh_tmp); + std::cout<<"long obt"<=10){ neighbours_pos_map.clear(); - raw_neighbours_pos_map.clear(); + //raw_neighbours_pos_map.clear(); timer_step=0; } } @@ -248,7 +249,7 @@ namespace rosbzz_node{ neighbours_pos_payload[1]=atan(longitude/latitude); neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude); } catch (std::overflow_error e) { - std::cout << e.what() << " Error in convertion to spherical coordinate system "; + std::cout << e.what() << " Error in convertion to spherical coordinate system "<