22 #include "tf_example_thread.h"
24 #include <tf/time_cache.h>
35 :
Thread(
"TfExampleThread",
Thread::OPMODE_WAITFORWAKEUP),
57 #define SOURCE "rx28/tilt"
58 #define TARGET "base_link"
66 if (!world_frame_exists || !robot_frame_exists) {
68 "Frame missing: %s %s %s %s",
70 world_frame_exists ?
"exists" :
"missing",
72 robot_frame_exists ?
"exists" :
"missing");
87 if (now >= transform.
stamp) {
88 diff = now - &transform.
stamp;
90 diff = transform.
stamp - &now;
93 tf::Quaternion q = transform.getRotation();
94 tf::Vector3 v = transform.getOrigin();
100 "Transform %s -> %s, %f sec old: "
101 "T(%f,%f,%f) Q(%f,%f,%f,%f)",
114 "World cache size: %zu Robot cache size: %zu",
115 world_cache->get_list_length(),
116 robot_cache->get_list_length());
120 if (angle_ >= 2 * M_PI)
124 tf::Transform t(tf::create_quaternion_from_yaw(angle_));
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 noexcept
Get primary string.
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.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
No connection between two frames in tree.
Fawkes library namespace.