frame_manager.cpp
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 #include "frame_manager.h"
31 #include "display.h"
32 #include "properties/property.h"
33 
34 #include <tf/transform_listener.h>
35 #include <ros/ros.h>
36 
37 #include <std_msgs/Float32.h>
38 
39 namespace rviz
40 {
41 
43 {
44  if (!tf) tf_.reset(new tf::TransformListener(ros::NodeHandle(), ros::Duration(10*60), true));
45  else tf_ = tf;
46 
48  setPause(false);
49 }
50 
52 {
53 }
54 
56 {
57  boost::mutex::scoped_lock lock(cache_mutex_);
58  if ( !pause_ )
59  {
60  cache_.clear();
61  }
62 
63  if ( !pause_ )
64  {
65  switch ( sync_mode_ )
66  {
67  case SyncOff:
69  break;
70  case SyncExact:
71  break;
72  case SyncApprox:
73  // adjust current time offset to sync source
75  try
76  {
78  }
79  catch (...)
80  {
82  }
83  break;
84  }
85  }
86 }
87 
88 void FrameManager::setFixedFrame(const std::string& frame)
89 {
90  bool emit = false;
91  {
92  boost::mutex::scoped_lock lock(cache_mutex_);
93  if( fixed_frame_ != frame )
94  {
95  fixed_frame_ = frame;
96  cache_.clear();
97  emit = true;
98  }
99  }
100  if( emit )
101  {
102  // This emission must be kept outside of the mutex lock to avoid deadlocks.
103  Q_EMIT fixedFrameChanged();
104  }
105 }
106 
107 void FrameManager::setPause( bool pause )
108 {
109  pause_ = pause;
110 }
111 
113 {
114  sync_mode_ = mode;
115  sync_time_ = ros::Time(0);
116  current_delta_ = 0;
117  sync_delta_ = 0;
118 }
119 
121 {
122  switch ( sync_mode_ )
123  {
124  case SyncOff:
125  break;
126  case SyncExact:
127  sync_time_ = time;
128  break;
129  case SyncApprox:
130  if ( time == ros::Time(0) )
131  {
132  sync_delta_ = 0;
133  return;
134  }
135  // avoid exception due to negative time
136  if ( ros::Time::now() >= time )
137  {
138  sync_delta_ = (ros::Time::now() - time).toSec();
139  }
140  else
141  {
143  }
144  break;
145  }
146 }
147 
148 bool FrameManager::adjustTime( const std::string &frame, ros::Time& time )
149 {
150  // we only need to act if we get a zero timestamp, which means "latest"
151  if ( time != ros::Time() )
152  {
153  return true;
154  }
155 
156  switch ( sync_mode_ )
157  {
158  case SyncOff:
159  break;
160  case SyncExact:
161  time = sync_time_;
162  break;
163  case SyncApprox:
164  {
165  // if we don't have tf info for the given timestamp, use the latest available
166  ros::Time latest_time;
167  std::string error_string;
168  int error_code;
169  error_code = tf_->getLatestCommonTime( fixed_frame_, frame, latest_time, &error_string );
170 
171  if ( error_code != 0 )
172  {
173  ROS_ERROR("Error getting latest time from frame '%s' to frame '%s': %s (Error code: %d)", frame.c_str(), fixed_frame_.c_str(), error_string.c_str(), error_code);
174  return false;
175  }
176 
177  if ( latest_time > sync_time_ )
178  {
179  time = sync_time_;
180  }
181  }
182  break;
183  }
184  return true;
185 }
186 
187 
188 
189 bool FrameManager::getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation)
190 {
191  if ( !adjustTime(frame, time) )
192  {
193  return false;
194  }
195 
196  boost::mutex::scoped_lock lock(cache_mutex_);
197 
198  position = Ogre::Vector3(9999999, 9999999, 9999999);
199  orientation = Ogre::Quaternion::IDENTITY;
200 
201  if (fixed_frame_.empty())
202  {
203  return false;
204  }
205 
206  M_Cache::iterator it = cache_.find(CacheKey(frame, time));
207  if (it != cache_.end())
208  {
209  position = it->second.position;
210  orientation = it->second.orientation;
211  return true;
212  }
213 
214  geometry_msgs::Pose pose;
215  pose.orientation.w = 1.0f;
216 
217  if (!transform(frame, time, pose, position, orientation))
218  {
219  return false;
220  }
221 
222  cache_.insert(std::make_pair(CacheKey(frame, time), CacheEntry(position, orientation)));
223 
224  return true;
225 }
226 
227 bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, Ogre::Vector3& position, Ogre::Quaternion& orientation)
228 {
229  if ( !adjustTime(frame, time) )
230  {
231  return false;
232  }
233 
234  position = Ogre::Vector3::ZERO;
235  orientation = Ogre::Quaternion::IDENTITY;
236 
237  // put all pose data into a tf stamped pose
238  tf::Quaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
239  tf::Vector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z);
240 
241  if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0)
242  {
243  bt_orientation.setW(1.0);
244  }
245 
246  tf::Stamped<tf::Pose> pose_in(tf::Transform(bt_orientation,bt_position), time, frame);
247  tf::Stamped<tf::Pose> pose_out;
248 
249  // convert pose into new frame
250  try
251  {
252  tf_->transformPose( fixed_frame_, pose_in, pose_out );
253  }
254  catch(std::runtime_error& e)
255  {
256  ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what());
257  return false;
258  }
259 
260  bt_position = pose_out.getOrigin();
261  position = Ogre::Vector3(bt_position.x(), bt_position.y(), bt_position.z());
262 
263  bt_orientation = pose_out.getRotation();
264  orientation = Ogre::Quaternion( bt_orientation.w(), bt_orientation.x(), bt_orientation.y(), bt_orientation.z() );
265 
266  return true;
267 }
268 
269 bool FrameManager::frameHasProblems(const std::string& frame, ros::Time time, std::string& error)
270 {
271  if (!tf_->frameExists(frame))
272  {
273  error = "Frame [" + frame + "] does not exist";
274  if (frame == fixed_frame_)
275  {
276  error = "Fixed " + error;
277  }
278  return true;
279  }
280 
281  return false;
282 }
283 
284 bool FrameManager::transformHasProblems(const std::string& frame, ros::Time time, std::string& error)
285 {
286  if ( !adjustTime(frame, time) )
287  {
288  return false;
289  }
290 
291  std::string tf_error;
292  bool transform_succeeded = tf_->canTransform(fixed_frame_, frame, time, &tf_error);
293  if (transform_succeeded)
294  {
295  return false;
296  }
297 
298  bool ok = true;
299  ok = ok && !frameHasProblems(fixed_frame_, time, error);
300  ok = ok && !frameHasProblems(frame, time, error);
301 
302  if (ok)
303  {
304  std::stringstream ss;
305  ss << "No transform to fixed frame [" << fixed_frame_ << "]. TF error: [" << tf_error << "]";
306  error = ss.str();
307  ok = false;
308  }
309 
310  {
311  std::stringstream ss;
312  ss << "For frame [" << frame << "]: " << error;
313  error = ss.str();
314  }
315 
316  return !ok;
317 }
318 
319 std::string getTransformStatusName(const std::string& caller_id)
320 {
321  std::stringstream ss;
322  ss << "Transform [sender=" << caller_id << "]";
323  return ss.str();
324 }
325 
326 std::string FrameManager::discoverFailureReason(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason)
327 {
329  {
330  std::stringstream ss;
331  ss << "Message removed because it is too old (frame=[" << frame_id << "], stamp=[" << stamp << "])";
332  return ss.str();
333  }
334  else
335  {
336  std::string error;
337  if (transformHasProblems(frame_id, stamp, error))
338  {
339  return error;
340  }
341  }
342 
343  return "Unknown reason for transform failure";
344 }
345 
346 void FrameManager::messageArrived( const std::string& frame_id, const ros::Time& stamp,
347  const std::string& caller_id, Display* display )
348 {
349  display->setStatusStd( StatusProperty::Ok, getTransformStatusName( caller_id ), "Transform OK" );
350 }
351 
352 void FrameManager::messageFailed( const std::string& frame_id, const ros::Time& stamp,
353  const std::string& caller_id, tf::FilterFailureReason reason, Display* display )
354 {
355  std::string status_name = getTransformStatusName( caller_id );
356  std::string status_text = discoverFailureReason( frame_id, stamp, caller_id, reason );
357 
358  display->setStatusStd(StatusProperty::Error, status_name, status_text );
359 }
360 
361 }
bool adjustTime(const std::string &frame, ros::Time &time)
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
Definition: display.h:157
std::string discoverFailureReason(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, tf::FilterFailureReason reason)
Create a description of a transform problem.
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.
std::string getTransformStatusName(const std::string &caller_id)
boost::mutex cache_mutex_
void update()
Clear the internal cache.
void messageArrived(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, Display *display)
boost::shared_ptr< tf::TransformListener > tf_
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
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 setPause(bool pause)
Enable/disable pause mode.
std::string fixed_frame_
FrameManager(boost::shared_ptr< tf::TransformListener > tf=boost::shared_ptr< tf::TransformListener >())
Constructor.
void syncTime(ros::Time time)
Synchronize with given time.
void setFixedFrame(const std::string &frame)
Set the frame to consider "fixed", into which incoming data is transformed.
void messageFailed(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, tf::FilterFailureReason reason, Display *display)
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 frameHasProblems(const std::string &frame, ros::Time time, std::string &error)
Check to see if a frame exists in the tf::TransformListener.
void setSyncMode(SyncMode mode)
Set synchronization mode (off/exact/approximate)
void fixedFrameChanged()
Emitted whenever the fixed frame changes.
static Time now()
#define ROS_ERROR(...)
~FrameManager()
Destructor.
#define ROS_DEBUG(...)


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