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 <OgreVector3.h>
00040 #include <OgreQuaternion.h>
00041
00042 #include <boost/thread/mutex.hpp>
00043
00044 #include <geometry_msgs/Pose.h>
00045
00046 #ifndef Q_MOC_RUN
00047 #include <tf/message_filter.h>
00048 #endif
00049
00050 namespace tf
00051 {
00052 class TransformListener;
00053 }
00054
00055 namespace rviz
00056 {
00057 class Display;
00058
00063 class FrameManager: public QObject
00064 {
00065 Q_OBJECT
00066 public:
00067
00068 enum SyncMode {
00069 SyncOff = 0,
00070 SyncExact,
00071 SyncApprox
00072 };
00073
00077 FrameManager(boost::shared_ptr<tf::TransformListener> tf = boost::shared_ptr<tf::TransformListener>());
00078
00084 ~FrameManager();
00085
00090 void setFixedFrame(const std::string& frame);
00091
00093 void setPause( bool pause );
00094
00095 bool getPause() { return pause_; }
00096
00098 void setSyncMode( SyncMode mode );
00099
00100 SyncMode getSyncMode() { return sync_mode_; }
00101
00103 void syncTime( ros::Time time );
00104
00106 ros::Time getTime() { return sync_time_; }
00107
00113 template<typename Header>
00114 bool getTransform(const Header& header, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00115 {
00116 return getTransform(header.frame_id, header.stamp, position, orientation);
00117 }
00118
00125 bool getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation);
00126
00133 template<typename Header>
00134 bool transform(const Header& header, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00135 {
00136 return transform(header.frame_id, header.stamp, pose, position, orientation);
00137 }
00138
00146 bool transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation);
00147
00149 void update();
00150
00156 bool frameHasProblems(const std::string& frame, ros::Time time, std::string& error);
00157
00163 bool transformHasProblems(const std::string& frame, ros::Time time, std::string& error);
00164
00173 template<class M>
00174 void registerFilterForTransformStatusCheck(tf::MessageFilter<M>* filter, Display* display)
00175 {
00176 filter->registerCallback(boost::bind(&FrameManager::messageCallback<M>, this, _1, display));
00177 filter->registerFailureCallback(boost::bind(&FrameManager::failureCallback<M>, this, _1, _2, display));
00178 }
00179
00181 const std::string& getFixedFrame() { return fixed_frame_; }
00182
00184 tf::TransformListener* getTFClient() { return tf_.get(); }
00185
00187 const boost::shared_ptr<tf::TransformListener>& getTFClientPtr() { return tf_; }
00188
00198 std::string discoverFailureReason(const std::string& frame_id,
00199 const ros::Time& stamp,
00200 const std::string& caller_id,
00201 tf::FilterFailureReason reason);
00202
00203 Q_SIGNALS:
00205 void fixedFrameChanged();
00206
00207 private:
00208
00209 bool adjustTime( const std::string &frame, ros::Time &time );
00210
00211 template<class M>
00212 void messageCallback(const ros::MessageEvent<M const>& msg_evt, Display* display)
00213 {
00214 boost::shared_ptr<M const> const &msg = msg_evt.getConstMessage();
00215 std::string authority = msg_evt.getPublisherName();
00216
00217 messageArrived(msg->info.header.frame_id, msg->info.header.stamp, authority, display);
00218 }
00219
00220 template<class M>
00221 void failureCallback(const ros::MessageEvent<M const>& msg_evt, tf::FilterFailureReason reason, Display* display)
00222 {
00223 boost::shared_ptr<M const> const &msg = msg_evt.getConstMessage();
00224 std::string authority = msg_evt.getPublisherName();
00225
00226 messageFailed(msg->info.header.frame_id, msg->info.header.stamp, authority, reason, display);
00227 }
00228
00229 void messageArrived(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, Display* display);
00230 void messageFailed(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason, Display* display);
00231
00232 struct CacheKey
00233 {
00234 CacheKey(const std::string& f, ros::Time t)
00235 : frame(f)
00236 , time(t)
00237 {}
00238
00239 bool operator<(const CacheKey& rhs) const
00240 {
00241 if (frame != rhs.frame)
00242 {
00243 return frame < rhs.frame;
00244 }
00245
00246 return time < rhs.time;
00247 }
00248
00249 std::string frame;
00250 ros::Time time;
00251 };
00252
00253 struct CacheEntry
00254 {
00255 CacheEntry(const Ogre::Vector3& p, const Ogre::Quaternion& o)
00256 : position(p)
00257 , orientation(o)
00258 {}
00259
00260 Ogre::Vector3 position;
00261 Ogre::Quaternion orientation;
00262 };
00263 typedef std::map<CacheKey, CacheEntry > M_Cache;
00264
00265 boost::mutex cache_mutex_;
00266 M_Cache cache_;
00267
00268 boost::shared_ptr<tf::TransformListener> tf_;
00269 std::string fixed_frame_;
00270
00271 bool pause_;
00272
00273 SyncMode sync_mode_;
00274
00275
00276 ros::Time sync_time_;
00277
00278
00279 double sync_delta_;
00280 double current_delta_;
00281 };
00282
00283 }
00284
00285 #endif // RVIZ_FRAME_MANAGER_H