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 #include <tf2_ros/message_filter.h>
49 #endif
50 
51 namespace tf
52 {
53 class TransformListener;
54 }
55 
56 namespace tf2_ros
57 {
58 class Buffer;
59 }
60 
61 namespace rviz
62 {
63 class Display;
64 
70 class FrameManager : public QObject
71 {
72  Q_OBJECT
73 public:
74  enum SyncMode
75  {
76  SyncOff = 0,
78  SyncApprox
79  };
80 
82  FrameManager();
83 
88  [[deprecated("This constructor signature will be removed in the next version. "
89  "If you still need to pass a boost::shared_ptr<tf::TransformListener>, "
90  "disable the warning explicitly. "
91  "When this constructor is removed, a new constructor with a single, "
92  "optional argument will take a std::pair<> containing a "
93  "std::shared_ptr<tf2_ros::Buffer> and a "
94  "std::shared_ptr<tf2_ros::TransformListener>. "
95  "However, that cannot occur until the use of tf::TransformListener is "
96  "removed internally.")]] explicit FrameManager(boost::shared_ptr<tf::TransformListener> tf);
97 
103  ~FrameManager() override;
104 
109  void setFixedFrame(const std::string& frame);
110 
112  void setPause(bool pause);
113 
114  bool getPause()
115  {
116  return pause_;
117  }
118 
120  void setSyncMode(SyncMode mode);
121 
123  {
124  return sync_mode_;
125  }
126 
128  void syncTime(ros::Time time);
129 
132  {
133  return sync_time_;
134  }
135 
141  template <typename Header>
142  bool getTransform(const Header& header, Ogre::Vector3& position, Ogre::Quaternion& orientation)
143  {
144  return getTransform(header.frame_id, header.stamp, position, orientation);
145  }
146 
153  bool getTransform(const std::string& frame,
154  ros::Time time,
155  Ogre::Vector3& position,
156  Ogre::Quaternion& orientation);
157 
164  template <typename Header>
165  bool transform(const Header& header,
166  const geometry_msgs::Pose& pose,
167  Ogre::Vector3& position,
168  Ogre::Quaternion& orientation)
169  {
170  return transform(header.frame_id, header.stamp, pose, position, orientation);
171  }
172 
180  bool transform(const std::string& frame,
181  ros::Time time,
182  const geometry_msgs::Pose& pose,
183  Ogre::Vector3& position,
184  Ogre::Quaternion& orientation);
185 
187  void update();
188 
194  bool frameHasProblems(const std::string& frame, ros::Time time, std::string& error);
195 
201  bool transformHasProblems(const std::string& frame, ros::Time time, std::string& error);
202 
212  template <class M>
213  [[deprecated("use a tf2_ros::MessageFilter instead")]] void
215  {
216  filter->registerCallback(boost::bind(&FrameManager::messageCallback<M>, this, _1, display));
217  filter->registerFailureCallback(
218  boost::bind(&FrameManager::failureCallback<M, tf::FilterFailureReason>, this, _1, _2, display));
219  }
220 
229  template <class M>
231  {
232  filter->registerCallback(boost::bind(&FrameManager::messageCallback<M>, this, _1, display));
233  filter->registerFailureCallback(boost::bind(
234  &FrameManager::failureCallback<M, tf2_ros::FilterFailureReason>, this, _1, _2, display));
235  }
236 
238  const std::string& getFixedFrame()
239  {
240  return fixed_frame_;
241  }
242 
244  [[deprecated("use getTF2BufferPtr() instead")]] tf::TransformListener* getTFClient()
245  {
246  return tf_.get();
247  }
248 
251  [[deprecated("use getTF2BufferPtr() instead")]] const boost::shared_ptr<tf::TransformListener>&
253  {
254  return tf_;
255  }
256 
257  const std::shared_ptr<tf2_ros::Buffer> getTF2BufferPtr()
258  {
259  return tf_->getTF2BufferPtr();
260  }
261 
271  [[deprecated("used tf2 version instead")]] std::string
272  discoverFailureReason(const std::string& frame_id,
273  const ros::Time& stamp,
274  const std::string& caller_id,
275  tf::FilterFailureReason reason);
276 
286  std::string discoverFailureReason(const std::string& frame_id,
287  const ros::Time& stamp,
288  const std::string& caller_id,
290 
291 Q_SIGNALS:
293  void fixedFrameChanged();
294 
295 private:
296  bool adjustTime(const std::string& frame, ros::Time& time);
297 
298  template <class M>
299  void messageCallback(const ros::MessageEvent<M const>& msg_evt, Display* display)
300  {
301  boost::shared_ptr<M const> const& msg = msg_evt.getConstMessage();
302  std::string authority = msg_evt.getPublisherName();
303 
304  messageArrived(msg->header.frame_id, msg->header.stamp, authority, display);
305  }
306 
307  template <class M, class TfFilterFailureReasonType>
309  TfFilterFailureReasonType reason,
310  Display* display)
311  {
312  boost::shared_ptr<M const> const& msg = msg_evt.getConstMessage();
313  std::string authority = msg_evt.getPublisherName();
314 
315  messageFailed(msg->header.frame_id, msg->header.stamp, authority, reason, display);
316  }
317 
318  void messageArrived(const std::string& frame_id,
319  const ros::Time& stamp,
320  const std::string& caller_id,
321  Display* display);
322 
323  void messageFailedImpl(const std::string& caller_id, const std::string& status_text, Display* display);
324 
325  template <class TfFilterFailureReasonType>
326  void messageFailed(const std::string& frame_id,
327  const ros::Time& stamp,
328  const std::string& caller_id,
329  TfFilterFailureReasonType reason,
330  Display* display)
331  {
332 // TODO(wjwwood): remove this when only Tf2 is supported
333 #ifndef _WIN32
334 #pragma GCC diagnostic push
335 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
336 #endif
337 
338  std::string status_text = discoverFailureReason(frame_id, stamp, caller_id, reason);
339 
340 #ifndef _WIN32
341 #pragma GCC diagnostic pop
342 #endif
343 
344  messageFailedImpl(caller_id, status_text, display);
345  }
346 
347  struct CacheKey
348  {
349  CacheKey(const std::string& f, ros::Time t) : frame(f), time(t)
350  {
351  }
352 
353  bool operator<(const CacheKey& rhs) const
354  {
355  if (frame != rhs.frame)
356  {
357  return frame < rhs.frame;
358  }
359 
360  return time < rhs.time;
361  }
362 
363  std::string frame;
365  };
366 
367  struct CacheEntry
368  {
369  CacheEntry(const Ogre::Vector3& p, const Ogre::Quaternion& o) : position(p), orientation(o)
370  {
371  }
372 
373  Ogre::Vector3 position;
374  Ogre::Quaternion orientation;
375  };
376  typedef std::map<CacheKey, CacheEntry> M_Cache;
377 
378  boost::mutex cache_mutex_;
379  M_Cache cache_;
380 
382  std::string fixed_frame_;
383 
384  bool pause_;
385 
387 
388  // the current synchronized time, used to overwrite ros:Time(0)
390 
391  // used for approx. syncing
392  double sync_delta_;
394 };
395 
396 } // namespace rviz
397 
398 #endif // RVIZ_FRAME_MANAGER_H
const boost::shared_ptr< ConstMessage > & getConstMessage() const
boost::mutex cache_mutex_
boost::shared_ptr< tf::TransformListener > tf_
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
ros::Time getTime()
Get current time, depending on the sync mode.
CacheKey(const std::string &f, ros::Time t)
void messageFailed(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, TfFilterFailureReasonType reason, Display *display)
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
Helper class for transforming data into Ogre&#39;s world frame (the fixed frame).
Definition: frame_manager.h:70
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)
std::map< CacheKey, CacheEntry > M_Cache
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.
void failureCallback(const ros::MessageEvent< M const > &msg_evt, TfFilterFailureReasonType reason, Display *display)
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.
void registerFilterForTransformStatusCheck(tf2_ros::MessageFilter< M > *filter, Display *display)
void messageCallback(const ros::MessageEvent< M const > &msg_evt, Display *display)
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.
const std::string & getPublisherName() const
CacheEntry(const Ogre::Vector3 &p, const Ogre::Quaternion &o)
const std::shared_ptr< tf2_ros::Buffer > getTF2BufferPtr()
ros::Time stamp
bool operator<(const CacheKey &rhs) const
Connection registerCallback(const C &callback)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Fri Aug 12 2022 02:06:08