tf_monitor.cpp
Go to the documentation of this file.
1 
2 
3 /*
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Willow Garage, Inc. nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
35 #include "tf/tf.h"
36 #include "tf/transform_listener.h"
37 #include <string>
38 #include <boost/bind.hpp>
39 #include <boost/thread.hpp>
40 #include "ros/ros.h"
41 
42 using namespace tf;
43 using namespace ros;
44 using namespace std;
45 
46 
47 class TFMonitor
48 {
49 public:
50  std::string framea_, frameb_;
52 
55  std::vector<std::string> chain_;
56  std::map<std::string, std::string> frame_authority_map;
57  std::map<std::string, std::vector<double> > delay_map;
58  std::map<std::string, std::vector<double> > authority_map;
59  std::map<std::string, std::vector<double> > authority_frequency_map;
60 
62 
63  tf::tfMessage message_;
64 
65  boost::mutex map_lock_;
67  {
68  const tf::tfMessage& message = *(msg_evt.getConstMessage());
69  std::string authority = msg_evt.getPublisherName(); // lookup the authority
70  process_callback(message, authority, false);
71  }
72 
74  {
75  const tf::tfMessage& message = *(msg_evt.getConstMessage());
76  std::string authority = msg_evt.getPublisherName() + std::string("(static)"); // lookup the authority
77  process_callback(message, authority, true);
78  }
79 
80 
81  void process_callback(const tf::tfMessage& message, const std::string & authority, bool is_static)
82  {
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++)
86  {
87  frame_authority_map[message.transforms[i].child_frame_id] = authority;
88 
89  double offset;
90  if (is_static)
91  {
92  offset = 0.0;
93  }
94  else
95  {
96  offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec();
97  }
98  average_offset += offset;
99 
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())
102  {
103  delay_map[message.transforms[i].child_frame_id] = std::vector<double>(1,offset);
104  }
105  else
106  {
107  it->second.push_back(offset);
108  if (it->second.size() > 1000)
109  it->second.erase(it->second.begin());
110  }
111 
112  }
113 
114  average_offset /= max((size_t) 1, message.transforms.size());
115 
116  //create the authority log
117  std::map<std::string, std::vector<double> >::iterator it2 = authority_map.find(authority);
118  if (it2 == authority_map.end())
119  {
120  authority_map[authority] = std::vector<double>(1,average_offset);
121  }
122  else
123  {
124  it2->second.push_back(average_offset);
125  if (it2->second.size() > 1000)
126  it2->second.erase(it2->second.begin());
127  }
128 
129  //create the authority frequency log
130  std::map<std::string, std::vector<double> >::iterator it3 = authority_frequency_map.find(authority);
131  if (it3 == authority_frequency_map.end())
132  {
133  authority_frequency_map[authority] = std::vector<double>(1,ros::Time::now().toSec());
134  }
135  else
136  {
137  it3->second.push_back(ros::Time::now().toSec());
138  if (it3->second.size() > 1000)
139  it3->second.erase(it3->second.begin());
140  }
141 
142  };
143 
144  TFMonitor(bool using_specific_chain, std::string framea = "", std::string frameb = ""):
145  framea_(framea), frameb_(frameb),
146  using_specific_chain_(using_specific_chain)
147  {
148 
149  if (using_specific_chain_)
150  {
151  cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush;
152  while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0)))
153  cout << "." << flush;
154  cout << endl;
155 
156  try{
157  tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_);
158  }
159  catch(tf::TransformException& ex){
160  ROS_WARN("Transform Exception %s", ex.what());
161  return;
162  }
163 
164  /* cout << "Chain currently is:" <<endl;
165  for (unsigned int i = 0; i < chain_.size(); i++)
166  {
167  cout << chain_[i] <<", ";
168  }
169  cout <<endl;*/
170  }
171  subscriber_tf_ = node_.subscribe<tf::tfMessage>("tf", 100, boost::bind(&TFMonitor::callback, this, _1));
172  subscriber_tf_static_ = node_.subscribe<tf::tfMessage>("tf_static", 100, boost::bind(&TFMonitor::static_callback, this, _1));
173 
174  }
175 
176  std::string outputFrameInfo(const std::map<std::string, std::vector<double> >::iterator& it, const std::string& frame_authority)
177  {
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++)
182  {
183  average_delay += it->second[i];
184  max_delay = std::max(max_delay, it->second[i]);
185  }
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;
188  return ss.str();
189  }
190 
191  void spin()
192  {
193 
194  // create tf listener
195  double max_diff = 0;
196  double avg_diff = 0;
197  double lowpass = 0.01;
198  unsigned int counter = 0;
199 
200 
201 
202  while (node_.ok()){
204  counter++;
205  // printf("looping %d\n", counter);
206  if (using_specific_chain_)
207  {
208  tf_.lookupTransform(framea_, frameb_, Time(), tmp);
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;
212  }
213  Duration(0.01).sleep();
214  if (counter > 20){
215  counter = 0;
216 
217  if (using_specific_chain_)
218  {
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++)
222  {
223  cout << chain_[i] ;
224  if (i != chain_.size()-1)
225  cout <<" -> ";
226  }
227  cout <<std::endl;
228  cout << "Net delay " << " avg = " << avg_diff <<": max = " << max_diff << endl;
229  }
230  else
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)
236  {
237  if (using_specific_chain_ )
238  {
239  for ( unsigned int i = 0 ; i < chain_.size(); i++)
240  {
241  if (it->first != chain_[i])
242  continue;
243 
244  cout << outputFrameInfo(it, frame_authority_map[it->first]);
245  }
246  }
247  else
248  cout << outputFrameInfo(it, frame_authority_map[it->first]);
249  }
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)
254  {
255  double average_delay = 0;
256  double max_delay = 0;
257  for (unsigned int i = 0; i < it1->second.size(); i++)
258  {
259  average_delay += it1->second[i];
260  max_delay = std::max(max_delay, it1->second[i]);
261  }
262  average_delay /= it1->second.size();
263  double frequency_out = (double)(it2->second.size())/std::max(0.00000001, (it2->second.back() - it2->second.front()));
264  //cout << "output" <<&(*it2) <<" " << it2->second.back() <<" " << it2->second.front() <<" " << std::max((size_t)1, it2->second.size()) << " " << frequency_out << endl;
265  cout << "Node: " <<it1->first << " " << frequency_out <<" Hz, Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl;
266  }
267 
268  }
269  }
270 
271  }
272 };
273 
274 
275 int main(int argc, char ** argv)
276 {
277  //Initialize ROS
278  init(argc, argv, "tf_monitor", ros::init_options::AnonymousName);
279 
280 
281  string framea, frameb;
282  bool using_specific_chain = true;
283  if (argc == 3){
284  framea = argv[1];
285  frameb = argv[2];
286  }
287  else if (argc == 1)
288  using_specific_chain = false;
289  else{
290  ROS_INFO("TF_Monitor: usage: tf_monitor framea frameb");
291  return -1;
292  }
293 
294  std::string searched_param;
295  std::string tf_prefix;
296  ros::NodeHandle local_nh("~");
297  local_nh.searchParam("tf_prefix", searched_param);
298  local_nh.getParam(searched_param, tf_prefix);
299 
300 
301  //Make sure we don't start before recieving time when in simtime
302  int iterations = 0;
303  while (ros::Time::now() == ros::Time())
304  {
305  if (++iterations > 10)
306  {
307  ROS_INFO("tf_monitor waiting for time to be published");
308  iterations = 0;
309  }
310  ros::WallDuration(0.1).sleep();
311  }
312 
313  ros::NodeHandle nh;
314  boost::thread spinner( boost::bind( &ros::spin ));
315  TFMonitor monitor(using_specific_chain, tf::resolve(tf_prefix, framea), tf::resolve(tf_prefix, frameb));
316  monitor.spin();
317  spinner.join();
318  return 0;
319 
320 }
ros::NodeHandle node_
Definition: tf_monitor.cpp:53
std::map< std::string, std::vector< double > > authority_frequency_map
Definition: tf_monitor.cpp:59
std::vector< std::string > chain_
Definition: tf_monitor.cpp:55
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)
tf::tfMessage message_
Definition: tf_monitor.cpp:63
void callback(const ros::MessageEvent< tf::tfMessage const > &msg_evt)
Definition: tf_monitor.cpp:66
Definition: exceptions.h:38
ros::Time stamp_
The timestamp associated with this transform.
#define ROS_WARN(...)
bool using_specific_chain_
Definition: tf_monitor.cpp:51
void spinner()
ROSCPP_DECL void spin(Spinner &spinner)
std::string frameb_
Definition: tf_monitor.cpp:50
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
Block until a transform is possible or it times out.
Definition: tf.cpp:348
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
Definition: tf.cpp:159
const std::string & getPublisherName() const
std::map< std::string, std::string > frame_authority_map
Definition: tf_monitor.cpp:56
const boost::shared_ptr< ConstMessage > & getConstMessage() const
void process_callback(const tf::tfMessage &message, const std::string &authority, bool is_static)
Definition: tf_monitor.cpp:81
bool sleep() const
void chainAsVector(const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const
Debugging function that will print the spanning chain of transforms. Possible exceptions tf::LookupEx...
Definition: tf.cpp:427
ros::Subscriber subscriber_tf_static_
Definition: tf_monitor.cpp:54
#define ROS_INFO(...)
bool searchParam(const std::string &key, std::string &result) const
int main(int argc, char **argv)
Definition: tf_monitor.cpp:275
void static_callback(const ros::MessageEvent< tf::tfMessage const > &msg_evt)
Definition: tf_monitor.cpp:73
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
Get the transform between two frames by frame ID.
Definition: tf.cpp:238
This class inherits from Transformer and automatically subscribes to ROS transform messages...
std::map< std::string, std::vector< double > > authority_map
Definition: tf_monitor.cpp:58
std::string outputFrameInfo(const std::map< std::string, std::vector< double > >::iterator &it, const std::string &frame_authority)
Definition: tf_monitor.cpp:176
boost::mutex map_lock_
Definition: tf_monitor.cpp:65
std::map< std::string, std::vector< double > > delay_map
Definition: tf_monitor.cpp:57
bool getParam(const std::string &key, std::string &s) const
TransformListener tf_
Definition: tf_monitor.cpp:61
static Time now()
bool ok() const
TFMonitor(bool using_specific_chain, std::string framea="", std::string frameb="")
Definition: tf_monitor.cpp:144
void spin()
Definition: tf_monitor.cpp:191
The Stamped Transform datatype used by tf.


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Tue Jan 2 2018 03:17:40