Fawkes API  Fawkes Development Version
tf_example_thread.cpp
1 
2 /***************************************************************************
3  * tf_example_thread.cpp - tf example thread
4  *
5  * Created: Tue Oct 25 18:01:36 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "tf_example_thread.h"
23 
24 #include <tf/time_cache.h>
25 
26 /** @class TfExampleThread "tf_example_thread.h"
27  * Main thread of tf example plugin.
28  * @author Tim Niemueller
29  */
30 
31 using namespace fawkes;
32 
33 /** Constructor. */
35 : Thread("TfExampleThread", Thread::OPMODE_WAITFORWAKEUP),
36  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_THINK),
37  TransformAspect(TransformAspect::BOTH, "test_frame")
38 {
39 }
40 
41 /** Destructor. */
43 {
44 }
45 
46 void
48 {
49  angle_ = 0.;
50 }
51 
52 void
54 {
55 }
56 
57 #define SOURCE "rx28/tilt"
58 #define TARGET "base_link"
59 
60 void
62 {
63  bool world_frame_exists = tf_listener->frame_exists(SOURCE);
64  bool robot_frame_exists = tf_listener->frame_exists(TARGET);
65 
66  if (!world_frame_exists || !robot_frame_exists) {
67  logger->log_warn(name(),
68  "Frame missing: %s %s %s %s",
69  SOURCE,
70  world_frame_exists ? "exists" : "missing",
71  TARGET,
72  robot_frame_exists ? "exists" : "missing");
73  } else {
74  tf::StampedTransform transform;
75  try {
76  tf_listener->lookup_transform(TARGET, SOURCE, transform);
77  } catch (tf::ExtrapolationException &e) {
78  logger->log_debug(name(), "Extrapolation error");
79  return;
80  } catch (tf::ConnectivityException &e) {
81  logger->log_debug(name(), "Connectivity exception: %s", e.what());
82  return;
83  }
84 
85  fawkes::Time now;
86  double diff;
87  if (now >= transform.stamp) {
88  diff = now - &transform.stamp;
89  } else {
90  diff = transform.stamp - &now;
91  }
92 
93  tf::Quaternion q = transform.getRotation();
94  tf::Vector3 v = transform.getOrigin();
95 
96  const tf::TimeCacheInterfacePtr world_cache = tf_listener->get_frame_cache(SOURCE);
97  const tf::TimeCacheInterfacePtr robot_cache = tf_listener->get_frame_cache(TARGET);
98 
99  logger->log_info(name(),
100  "Transform %s -> %s, %f sec old: "
101  "T(%f,%f,%f) Q(%f,%f,%f,%f)",
102  transform.frame_id.c_str(),
103  transform.child_frame_id.c_str(),
104  diff,
105  v.x(),
106  v.y(),
107  v.z(),
108  q.x(),
109  q.y(),
110  q.z(),
111  q.w());
112 
113  logger->log_info(name(),
114  "World cache size: %zu Robot cache size: %zu",
115  world_cache->get_list_length(),
116  robot_cache->get_list_length());
117  }
118 
119  angle_ += M_PI / 4.;
120  if (angle_ >= 2 * M_PI)
121  angle_ = 0.;
122  fawkes::Time now;
123 
124  tf::Transform t(tf::create_quaternion_from_yaw(angle_));
125  tf::StampedTransform st(t, now, "base_link", "test_frame");
127 }
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:98
TfExampleThread::init
virtual void init()
Initialize the thread.
Definition: tf_example_thread.cpp:46
fawkes::tf::Transformer::frame_exists
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
Definition: transformer.cpp:173
TfExampleThread::~TfExampleThread
virtual ~TfExampleThread()
Destructor.
Definition: tf_example_thread.cpp:41
fawkes::tf::ConnectivityException
Definition: exceptions.h:43
fawkes::tf::Transformer::get_frame_cache
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
Definition: transformer.cpp:185
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::tf::StampedTransform::stamp
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:102
fawkes::tf::StampedTransform::child_frame_id
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:107
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
TfExampleThread::TfExampleThread
TfExampleThread()
Constructor.
Definition: tf_example_thread.cpp:33
TfExampleThread::finalize
virtual void finalize()
Finalize the thread.
Definition: tf_example_thread.cpp:52
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes
fawkes::tf::TransformPublisher::send_transform
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
Definition: transform_publisher.cpp:122
TfExampleThread::loop
virtual void loop()
Code to execute in the thread.
Definition: tf_example_thread.cpp:60
fawkes::TransformAspect
Definition: tf.h:41
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::tf::StampedTransform::frame_id
std::string frame_id
Parent/reference frame ID.
Definition: types.h:104
fawkes::tf::Transformer::lookup_transform
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Definition: transformer.cpp:272
fawkes::Time
Definition: time.h:96
fawkes::TransformAspect::tf_listener
tf::Transformer * tf_listener
Definition: tf.h:70
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:638
fawkes::Thread
Definition: thread.h:44
fawkes::TransformAspect::tf_publisher
tf::TransformPublisher * tf_publisher
Definition: tf.h:71
fawkes::tf::ExtrapolationException
Definition: exceptions.h:55
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0