22 #include "static_transforms_thread.h"
24 #include <utils/time/time.h>
41 :
Thread(
"StaticTransformsThread",
Thread::OPMODE_WAITFORWAKEUP),
45 __last_update =
new Time();
59 std::set<std::string> transforms;
60 std::set<std::string> ignored_transforms;
62 __cfg_update_interval = 1.0;
64 __cfg_update_interval =
config->
get_float(
"/plugins/static-transforms/update-interval");
67 std::string prefix =
"/plugins/static-transforms/transforms/";
68 std::auto_ptr<Configuration::ValueIterator> i(
config->
search(prefix.c_str()));
70 std::string cfg_name = std::string(i->
path()).substr(prefix.length());
71 cfg_name = cfg_name.substr(0, cfg_name.find(
"/"));
73 if ( (transforms.find(cfg_name) == transforms.end()) &&
74 (ignored_transforms.find(cfg_name) == ignored_transforms.end()) ) {
76 std::string cfg_prefix = prefix + cfg_name +
"/";
86 std::string child_frame =
89 float tx = 0., ty = 0., tz = 0.;
99 bool use_quaternion =
false;
100 float rx = 0., ry = 0., rz = 0., rw = 1.,
101 ryaw = 0., rpitch = 0., rroll = 0.;
108 use_quaternion =
true;
114 }
else if (
config->
exists((cfg_prefix +
"rot_roll").c_str()) ||
123 if (frame == child_frame) {
124 throw Exception(
"Parent and child frames may not be the same");
132 if (use_quaternion) {
133 tf::Quaternion q(rx, ry, rz, rw);
134 tf::assert_quaternion_valid(q);
135 tf::Transform t(q, tf::Vector3(tx, ty, tz));
138 tf::Quaternion q; q.setEulerZYX(ryaw, rpitch, rroll);
139 tf::Transform t(q, tf::Vector3(tx, ty, tz));
143 tf::Quaternion q = e.transform->getRotation();
145 tf::assert_quaternion_valid(q);
147 tf::Vector3 &v = e.transform->getOrigin();
149 "T(%f,%f,%f) Q(%f,%f,%f,%f)", e.name.c_str(),
150 e.transform->frame_id.c_str(),
151 e.transform->child_frame_id.c_str(),
152 v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
154 __entries.push_back(e);
156 std::list<Entry>::iterator i;
157 for (i = __entries.begin(); i != __entries.end(); ++i) {
165 e.
prepend(
"Transform %s: wrong or incomplete transform data",
170 transforms.insert(cfg_name);
173 ignored_transforms.insert(cfg_name);
178 if ( __entries.empty() ) {
179 throw Exception(
"No transforms configured");
190 std::list<Entry>::iterator i;
191 for (i = __entries.begin(); i != __entries.end(); ++i) {
202 if ((now - __last_update) > __cfg_update_interval) {
203 __last_update->
stamp();
207 fawkes::Time timestamp = now + (__cfg_update_interval * 1.1);
209 std::list<Entry>::iterator i;
210 for (i = __entries.begin(); i != __entries.end(); ++i) {
211 i->transform->stamp = timestamp;
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
A class for handling time.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
void set_clock(Clock *clock)
Set clock for this instance.
void prepend(const char *format,...)
Prepend messages to the message list.
virtual const char * path() const =0
Path of value.
const char * name() const
Get name of thread.
void set_time(const timeval *tv)
Sets the time.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual bool exists(const char *path)=0
Check if a given value exists.
Time & stamp()
Set this time to the current time.
Configuration * config
This is the Configuration member used to access the configuration.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.