00001 #ifndef OROCOS_RTT_TF_COMPONENT_HPP 00002 #define OROCOS_RTT_TF_COMPONENT_HPP 00003 00004 #include <rtt/RTT.hpp> 00005 #include <tf/tf.h> 00006 #include <tf/tfMessage.h> 00007 00008 namespace rtt_tf 00009 { 00010 // Inherit from TaskContext and Transformer, the second is required in order to use tf_prefix 00011 class RTT_TF: public RTT::TaskContext, protected tf::Transformer 00012 { 00013 static const int DEFAULT_BUFFER_SIZE = 100; 00014 00015 boost::shared_ptr<tf::Transformer> m_transformer; 00016 double prop_cache_time; 00017 double prop_buffer_size; 00018 std::string prop_tf_prefix; 00019 00020 RTT::InputPort<tf::tfMessage> port_tf_in; 00021 RTT::OutputPort<tf::tfMessage> port_tf_out; 00022 00023 bool canTransform( 00024 const std::string& parent, 00025 const std::string& child); 00026 00027 geometry_msgs::TransformStamped lookupTransform( 00028 const std::string& parent, 00029 const std::string& child); 00030 00031 geometry_msgs::TransformStamped lookupTransformAtTime( 00032 const std::string& parent, 00033 const std::string& child, 00034 const ros::Time& common_time); 00035 00036 void broadcastTransform( 00037 const geometry_msgs::TransformStamped &tform); 00038 00039 void broadcastTransforms( 00040 const std::vector<geometry_msgs::TransformStamped> &tforms); 00041 00042 void addTFOperations(RTT::Service::shared_ptr service); 00043 00044 public: 00045 RTT_TF(std::string const& name); 00046 00047 bool configureHook(); 00048 00049 bool startHook(){return true;}; 00050 00051 void updateHook(); 00052 00053 void stopHook(){}; 00054 00055 void cleanupHook(){}; 00056 }; 00057 }//namespace rtt_tf 00058 #endif