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 #include <tf2_ros/buffer.h>
39 #include <geometry_msgs/Pose.h>
40 
42 #include <OgreQuaternion.h>
43 
44 #include <boost/thread/mutex.hpp>
45 
46 #ifndef Q_MOC_RUN
47 #include <tf2_ros/message_filter.h>
48 #endif
49 
50 namespace tf2_ros
51 {
52 class TransformListener;
53 }
54 
55 namespace rviz
56 {
57 class Display;
58 
64 class FrameManager : public QObject
65 {
66  Q_OBJECT
67 public:
68  enum SyncMode
69  {
70  SyncOff = 0, // use latest TF updates
71  SyncExact, // sync to incoming messages of a display (emitting timeSignal())
73  SyncFrame, // synchronize frame lookups to start of update() loop
74  };
75 
77  explicit FrameManager(std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
78  std::shared_ptr<tf2_ros::TransformListener> tf_listener =
79  std::shared_ptr<tf2_ros::TransformListener>());
80 
86  ~FrameManager() override;
87 
92  void setFixedFrame(const std::string& frame);
93 
95  void setPause(bool pause);
96 
97  bool getPause()
98  {
99  return pause_;
100  }
101 
103  void setSyncMode(SyncMode mode);
104 
106  {
107  return sync_mode_;
108  }
109 
111  void syncTime(ros::Time time);
112 
115  {
116  return sync_time_;
117  }
118 
124  template <typename Header>
125  bool getTransform(const Header& header, Ogre::Vector3& position, Ogre::Quaternion& orientation)
126  {
127  return getTransform(header.frame_id, header.stamp, position, orientation);
128  }
129 
136  bool getTransform(const std::string& frame,
137  ros::Time time,
138  Ogre::Vector3& position,
139  Ogre::Quaternion& orientation);
140 
147  template <typename Header>
148  bool transform(const Header& header,
149  const geometry_msgs::Pose& pose,
150  Ogre::Vector3& position,
151  Ogre::Quaternion& orientation)
152  {
153  return transform(header.frame_id, header.stamp, pose, position, orientation);
154  }
155 
163  bool transform(const std::string& frame,
164  ros::Time time,
165  const geometry_msgs::Pose& pose,
166  Ogre::Vector3& position,
167  Ogre::Quaternion& orientation);
168 
170  void update();
171 
177  bool frameHasProblems(const std::string& frame, ros::Time time, std::string& error);
178 
184  bool transformHasProblems(const std::string& frame, ros::Time time, std::string& error);
185 
193  template <class M>
195  {
196  filter->registerCallback(
197  boost::bind(&FrameManager::messageCallback<M>, this, boost::placeholders::_1, display));
198  filter->registerFailureCallback(
199  boost::bind(&FrameManager::failureCallback<M, tf2_ros::FilterFailureReason>, this,
200  boost::placeholders::_1, boost::placeholders::_2, display));
201  }
202 
204  const std::string& getFixedFrame()
205  {
206  return fixed_frame_;
207  }
208 
209  const std::shared_ptr<tf2_ros::Buffer> getTF2BufferPtr()
210  {
211  return tf_buffer_;
212  }
213 
223  std::string discoverFailureReason(const std::string& frame_id,
224  const ros::Time& stamp,
225  const std::string& caller_id,
227 
228 Q_SIGNALS:
230  void fixedFrameChanged();
231 
232 private:
233  void adjustTime(ros::Time& time);
234 
235  template <class M>
236  void messageCallback(const ros::MessageEvent<M const>& msg_evt, Display* display)
237  {
238  boost::shared_ptr<M const> const& msg = msg_evt.getConstMessage();
239  const std::string& authority = msg_evt.getPublisherName();
240 
241  messageArrived(msg->header.frame_id, msg->header.stamp, authority, display);
242  }
243 
244  template <class M, class TfFilterFailureReasonType>
246  TfFilterFailureReasonType reason,
247  Display* display)
248  {
249  boost::shared_ptr<M const> const& msg = msg_evt.getConstMessage();
250  const std::string& authority = msg_evt.getPublisherName();
251 
252  messageFailed(msg->header.frame_id, msg->header.stamp, authority, reason, display);
253  }
254 
255  void messageArrived(const std::string& frame_id,
256  const ros::Time& stamp,
257  const std::string& caller_id,
258  Display* display);
259 
260  void messageFailedImpl(const std::string& caller_id, const std::string& status_text, Display* display);
261 
262  template <class TfFilterFailureReasonType>
263  void messageFailed(const std::string& frame_id,
264  const ros::Time& stamp,
265  const std::string& caller_id,
266  TfFilterFailureReasonType reason,
267  Display* display)
268  {
269  std::string status_text = discoverFailureReason(frame_id, stamp, caller_id, reason);
270  messageFailedImpl(caller_id, status_text, display);
271  }
272 
273  struct CacheKey
274  {
275  CacheKey(const std::string& f, ros::Time t) : frame(f), time(t)
276  {
277  }
278 
279  bool operator<(const CacheKey& rhs) const
280  {
281  if (frame != rhs.frame)
282  {
283  return frame < rhs.frame;
284  }
285 
286  return time < rhs.time;
287  }
288 
289  std::string frame;
291  };
292 
293  struct CacheEntry
294  {
295  CacheEntry(const Ogre::Vector3& p, const Ogre::Quaternion& o) : position(p), orientation(o)
296  {
297  }
298 
299  Ogre::Vector3 position;
300  Ogre::Quaternion orientation;
301  };
302  typedef std::map<CacheKey, CacheEntry> M_Cache;
303 
304  boost::mutex cache_mutex_;
306 
307  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
308  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
309  std::string fixed_frame_;
310 
311  bool pause_;
312 
314 
315  // the current synchronized time, used to overwrite ros:Time(0)
317 
318  // used for approx. syncing
319  double current_delta_; // current time delay between received sync msg's time stamp and now()
320  double sync_delta_; // sliding average of current_delta_, used to compute sync_time_
321 };
322 
323 } // namespace rviz
324 
325 #endif // RVIZ_FRAME_MANAGER_H
rviz::FrameManager::getTF2BufferPtr
const std::shared_ptr< tf2_ros::Buffer > getTF2BufferPtr()
Definition: frame_manager.h:209
rviz::FrameManager::FrameManager
FrameManager(std::shared_ptr< tf2_ros::Buffer > tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), std::shared_ptr< tf2_ros::TransformListener > tf_listener=std::shared_ptr< tf2_ros::TransformListener >())
Constructor, will create a TransformListener (and Buffer) automatically if not provided.
Definition: frame_manager.cpp:42
rviz::FrameManager
Helper class for transforming data into Ogre's world frame (the fixed frame).
Definition: frame_manager.h:64
rviz::FrameManager::cache_
M_Cache cache_
Definition: frame_manager.h:305
rviz::FrameManager::messageArrived
void messageArrived(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, Display *display)
Definition: frame_manager.cpp:334
rviz::FrameManager::messageCallback
void messageCallback(const ros::MessageEvent< M const > &msg_evt, Display *display)
Definition: frame_manager.h:236
tf2_ros::MessageFilter::registerFailureCallback
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
rviz::FrameManager::cache_mutex_
boost::mutex cache_mutex_
Definition: frame_manager.h:304
rviz::FrameManager::setFixedFrame
void setFixedFrame(const std::string &frame)
Set the frame to consider "fixed", into which incoming data is transformed.
Definition: frame_manager.cpp:91
tf2_ros::MessageFilter
Header
boost::shared_ptr
rviz::FrameManager::fixedFrameChanged
void fixedFrameChanged()
Emitted whenever the fixed frame changes.
rviz::FrameManager::adjustTime
void adjustTime(ros::Time &time)
Definition: frame_manager.cpp:152
tf2_ros::filter_failure_reasons::FilterFailureReason
FilterFailureReason
time.h
rviz::FrameManager::sync_time_
ros::Time sync_time_
Definition: frame_manager.h:316
ros::MessageEvent::getPublisherName
const std::string & getPublisherName() const
buffer.h
rviz::FrameManager::CacheEntry::CacheEntry
CacheEntry(const Ogre::Vector3 &p, const Ogre::Quaternion &o)
Definition: frame_manager.h:295
ros::MessageEvent::getConstMessage
const boost::shared_ptr< ConstMessage > & getConstMessage() const
rviz::FrameManager::messageFailed
void messageFailed(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, TfFilterFailureReasonType reason, Display *display)
Definition: frame_manager.h:263
rviz::FrameManager::CacheKey::CacheKey
CacheKey(const std::string &f, ros::Time t)
Definition: frame_manager.h:275
f
f
rviz::FrameManager::SyncOff
@ SyncOff
Definition: frame_manager.h:70
rviz::FrameManager::sync_mode_
SyncMode sync_mode_
Definition: frame_manager.h:313
rviz::FrameManager::setSyncMode
void setSyncMode(SyncMode mode)
Set synchronization mode (off/exact/approximate)
Definition: frame_manager.cpp:115
rviz::Display
Definition: display.h:63
rviz::FrameManager::getTime
ros::Time getTime()
Get current time, depending on the sync mode.
Definition: frame_manager.h:114
rviz::FrameManager::current_delta_
double current_delta_
Definition: frame_manager.h:319
rviz::FrameManager::CacheEntry::position
Ogre::Vector3 position
Definition: frame_manager.h:299
rviz::FrameManager::getFixedFrame
const std::string & getFixedFrame()
Return the current fixed frame name.
Definition: frame_manager.h:204
tf2_ros
message_filter.h
rviz::FrameManager::tf_listener_
std::shared_ptr< tf2_ros::TransformListener > tf_listener_
Definition: frame_manager.h:308
rviz::FrameManager::messageFailedImpl
void messageFailedImpl(const std::string &caller_id, const std::string &status_text, Display *display)
Definition: frame_manager.cpp:342
rviz
Definition: add_display_dialog.cpp:54
rviz::FrameManager::CacheKey::time
ros::Time time
Definition: frame_manager.h:290
rviz::FrameManager::getPause
bool getPause()
Definition: frame_manager.h:97
rviz::FrameManager::failureCallback
void failureCallback(const ros::MessageEvent< M const > &msg_evt, TfFilterFailureReasonType reason, Display *display)
Definition: frame_manager.h:245
rviz::FrameManager::CacheEntry::orientation
Ogre::Quaternion orientation
Definition: frame_manager.h:300
ogre_vector.h
rviz::FrameManager::~FrameManager
~FrameManager() override
Destructor.
Definition: frame_manager.cpp:56
rviz::FrameManager::fixed_frame_
std::string fixed_frame_
Definition: frame_manager.h:309
rviz::FrameManager::frameHasProblems
bool frameHasProblems(const std::string &frame, ros::Time time, std::string &error)
Check to see if a frame exists in our tf buffer.
Definition: frame_manager.cpp:257
rviz::FrameManager::sync_delta_
double sync_delta_
Definition: frame_manager.h:320
rviz::FrameManager::syncTime
void syncTime(ros::Time time)
Synchronize with given time.
Definition: frame_manager.cpp:123
rviz::FrameManager::CacheEntry
Definition: frame_manager.h:293
rviz::FrameManager::M_Cache
std::map< CacheKey, CacheEntry > M_Cache
Definition: frame_manager.h:302
rviz::FrameManager::SyncMode
SyncMode
Definition: frame_manager.h:68
rviz::FrameManager::SyncApprox
@ SyncApprox
Definition: frame_manager.h:72
rviz::FrameManager::getTransform
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.
Definition: frame_manager.h:125
rviz::FrameManager::getSyncMode
SyncMode getSyncMode()
Definition: frame_manager.h:105
rviz::FrameManager::SyncFrame
@ SyncFrame
Definition: frame_manager.h:73
ros::Time
rviz::FrameManager::registerFilterForTransformStatusCheck
void registerFilterForTransformStatusCheck(tf2_ros::MessageFilter< M > *filter, Display *display)
Definition: frame_manager.h:194
rviz::FrameManager::CacheKey::frame
std::string frame
Definition: frame_manager.h:289
rviz::FrameManager::transform
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.
Definition: frame_manager.h:148
rviz::FrameManager::update
void update()
Clear the internal cache.
Definition: frame_manager.cpp:60
rviz::FrameManager::CacheKey::operator<
bool operator<(const CacheKey &rhs) const
Definition: frame_manager.h:279
header
const std::string header
rviz::FrameManager::transformHasProblems
bool transformHasProblems(const std::string &frame, ros::Time time, std::string &error)
Check to see if a transform is known between a given frame and the fixed frame.
Definition: frame_manager.cpp:272
t
geometry_msgs::TransformStamped t
rviz::FrameManager::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: frame_manager.h:307
rviz::FrameManager::setPause
void setPause(bool pause)
Enable/disable pause mode.
Definition: frame_manager.cpp:110
rviz::FrameManager::CacheKey
Definition: frame_manager.h:273
ros::MessageEvent
rviz::FrameManager::discoverFailureReason
std::string discoverFailureReason(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, tf2_ros::FilterFailureReason reason)
Definition: frame_manager.cpp:311
rviz::FrameManager::pause_
bool pause_
Definition: frame_manager.h:311
rviz::FrameManager::SyncExact
@ SyncExact
Definition: frame_manager.h:71


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Dec 13 2024 03:31:02