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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:30