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 }
virtual ~TfExampleThread()
Destructor.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
TfExampleThread()
Constructor.
virtual void finalize()
Finalize the thread.
Thread aspect to use blocked timing.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
No connection between two frames in tree.
Definition: exceptions.h:37
Request would have required extrapolation beyond current limits.
Definition: exceptions.h:49
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:95
std::string frame_id
Parent/reference frame ID.
Definition: types.h:97
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:100
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
Fawkes library namespace.