tf_republish.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  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
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 #include <boost/thread/mutex.hpp>
00035 
00036 #include <map>  
00037 
00038 #include <ros/ros.h>
00039 #include <tf/tfMessage.h>
00040 #include <tf/transform_datatypes.h>
00041 
00042 #include <geometry_msgs/PoseStamped.h>
00043 
00044 namespace pr2_interactive_manipulation
00045 {
00046 
00047 class TFRepublish
00048 {
00049 protected:
00050 
00051   class TransformInfo {
00052   public:          
00053     geometry_msgs::TransformStamped transform_;
00054     ros::Time last_received_;
00055 
00056     TransformInfo(geometry_msgs::TransformStamped trf) 
00057     {
00058       last_received_ = ros::Time(0);
00059       transform_ = trf;
00060     }
00061   };
00062   
00063   std::map<std::string, TransformInfo> transforms_;
00064   ros::NodeHandle root_nh_;
00065   ros::NodeHandle priv_nh_;
00066   ros::Publisher pub_;
00067   ros::Subscriber sub_;
00068   boost::mutex mutex_;
00069   ros::Timer sync_timer_;
00070   ros::Duration expiration_time_;
00071 
00072   void callback(const tf::tfMessageConstPtr &msg)
00073   {
00074     boost::mutex::scoped_lock lock(mutex_);
00075     for (size_t i=0; i<msg->transforms.size(); i++)
00076     {
00077       std::string key = msg->transforms[i].header.frame_id + msg->transforms[i].child_frame_id;
00078       std::map<std::string, TransformInfo>::iterator it = transforms_.find(key);
00079       if (it == transforms_.end())
00080       {
00081         transforms_.insert( std::pair<std::string, TransformInfo>(key, TransformInfo(msg->transforms[i])) );
00082       }
00083       else
00084       {
00085         if (msg->transforms[i].header.stamp > it->second.transform_.header.stamp)
00086         {
00087           it->second.transform_ = msg->transforms[i];
00088         }
00089       }      
00090     }
00091   }
00092 
00093   void sync()
00094   {
00095     mutex_.lock();
00096     tf::tfMessage msg;
00097     ros::Time time_now = ros::Time::now();
00098     ros::Time time_latest = ros::Time(0);
00099     //prune the list of transforms, and also find out the most recent one
00100     std::map<std::string, TransformInfo>::iterator it = transforms_.begin();
00101     while (it!=transforms_.end())
00102     {
00103       if ( time_now - it->second.transform_.header.stamp > expiration_time_ )
00104       {
00105         ROS_WARN_STREAM("TF Republisher: dropping transform " << it->first);
00106         transforms_.erase(it++);
00107       }
00108       else if ( (it++)->second.transform_.header.stamp > time_latest )
00109       {
00110         time_latest = it->second.transform_.header.stamp;
00111       }
00112     }
00113     if (transforms_.empty()) {mutex_.unlock(); return;}
00114     //publish all transforms we have; all get the time stamp of the most recent one
00115     for (it=transforms_.begin(); it!=transforms_.end(); it++)
00116     {
00117       msg.transforms.push_back(it->second.transform_);
00118       msg.transforms.back().header.stamp = time_latest;
00119     }
00120     mutex_.unlock();
00121     pub_.publish(msg);
00122   }
00123 
00124 public:
00125   TFRepublish() : root_nh_(""), priv_nh_("~")
00126   {
00127     std::string pub_topic;
00128     priv_nh_.param<std::string>("publish_topic", pub_topic, "/tf_republished");
00129     pub_ = root_nh_.advertise<tf::tfMessage>(pub_topic, 10);
00130     std::string sub_topic;
00131     priv_nh_.param<std::string>("subscribe_topic", sub_topic, "/tf_throttled");
00132     sub_ = root_nh_.subscribe(sub_topic, 10, &TFRepublish::callback, this);    
00133     double rate;
00134     priv_nh_.param<double>("rate", rate, 100.0);
00135     sync_timer_ =  root_nh_.createTimer(ros::Duration(1.0/rate), boost::bind( &TFRepublish::sync, this ) );
00136     double expiration_time;
00137     priv_nh_.param<double>("expiration_time", expiration_time, 1.1);
00138     expiration_time_ = ros::Duration(expiration_time);    
00139 
00140     ROS_INFO("TF republish started; listening to %s and publishing on %s at %f Hz", sub_topic.c_str(),
00141              pub_topic.c_str(), rate);
00142   }
00143 
00144 
00145 };
00146 
00147 }
00148 
00149 int main(int argc, char **argv)
00150 {
00151   ros::init(argc, argv, "tf_republish");
00152   pr2_interactive_manipulation::TFRepublish republish;
00153   ros::spin();
00154   return 0;
00155 }


tf_throttle
Author(s): Matei Ciocarlie
autogenerated on Fri Jan 3 2014 12:05:59