frame_manager.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef RVIZ_FRAME_MANAGER_H
31 #define RVIZ_FRAME_MANAGER_H
32 
33 #include <map>
34 
35 #include <QObject>
36 
37 #include <ros/time.h>
38 
39 #include <OgreVector3.h>
40 #include <OgreQuaternion.h>
41 
42 #include <boost/thread/mutex.hpp>
43 
44 #include <geometry_msgs/Pose.h>
45 
46 #ifndef Q_MOC_RUN
47 #include <tf/message_filter.h>
48 #endif
49 
50 namespace tf
51 {
52 class TransformListener;
53 }
54 
55 namespace rviz
56 {
57 class Display;
58 
63 class FrameManager: public QObject
64 {
65 Q_OBJECT
66 public:
67 
68  enum SyncMode {
69  SyncOff = 0,
71  SyncApprox
72  };
73 
78 
84  ~FrameManager();
85 
90  void setFixedFrame(const std::string& frame);
91 
93  void setPause( bool pause );
94 
95  bool getPause() { return pause_; }
96 
98  void setSyncMode( SyncMode mode );
99 
100  SyncMode getSyncMode() { return sync_mode_; }
101 
103  void syncTime( ros::Time time );
104 
106  ros::Time getTime() { return sync_time_; }
107 
113  template<typename Header>
114  bool getTransform(const Header& header, Ogre::Vector3& position, Ogre::Quaternion& orientation)
115  {
116  return getTransform(header.frame_id, header.stamp, position, orientation);
117  }
118 
125  bool getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation);
126 
133  template<typename Header>
134  bool transform(const Header& header, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation)
135  {
136  return transform(header.frame_id, header.stamp, pose, position, orientation);
137  }
138 
146  bool transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation);
147 
149  void update();
150 
156  bool frameHasProblems(const std::string& frame, ros::Time time, std::string& error);
157 
163  bool transformHasProblems(const std::string& frame, ros::Time time, std::string& error);
164 
173  template<class M>
175  {
176  filter->registerCallback(boost::bind(&FrameManager::messageCallback<M>, this, _1, display));
177  filter->registerFailureCallback(boost::bind(&FrameManager::failureCallback<M>, this, _1, _2, display));
178  }
179 
181  const std::string& getFixedFrame() { return fixed_frame_; }
182 
184  tf::TransformListener* getTFClient() { return tf_.get(); }
185 
188 
198  std::string discoverFailureReason(const std::string& frame_id,
199  const ros::Time& stamp,
200  const std::string& caller_id,
201  tf::FilterFailureReason reason);
202 
203 Q_SIGNALS:
205  void fixedFrameChanged();
206 
207 private:
208 
209  bool adjustTime( const std::string &frame, ros::Time &time );
210 
211  template<class M>
212  void messageCallback(const ros::MessageEvent<M const>& msg_evt, Display* display)
213  {
214  boost::shared_ptr<M const> const &msg = msg_evt.getConstMessage();
215  std::string authority = msg_evt.getPublisherName();
216 
217  messageArrived(msg->header.frame_id, msg->header.stamp, authority, display);
218  }
219 
220  template<class M>
222  {
223  boost::shared_ptr<M const> const &msg = msg_evt.getConstMessage();
224  std::string authority = msg_evt.getPublisherName();
225 
226  messageFailed(msg->header.frame_id, msg->header.stamp, authority, reason, display);
227  }
228 
229  void messageArrived(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, Display* display);
230  void messageFailed(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason, Display* display);
231 
232  struct CacheKey
233  {
234  CacheKey(const std::string& f, ros::Time t)
235  : frame(f)
236  , time(t)
237  {}
238 
239  bool operator<(const CacheKey& rhs) const
240  {
241  if (frame != rhs.frame)
242  {
243  return frame < rhs.frame;
244  }
245 
246  return time < rhs.time;
247  }
248 
249  std::string frame;
251  };
252 
253  struct CacheEntry
254  {
255  CacheEntry(const Ogre::Vector3& p, const Ogre::Quaternion& o)
256  : position(p)
257  , orientation(o)
258  {}
259 
260  Ogre::Vector3 position;
261  Ogre::Quaternion orientation;
262  };
263  typedef std::map<CacheKey, CacheEntry > M_Cache;
264 
265  boost::mutex cache_mutex_;
266  M_Cache cache_;
267 
269  std::string fixed_frame_;
270 
271  bool pause_;
272 
274 
275  // the current synchronized time, used to overwrite ros:Time(0)
277 
278  // used for approx. syncing
279  double sync_delta_;
281 };
282 
283 }
284 
285 #endif // RVIZ_FRAME_MANAGER_H
void failureCallback(const ros::MessageEvent< M const > &msg_evt, tf::FilterFailureReason reason, Display *display)
boost::mutex cache_mutex_
boost::shared_ptr< tf::TransformListener > tf_
ros::Time getTime()
Get current time, depending on the sync mode.
CacheKey(const std::string &f, ros::Time t)
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
const std::string & getPublisherName() const
const boost::shared_ptr< ConstMessage > & getConstMessage() const
Helper class for transforming data into Ogre&#39;s world frame (the fixed frame).
Definition: frame_manager.h:63
const boost::shared_ptr< tf::TransformListener > & getTFClientPtr()
Return a boost shared pointer to the tf::TransformListener used to receive transform data...
void registerFilterForTransformStatusCheck(tf::MessageFilter< M > *filter, Display *display)
Connect a tf::MessageFilter&#39;s callbacks to success and failure handler functions in this FrameManager...
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Transform a pose from a frame into the fixed frame.
tf::TransformListener * getTFClient()
Return the tf::TransformListener used to receive transform data.
std::string fixed_frame_
const std::string & getFixedFrame()
Return the current fixed frame name.
SyncMode getSyncMode()
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
bool operator<(const CacheKey &rhs) const
void messageCallback(const ros::MessageEvent< M const > &msg_evt, Display *display)
std::map< CacheKey, CacheEntry > M_Cache
CacheEntry(const Ogre::Vector3 &p, const Ogre::Quaternion &o)
ros::Time stamp
Connection registerCallback(const C &callback)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41