00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef RVIZ_FRAME_MANAGER_H
00031 #define RVIZ_FRAME_MANAGER_H
00032
00033 #include <map>
00034
00035 #include <QObject>
00036
00037 #include <ros/time.h>
00038
00039 #include <OGRE/OgreVector3.h>
00040 #include <OGRE/OgreQuaternion.h>
00041
00042 #include <boost/thread/mutex.hpp>
00043
00044 #include <geometry_msgs/Pose.h>
00045
00046 #include <tf/message_filter.h>
00047
00048 namespace tf
00049 {
00050 class TransformListener;
00051 }
00052
00053 namespace rviz
00054 {
00055 class Display;
00056
00061 class FrameManager: public QObject
00062 {
00063 Q_OBJECT
00064 public:
00065 FrameManager();
00066
00072 ~FrameManager();
00073
00078 void setFixedFrame(const std::string& frame);
00079
00085 template<typename Header>
00086 bool getTransform(const Header& header, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00087 {
00088 return getTransform(header.frame_id, header.stamp, position, orientation);
00089 }
00090
00097 bool getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation);
00098
00105 template<typename Header>
00106 bool transform(const Header& header, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00107 {
00108 return transform(header.frame_id, header.stamp, pose, position, orientation);
00109 }
00110
00118 bool transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation);
00119
00121 void update();
00122
00128 bool frameHasProblems(const std::string& frame, ros::Time time, std::string& error);
00129
00135 bool transformHasProblems(const std::string& frame, ros::Time time, std::string& error);
00136
00145 template<class M>
00146 void registerFilterForTransformStatusCheck(tf::MessageFilter<M>* filter, Display* display)
00147 {
00148 filter->registerCallback(boost::bind(&FrameManager::messageCallback<M>, this, _1, display));
00149 filter->registerFailureCallback(boost::bind(&FrameManager::failureCallback<M>, this, _1, _2, display));
00150 }
00151
00153 const std::string& getFixedFrame() { return fixed_frame_; }
00154
00156 tf::TransformListener* getTFClient() { return tf_; }
00157
00167 std::string discoverFailureReason(const std::string& frame_id,
00168 const ros::Time& stamp,
00169 const std::string& caller_id,
00170 tf::FilterFailureReason reason);
00171
00172 Q_SIGNALS:
00174 void fixedFrameChanged();
00175
00176 private:
00177 template<class M>
00178 void messageCallback(const boost::shared_ptr<M const>& msg, Display* display)
00179 {
00180 messageArrived(msg->header.frame_id, msg->header.stamp, msg->__connection_header ? (*msg->__connection_header)["callerid"] : "unknown", display);
00181 }
00182
00183 template<class M>
00184 void failureCallback(const boost::shared_ptr<M const>& msg, tf::FilterFailureReason reason, Display* display)
00185 {
00186 messageFailed(msg->header.frame_id, msg->header.stamp, msg->__connection_header ? (*msg->__connection_header)["callerid"] : "unknown", reason, display);
00187 }
00188
00189 void messageArrived(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, Display* display);
00190 void messageFailed(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason, Display* display);
00191
00192 struct CacheKey
00193 {
00194 CacheKey(const std::string& f, ros::Time t)
00195 : frame(f)
00196 , time(t)
00197 {}
00198
00199 bool operator<(const CacheKey& rhs) const
00200 {
00201 if (frame != rhs.frame)
00202 {
00203 return frame < rhs.frame;
00204 }
00205
00206 return time < rhs.time;
00207 }
00208
00209 std::string frame;
00210 ros::Time time;
00211 };
00212
00213 struct CacheEntry
00214 {
00215 CacheEntry(const Ogre::Vector3& p, const Ogre::Quaternion& o)
00216 : position(p)
00217 , orientation(o)
00218 {}
00219
00220 Ogre::Vector3 position;
00221 Ogre::Quaternion orientation;
00222 };
00223 typedef std::map<CacheKey, CacheEntry > M_Cache;
00224
00225 boost::mutex cache_mutex_;
00226 M_Cache cache_;
00227
00228 tf::TransformListener* tf_;
00229 std::string fixed_frame_;
00230 };
00231
00232 }
00233
00234 #endif // RVIZ_FRAME_MANAGER_H