uav_statistic.cpp
Go to the documentation of this file.
1 
23 #include <iostream>
24 #include <fstream>
25 #include <sstream>
26 #include <ros/ros.h>
27 #include <math.h>
28 
29 #include <boost/function.hpp>
30 #include <boost/bind.hpp>
31 #include <boost/thread.hpp>
32 
33 #include "std_msgs/String.h"
34 #include "nav_msgs/Odometry.h"
35 #include "geometry_msgs/Twist.h"
36 
37 using namespace std;
38 
39 int SWARM_SIZE = 3;
40 int NEI_DIS = 5;
41 int COM_DIS = 12;
42 
43 struct Position{
44  float x;
45  float y;
46 };
47 
48 float pos_dis(Position self, Position nei)
49 {
50  return sqrt((self.x - nei.x)*(self.x - nei.x) + (self.y - nei.y)*(self.y - nei.y));
51 }
52 
53 std::map<int, Position> pos_map;
54 std::map<int, std::vector<int> > neighbor_map;
55 
56 void callback(const nav_msgs::OdometryConstPtr &lmsg, int r_id)
57 {
58  Position p;
59  p.x = lmsg->pose.pose.position.x;
60  p.y = lmsg->pose.pose.position.y;
61  std::map<int, Position >::iterator pos_it;
62  pos_it = pos_map.find(r_id);
63  if(pos_it != pos_map.end()) {
64  pos_it->second = p;
65  }
66  else {
67  pos_map.insert(std::pair<int, Position>(r_id, p));
68  }
69 
70  /*Position self = p;
71  Position nei;
72  std::vector<int> neis;
73  std::map<int, Position >::iterator it;
74  for(it = pos_map.begin(); it != pos_map.end(); it++) {
75  if(it->first == r_id) {
76  continue;
77  }
78  else {
79  nei = it->second;
80  if (pos_dis(self, nei) < COM_DIS) {
81  neis.push_back(it->first);
82  }
83  }
84  }
85 
86  std::map<int, std::vector<int> >::iterator nei_it;
87  nei_it = neighbor_map.find(r_id);
88  if(nei_it != neighbor_map.end()) {
89  nei_it->second = neis;
90  }
91  else {
92  neighbor_map.insert(std::pair < int, std::vector < int > > (r_id, neis));
93  }*/
94 }
95 
96 Position get_pos(int r_id)
97 {
98  std::map<int, Position >::iterator it;
99  it = pos_map.find(r_id);
100  if(it != pos_map.end()) {
101  return it->second;
102  }
103 
104  Position p;
105  p.x = 0;
106  p.y = 0;
107  return p;
108 }
109 
110 std::vector<int> get_neighbor(int r_id)
111 {
112  std::map<int, std::vector<int> >::iterator it;
113  it = neighbor_map.find(r_id);
114  if(it != neighbor_map.end()) {
115  return it->second;
116  }
117 
118  std::vector<int> nei;
119  nei.clear();
120  return nei;
121 }
122 
123 float nei_dis(int id1, int id2)
124 {
125  Position p1 = get_pos(id1);
126  Position p2 = get_pos(id2);
127 
128  return pos_dis(p1, p2);
129 }
130 
131 float cal_error()
132 {
133  float err = 0;
134  int count = 0;
135  for(int i = 0; i < SWARM_SIZE; i++) {
136  std::vector<int> nei = get_neighbor(i);
137  for(int j = 0; j < nei.size(); j++) {
138  err += abs((nei_dis(i, nei[j]) - NEI_DIS));
139  count++;
140  }
141  }
142 
143  if(count > 0) {
144  return err/count;
145  }
146  else {
147  return -1;
148  }
149 }
150 
151 void process()
152 {
153  while(1) {
154  for (int i = 0; i < SWARM_SIZE; i++) {
155  int r_id = i;
156  Position self = get_pos(i);
157  Position nei;
158  std::vector<int> neis;
159  std::map<int, Position>::iterator it;
160  for (it = pos_map.begin(); it != pos_map.end(); it++) {
161  if (it->first == r_id) {
162  continue;
163  } else {
164  nei = it->second;
165  if (pos_dis(self, nei) < COM_DIS) {
166  neis.push_back(it->first);
167  }
168  }
169  }
170 
171  std::map < int, std::vector < int > > ::iterator nei_it;
172  nei_it = neighbor_map.find(r_id);
173  if (nei_it != neighbor_map.end()) {
174  nei_it->second = neis;
175  } else {
176  neighbor_map.insert(std::pair < int, std::vector < int > > (r_id, neis));
177  }
178  }
179  ros::Duration(0.1).sleep();
180  }
181 }
182 
183 int main(int argc, char** argv)
184 {
185  ros::init(argc, argv, "app3_statistic");
186  ros::NodeHandle nh;
187 
188  std::vector<ros::Subscriber> sub_vec;
189  sub_vec.resize(SWARM_SIZE);
190 
191  for(int i = 0; i < SWARM_SIZE; i++) {
192  stringstream ss;
193  ss<<"/uav"<<i<<"/fix_odom";
194  sub_vec[i] = nh.subscribe<nav_msgs::Odometry>(ss.str(), 1000, boost::bind(callback, _1, i));
195  }
196 
197  ros::Duration(1).sleep();
198 
199  boost::thread* process_thread_ = new boost::thread(process);
200 
201  ofstream file;
202  file.open("uav_statistic.txt");
203 
204  int step = 0;
205  while(ros::ok()) {
206  float err = cal_error();
207  std::cout<<step<<" "<<err<<std::endl;
208  file<<step<<" "<<err<<std::endl;
209  step++;
210  ros::spinOnce();
211  ros::Duration(0.5).sleep();
212  }
213 
214  ros::spin();
215  return 0;
216 }
void process()
float cal_error()
Position get_pos(int r_id)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void callback(const nav_msgs::OdometryConstPtr &lmsg, int r_id)
int SWARM_SIZE
int COM_DIS
ROSCPP_DECL bool ok()
ROSCPP_DECL void spin()
std::map< int, Position > pos_map
float pos_dis(Position self, Position nei)
int NEI_DIS
std::vector< int > get_neighbor(int r_id)
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()
std::map< int, std::vector< int > > neighbor_map
float nei_dis(int id1, int id2)


test_statistic
Author(s):
autogenerated on Mon Jun 10 2019 14:02:29