rtt_tf-component.cpp
Go to the documentation of this file.
00001 /* (C) 2011 Ruben Smits, ruben.smits@mech.kuleuven.be, Department of Mechanical
00002  * Engineering, Katholieke Universiteit Leuven, Belgium.
00003  *
00004  * This library is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU Lesser General Public
00006  * License as published by the Free Software Foundation; either
00007  * version 2.1 of the License, or (at your option) any later version.
00008  *
00009  * This library is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012  * Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public
00015  * License along with this library; if not, write to the Free Software
00016  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00017  */
00018 
00019 /*
00020  * Copyright (c) 2008, Willow Garage, Inc.
00021  * All rights reserved.
00022  *
00023  * Redistribution and use in source and binary forms, with or without
00024  * modification, are permitted provided that the following conditions are met:
00025  *
00026  *     * Redistributions of source code must retain the above copyright
00027  *       notice, this list of conditions and the following disclaimer.
00028  *     * Redistributions in binary form must reproduce the above copyright
00029  *       notice, this list of conditions and the following disclaimer in the
00030  *       documentation and/or other materials provided with the distribution.
00031  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00032  *       contributors may be used to endorse or promote products derived from
00033  *       this software without specific prior written permission.
00034  *
00035  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00036  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00037  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00038  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00039  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00040  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00041  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00042  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00043  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00044  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00045  * POSSIBILITY OF SUCH DAMAGE.
00046  */
00047 
00048 #include "rtt_tf-component.hpp"
00049 #include <rtt/Component.hpp>
00050 #include <ros/ros.h>
00051 
00052 namespace rtt_tf
00053 {
00054 
00055   using namespace RTT;
00056   using namespace tf;
00057 
00058   RTT_TF::RTT_TF(const std::string& name) :
00059     TaskContext(name, PreOperational), 
00060     tf::Transformer(true, ros::Duration(Transformer::DEFAULT_CACHE_TIME)),
00061     prop_cache_time(Transformer::DEFAULT_CACHE_TIME),
00062     prop_buffer_size(DEFAULT_BUFFER_SIZE)
00063   {
00064     this->addProperty("cache_time", prop_cache_time);
00065     this->addProperty("buffer_size", prop_buffer_size);
00066     this->addProperty("tf_prefix", prop_tf_prefix);
00067     this->addEventPort("tf_in", port_tf_in);
00068     this->addPort("tf_out", port_tf_out);
00069 
00070     this->addTFOperations(this->provides());
00071     this->addTFOperations(this->provides("tf"));
00072   }
00073 
00074   void RTT_TF::addTFOperations(RTT::Service::shared_ptr service)
00075   {
00076     service->addOperation("lookupTransform", &RTT_TF::lookupTransform, this)
00077       .doc("Lookup the most recent transform from source to target.")
00078       .arg("target", "target frame")
00079       .arg("source", "source frame");
00080 
00081     service->addOperation("lookupTransformAtTime", &RTT_TF::lookupTransformAtTime, this)
00082       .doc("Lookup the most recent transform from source to target at a specific time.")
00083       .arg("target", "Target frame")
00084       .arg("source", "Source frame")
00085       .arg("common_time", "[ros::Time] The common time at which the transform should be computed");
00086 
00087     service->addOperation("broadcastTransform", &RTT_TF::broadcastTransform, this, RTT::OwnThread)
00088       .doc("Broadcast a stamped transform immediately.")
00089       .arg("transform", "[geometry_msgs::TransformStamped]");
00090 
00091     service->addOperation("broadcastTransforms", &RTT_TF::broadcastTransforms, this, RTT::OwnThread)
00092       .doc("Broadcast a stamped transform immediately.")
00093       .arg("transforms", "[std::vector<geometry_msgs::TransformStamped>]");
00094 
00095     service->addOperation("canTransform", &RTT_TF::canTransform, this)
00096       .doc("Check if the transform from source to target can be resolved..")
00097       .arg("target", "Target frame")
00098       .arg("source", "Source frame");
00099   }
00100 
00101   bool RTT_TF::configureHook()
00102   {
00103     Logger::In(this->getName());
00104 
00105     // Get tf prefix rosparam
00106     ros::NodeHandle nh("~");
00107     std::string tf_prefix_param_key;
00108     if(nh.searchParam("tf_prefix",tf_prefix_param_key)) {
00109       nh.getParam(tf_prefix_param_key, prop_tf_prefix);
00110     }
00111     
00112     // Update the tf::Transformer prefix
00113     tf_prefix_ = prop_tf_prefix;
00114     
00115     // Connect to tf topic
00116     ConnPolicy cp = ConnPolicy::buffer(prop_buffer_size);
00117     cp.transport = 3; //3=ROS
00118     cp.name_id = "/tf";
00119 
00120     return (port_tf_in.createStream(cp) && port_tf_out.createStream(cp));
00121   }
00122 
00123   void RTT_TF::updateHook()
00124   {
00125     Logger::In(this->getName());
00126 #ifndef NDEBUG
00127     //log(Debug) << "In update" << endlog();
00128 #endif
00129     try {
00130       tf::tfMessage msg_in;
00131 
00132       while (port_tf_in.read(msg_in) == NewData) {
00133         for (unsigned int i = 0; i < msg_in.transforms.size(); i++) {
00134           StampedTransform trans;
00135           transformStampedMsgToTF(msg_in.transforms[i], trans);
00136           try {
00137 #if ROS_VERSION_MINIMUM(1,11,0)
00138             this->setTransform(trans);
00139 #else
00140             std::map<std::string, std::string>* msg_header_map =
00141               msg_in.__connection_header.get();
00142             std::string authority;
00143             std::map<std::string, std::string>::iterator it =
00144               msg_header_map->find("callerid");
00145 
00146             if (it == msg_header_map->end()) {
00147               log(Warning) << "Message received without callerid" << endlog();
00148               authority = "no callerid";
00149             } else {
00150               authority = it->second;
00151             }
00152             this->setTransform(trans, authority);
00153 #endif
00154           } catch (TransformException& ex) {
00155             log(Error) << "Failure to set received transform from "
00156               << msg_in.transforms[i].child_frame_id << " to "
00157               << msg_in.transforms[i].header.frame_id
00158               << " with error: " << ex.what() << endlog();
00159           }
00160         }
00161       }
00162     } catch (std::exception& ex) {
00163       log(Error) << ex.what() << endlog();
00164     }
00165   }
00166 
00167   geometry_msgs::TransformStamped RTT_TF::lookupTransform(
00168       const std::string& target,
00169       const std::string& source)
00170   {
00171     tf::StampedTransform stamped_tf;
00172     ros::Time common_time;
00173     this->getLatestCommonTime(source, target, common_time,NULL);
00174     static_cast<tf::Transformer*>(this)->lookupTransform(target, source, common_time, stamped_tf);
00175     geometry_msgs::TransformStamped msg;
00176     tf::transformStampedTFToMsg(stamped_tf,msg);
00177     return msg;
00178   }
00179 
00180   bool RTT_TF::canTransform(
00181       const std::string& target,
00182       const std::string& source)
00183   {
00184     tf::StampedTransform stamped_tf;
00185     ros::Time common_time;
00186     this->getLatestCommonTime(source, target, common_time,NULL);
00187     return static_cast<tf::Transformer*>(this)->canTransform(target, source, common_time);
00188   }
00189 
00190   geometry_msgs::TransformStamped RTT_TF::lookupTransformAtTime(
00191       const std::string& target,
00192       const std::string& source,
00193       const ros::Time& common_time)
00194   {
00195     tf::StampedTransform stamped_tf;
00196     static_cast<tf::Transformer*>(this)->lookupTransform(target, source, common_time, stamped_tf);
00197     geometry_msgs::TransformStamped msg;
00198     tf::transformStampedTFToMsg(stamped_tf,msg);
00199     return msg;
00200   }
00201 
00202   void RTT_TF::broadcastTransform(
00203       const geometry_msgs::TransformStamped &tform)
00204   {
00205     // Populate the TF message
00206     tf::tfMessage msg_out;
00207     msg_out.transforms.push_back(tform);
00208 
00209     // Resolve names
00210     msg_out.transforms.back().header.frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().header.frame_id);
00211     msg_out.transforms.back().child_frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().child_frame_id);
00212 
00213     port_tf_out.write(msg_out);
00214   }
00215 
00216   void RTT_TF::broadcastTransforms(
00217       const std::vector<geometry_msgs::TransformStamped> &tform)
00218   {
00219     // Populate the TF message
00220     tf::tfMessage msg_out;
00221 
00222     // Resolve names
00223     for(std::vector<geometry_msgs::TransformStamped>::const_iterator it = tform.begin();
00224         it != tform.end();
00225         ++it)
00226     {
00227       msg_out.transforms.push_back(*it);
00228       msg_out.transforms.back().header.frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().header.frame_id);
00229       msg_out.transforms.back().child_frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().child_frame_id);
00230     }
00231 
00232     port_tf_out.write(msg_out);
00233   }
00234 
00235 }//namespace
00236 
00237 /*
00238  * Using this macro, only one component may live
00239  * in one library *and* you may *not* link this library
00240  * with another component library. Use 
00241  * ORO_CREATE_COMPONENT_TYPE()
00242  * ORO_LIST_COMPONENT_TYPE(Rtt_tf)
00243  * In case you want to link with another library that
00244  * already contains components.
00245  *
00246  * If you have put your component class
00247  * in a namespace, don't forget to add it here too:
00248  */
00249 ORO_CREATE_COMPONENT(rtt_tf::RTT_TF)


rtt_tf
Author(s): Ruben Smits
autogenerated on Wed Sep 16 2015 06:58:36