Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 {
38 }
39 
40 
41 /** Destructor. */
43 {
44 }
45 
46 
47 void
49 {
50 }
51 
52 
53 void
55 {
56 }
57 
58 
59 #define SOURCE "/rx28/tilt"
60 #define TARGET "/base_link"
61 
62 void
64 {
65  bool world_frame_exists = tf_listener->frame_exists(SOURCE);
66  bool robot_frame_exists = tf_listener->frame_exists(TARGET);
67 
68  if (! world_frame_exists || ! robot_frame_exists) {
69  logger->log_warn(name(), "Frame missing: %s %s %s %s",
70  SOURCE, world_frame_exists ? "exists" : "missing",
71  TARGET, robot_frame_exists ? "exists" : "missing");
72  } else {
73  tf::StampedTransform transform;
74  try {
75  tf_listener->lookup_transform(TARGET, SOURCE, transform);
76  } catch (tf::ExtrapolationException &e) {
77  logger->log_debug(name(), "Extrapolation error");
78  return;
79  } catch (tf::ConnectivityException &e) {
80  logger->log_debug(name(), "Connectivity exception: %s", e.what());
81  return;
82  }
83 
84  fawkes::Time now;
85  double diff;
86  if (now >= transform.stamp) {
87  diff = now - &transform.stamp;
88  } else {
89  diff = transform.stamp - &now;
90  }
91 
92  tf::Quaternion q = transform.getRotation();
93  tf::Vector3 v = transform.getOrigin();
94 
95  const tf::TimeCache *world_cache = tf_listener->get_frame_cache(SOURCE);
96  const tf::TimeCache *robot_cache = tf_listener->get_frame_cache(TARGET);
97 
98  logger->log_info(name(), "Transform %s -> %s, %f sec old: "
99  "T(%f,%f,%f) Q(%f,%f,%f,%f)",
100  transform.frame_id.c_str(), transform.child_frame_id.c_str(),
101  diff, v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
102 
103  logger->log_info(name(), "World cache size: %zu Robot cache size: %zu",
104  world_cache->get_list_length(),
105  robot_cache->get_list_length());
106 
107  }
108 }
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::Time stamp
Timestamp of this transform.
Definition: types.h:95
A class for handling time.
Definition: time.h:91
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
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Time based transform cache.
Definition: time_cache.h:104
Thread aspect to use blocked timing.
TfExampleThread()
Constructor.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
virtual void finalize()
Finalize the thread.
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
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:100
const char * name() const
Get name of thread.
Definition: thread.h:95
unsigned int get_list_length() const
Debugging information methods.
Definition: time_cache.cpp:356
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:97
tf::TransformListener * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:55
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
const TimeCache * get_frame_cache(const std::string &frame_id) const
An accessor to get access to the frame cache.