Fawkes API
Fawkes Development Version
|
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 }