23 #include "static_transforms_thread.h" 25 #include <utils/time/time.h> 28 #include <core/threading/mutex_locker.h> 41 #define CFG_PREFIX "/plugins/static-transforms/" 45 :
Thread(
"StaticTransformsThread",
Thread::OPMODE_WAITFORWAKEUP),
61 entries_get_from_config();
75 StaticTransformsThread::entries_get_from_config()
77 std::set<std::string> transforms;
78 std::set<std::string> ignored_transforms;
80 std::string prefix = CFG_PREFIX
"transforms/";
81 #if __cplusplus >= 201103L 82 std::unique_ptr<Configuration::ValueIterator> i(
config->
search(prefix.c_str()));
84 std::auto_ptr<Configuration::ValueIterator> i(
config->
search(prefix.c_str()));
87 std::string cfg_name = std::string(i->
path()).substr(prefix.length());
88 cfg_name = cfg_name.substr(0, cfg_name.find(
"/"));
90 if ( (transforms.find(cfg_name) == transforms.end()) &&
91 (ignored_transforms.find(cfg_name) == ignored_transforms.end()) ) {
93 std::string cfg_prefix = prefix + cfg_name +
"/";
103 std::string child_frame =
106 if (frame[0] ==
'/') {
108 "removing leading slash.", cfg_name.c_str(), frame.c_str());
109 frame = frame.substr(1);
111 if (child_frame[0] ==
'/') {
113 "removing leading slash.", cfg_name.c_str(), frame.c_str());
114 child_frame = child_frame.substr(1);
117 float tx = 0., ty = 0., tz = 0.;
127 bool use_quaternion =
false;
128 float rx = 0., ry = 0., rz = 0., rw = 1.,
129 ryaw = 0., rpitch = 0., rroll = 0.;
136 use_quaternion =
true;
142 }
else if (
config->
exists((cfg_prefix +
"rot_roll").c_str()) ||
151 if (frame == child_frame) {
152 throw Exception(
"Parent and child frames may not be the same");
160 if (use_quaternion) {
161 tf::Quaternion q(rx, ry, rz, rw);
162 tf::assert_quaternion_valid(q);
163 tf::Transform t(q, tf::Vector3(tx, ty, tz));
166 tf::Quaternion q; q.setEulerZYX(ryaw, rpitch, rroll);
167 tf::Transform t(q, tf::Vector3(tx, ty, tz));
171 tf::Quaternion q = e.transform->getRotation();
173 tf::assert_quaternion_valid(q);
175 tf::Vector3 &v = e.transform->getOrigin();
177 "T(%f,%f,%f) Q(%f,%f,%f,%f)", e.name.c_str(),
178 e.transform->frame_id.c_str(),
179 e.transform->child_frame_id.c_str(),
180 v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
182 __entries.push_back(e);
190 e.
prepend(
"Transform %s: wrong or incomplete transform data", cfg_name.c_str());
194 transforms.insert(cfg_name);
197 ignored_transforms.insert(cfg_name);
202 if ( __entries.empty() ) {
203 throw Exception(
"No transforms configured");
206 for (std::list<Entry>::iterator i = __entries.begin(); i != __entries.end(); ++i) {
207 i->transform->stamp.stamp();
208 tf_publishers[i->transform->child_frame_id]->send_transform(*(i->transform),
true);
213 StaticTransformsThread::entries_delete()
215 std::list<Entry>::iterator i;
216 for (i = __entries.begin(); i != __entries.end(); ++i) {
230 entries_get_from_config();
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Interface for configuration change handling.
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.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
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.
Base class for exceptions in Fawkes.
virtual void rem_change_handler(ConfigurationChangeHandler *h)
Remove a configuration change handler.
void prepend(const char *format,...)
Prepend messages to the message list.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual const char * path() const =0
Path of value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Iterator interface to iterate over config values.
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
virtual bool exists(const char *path)=0
Check if a given value exists.
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.