23#include "static_transforms_thread.h"
25#include <core/threading/mutex_locker.h>
26#include <utils/time/time.h>
42#define CFG_PREFIX "/plugins/static-transforms/"
46:
Thread(
"StaticTransformsThread",
Thread::OPMODE_WAITFORWAKEUP),
60 entries_get_from_config();
73StaticTransformsThread::entries_get_from_config()
75 std::set<std::string> transforms;
76 std::set<std::string> ignored_transforms;
78 std::string prefix = CFG_PREFIX
"transforms/";
79#if __cplusplus >= 201103L
80 std::unique_ptr<Configuration::ValueIterator> i(
config->
search(prefix.c_str()));
82 std::auto_ptr<Configuration::ValueIterator> i(
config->
search(prefix.c_str()));
85 std::string cfg_name = std::string(i->path()).substr(prefix.length());
86 cfg_name = cfg_name.substr(0, cfg_name.find(
"/"));
88 if ((transforms.find(cfg_name) == transforms.end())
89 && (ignored_transforms.find(cfg_name) == ignored_transforms.end())) {
90 std::string cfg_prefix = prefix + cfg_name +
"/";
101 std::string child_frame =
config->
get_string((cfg_prefix +
"child_frame").c_str());
103 if (frame[0] ==
'/') {
105 "Transform %s parent frame %s starts with /,"
106 "removing leading slash.",
109 frame = frame.substr(1);
111 if (child_frame[0] ==
'/') {
113 "Transform %s child frame %s starts with /,"
114 "removing leading slash.",
117 child_frame = child_frame.substr(1);
120 float tx = 0., ty = 0., tz = 0.;
129 bool use_quaternion =
false;
130 float rx = 0., ry = 0., rz = 0., rw = 1., ryaw = 0., rpitch = 0., rroll = 0.;
136 use_quaternion =
true;
142 }
else if (
config->
exists((cfg_prefix +
"rot_roll").c_str())
150 if (frame == child_frame) {
151 throw Exception(
"Parent and child frames may not be the same");
159 if (use_quaternion) {
160 tf::Quaternion q(rx, ry, rz, rw);
161 tf::assert_quaternion_valid(q);
162 tf::Transform t(q, tf::Vector3(tx, ty, tz));
166 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 "Adding transform '%s' (%s -> %s): "
178 "T(%f,%f,%f) Q(%f,%f,%f,%f)",
180 e.transform->frame_id.c_str(),
181 e.transform->child_frame_id.c_str(),
190 entries_.push_back(e);
198 e.
prepend(
"Transform %s: wrong or incomplete transform data", cfg_name.c_str());
202 transforms.insert(cfg_name);
205 ignored_transforms.insert(cfg_name);
210 if (entries_.empty()) {
211 throw Exception(
"No transforms configured");
214 for (std::list<Entry>::iterator i = entries_.begin(); i != entries_.end(); ++i) {
215 i->transform->stamp.stamp();
216 tf_publishers[i->transform->child_frame_id]->send_transform(*(i->transform),
222StaticTransformsThread::entries_delete()
224 std::list<Entry>::iterator i;
225 for (i = entries_.begin(); i != entries_.end(); ++i) {
239 entries_get_from_config();
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
Interface for configuration change handling.
Iterator interface to iterate over config values.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void rem_change_handler(ConfigurationChangeHandler *h)
Remove a configuration change handler.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual bool exists(const char *path)=0
Check if a given value exists.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Base class for exceptions in Fawkes.
void prepend(const char *format,...) noexcept
Prepend messages to the message list.
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.
Logger * logger
This is the Logger member used to access the logger.
Thread class encapsulation of pthreads.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
const char * name() const
Get name of thread.
A class for handling time.
Fawkes library namespace.