22 #include "tf_example_thread.h"
24 #include <tf/time_cache.h>
31 using namespace fawkes;
35 :
Thread(
"TfExampleThread",
Thread::OPMODE_WAITFORWAKEUP),
59 #define SOURCE "/rx28/tilt"
60 #define TARGET "/base_link"
68 if (! world_frame_exists || ! robot_frame_exists) {
70 SOURCE, world_frame_exists ?
"exists" :
"missing",
71 TARGET, robot_frame_exists ?
"exists" :
"missing");
86 if (now >= transform.
stamp) {
87 diff = now - &transform.
stamp;
89 diff = transform.
stamp - &now;
92 tf::Quaternion q = transform.getRotation();
93 tf::Vector3 v = transform.getOrigin();
99 "T(%f,%f,%f) Q(%f,%f,%f,%f)",
101 diff, v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
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.
A class for handling time.
Thread class encapsulation of pthreads.
No connection between two frames in tree.
Logger * logger
This is the Logger member used to access the logger.
Time based transform cache.
Thread aspect to use blocked timing.
TfExampleThread()
Constructor.
virtual const char * what() const
Get primary string.
virtual void finalize()
Finalize the thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
unsigned int get_list_length() const
Debugging information methods.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.