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 <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


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32