Fawkes API Fawkes Development Version
static_transforms_thread.cpp
1
2/***************************************************************************
3 * static_transform_thread.cpp - Static transform publisher thread
4 *
5 * Created: Tue Oct 25 16:36:04 2011
6 * Copyright 2011 Tim Niemueller [www.niemueller.de]
7 * 2014 Tobias Neumann
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "static_transforms_thread.h"
24
25#include <core/threading/mutex_locker.h>
26#include <utils/time/time.h>
27
28#include <memory>
29#include <set>
30
31using namespace fawkes;
32
33/** @class StaticTransformsThread "static_transforms_thread.h"
34 * Thread to regularly publish static transforms.
35 * This thread runs at the sensor hook and publishes a set of
36 * transforms. The transforms are set in the configuration and
37 * are static at run-time. Only the timestamp is updated between
38 * writes.
39 * @author Tim Niemueller
40 */
41
42#define CFG_PREFIX "/plugins/static-transforms/"
43
44/** Constructor. */
46: Thread("StaticTransformsThread", Thread::OPMODE_WAITFORWAKEUP),
47 TransformAspect(TransformAspect::DEFER_PUBLISHER),
49{
50}
51
52/** Destructor. */
54{
55}
56
57void
59{
60 entries_get_from_config();
61
63}
64
65void
67{
69 entries_delete();
70}
71
72void
73StaticTransformsThread::entries_get_from_config()
74{
75 std::set<std::string> transforms;
76 std::set<std::string> ignored_transforms;
77
78 std::string prefix = CFG_PREFIX "transforms/";
79#if __cplusplus >= 201103L
80 std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
81#else
82 std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
83#endif
84 while (i->next()) {
85 std::string cfg_name = std::string(i->path()).substr(prefix.length());
86 cfg_name = cfg_name.substr(0, cfg_name.find("/"));
87
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 + "/";
91
92 bool active = true;
93 try {
94 active = config->get_bool((cfg_prefix + "active").c_str());
95 } catch (Exception &e) {
96 } // ignored, assume enabled
97
98 if (active) {
99 try {
100 std::string frame = config->get_string((cfg_prefix + "frame").c_str());
101 std::string child_frame = config->get_string((cfg_prefix + "child_frame").c_str());
102
103 if (frame[0] == '/') {
105 "Transform %s parent frame %s starts with /,"
106 "removing leading slash.",
107 cfg_name.c_str(),
108 frame.c_str());
109 frame = frame.substr(1);
110 }
111 if (child_frame[0] == '/') {
113 "Transform %s child frame %s starts with /,"
114 "removing leading slash.",
115 cfg_name.c_str(),
116 frame.c_str());
117 child_frame = child_frame.substr(1);
118 }
119
120 float tx = 0., ty = 0., tz = 0.;
121 if (config->exists((cfg_prefix + "trans_x").c_str())
122 || config->exists((cfg_prefix + "trans_y").c_str())
123 || config->exists((cfg_prefix + "trans_z").c_str())) {
124 tx = config->get_float((cfg_prefix + "trans_x").c_str());
125 ty = config->get_float((cfg_prefix + "trans_y").c_str());
126 tz = config->get_float((cfg_prefix + "trans_z").c_str());
127 } // else assume no translation
128
129 bool use_quaternion = false;
130 float rx = 0., ry = 0., rz = 0., rw = 1., ryaw = 0., rpitch = 0., rroll = 0.;
131
132 if (config->exists((cfg_prefix + "rot_x").c_str())
133 || config->exists((cfg_prefix + "rot_y").c_str())
134 || config->exists((cfg_prefix + "rot_z").c_str())
135 || config->exists((cfg_prefix + "rot_w").c_str())) {
136 use_quaternion = true;
137 rx = config->get_float((cfg_prefix + "rot_x").c_str());
138 ry = config->get_float((cfg_prefix + "rot_y").c_str());
139 rz = config->get_float((cfg_prefix + "rot_z").c_str());
140 rw = config->get_float((cfg_prefix + "rot_w").c_str());
141
142 } else if (config->exists((cfg_prefix + "rot_roll").c_str())
143 || config->exists((cfg_prefix + "rot_pitch").c_str())
144 || config->exists((cfg_prefix + "rot_yaw").c_str())) {
145 ryaw = config->get_float((cfg_prefix + "rot_yaw").c_str());
146 rpitch = config->get_float((cfg_prefix + "rot_pitch").c_str());
147 rroll = config->get_float((cfg_prefix + "rot_roll").c_str());
148 } // else assume no rotation
149
150 if (frame == child_frame) {
151 throw Exception("Parent and child frames may not be the same");
152 }
153
154 try {
155 Entry e;
156 e.name = cfg_name;
157
158 fawkes::Time time(clock);
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));
163 e.transform = new tf::StampedTransform(t, time, frame, child_frame);
164 } else {
165 tf::Quaternion q;
166 q.setEulerZYX(ryaw, rpitch, rroll);
167 tf::Transform t(q, tf::Vector3(tx, ty, tz));
168 e.transform = new tf::StampedTransform(t, time, frame, child_frame);
169 }
170
171 tf::Quaternion q = e.transform->getRotation();
172
173 tf::assert_quaternion_valid(q);
174
175 tf::Vector3 &v = e.transform->getOrigin();
177 "Adding transform '%s' (%s -> %s): "
178 "T(%f,%f,%f) Q(%f,%f,%f,%f)",
179 e.name.c_str(),
180 e.transform->frame_id.c_str(),
181 e.transform->child_frame_id.c_str(),
182 v.x(),
183 v.y(),
184 v.z(),
185 q.x(),
186 q.y(),
187 q.z(),
188 q.w());
189
190 entries_.push_back(e);
191 tf_add_publisher("%s", e.transform->child_frame_id.c_str());
192 } catch (Exception &e) {
193 entries_delete();
194 throw;
195 }
196
197 } catch (Exception &e) {
198 e.prepend("Transform %s: wrong or incomplete transform data", cfg_name.c_str());
199 throw;
200 }
201
202 transforms.insert(cfg_name);
203 } else {
204 //printf("Ignoring laser config %s\n", cfg_name.c_str());
205 ignored_transforms.insert(cfg_name);
206 }
207 }
208 }
209
210 if (entries_.empty()) {
211 throw Exception("No transforms configured");
212 }
213
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),
217 /* is_static */ true);
218 }
219}
220
221void
222StaticTransformsThread::entries_delete()
223{
224 std::list<Entry>::iterator i;
225 for (i = entries_.begin(); i != entries_.end(); ++i) {
226 delete tf_publishers[i->name];
227 tf_publishers.erase(i->name);
228 delete i->transform;
229 }
230 entries_.clear();
231}
232
233void
234StaticTransformsThread::config_value_changed(const fawkes::Configuration::ValueIterator *v)
235{
237
238 entries_delete();
239 entries_get_from_config();
240}
241
242void
244{
245}
virtual void init()
Initialize the thread.
virtual ~StaticTransformsThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Interface for configuration change handling.
Iterator interface to iterate over config values.
Definition: config.h:75
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.
Definition: config.cpp:619
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.
Definition: config.cpp:603
Base class for exceptions in Fawkes.
Definition: exception.h:36
void prepend(const char *format,...) noexcept
Prepend messages to the message list.
Definition: exception.cpp:314
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.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
Thread class encapsulation of pthreads.
Definition: thread.h:46
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
std::map< std::string, tf::TransformPublisher * > tf_publishers
Map of transform publishers created through the aspect.
Definition: tf.h:70
void tf_add_publisher(const char *frame_id_format,...)
Late add of publisher.
Definition: tf.cpp:186
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Fawkes library namespace.