frame_manager.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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->header.frame_id, msg->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->header.frame_id, msg->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   // the current synchronized time, used to overwrite ros:Time(0)
00276   ros::Time sync_time_;
00277 
00278   // used for approx. syncing
00279   double sync_delta_;
00280   double current_delta_;
00281 };
00282 
00283 }
00284 
00285 #endif // RVIZ_FRAME_MANAGER_H


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15