22 #include "tf_example_thread.h" 24 #include <tf/time_cache.h> 35 :
Thread(
"TfExampleThread",
Thread::OPMODE_WAITFORWAKEUP),
61 #define SOURCE "rx28/tilt" 62 #define TARGET "base_link" 70 if (! world_frame_exists || ! robot_frame_exists) {
72 SOURCE, world_frame_exists ?
"exists" :
"missing",
73 TARGET, robot_frame_exists ?
"exists" :
"missing");
88 if (now >= transform.
stamp) {
89 diff = now - &transform.
stamp;
91 diff = transform.
stamp - &now;
94 tf::Quaternion q = transform.getRotation();
95 tf::Vector3 v = transform.getOrigin();
101 "T(%f,%f,%f) Q(%f,%f,%f,%f)",
103 diff, v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
106 world_cache->get_list_length(),
107 robot_cache->get_list_length());
111 if (angle_ >= 2*M_PI) angle_ = 0.;
114 tf::Transform t(tf::create_quaternion_from_yaw(angle_));
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.
A class for handling time.
virtual const char * what() const
Get primary string.
Thread class encapsulation of pthreads.
No connection between two frames in tree.
Logger * logger
This is the Logger member used to access the logger.
Thread aspect to use blocked timing.
TfExampleThread()
Constructor.
virtual void finalize()
Finalize the thread.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.