frame_manager.cpp
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 #include "frame_manager.h"
00031 #include "display.h"
00032 #include "properties/property.h"
00033 
00034 #include <tf/transform_listener.h>
00035 #include <ros/ros.h>
00036 
00037 #include <std_msgs/Float32.h>
00038 
00039 namespace rviz
00040 {
00041 
00042 FrameManager::FrameManager()
00043 {
00044   tf_.reset(new tf::TransformListener(ros::NodeHandle(), ros::Duration(10*60), true));
00045   setSyncMode( SyncOff );
00046   setPause(false);
00047 }
00048 
00049 FrameManager::~FrameManager()
00050 {
00051 }
00052 
00053 void FrameManager::update()
00054 {
00055   boost::mutex::scoped_lock lock(cache_mutex_);
00056   if ( !pause_ )
00057   {
00058     cache_.clear();
00059   }
00060 
00061   if ( !pause_ )
00062   {
00063     switch ( sync_mode_ )
00064     {
00065       case SyncOff:
00066         sync_time_ = ros::Time::now();
00067         break;
00068       case SyncExact:
00069         break;
00070       case SyncApprox:
00071         // adjust current time offset to sync source
00072         current_delta_ = 0.7*current_delta_ + 0.3*sync_delta_;
00073         try
00074         {
00075           sync_time_ = ros::Time::now()-ros::Duration(current_delta_);
00076         }
00077         catch (...)
00078         {
00079           sync_time_ = ros::Time::now();
00080         }
00081         break;
00082     }
00083   }
00084 }
00085 
00086 void FrameManager::setFixedFrame(const std::string& frame)
00087 {
00088   bool emit = false;
00089   {
00090     boost::mutex::scoped_lock lock(cache_mutex_);
00091     if( fixed_frame_ != frame )
00092     {
00093       fixed_frame_ = frame;
00094       cache_.clear();
00095       emit = true;
00096     }
00097   }
00098   if( emit )
00099   {
00100     // This emission must be kept outside of the mutex lock to avoid deadlocks.
00101     Q_EMIT fixedFrameChanged();
00102   }
00103 }
00104 
00105 void FrameManager::setPause( bool pause )
00106 {
00107   pause_ = pause;
00108 }
00109 
00110 void FrameManager::setSyncMode( SyncMode mode )
00111 {
00112   sync_mode_ = mode;
00113   sync_time_ = ros::Time(0);
00114   current_delta_ = 0;
00115   sync_delta_ = 0;
00116 }
00117 
00118 void FrameManager::syncTime( ros::Time time )
00119 {
00120   switch ( sync_mode_ )
00121   {
00122     case SyncOff:
00123       break;
00124     case SyncExact:
00125       sync_time_ = time;
00126       break;
00127     case SyncApprox:
00128       if ( time == ros::Time(0) )
00129       {
00130         sync_delta_ = 0;
00131         return;
00132       }
00133       // avoid exception due to negative time
00134       if ( ros::Time::now() >= time )
00135       {
00136         sync_delta_ = (ros::Time::now() - time).toSec();
00137       }
00138       else
00139       {
00140         setSyncMode( SyncApprox );
00141       }
00142       break;
00143   }
00144 }
00145 
00146 bool FrameManager::adjustTime( const std::string &frame, ros::Time& time )
00147 {
00148   // we only need to act if we get a zero timestamp, which means "latest"
00149   if ( time != ros::Time() )
00150   {
00151     return true;
00152   }
00153 
00154   switch ( sync_mode_ )
00155   {
00156     case SyncOff:
00157       break;
00158     case SyncExact:
00159       time = sync_time_;
00160       break;
00161     case SyncApprox:
00162       {
00163         // if we don't have tf info for the given timestamp, use the latest available
00164         ros::Time latest_time;
00165         std::string error_string;
00166         int error_code;
00167         error_code = tf_->getLatestCommonTime( fixed_frame_, frame, latest_time, &error_string );
00168 
00169         if ( error_code != 0 )
00170         {
00171           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);
00172           return false;
00173         }
00174 
00175         if ( latest_time > sync_time_ )
00176         {
00177           time = sync_time_;
00178         }
00179       }
00180       break;
00181   }
00182   return true;
00183 }
00184 
00185 
00186 
00187 bool FrameManager::getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00188 {
00189   if ( !adjustTime(frame, time) )
00190   {
00191     return false;
00192   }
00193 
00194   boost::mutex::scoped_lock lock(cache_mutex_);
00195 
00196   position = Ogre::Vector3(9999999, 9999999, 9999999);
00197   orientation = Ogre::Quaternion::IDENTITY;
00198 
00199   if (fixed_frame_.empty())
00200   {
00201     return false;
00202   }
00203 
00204   M_Cache::iterator it = cache_.find(CacheKey(frame, time));
00205   if (it != cache_.end())
00206   {
00207     position = it->second.position;
00208     orientation = it->second.orientation;
00209     return true;
00210   }
00211 
00212   geometry_msgs::Pose pose;
00213   pose.orientation.w = 1.0f;
00214 
00215   if (!transform(frame, time, pose, position, orientation))
00216   {
00217     return false;
00218   }
00219 
00220   cache_.insert(std::make_pair(CacheKey(frame, time), CacheEntry(position, orientation)));
00221 
00222   return true;
00223 }
00224 
00225 bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00226 {
00227   if ( !adjustTime(frame, time) )
00228   {
00229     return false;
00230   }
00231 
00232   position = Ogre::Vector3::ZERO;
00233   orientation = Ogre::Quaternion::IDENTITY;
00234 
00235   // put all pose data into a tf stamped pose
00236   tf::Quaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
00237   tf::Vector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z);
00238 
00239   if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0)
00240   {
00241     bt_orientation.setW(1.0);
00242   }
00243 
00244   tf::Stamped<tf::Pose> pose_in(tf::Transform(bt_orientation,bt_position), time, frame);
00245   tf::Stamped<tf::Pose> pose_out;
00246 
00247   // convert pose into new frame
00248   try
00249   {
00250     tf_->transformPose( fixed_frame_, pose_in, pose_out );
00251   }
00252   catch(std::runtime_error& e)
00253   {
00254     ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what());
00255     return false;
00256   }
00257 
00258   bt_position = pose_out.getOrigin();
00259   position = Ogre::Vector3(bt_position.x(), bt_position.y(), bt_position.z());
00260 
00261   bt_orientation = pose_out.getRotation();
00262   orientation = Ogre::Quaternion( bt_orientation.w(), bt_orientation.x(), bt_orientation.y(), bt_orientation.z() );
00263 
00264   return true;
00265 }
00266 
00267 bool FrameManager::frameHasProblems(const std::string& frame, ros::Time time, std::string& error)
00268 {
00269   if (!tf_->frameExists(frame))
00270   {
00271     error = "Frame [" + frame + "] does not exist";
00272     if (frame == fixed_frame_)
00273     {
00274       error = "Fixed " + error;
00275     }
00276     return true;
00277   }
00278 
00279   return false;
00280 }
00281 
00282 bool FrameManager::transformHasProblems(const std::string& frame, ros::Time time, std::string& error)
00283 {
00284   if ( !adjustTime(frame, time) )
00285   {
00286     return false;
00287   }
00288 
00289   std::string tf_error;
00290   bool transform_succeeded = tf_->canTransform(fixed_frame_, frame, time, &tf_error);
00291   if (transform_succeeded)
00292   {
00293     return false;
00294   }
00295 
00296   bool ok = true;
00297   ok = ok && !frameHasProblems(fixed_frame_, time, error);
00298   ok = ok && !frameHasProblems(frame, time, error);
00299 
00300   if (ok)
00301   {
00302     std::stringstream ss;
00303     ss << "No transform to fixed frame [" << fixed_frame_ << "].  TF error: [" << tf_error << "]";
00304     error = ss.str();
00305     ok = false;
00306   }
00307 
00308   {
00309     std::stringstream ss;
00310     ss << "For frame [" << frame << "]: " << error;
00311     error = ss.str();
00312   }
00313 
00314   return !ok;
00315 }
00316 
00317 std::string getTransformStatusName(const std::string& caller_id)
00318 {
00319   std::stringstream ss;
00320   ss << "Transform [sender=" << caller_id << "]";
00321   return ss.str();
00322 }
00323 
00324 std::string FrameManager::discoverFailureReason(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason)
00325 {
00326   if (reason == tf::filter_failure_reasons::OutTheBack)
00327   {
00328     std::stringstream ss;
00329     ss << "Message removed because it is too old (frame=[" << frame_id << "], stamp=[" << stamp << "])";
00330     return ss.str();
00331   }
00332   else
00333   {
00334     std::string error;
00335     if (transformHasProblems(frame_id, stamp, error))
00336     {
00337       return error;
00338     }
00339   }
00340 
00341   return "Unknown reason for transform failure";
00342 }
00343 
00344 void FrameManager::messageArrived( const std::string& frame_id, const ros::Time& stamp,
00345                                    const std::string& caller_id, Display* display )
00346 {
00347   display->setStatusStd( StatusProperty::Ok, getTransformStatusName( caller_id ), "Transform OK" );
00348 }
00349 
00350 void FrameManager::messageFailed( const std::string& frame_id, const ros::Time& stamp,
00351                                   const std::string& caller_id, tf::FilterFailureReason reason, Display* display )
00352 {
00353   std::string status_name = getTransformStatusName( caller_id );
00354   std::string status_text = discoverFailureReason( frame_id, stamp, caller_id, reason );
00355 
00356   display->setStatusStd(StatusProperty::Error, status_name, status_text );
00357 }
00358 
00359 }


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27