Fawkes API  Fawkes Development Version
tf_example_thread.cpp
00001 
00002 /***************************************************************************
00003  *  tf_example_thread.cpp - tf example thread
00004  *
00005  *  Created: Tue Oct 25 18:01:36 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include "tf_example_thread.h"
00023 
00024 #include <tf/time_cache.h>
00025 
00026 /** @class TfExampleThread "tf_example_thread.h"
00027  * Main thread of tf example plugin.
00028  * @author Tim Niemueller
00029  */
00030 
00031 using namespace fawkes;
00032 
00033 /** Constructor. */
00034 TfExampleThread::TfExampleThread()
00035   : Thread("TfExampleThread", Thread::OPMODE_WAITFORWAKEUP),
00036     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_THINK)
00037 {
00038 }
00039 
00040 
00041 /** Destructor. */
00042 TfExampleThread::~TfExampleThread()
00043 {
00044 }
00045 
00046 
00047 void
00048 TfExampleThread::init()
00049 {
00050 }
00051 
00052 
00053 void
00054 TfExampleThread::finalize()
00055 {
00056 }
00057 
00058 
00059 #define SOURCE "/rx28/tilt"
00060 #define TARGET "/base_link"
00061 
00062 void
00063 TfExampleThread::loop()
00064 {
00065   bool world_frame_exists = tf_listener->frame_exists(SOURCE);
00066   bool robot_frame_exists = tf_listener->frame_exists(TARGET);
00067 
00068   if (! world_frame_exists || ! robot_frame_exists) {
00069     logger->log_warn(name(), "Frame missing: %s %s   %s %s",
00070                      SOURCE, world_frame_exists ? "exists" : "missing",
00071                      TARGET, robot_frame_exists ? "exists" : "missing");
00072   } else {
00073     tf::StampedTransform transform;
00074     try {
00075       tf_listener->lookup_transform(TARGET, SOURCE, transform);
00076     } catch (tf::ExtrapolationException &e) {
00077       logger->log_debug(name(), "Extrapolation error");
00078       return;
00079     } catch (tf::ConnectivityException &e) {
00080       logger->log_debug(name(), "Connectivity exception: %s", e.what());
00081       return;   
00082     }
00083 
00084     fawkes::Time now;
00085     double diff;
00086     if (now >= transform.stamp) {
00087       diff = now - &transform.stamp;
00088     } else {
00089       diff = transform.stamp - &now;
00090     }
00091 
00092     tf::Quaternion q = transform.getRotation();
00093     tf::Vector3 v   = transform.getOrigin();
00094 
00095     const tf::TimeCache *world_cache = tf_listener->get_frame_cache(SOURCE);
00096     const tf::TimeCache *robot_cache = tf_listener->get_frame_cache(TARGET);
00097 
00098     logger->log_info(name(), "Transform %s -> %s, %f sec old: "
00099                      "T(%f,%f,%f)  Q(%f,%f,%f,%f)",
00100                      transform.frame_id.c_str(), transform.child_frame_id.c_str(),
00101                      diff, v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
00102 
00103     logger->log_info(name(), "World cache size: %zu  Robot cache size: %zu",
00104                      world_cache->get_list_length(),
00105                      robot_cache->get_list_length());
00106 
00107   }
00108 }