38 #include <boost/bind.hpp>
39 #include <boost/thread.hpp>
57 std::map<std::string, std::vector<double> >
delay_map;
70 process_callback(message, authority,
false);
76 std::string authority = msg_evt.
getPublisherName() + std::string(
"(static)");
77 process_callback(message, authority,
true);
81 void process_callback(
const tf::tfMessage& message,
const std::string & authority,
bool is_static)
83 double average_offset = 0;
84 boost::mutex::scoped_lock my_lock(map_lock_);
85 for (
unsigned int i = 0; i < message.transforms.size(); i++)
87 frame_authority_map[message.transforms[i].child_frame_id] = authority;
96 offset = (
ros::Time::now() - message.transforms[i].header.stamp).toSec();
98 average_offset += offset;
100 std::map<std::string, std::vector<double> >::iterator it = delay_map.find(message.transforms[i].child_frame_id);
101 if (it == delay_map.end())
103 delay_map[message.transforms[i].child_frame_id] = std::vector<double>(1,offset);
107 it->second.push_back(offset);
108 if (it->second.size() > 1000)
109 it->second.erase(it->second.begin());
114 average_offset /= max((
size_t) 1, message.transforms.size());
117 std::map<std::string, std::vector<double> >::iterator it2 = authority_map.find(authority);
118 if (it2 == authority_map.end())
120 authority_map[authority] = std::vector<double>(1,average_offset);
124 it2->second.push_back(average_offset);
125 if (it2->second.size() > 1000)
126 it2->second.erase(it2->second.begin());
130 std::map<std::string, std::vector<double> >::iterator it3 = authority_frequency_map.find(authority);
131 if (it3 == authority_frequency_map.end())
133 authority_frequency_map[authority] = std::vector<double>(1,
ros::Time::now().toSec());
138 if (it3->second.size() > 1000)
139 it3->second.erase(it3->second.begin());
144 TFMonitor(
bool using_specific_chain, std::string framea =
"", std::string frameb =
""):
145 framea_(framea), frameb_(frameb),
146 using_specific_chain_(using_specific_chain)
149 if (using_specific_chain_)
151 cout <<
"Waiting for transform chain to become available between "<< framea_ <<
" and " << frameb_<<
" " << flush;
153 cout <<
"." << flush;
160 ROS_WARN(
"Transform Exception %s", ex.what());
176 std::string
outputFrameInfo(
const std::map<std::string, std::vector<double> >::iterator& it,
const std::string& frame_authority)
178 std::stringstream ss;
179 double average_delay = 0;
180 double max_delay = 0;
181 for (
unsigned int i = 0; i < it->second.size(); i++)
183 average_delay += it->second[i];
184 max_delay = std::max(max_delay, it->second[i]);
186 average_delay /= it->second.size();
187 ss <<
"Frame: " << it->first <<
" published by "<< frame_authority <<
" Average Delay: " << average_delay <<
" Max Delay: " << max_delay << std::endl;
197 double lowpass = 0.01;
198 unsigned int counter = 0;
206 if (using_specific_chain_)
209 double diff = (Time::now() - tmp.
stamp_).toSec();
210 avg_diff = lowpass * diff + (1-lowpass)*avg_diff;
211 if (diff > max_diff) max_diff = diff;
217 if (using_specific_chain_)
219 std::cout <<std::endl<< std::endl<< std::endl<<
"RESULTS: for "<< framea_ <<
" to " << frameb_ <<std::endl;
220 cout <<
"Chain is: ";
221 for (
unsigned int i = 0; i < chain_.size(); i++)
224 if (i != chain_.size()-1)
228 cout <<
"Net delay " <<
" avg = " << avg_diff <<
": max = " << max_diff << endl;
231 std::cout <<std::endl<< std::endl<< std::endl<<
"RESULTS: for all Frames" <<std::endl;
232 boost::mutex::scoped_lock lock(map_lock_);
233 std::cout <<std::endl <<
"Frames:" <<std::endl;
234 std::map<std::string, std::vector<double> >::iterator it = delay_map.begin();
235 for ( ; it != delay_map.end() ; ++it)
237 if (using_specific_chain_ )
239 for (
unsigned int i = 0 ; i < chain_.size(); i++)
241 if (it->first != chain_[i])
244 cout << outputFrameInfo(it, frame_authority_map[it->first]);
248 cout << outputFrameInfo(it, frame_authority_map[it->first]);
250 std::cerr <<std::endl<<
"All Broadcasters:" << std::endl;
251 std::map<std::string, std::vector<double> >::iterator it1 = authority_map.begin();
252 std::map<std::string, std::vector<double> >::iterator it2 = authority_frequency_map.begin();
253 for ( ; it1 != authority_map.end() ; ++it1, ++it2)
255 double average_delay = 0;
256 double max_delay = 0;
257 for (
unsigned int i = 0; i < it1->second.size(); i++)
259 average_delay += it1->second[i];
260 max_delay = std::max(max_delay, it1->second[i]);
262 average_delay /= it1->second.size();
263 double frequency_out = (double)(it2->second.size())/std::max(0.00000001, (it2->second.back() - it2->second.front()));
265 cout <<
"Node: " <<it1->first <<
" " << frequency_out <<
" Hz, Average Delay: " << average_delay <<
" Max Delay: " << max_delay << std::endl;
275 int main(
int argc,
char ** argv)
281 string framea, frameb;
282 bool using_specific_chain =
true;
288 using_specific_chain =
false;
290 ROS_INFO(
"TF_Monitor: usage: tf_monitor framea frameb");
294 std::string searched_param;
295 std::string tf_prefix;
298 local_nh.
getParam(searched_param, tf_prefix);
305 if (++iterations > 10)
307 ROS_INFO(
"tf_monitor waiting for time to be published");