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 
42 /** Destructor. */
44 {
45 }
46 
47 
48 void
50 {
51  angle_ = 0.;
52 }
53 
54 
55 void
57 {
58 }
59 
60 
61 #define SOURCE "rx28/tilt"
62 #define TARGET "base_link"
63 
64 void
66 {
67  bool world_frame_exists = tf_listener->frame_exists(SOURCE);
68  bool robot_frame_exists = tf_listener->frame_exists(TARGET);
69 
70  if (! world_frame_exists || ! robot_frame_exists) {
71  logger->log_warn(name(), "Frame missing: %s %s %s %s",
72  SOURCE, world_frame_exists ? "exists" : "missing",
73  TARGET, robot_frame_exists ? "exists" : "missing");
74  } else {
75  tf::StampedTransform transform;
76  try {
77  tf_listener->lookup_transform(TARGET, SOURCE, transform);
78  } catch (tf::ExtrapolationException &e) {
79  logger->log_debug(name(), "Extrapolation error");
80  return;
81  } catch (tf::ConnectivityException &e) {
82  logger->log_debug(name(), "Connectivity exception: %s", e.what());
83  return;
84  }
85 
86  fawkes::Time now;
87  double diff;
88  if (now >= transform.stamp) {
89  diff = now - &transform.stamp;
90  } else {
91  diff = transform.stamp - &now;
92  }
93 
94  tf::Quaternion q = transform.getRotation();
95  tf::Vector3 v = transform.getOrigin();
96 
97  const tf::TimeCacheInterfacePtr world_cache = tf_listener->get_frame_cache(SOURCE);
98  const tf::TimeCacheInterfacePtr robot_cache = tf_listener->get_frame_cache(TARGET);
99 
100  logger->log_info(name(), "Transform %s -> %s, %f sec old: "
101  "T(%f,%f,%f) Q(%f,%f,%f,%f)",
102  transform.frame_id.c_str(), transform.child_frame_id.c_str(),
103  diff, v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
104 
105  logger->log_info(name(), "World cache size: %zu Robot cache size: %zu",
106  world_cache->get_list_length(),
107  robot_cache->get_list_length());
108  }
109 
110  angle_ += M_PI / 4.;
111  if (angle_ >= 2*M_PI) angle_ = 0.;
112  fawkes::Time now;
113 
114  tf::Transform t(tf::create_quaternion_from_yaw(angle_));
115  tf::StampedTransform st(t, now, "base_link", "test_frame");
117 }
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:71
virtual void loop()
Code to execute in the thread.
virtual ~TfExampleThread()
Destructor.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
Fawkes library namespace.
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:100
A class for handling time.
Definition: time.h:91
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
Thread class encapsulation of pthreads.
Definition: thread.h:42
Request would have required extrapolation beyond current limits.
Definition: exceptions.h:52
No connection between two frames in tree.
Definition: exceptions.h:40
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to access the transform system.
Definition: tf.h:42
Thread aspect to use blocked timing.
TfExampleThread()
Constructor.
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
virtual void finalize()
Finalize the thread.
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:105
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::string frame_id
Parent/reference frame ID.
Definition: types.h:102
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:70
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.