Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "static_transforms_thread.h"
23 
24 #include <utils/time/time.h>
25 #include <set>
26 #include <memory>
27 
28 using namespace fawkes;
29 
30 /** @class StaticTransformsThread "static_transforms_thread.h"
31  * Thread to regularly publish static transforms.
32  * This thread runs at the sensor hook and publishes a set of
33  * transforms. The transforms are set in the configuration and
34  * are static at run-time. Only the timestamp is updated between
35  * writes.
36  * @author Tim Niemueller
37  */
38 
39 /** Constructor. */
41  : Thread("StaticTransformsThread", Thread::OPMODE_WAITFORWAKEUP),
42  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
43  TransformAspect(TransformAspect::ONLY_PUBLISHER, "static")
44 {
45  __last_update = new Time();
46 }
47 
48 
49 /** Destructor. */
51 {
52  delete __last_update;
53 }
54 
55 
56 void
58 {
59  std::set<std::string> transforms;
60  std::set<std::string> ignored_transforms;
61 
62  __cfg_update_interval = 1.0;
63  try {
64  __cfg_update_interval = config->get_float("/plugins/static-transforms/update-interval");
65  } catch (Exception &e) {} // ignored, use default
66 
67  std::string prefix = "/plugins/static-transforms/transforms/";
68  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
69  while (i->next()) {
70  std::string cfg_name = std::string(i->path()).substr(prefix.length());
71  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
72 
73  if ( (transforms.find(cfg_name) == transforms.end()) &&
74  (ignored_transforms.find(cfg_name) == ignored_transforms.end()) ) {
75 
76  std::string cfg_prefix = prefix + cfg_name + "/";
77 
78  bool active = true;
79  try {
80  active = config->get_bool((cfg_prefix + "active").c_str());
81  } catch (Exception &e) {} // ignored, assume enabled
82 
83  if (active) {
84  try {
85  std::string frame = config->get_string((cfg_prefix + "frame").c_str());
86  std::string child_frame =
87  config->get_string((cfg_prefix + "child_frame").c_str());
88 
89  float tx = 0., ty = 0., tz = 0.;
90  if (config->exists((cfg_prefix + "trans_x").c_str()) ||
91  config->exists((cfg_prefix + "trans_y").c_str()) ||
92  config->exists((cfg_prefix + "trans_z").c_str()))
93  {
94  tx = config->get_float((cfg_prefix + "trans_x").c_str());
95  ty = config->get_float((cfg_prefix + "trans_y").c_str());
96  tz = config->get_float((cfg_prefix + "trans_z").c_str());
97  } // else assume no translation
98 
99  bool use_quaternion = false;
100  float rx = 0., ry = 0., rz = 0., rw = 1.,
101  ryaw = 0., rpitch = 0., rroll = 0.;
102 
103  if (config->exists((cfg_prefix + "rot_x").c_str()) ||
104  config->exists((cfg_prefix + "rot_y").c_str()) ||
105  config->exists((cfg_prefix + "rot_z").c_str()) ||
106  config->exists((cfg_prefix + "rot_w").c_str()) )
107  {
108  use_quaternion = true;
109  rx = config->get_float((cfg_prefix + "rot_x").c_str());
110  ry = config->get_float((cfg_prefix + "rot_y").c_str());
111  rz = config->get_float((cfg_prefix + "rot_z").c_str());
112  rw = config->get_float((cfg_prefix + "rot_w").c_str());
113 
114  } else if (config->exists((cfg_prefix + "rot_roll").c_str()) ||
115  config->exists((cfg_prefix + "rot_pitch").c_str()) ||
116  config->exists((cfg_prefix + "rot_yaw").c_str()))
117  {
118  ryaw = config->get_float((cfg_prefix + "rot_yaw").c_str());
119  rpitch = config->get_float((cfg_prefix + "rot_pitch").c_str());
120  rroll = config->get_float((cfg_prefix + "rot_roll").c_str());
121  } // else assume no rotation
122 
123  if (frame == child_frame) {
124  throw Exception("Parent and child frames may not be the same");
125  }
126 
127  try {
128  Entry e;
129  e.name = cfg_name;
130 
131  fawkes::Time time(clock);
132  if (use_quaternion) {
133  tf::Quaternion q(rx, ry, rz, rw);
134  tf::assert_quaternion_valid(q);
135  tf::Transform t(q, tf::Vector3(tx, ty, tz));
136  e.transform = new tf::StampedTransform(t, time, frame, child_frame);
137  } else {
138  tf::Quaternion q; q.setEulerZYX(ryaw, rpitch, rroll);
139  tf::Transform t(q, tf::Vector3(tx, ty, tz));
140  e.transform = new tf::StampedTransform(t, time, frame, child_frame);
141  }
142 
143  tf::Quaternion q = e.transform->getRotation();
144 
145  tf::assert_quaternion_valid(q);
146 
147  tf::Vector3 &v = e.transform->getOrigin();
148  logger->log_debug(name(), "Adding transform '%s' (%s -> %s): "
149  "T(%f,%f,%f) Q(%f,%f,%f,%f)", e.name.c_str(),
150  e.transform->frame_id.c_str(),
151  e.transform->child_frame_id.c_str(),
152  v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
153 
154  __entries.push_back(e);
155  } catch (Exception &e) {
156  std::list<Entry>::iterator i;
157  for (i = __entries.begin(); i != __entries.end(); ++i) {
158  delete i->transform;
159  }
160  __entries.clear();
161  throw;
162  }
163 
164  } catch (Exception &e) {
165  e.prepend("Transform %s: wrong or incomplete transform data",
166  cfg_name.c_str());
167  throw;
168  }
169 
170  transforms.insert(cfg_name);
171  } else {
172  //printf("Ignoring laser config %s\n", cfg_name.c_str());
173  ignored_transforms.insert(cfg_name);
174  }
175  }
176  }
177 
178  if ( __entries.empty() ) {
179  throw Exception("No transforms configured");
180  }
181 
182  __last_update->set_clock(clock);
183  __last_update->set_time(0, 0);
184 }
185 
186 
187 void
189 {
190  std::list<Entry>::iterator i;
191  for (i = __entries.begin(); i != __entries.end(); ++i) {
192  delete i->transform;
193  }
194  __entries.clear();
195 }
196 
197 
198 void
200 {
201  fawkes::Time now(clock);
202  if ((now - __last_update) > __cfg_update_interval) {
203  __last_update->stamp();
204 
205  // date time stamps slightly into the future so they are valid
206  // for longer and need less frequent updates.
207  fawkes::Time timestamp = now + (__cfg_update_interval * 1.1);
208 
209  std::list<Entry>::iterator i;
210  for (i = __entries.begin(); i != __entries.end(); ++i) {
211  i->transform->stamp = timestamp;
212  tf_publisher->send_transform(*(i->transform));
213  }
214  }
215 }
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:56
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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
virtual void send_transform(const StampedTransform &transform)
Publish transform.
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: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:45
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:329
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:91
virtual const char * path() const =0
Path of value.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void loop()
Code to execute in the thread.
void set_time(const timeval *tv)
Sets the time.
Definition: time.cpp:262
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual bool exists(const char *path)=0
Check if a given value exists.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
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.