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, public tf::Transformer 00012 { 00013 static const int DEFAULT_BUFFER_SIZE = 100; 00014 00015 boost::shared_ptr<tf::Transformer> m_transformer; 00016 bool prop_interpolating; 00017 double prop_cache_time; 00018 double prop_buffer_size; 00019 std::string prop_tf_prefix; 00020 00021 RTT::InputPort<tf::tfMessage> port_tf_in; 00022 RTT::OutputPort<tf::tfMessage> port_tf_out; 00023 00024 geometry_msgs::TransformStamped lookupTransformService( 00025 const std::string& parent, 00026 const std::string& child); 00027 00028 geometry_msgs::TransformStamped lookupTransformAtTimeService( 00029 const std::string& parent, 00030 const std::string& child, 00031 const ros::Time& common_time); 00032 00033 void broadcastTransformService( 00034 const geometry_msgs::TransformStamped &tform); 00035 00036 public: 00037 RTT_TF(std::string const& name); 00038 00039 bool configureHook(); 00040 00041 bool startHook(){return true;}; 00042 00043 void updateHook(); 00044 00045 void stopHook(){}; 00046 00047 void cleanupHook(){}; 00048 }; 00049 }//namespace rtt_tf 00050 #endif