00001 #ifndef __RTT_TF_TF_INTERFACE_H 00002 #define __RTT_TF_TF_INTERFACE_H 00003 00004 #include <ros/ros.h> 00005 #include <string> 00006 #include <vector> 00007 #include <rtt/RTT.hpp> 00008 #include <geometry_msgs/TransformStamped.h> 00009 00010 namespace rtt_tf { 00011 00013 class TFInterface { 00014 public: 00016 TFInterface(RTT::TaskContext *owner) : 00017 lookupTransform("lookupTransform"), 00018 lookupTransformAtTime("lookupTransformAtTime"), 00019 broadcastTransform("broadcastTransform"), 00020 broadcastTransforms("broadcastTransforms"), 00021 canTransform("canTransform") 00022 { 00023 owner->requires("tf")->addOperationCaller(lookupTransform); 00024 owner->requires("tf")->addOperationCaller(lookupTransformAtTime); 00025 owner->requires("tf")->addOperationCaller(broadcastTransform); 00026 owner->requires("tf")->addOperationCaller(broadcastTransforms); 00027 owner->requires("tf")->addOperationCaller(canTransform); 00028 } 00029 00031 bool ready() { 00032 return 00033 lookupTransform.ready() && 00034 lookupTransformAtTime.ready() && 00035 broadcastTransform.ready() && 00036 broadcastTransforms.ready() && 00037 canTransform.ready(); 00038 } 00039 00040 RTT::OperationCaller<geometry_msgs::TransformStamped(const std::string&, const std::string&)> lookupTransform; 00041 RTT::OperationCaller<geometry_msgs::TransformStamped(const std::string&, const std::string&, const ros::Time&)> lookupTransformAtTime; 00042 RTT::OperationCaller<void(const geometry_msgs::TransformStamped&)> broadcastTransform; 00043 RTT::OperationCaller<void(const std::vector<geometry_msgs::TransformStamped>&)> broadcastTransforms; 00044 RTT::OperationCaller<bool(const std::string&, const std::string&)> canTransform; 00045 }; 00046 } 00047 00048 #endif // ifndef __RTT_TF_TF_INTERFACE_H