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 
31 using 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),
48  ConfigurationChangeHandler(CFG_PREFIX)
49 {
50 }
51 
52 /** Destructor. */
54 {
55 }
56 
57 void
59 {
60  entries_get_from_config();
61 
63 }
64 
65 void
67 {
69  entries_delete();
70 }
71 
72 void
73 StaticTransformsThread::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] == '/') {
104  logger->log_warn(name(),
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] == '/') {
112  logger->log_warn(name(),
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();
176  logger->log_debug(name(),
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 
221 void
222 StaticTransformsThread::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 
233 void
234 StaticTransformsThread::config_value_changed(const fawkes::Configuration::ValueIterator *v)
235 {
236  MutexLocker lock(loop_mutex);
237 
238  entries_delete();
239  entries_get_from_config();
240 }
241 
242 void
244 {
245 }
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:92
Thread class encapsulation of pthreads.
Definition: thread.h:45
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread aspect to access the transform system.
Definition: tf.h:38
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:42
std::map< std::string, tf::TransformPublisher * > tf_publishers
Map of transform publishers created through the aspect.
Definition: tf.h:70
Base class for exceptions in Fawkes.
Definition: exception.h:35
virtual void rem_change_handler(ConfigurationChangeHandler *h)
Remove a configuration change handler.
Definition: config.cpp:619
void prepend(const char *format,...)
Prepend messages to the message list.
Definition: exception.cpp:314
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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:186
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Iterator interface to iterate over config values.
Definition: config.h:71
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Definition: config.cpp:603
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:41
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.