tf_monitor.cpp
Go to the documentation of this file.
00001 
00002 
00003 /*
00004  * Copyright (c) 2008, Willow Garage, Inc.
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *     * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *     * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00016  *       contributors may be used to endorse or promote products derived from
00017  *       this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00035 #include "tf/tf.h"
00036 #include "tf/transform_listener.h"
00037 #include <string>
00038 #include <boost/bind.hpp>
00039 #include <boost/thread.hpp>
00040 #include "ros/ros.h"
00041 
00042 using namespace tf;
00043 using namespace ros;
00044 using namespace std;
00045 
00046 
00047 class TFMonitor
00048 {
00049 public:
00050   std::string framea_, frameb_;
00051   bool using_specific_chain_;
00052   
00053   ros::NodeHandle node_;
00054   ros::Subscriber subscriber_tf_, subscriber_tf_static_;
00055   std::vector<std::string> chain_;
00056   std::map<std::string, std::string> frame_authority_map;
00057   std::map<std::string, std::vector<double> > delay_map;
00058   std::map<std::string, std::vector<double> > authority_map;
00059   std::map<std::string, std::vector<double> > authority_frequency_map;
00060   
00061   TransformListener tf_;
00062 
00063   tf::tfMessage message_;
00064 
00065   boost::mutex map_lock_;
00066   void callback(const ros::MessageEvent<tf::tfMessage const>& msg_evt)
00067   {
00068     const tf::tfMessage& message = *(msg_evt.getConstMessage());
00069     std::string authority = msg_evt.getPublisherName(); // lookup the authority 
00070     process_callback(message, authority, false);
00071   }
00072   
00073   void static_callback(const ros::MessageEvent<tf::tfMessage const>& msg_evt)
00074   {
00075     const tf::tfMessage& message = *(msg_evt.getConstMessage());
00076     std::string authority = msg_evt.getPublisherName() + std::string("(static)"); // lookup the authority 
00077     process_callback(message, authority, true);
00078   }
00079 
00080 
00081   void process_callback(const tf::tfMessage& message, const std::string & authority, bool is_static)
00082   {
00083     double average_offset = 0;
00084     boost::mutex::scoped_lock my_lock(map_lock_);
00085     for (unsigned int i = 0; i < message.transforms.size(); i++)
00086     {
00087       frame_authority_map[message.transforms[i].child_frame_id] = authority;
00088 
00089       double offset;
00090       if (is_static)
00091       {
00092         offset = 0.0;
00093       }
00094       else
00095       {
00096         offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec();
00097       }
00098       average_offset  += offset;
00099       
00100       std::map<std::string, std::vector<double> >::iterator it = delay_map.find(message.transforms[i].child_frame_id);
00101       if (it == delay_map.end())
00102       {
00103         delay_map[message.transforms[i].child_frame_id] = std::vector<double>(1,offset);
00104       }
00105       else
00106       {
00107         it->second.push_back(offset);
00108         if (it->second.size() > 1000) 
00109           it->second.erase(it->second.begin());
00110       }
00111       
00112     } 
00113     
00114     average_offset /= max((size_t) 1, message.transforms.size());
00115 
00116     //create the authority log
00117     std::map<std::string, std::vector<double> >::iterator it2 = authority_map.find(authority);
00118     if (it2 == authority_map.end())
00119     {
00120       authority_map[authority] = std::vector<double>(1,average_offset);
00121     }
00122     else
00123     {
00124       it2->second.push_back(average_offset);
00125       if (it2->second.size() > 1000) 
00126         it2->second.erase(it2->second.begin());
00127     }
00128     
00129     //create the authority frequency log
00130     std::map<std::string, std::vector<double> >::iterator it3 = authority_frequency_map.find(authority);
00131     if (it3 == authority_frequency_map.end())
00132     {
00133       authority_frequency_map[authority] = std::vector<double>(1,ros::Time::now().toSec());
00134     }
00135     else
00136     {
00137       it3->second.push_back(ros::Time::now().toSec());
00138       if (it3->second.size() > 1000) 
00139         it3->second.erase(it3->second.begin());
00140     }
00141     
00142   };
00143 
00144   TFMonitor(bool using_specific_chain, std::string framea  = "", std::string frameb = ""):
00145     framea_(framea), frameb_(frameb),
00146     using_specific_chain_(using_specific_chain)
00147   {
00148     
00149     if (using_specific_chain_)
00150     {
00151       cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush;
00152       while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0)))
00153         cout << "." << flush;
00154       cout << endl;
00155      
00156       try{
00157         tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_);
00158       }
00159       catch(tf::TransformException& ex){
00160         ROS_WARN("Transform Exception %s", ex.what());
00161         return;
00162       } 
00163 
00164       /*      cout << "Chain currently is:" <<endl;
00165       for (unsigned int i = 0; i < chain_.size(); i++)
00166       {
00167         cout << chain_[i] <<", ";
00168       }
00169       cout <<endl;*/
00170     }
00171     subscriber_tf_ = node_.subscribe<tf::tfMessage>("tf", 100, boost::bind(&TFMonitor::callback, this, _1));
00172     subscriber_tf_static_ = node_.subscribe<tf::tfMessage>("tf_static", 100, boost::bind(&TFMonitor::static_callback, this, _1));
00173     
00174   }
00175 
00176   std::string outputFrameInfo(const std::map<std::string, std::vector<double> >::iterator& it, const std::string& frame_authority)
00177   {
00178     std::stringstream ss;
00179     double average_delay = 0;
00180     double max_delay = 0;
00181     for (unsigned int i = 0; i < it->second.size(); i++)
00182     {
00183       average_delay += it->second[i];
00184       max_delay = std::max(max_delay, it->second[i]);
00185     }
00186     average_delay /= it->second.size();
00187     ss << "Frame: " << it->first <<" published by "<< frame_authority << " Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl;
00188     return ss.str();
00189   }
00190   
00191   void spin()
00192   {  
00193     
00194     // create tf listener
00195     double max_diff = 0;
00196     double avg_diff = 0;
00197     double lowpass = 0.01;
00198     unsigned int counter = 0;
00199     
00200   
00201 
00202   while (node_.ok()){
00203     tf::StampedTransform tmp;
00204     counter++;
00205     //    printf("looping %d\n", counter);
00206     if (using_specific_chain_)
00207     {
00208       tf_.lookupTransform(framea_, frameb_, Time(), tmp);
00209       double diff = (Time::now() - tmp.stamp_).toSec();
00210       avg_diff = lowpass * diff + (1-lowpass)*avg_diff;
00211       if (diff > max_diff) max_diff = diff;
00212     }
00213     Duration(0.01).sleep();
00214     if (counter > 20){
00215       counter = 0;
00216 
00217       if (using_specific_chain_)
00218       {
00219         std::cout <<std::endl<< std::endl<< std::endl<< "RESULTS: for "<< framea_ << " to " << frameb_  <<std::endl;
00220         cout << "Chain is: ";
00221         for (unsigned int i = 0; i < chain_.size(); i++)
00222         {
00223           cout << chain_[i] ;
00224           if (i != chain_.size()-1)
00225             cout <<" -> ";
00226         }
00227         cout <<std::endl;
00228         cout << "Net delay " << "    avg = " << avg_diff <<": max = " << max_diff << endl;
00229       }
00230       else
00231         std::cout <<std::endl<< std::endl<< std::endl<< "RESULTS: for all Frames" <<std::endl;
00232       boost::mutex::scoped_lock lock(map_lock_);  
00233       std::cout <<std::endl << "Frames:" <<std::endl;
00234       std::map<std::string, std::vector<double> >::iterator it = delay_map.begin();
00235       for ( ; it != delay_map.end() ; ++it)
00236       {
00237         if (using_specific_chain_ )
00238         {
00239           for ( unsigned int i = 0 ; i < chain_.size(); i++)
00240           {
00241             if (it->first != chain_[i])
00242               continue;
00243             
00244             cout << outputFrameInfo(it, frame_authority_map[it->first]);
00245           }
00246         }
00247         else
00248           cout << outputFrameInfo(it, frame_authority_map[it->first]);
00249       }          
00250       std::cerr <<std::endl<< "All Broadcasters:" << std::endl;
00251       std::map<std::string, std::vector<double> >::iterator it1 = authority_map.begin();
00252       std::map<std::string, std::vector<double> >::iterator it2 = authority_frequency_map.begin();
00253       for ( ; it1 != authority_map.end() ; ++it1, ++it2)
00254       {
00255         double average_delay = 0;
00256         double max_delay = 0;
00257         for (unsigned int i = 0; i < it1->second.size(); i++)
00258         {
00259           average_delay += it1->second[i];
00260           max_delay = std::max(max_delay, it1->second[i]);
00261         }
00262         average_delay /= it1->second.size();
00263         double frequency_out = (double)(it2->second.size())/std::max(0.00000001, (it2->second.back() - it2->second.front()));
00264         //cout << "output" <<&(*it2) <<" " << it2->second.back() <<" " << it2->second.front() <<" " << std::max((size_t)1, it2->second.size()) << " " << frequency_out << endl;
00265         cout << "Node: " <<it1->first << " " << frequency_out <<" Hz, Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl;
00266       }
00267       
00268     }
00269   }
00270 
00271   }
00272 };
00273 
00274 
00275 int main(int argc, char ** argv)
00276 {
00277   //Initialize ROS
00278   init(argc, argv, "tf_monitor", ros::init_options::AnonymousName);
00279 
00280 
00281   string framea, frameb;
00282   bool using_specific_chain = true;
00283   if (argc == 3){
00284     framea = argv[1];
00285     frameb = argv[2];
00286   }
00287   else if (argc == 1)
00288     using_specific_chain = false;
00289   else{
00290     ROS_INFO("TF_Monitor: usage: tf_monitor framea frameb");
00291     return -1;
00292   }
00293 
00294   std::string searched_param;
00295   std::string tf_prefix;
00296   ros::NodeHandle local_nh("~");
00297   local_nh.searchParam("tf_prefix", searched_param);
00298   local_nh.getParam(searched_param, tf_prefix);
00299   
00300 
00301   //Make sure we don't start before recieving time when in simtime
00302   int iterations = 0;
00303   while (ros::Time::now() == ros::Time())
00304   {
00305     if (++iterations > 10)
00306     {
00307       ROS_INFO("tf_monitor waiting for time to be published");
00308       iterations = 0;
00309     }
00310     ros::WallDuration(0.1).sleep();
00311   }
00312 
00313   ros::NodeHandle nh;
00314   boost::thread spinner( boost::bind( &ros::spin ));
00315   TFMonitor monitor(using_specific_chain, tf::resolve(tf_prefix, framea), tf::resolve(tf_prefix, frameb));
00316   monitor.spin();
00317   spinner.join();
00318   return 0;
00319 
00320 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jun 6 2019 18:45:32