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_message_;
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 tf::tfMessageConstPtr& msg_ptr)
00067   {
00068     const tf::tfMessage& message = *msg_ptr;
00069     //Lookup the authority 
00070     std::string authority;
00071     std::map<std::string, std::string>* msg_header_map = message.__connection_header.get();
00072     std::map<std::string, std::string>::iterator it = msg_header_map->find("callerid");
00073     if (it == msg_header_map->end())
00074     {
00075       ROS_WARN("Message recieved without callerid");
00076       authority = "no callerid";
00077     }
00078     else
00079     {
00080       authority = it->second;
00081     }
00082 
00083 
00084     double average_offset = 0;
00085     boost::mutex::scoped_lock my_lock(map_lock_);  
00086     for (unsigned int i = 0; i < message.transforms.size(); i++)
00087     {
00088       frame_authority_map[message.transforms[i].child_frame_id] = authority;
00089 
00090       double offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec();
00091       average_offset  += offset;
00092       
00093       std::map<std::string, std::vector<double> >::iterator it = delay_map.find(message.transforms[i].child_frame_id);
00094       if (it == delay_map.end())
00095       {
00096         delay_map[message.transforms[i].child_frame_id] = std::vector<double>(1,offset);
00097       }
00098       else
00099       {
00100         it->second.push_back(offset);
00101         if (it->second.size() > 1000) 
00102           it->second.erase(it->second.begin());
00103       }
00104       
00105     } 
00106     
00107     average_offset /= max((size_t) 1, message.transforms.size());
00108 
00109     //create the authority log
00110     std::map<std::string, std::vector<double> >::iterator it2 = authority_map.find(authority);
00111     if (it2 == authority_map.end())
00112     {
00113       authority_map[authority] = std::vector<double>(1,average_offset);
00114     }
00115     else
00116     {
00117       it2->second.push_back(average_offset);
00118       if (it2->second.size() > 1000) 
00119         it2->second.erase(it2->second.begin());
00120     }
00121     
00122     //create the authority frequency log
00123     std::map<std::string, std::vector<double> >::iterator it3 = authority_frequency_map.find(authority);
00124     if (it3 == authority_frequency_map.end())
00125     {
00126       authority_frequency_map[authority] = std::vector<double>(1,ros::Time::now().toSec());
00127     }
00128     else
00129     {
00130       it3->second.push_back(ros::Time::now().toSec());
00131       if (it3->second.size() > 1000) 
00132         it3->second.erase(it3->second.begin());
00133     }
00134     
00135   };
00136 
00137   TFMonitor(bool using_specific_chain, std::string framea  = "", std::string frameb = ""):
00138     framea_(framea), frameb_(frameb),
00139     using_specific_chain_(using_specific_chain)
00140   {
00141     
00142     if (using_specific_chain_)
00143     {
00144       cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush;
00145       while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0)))
00146         cout << "." << flush;
00147       cout << endl;
00148      
00149       try{
00150         tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_);
00151       }
00152       catch(tf::TransformException& ex){
00153         ROS_WARN("Transform Exception %s", ex.what());
00154         return;
00155       } 
00156 
00157       /*      cout << "Chain currently is:" <<endl;
00158       for (unsigned int i = 0; i < chain_.size(); i++)
00159       {
00160         cout << chain_[i] <<", ";
00161       }
00162       cout <<endl;*/
00163     }
00164     subscriber_tf_ = node_.subscribe<tf::tfMessage>("tf", 100, boost::bind(&TFMonitor::callback, this, _1));
00165     subscriber_tf_message_ = node_.subscribe<tf::tfMessage>("tf_message", 100, boost::bind(&TFMonitor::callback, this, _1));
00166     
00167   }
00168 
00169   std::string outputFrameInfo(const std::map<std::string, std::vector<double> >::iterator& it, const std::string& frame_authority)
00170   {
00171     std::stringstream ss;
00172     double average_delay = 0;
00173     double max_delay = 0;
00174     for (unsigned int i = 0; i < it->second.size(); i++)
00175     {
00176       average_delay += it->second[i];
00177       max_delay = std::max(max_delay, it->second[i]);
00178     }
00179     average_delay /= it->second.size();
00180     ss << "Frame: " << it->first <<" published by "<< frame_authority << " Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl;
00181     return ss.str();
00182   }
00183   
00184   void spin()
00185   {  
00186     
00187     // create tf listener
00188     double max_diff = 0;
00189     double avg_diff = 0;
00190     double lowpass = 0.01;
00191     unsigned int counter = 0;
00192     
00193   
00194 
00195   while (node_.ok()){
00196     tf::StampedTransform tmp;
00197     counter++;
00198     //    printf("looping %d\n", counter);
00199     if (using_specific_chain_)
00200     {
00201       tf_.lookupTransform(framea_, frameb_, Time(), tmp);
00202       double diff = (Time::now() - tmp.stamp_).toSec();
00203       avg_diff = lowpass * diff + (1-lowpass)*avg_diff;
00204       if (diff > max_diff) max_diff = diff;
00205     }
00206     Duration(0.01).sleep();
00207     if (counter > 20){
00208       counter = 0;
00209 
00210       if (using_specific_chain_)
00211       {
00212         std::cout <<std::endl<< std::endl<< std::endl<< "RESULTS: for "<< framea_ << " to " << frameb_  <<std::endl;
00213         cout << "Chain is: ";
00214         for (unsigned int i = 0; i < chain_.size(); i++)
00215         {
00216           cout << chain_[i] ;
00217           if (i != chain_.size()-1)
00218             cout <<" -> ";
00219         }
00220         cout <<std::endl;
00221         cout << "Net delay " << "    avg = " << avg_diff <<": max = " << max_diff << endl;
00222       }
00223       else
00224         std::cout <<std::endl<< std::endl<< std::endl<< "RESULTS: for all Frames" <<std::endl;
00225       boost::mutex::scoped_lock lock(map_lock_);  
00226       std::cout <<std::endl << "Frames:" <<std::endl;
00227       std::map<std::string, std::vector<double> >::iterator it = delay_map.begin();
00228       for ( ; it != delay_map.end() ; ++it)
00229       {
00230         if (using_specific_chain_ )
00231         {
00232           for ( unsigned int i = 0 ; i < chain_.size(); i++)
00233           {
00234             if (it->first != chain_[i])
00235               continue;
00236             
00237             cout << outputFrameInfo(it, frame_authority_map[it->first]);
00238           }
00239         }
00240         else
00241           cout << outputFrameInfo(it, frame_authority_map[it->first]);
00242       }          
00243       std::cerr <<std::endl<< "All Broadcasters:" << std::endl;
00244       std::map<std::string, std::vector<double> >::iterator it1 = authority_map.begin();
00245       std::map<std::string, std::vector<double> >::iterator it2 = authority_frequency_map.begin();
00246       for ( ; it1 != authority_map.end() ; ++it1, ++it2)
00247       {
00248         double average_delay = 0;
00249         double max_delay = 0;
00250         for (unsigned int i = 0; i < it1->second.size(); i++)
00251         {
00252           average_delay += it1->second[i];
00253           max_delay = std::max(max_delay, it1->second[i]);
00254         }
00255         average_delay /= it1->second.size();
00256         double frequency_out = (double)(it2->second.size())/std::max(0.00000001, (it2->second.back() - it2->second.front()));
00257         //cout << "output" <<&(*it2) <<" " << it2->second.back() <<" " << it2->second.front() <<" " << std::max((size_t)1, it2->second.size()) << " " << frequency_out << endl;
00258         cout << "Node: " <<it1->first << " " << frequency_out <<" Hz, Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl;
00259       }
00260       
00261     }
00262   }
00263 
00264   }
00265 };
00266 
00267 
00268 int main(int argc, char ** argv)
00269 {
00270   //Initialize ROS
00271   init(argc, argv, "tf_monitor", ros::init_options::AnonymousName);
00272 
00273 
00274   string framea, frameb;
00275   bool using_specific_chain = true;
00276   if (argc == 3){
00277     framea = argv[1];
00278     frameb = argv[2];
00279   }
00280   else if (argc == 1)
00281     using_specific_chain = false;
00282   else{
00283     ROS_INFO("TF_Monitor: usage: tf_monitor framea frameb");
00284     return -1;
00285   }
00286 
00287   std::string searched_param;
00288   std::string tf_prefix;
00289   ros::NodeHandle local_nh("~");
00290   local_nh.searchParam("tf_prefix", searched_param);
00291   local_nh.getParam(searched_param, tf_prefix);
00292   
00293 
00294   //Make sure we don't start before recieving time when in simtime
00295   int iterations = 0;
00296   while (ros::Time::now() == ros::Time())
00297   {
00298     if (++iterations > 10)
00299     {
00300       ROS_INFO("tf_monitor waiting for time to be published");
00301       iterations = 0;
00302     }
00303     ros::WallDuration(0.1).sleep();
00304   }
00305 
00306   ros::NodeHandle nh;
00307   boost::thread spinner( boost::bind( &ros::spin ));
00308   TFMonitor monitor(using_specific_chain, tf::resolve(tf_prefix, framea), tf::resolve(tf_prefix, frameb));
00309   monitor.spin();
00310   spinner.join();
00311   return 0;
00312 
00313 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 27 2015 13:02:09