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 <utils/time/time.h>
26 #include <set>
27 #include <memory>
28 #include <core/threading/mutex_locker.h>
29 
30 using namespace fawkes;
31 
32 /** @class StaticTransformsThread "static_transforms_thread.h"
33  * Thread to regularly publish static transforms.
34  * This thread runs at the sensor hook and publishes a set of
35  * transforms. The transforms are set in the configuration and
36  * are static at run-time. Only the timestamp is updated between
37  * writes.
38  * @author Tim Niemueller
39  */
40 
41 #define CFG_PREFIX "/plugins/static-transforms/"
42 
43 /** Constructor. */
45  : Thread("StaticTransformsThread", Thread::OPMODE_WAITFORWAKEUP),
46  TransformAspect(TransformAspect::DEFER_PUBLISHER),
47  ConfigurationChangeHandler(CFG_PREFIX)
48 {
49 }
50 
51 
52 /** Destructor. */
54 {
55 }
56 
57 
58 void
60 {
61  entries_get_from_config();
62 
64 }
65 
66 
67 void
69 {
71  entries_delete();
72 }
73 
74 void
75 StaticTransformsThread::entries_get_from_config()
76 {
77  std::set<std::string> transforms;
78  std::set<std::string> ignored_transforms;
79 
80  std::string prefix = CFG_PREFIX"transforms/";
81 #if __cplusplus >= 201103L
82  std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
83 #else
84  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
85 #endif
86  while (i->next()) {
87  std::string cfg_name = std::string(i->path()).substr(prefix.length());
88  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
89 
90  if ( (transforms.find(cfg_name) == transforms.end()) &&
91  (ignored_transforms.find(cfg_name) == ignored_transforms.end()) ) {
92 
93  std::string cfg_prefix = prefix + cfg_name + "/";
94 
95  bool active = true;
96  try {
97  active = config->get_bool((cfg_prefix + "active").c_str());
98  } catch (Exception &e) {} // ignored, assume enabled
99 
100  if (active) {
101  try {
102  std::string frame = config->get_string((cfg_prefix + "frame").c_str());
103  std::string child_frame =
104  config->get_string((cfg_prefix + "child_frame").c_str());
105 
106  if (frame[0] == '/') {
107  logger->log_warn(name(), "Transform %s parent frame %s starts with /,"
108  "removing leading slash.", cfg_name.c_str(), frame.c_str());
109  frame = frame.substr(1);
110  }
111  if (child_frame[0] == '/') {
112  logger->log_warn(name(), "Transform %s child frame %s starts with /,"
113  "removing leading slash.", cfg_name.c_str(), frame.c_str());
114  child_frame = child_frame.substr(1);
115  }
116 
117  float tx = 0., ty = 0., tz = 0.;
118  if (config->exists((cfg_prefix + "trans_x").c_str()) ||
119  config->exists((cfg_prefix + "trans_y").c_str()) ||
120  config->exists((cfg_prefix + "trans_z").c_str()))
121  {
122  tx = config->get_float((cfg_prefix + "trans_x").c_str());
123  ty = config->get_float((cfg_prefix + "trans_y").c_str());
124  tz = config->get_float((cfg_prefix + "trans_z").c_str());
125  } // else assume no translation
126 
127  bool use_quaternion = false;
128  float rx = 0., ry = 0., rz = 0., rw = 1.,
129  ryaw = 0., rpitch = 0., rroll = 0.;
130 
131  if (config->exists((cfg_prefix + "rot_x").c_str()) ||
132  config->exists((cfg_prefix + "rot_y").c_str()) ||
133  config->exists((cfg_prefix + "rot_z").c_str()) ||
134  config->exists((cfg_prefix + "rot_w").c_str()) )
135  {
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  {
146  ryaw = config->get_float((cfg_prefix + "rot_yaw").c_str());
147  rpitch = config->get_float((cfg_prefix + "rot_pitch").c_str());
148  rroll = config->get_float((cfg_prefix + "rot_roll").c_str());
149  } // else assume no rotation
150 
151  if (frame == child_frame) {
152  throw Exception("Parent and child frames may not be the same");
153  }
154 
155  try {
156  Entry e;
157  e.name = cfg_name;
158 
159  fawkes::Time time(clock);
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));
164  e.transform = new tf::StampedTransform(t, time, frame, child_frame);
165  } else {
166  tf::Quaternion q; 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();
176  logger->log_debug(name(), "Adding transform '%s' (%s -> %s): "
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());
181 
182  __entries.push_back(e);
183  tf_add_publisher("%s", e.transform->child_frame_id.c_str());
184  } catch (Exception &e) {
185  entries_delete();
186  throw;
187  }
188 
189  } catch (Exception &e) {
190  e.prepend("Transform %s: wrong or incomplete transform data", cfg_name.c_str());
191  throw;
192  }
193 
194  transforms.insert(cfg_name);
195  } else {
196  //printf("Ignoring laser config %s\n", cfg_name.c_str());
197  ignored_transforms.insert(cfg_name);
198  }
199  }
200  }
201 
202  if ( __entries.empty() ) {
203  throw Exception("No transforms configured");
204  }
205 
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), /* is_static */ true);
209  }
210 }
211 
212 void
213 StaticTransformsThread::entries_delete()
214 {
215  std::list<Entry>::iterator i;
216  for (i = __entries.begin(); i != __entries.end(); ++i) {
217  delete tf_publishers[i->name];
218  tf_publishers.erase(i->name);
219  delete i->transform;
220  }
221  __entries.clear();
222 }
223 
224 void
225 StaticTransformsThread::config_value_changed(const fawkes::Configuration::ValueIterator *v)
226 {
227  MutexLocker lock(loop_mutex);
228 
229  entries_delete();
230  entries_get_from_config();
231 }
232 
233 void
235 {
236 }
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Mutex locking helper.
Definition: mutex_locker.h:33
Interface for configuration change handling.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
A class for handling time.
Definition: time.h:91
virtual bool next()=0
Check if there is another element and advance to this if possible.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:139
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to access the transform system.
Definition: tf.h:42
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
std::map< std::string, tf::TransformPublisher * > tf_publishers
Map of transform publishers created through the aspect.
Definition: tf.h:73
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void rem_change_handler(ConfigurationChangeHandler *h)
Remove a configuration change handler.
Definition: config.cpp:564
void prepend(const char *format,...)
Prepend messages to the message list.
Definition: exception.cpp:322
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
const char * name() const
Get name of thread.
Definition: thread.h:95
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 loop()
Code to execute in the thread.
void tf_add_publisher(const char *frame_id_format,...)
Late add of publisher.
Definition: tf.cpp:198
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Iterator interface to iterate over config values.
Definition: config.h:72
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Definition: config.cpp:547
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.
Definition: configurable.h:44
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.
virtual ~StaticTransformsThread()
Destructor.