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 <ros/time.h>
00036
00037 #include <OGRE/OgreVector3.h>
00038 #include <OGRE/OgreQuaternion.h>
00039
00040 #include <boost/thread/mutex.hpp>
00041
00042 #include <geometry_msgs/Pose.h>
00043
00044 #include <tf/message_filter.h>
00045
00046 namespace tf
00047 {
00048 class TransformListener;
00049 }
00050
00051 namespace rviz
00052 {
00053 class Display;
00054
00055 class FrameManager;
00056 typedef boost::shared_ptr<FrameManager> FrameManagerPtr;
00057 typedef boost::weak_ptr<FrameManager> FrameManagerWPtr;
00058
00063 class FrameManager
00064 {
00065 public:
00071 static FrameManagerPtr instance();
00072
00078 ~FrameManager();
00079
00084 void setFixedFrame(const std::string& frame);
00085
00091 template<typename Header>
00092 bool getTransform(const Header& header, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00093 {
00094 return getTransform(header.frame_id, header.stamp, position, orientation);
00095 }
00096
00103 bool getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation);
00104
00111 template<typename Header>
00112 bool transform(const Header& header, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00113 {
00114 return transform(header.frame_id, header.stamp, pose, position, orientation);
00115 }
00116
00124 bool transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation);
00125
00127 void update();
00128
00134 bool frameHasProblems(const std::string& frame, ros::Time time, std::string& error);
00135
00141 bool transformHasProblems(const std::string& frame, ros::Time time, std::string& error);
00142
00151 template<class M>
00152 void registerFilterForTransformStatusCheck(tf::MessageFilter<M>* filter, Display* display)
00153 {
00154 filter->registerCallback(boost::bind(&FrameManager::messageCallback<M>, this, _1, display));
00155 filter->registerFailureCallback(boost::bind(&FrameManager::failureCallback<M>, this, _1, _2, display));
00156 }
00157
00159 const std::string& getFixedFrame() { return fixed_frame_; }
00160
00162 tf::TransformListener* getTFClient() { return tf_; }
00163
00173 std::string discoverFailureReason(const std::string& frame_id,
00174 const ros::Time& stamp,
00175 const std::string& caller_id,
00176 tf::FilterFailureReason reason);
00177
00178 private:
00179 FrameManager();
00180
00181 template<class M>
00182 void messageCallback(const boost::shared_ptr<M const>& msg, Display* display)
00183 {
00184 messageArrived(msg->header.frame_id, msg->header.stamp, msg->__connection_header ? (*msg->__connection_header)["callerid"] : "unknown", display);
00185 }
00186
00187 template<class M>
00188 void failureCallback(const boost::shared_ptr<M const>& msg, tf::FilterFailureReason reason, Display* display)
00189 {
00190 messageFailed(msg->header.frame_id, msg->header.stamp, msg->__connection_header ? (*msg->__connection_header)["callerid"] : "unknown", reason, display);
00191 }
00192
00193 void messageArrived(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, Display* display);
00194 void messageFailed(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason, Display* display);
00195
00196 struct CacheKey
00197 {
00198 CacheKey(const std::string& f, ros::Time t)
00199 : frame(f)
00200 , time(t)
00201 {}
00202
00203 bool operator<(const CacheKey& rhs) const
00204 {
00205 if (frame != rhs.frame)
00206 {
00207 return frame < rhs.frame;
00208 }
00209
00210 return time < rhs.time;
00211 }
00212
00213 std::string frame;
00214 ros::Time time;
00215 };
00216
00217 struct CacheEntry
00218 {
00219 CacheEntry(const Ogre::Vector3& p, const Ogre::Quaternion& o)
00220 : position(p)
00221 , orientation(o)
00222 {}
00223
00224 Ogre::Vector3 position;
00225 Ogre::Quaternion orientation;
00226 };
00227 typedef std::map<CacheKey, CacheEntry > M_Cache;
00228
00229 boost::mutex cache_mutex_;
00230 M_Cache cache_;
00231
00232 tf::TransformListener* tf_;
00233 std::string fixed_frame_;
00234 };
00235
00236 }
00237
00238 #endif // RVIZ_FRAME_MANAGER_H