tf_smart_throttle.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00031 #include "tf_smart_throttle/tf_smart_throttle.h"
00032 
00033 namespace tf_smart_throttle {
00034 
00035 TransformManager::TransformManager(ros::NodeHandle& node)
00036 : node_(node), publish_all_(false)
00037 {
00038   ros::NodeHandle local_node("~");
00039 
00040   local_node.param(std::string("polling_frequency"),             polling_frequency_,              10.0);
00041   local_node.param(std::string("translational_update_distance"), translational_update_distance_,  0.10);
00042   local_node.param(std::string("angular_update_distance"),       angular_update_distance_,        0.10);
00043 
00044   // subscribe to tf messages
00045   subscriber_tf_ = node_.subscribe<tf::tfMessage>("tf", 100, boost::bind(&TransformManager::tfCallback, this, _1));
00046 
00047   // Advertise the service
00048   pub_ = node.advertise<tf::tfMessage>("tf_changes", 1, true);
00049   publish_all_srv_ = local_node.advertiseService("publish_all_transforms", &TransformManager::publishAllTransforms, this);
00050 }
00051 
00052 TransformManager::~TransformManager()
00053 {
00054 }
00055 
00056 void TransformManager::tfCallback(const tf::tfMessageConstPtr& msg_ptr)
00057 {
00058   const tf::tfMessage& message = *msg_ptr;
00059   boost::mutex::scoped_lock my_lock(frame_pairs_mutex_);
00060   for (unsigned int i = 0; i < message.transforms.size(); i++)
00061   {
00062     std::string frame_id = message.transforms[i].header.frame_id;
00063     std::string child_frame_id = message.transforms[i].child_frame_id;
00064     std::string id = frame_id + "<--" + child_frame_id;
00065     if(frame_pairs_.find(id) == frame_pairs_.end()) {
00066       ROS_INFO("Adding frame pair %s",id.c_str());
00067       FramePair frame_pair(child_frame_id, frame_id, translational_update_distance_, angular_update_distance_);
00068       frame_pairs_.insert(std::make_pair(id,frame_pair));
00069     }
00070   }
00071 }
00072 
00073 bool TransformManager::publishAllTransforms(tf_smart_throttle::PublishAllTransforms::Request& req, tf_smart_throttle::PublishAllTransforms::Response& resp)
00074 {
00075   publish_all_ = true;
00076   return true;
00077 }
00078 
00079 
00080 void TransformManager::publishAll()
00081 {
00082   tf::tfMessage msg;
00083   for (std::map<std::string,FramePair>::iterator i = frame_pairs_.begin(); i != frame_pairs_.end(); i++)
00084   {
00085    FramePair& fp = i->second;
00086 
00087    tfl_.transformPose(fp.target_frame_, fp.pose_in_, fp.pose_out_);
00088 
00089    const tf::Vector3&    origin   = fp.pose_out_.getOrigin();
00090    const tf::Quaternion& rotation = fp.pose_out_.getRotation();
00091 
00092    tf::StampedTransform stampedTf(tf::Transform(rotation, origin), fp.pose_out_.stamp_, fp.target_frame_, fp.source_frame_);
00093    geometry_msgs::TransformStamped msgtf;
00094    transformStampedTFToMsg(stampedTf, msgtf);
00095    msg.transforms.push_back(msgtf);
00096   }
00097 
00098   if (msg.transforms.size() > 0)
00099    pub_.publish(msg);
00100 }
00101 
00102 void TransformManager::publishChanged()
00103 {
00104   tf::tfMessage msg;
00105   for (std::map<std::string,FramePair>::iterator i = frame_pairs_.begin(); i != frame_pairs_.end(); i++)
00106   {
00107     FramePair& fp = i->second;
00108 
00109     tfl_.transformPose(fp.target_frame_, fp.pose_in_, fp.pose_out_);
00110 
00111     const tf::Vector3&    origin   = fp.pose_out_.getOrigin();
00112     const tf::Quaternion& rotation = fp.pose_out_.getRotation();
00113 
00114     if (origin.distance(fp.last_sent_pose_.getOrigin()) > fp.translational_update_distance_ ||
00115         rotation.angle(fp.last_sent_pose_.getRotation()) > fp.angular_update_distance_)
00116     {
00117       fp.last_sent_pose_ = fp.pose_out_;
00118 
00119       tf::StampedTransform stampedTf(tf::Transform(rotation, origin), fp.pose_out_.stamp_, fp.target_frame_, fp.source_frame_);
00120       geometry_msgs::TransformStamped msgtf;
00121       transformStampedTFToMsg(stampedTf, msgtf);
00122       msg.transforms.push_back(msgtf);
00123     }
00124   }
00125 
00126   if (msg.transforms.size() > 0)
00127     pub_.publish(msg);
00128 }
00129 
00130 void TransformManager::spin()
00131 {
00132 
00133   while (node_.ok())
00134   {
00135     try
00136     {
00137       boost::mutex::scoped_lock my_lock(frame_pairs_mutex_);
00138 
00139       if(publish_all_) {
00140         publishAll();
00141         //publish_all_ = false;
00142       }
00143       else {
00144         publishChanged();
00145       }
00146     }
00147     catch (tf::TransformException& ex)
00148     {
00149       ROS_DEBUG("Exception: %s\n", ex.what());
00150     }
00151     
00152     // Sleep until next polling
00153     if (polling_frequency_ > 0)
00154       ros::Duration().fromSec(1.0 / polling_frequency_).sleep();
00155   }
00156 }
00157 
00158 }
00159 
00164 int main(int argc, char** argv)
00165 {
00166   ros::init(argc, argv, "tf_smart_throttle");
00167 
00168   ros::NodeHandle nh;
00169   boost::thread spinner( boost::bind( &ros::spin ));
00170   tf_smart_throttle::TransformManager manager(nh);
00171   manager.spin();
00172   spinner.join();
00173   return 0;
00174 }


tf_smart_throttle
Author(s): bener
autogenerated on Thu Jan 2 2014 11:53:26