Fawkes API  Fawkes Development Version
tf.cpp
1 
2 /***************************************************************************
3  * tf.cpp - Transform aspect for Fawkes
4  *
5  * Created: Tue Oct 25 21:35:14 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
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. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <aspect/tf.h>
25 #include <blackboard/ownership.h>
26 #include <core/exceptions/system.h>
27 #include <core/threading/thread_initializer.h>
28 #include <tf/transform_listener.h>
29 
30 #include <cstdarg>
31 #include <cstdlib>
32 #include <cstring>
33 
34 namespace fawkes {
35 
36 /** @class TransformAspect <aspect/tf.h>
37  * Thread aspect to access the transform system.
38 
39  * Give this aspect to your thread to gain access to the transform
40  * library. Depending on the parameters to the ctor only the listener
41  * or additionaly the publisher is created.
42  * It is guaranteed that if used properly from within plugins that the
43  * blackboard member has been initialized properly.
44  * @ingroup Aspects
45  * @author Tim Niemueller
46  */
47 
48 /** @var tf::TransformListener * TransformAspect::tf_listener
49  * This is the transform listener which saves transforms published by
50  * other threads in the system.
51  */
52 
53 /** @var tf::TransformPublisher * TransformAspect::tf_publisher
54  * This is the transform publisher which can be used to publish
55  * transforms via the blackboard. It is only created if the constructor
56  * taking the blackboard interface ID parameter is used!
57  */
58 
59 /** @var std::map<std::string, tf::TransformPublisher *> TransformAspect::tf_publishers
60  * Map of transform publishers created through the aspect.
61 
62  * The maps key is the blackboard interface ID passed to either the
63  * constructor or tf_add_publisher(). The ID is used as passed, i.e.,
64  * not with the /tf/ prefix which might be added by the
65  * TransformPublisher. The singular tf_publisher is also added to the
66  * map.
67  */
68 
69 /** Constructor.
70  * @param mode mode of operation
71  * @param frame_id ID of frame to create publisher for, can be zero if
72  * creating of publisher is omitted or deferred.
73  */
74 TransformAspect::TransformAspect(Mode mode, const char *frame_id) : tf_aspect_mode_(mode)
75 {
76  add_aspect("TransformAspect");
77  if (((mode == ONLY_PUBLISHER) || (mode == BOTH) || (mode == BOTH_DEFER_PUBLISHER)
78  || (mode == DEFER_PUBLISHER))
79  && frame_id) {
80  tf_aspect_frame_id_ = strdup(frame_id);
81  } else {
82  tf_aspect_frame_id_ = 0;
83  }
84  tf_aspect_blackboard_ = 0;
85 }
86 
87 /** Virtual empty destructor. */
89 {
90  if (tf_aspect_frame_id_)
91  free(tf_aspect_frame_id_);
92 }
93 
94 /** Init transform aspect.
95  * This creates the listener and potentially publisher.
96  * @param blackboard blackboard used to create listener and/or publisher.
97  * @param transformer system-wide shared transformer to pass to threads
98  * @param thread_name name of thread opening publishers
99  */
100 void
102  tf::Transformer *transformer,
103  const char * thread_name)
104 {
105  if (((tf_aspect_mode_ == ONLY_PUBLISHER) || (tf_aspect_mode_ == BOTH))
106  && (tf_aspect_frame_id_ == NULL)) {
107  throw CannotInitializeThreadException("TransformAspect was initialized "
108  "in mode %s but BB interface ID"
109  "is not set",
110  (tf_aspect_mode_ == BOTH) ? "BOTH" : "ONLY_PUBLISHER");
111  }
112 
113  tf_aspect_blackboard_ = new BlackBoardWithOwnership(blackboard, thread_name);
114 
115  if ((tf_aspect_mode_ == ONLY_LISTENER) || (tf_aspect_mode_ == BOTH)
116  || (tf_aspect_mode_ == BOTH_DEFER_PUBLISHER)) {
117  tf_listener = transformer;
118  } else {
119  tf_listener = NULL;
120  }
121 
122  if ((tf_aspect_mode_ == ONLY_PUBLISHER) || (tf_aspect_mode_ == BOTH)) {
123  tf_publisher = new tf::TransformPublisher(tf_aspect_blackboard_, tf_aspect_frame_id_);
124  tf_publishers[tf_aspect_frame_id_] = tf_publisher;
125  } else {
126  tf_publisher = new tf::TransformPublisher(NULL, NULL);
127  }
128 }
129 
130 /** Late enabling of publisher.
131  * If and only if the TransformAspect has been initialized in
132  * DEFER_PUBLISHER or BOTH_DEFER_PUBLISHER mode the transform
133  * publisher can be enabled using this method. It will create a new
134  * transform publisher with the interface ID given as constructor
135  * parameter.
136  *
137  * This method is intended to be used if it is unclear at construction
138  * time whether the publisher will be needed or not.
139  * @param frame_id Frame ID to use for publisher. This can only be passed if
140  * the frame_id passed to the constructor was null.
141  * @exception Exception thrown if the TransformAspect is not initialized in
142  * DEFER_PUBLISHER or BOTH_DEFER_PUBLISHER mode.
143  */
144 void
146 {
147  if ((tf_aspect_mode_ != DEFER_PUBLISHER) && (tf_aspect_mode_ != BOTH_DEFER_PUBLISHER)) {
148  throw Exception("Publisher can only be enabled later in (BOTH_)DEFER_PUBLISHER mode");
149  }
150  if (frame_id) {
151  if (tf_aspect_frame_id_) {
152  throw Exception("Cannot overwrite frame_id '%s' with '%s' in tf_enable_publisher",
153  tf_aspect_frame_id_,
154  frame_id);
155  } else {
156  tf_aspect_frame_id_ = strdup(frame_id);
157  }
158  }
159  if (tf_aspect_frame_id_ == 0) {
160  throw Exception("TransformAspect in %s mode "
161  "requires a valid blackboard interface ID to enable the publisher",
162  tf_aspect_mode_ == DEFER_PUBLISHER ? "DEFER_PUBLISHER"
163  : "BOTH_DEFER_PUBLISHER");
164  }
165 
166  delete tf_publisher;
167  tf_publisher = new tf::TransformPublisher(tf_aspect_blackboard_, tf_aspect_frame_id_);
168  tf_publishers[tf_aspect_frame_id_] = tf_publisher;
169 }
170 
171 /** Late add of publisher.
172  * If and only if the TransformAspect has been initialized in
173  * DEFER_PUBLISHER or BOTH_DEFER_PUBLISHER mode additional transform
174  * publishers can be added using this method. It will create a new
175  * transform publisher with the given interface ID.
176  *
177  * This method is intended to be used if it is unclear at construction
178  * time whether the publisher will be needed or not.
179  * @exception Exception thrown if the TransformAspect is not initialized in
180  * DEFER_PUBLISHER or BOTH_DEFER_PUBLISHER mode.
181  * @param frame_id_format format string of interface ID to create. See man printf
182  * for accepted patterns. If string starts with / is taken as is, otherwise "/tf/" is
183  * prepended to interface ID implicitly.
184  */
185 void
186 TransformAspect::tf_add_publisher(const char *frame_id_format, ...)
187 {
188  if ((tf_aspect_mode_ != DEFER_PUBLISHER) && (tf_aspect_mode_ != BOTH_DEFER_PUBLISHER)) {
189  throw Exception("Publisher can only be enabled later in (BOTH_)DEFER_PUBLISHER mode");
190  }
191 
192  va_list arg;
193  va_start(arg, frame_id_format);
194 
195  char *msg;
196  if (vasprintf(&msg, frame_id_format, arg) == -1) {
197  throw OutOfMemoryException("Cannot format transform publisher BB interface ID");
198  }
199  va_end(arg);
200  std::string frame_id = msg;
201  free(msg);
202 
203  if (tf_publishers.find(frame_id) != tf_publishers.end()) {
204  throw Exception("Publisher for %s has already been added", frame_id.c_str());
205  }
206 
207  tf_publishers[frame_id] = new tf::TransformPublisher(tf_aspect_blackboard_, frame_id.c_str());
208 }
209 
210 /** Finalize transform aspect.
211  * This deletes the transform listener and publisher.
212  */
213 void
215 {
216  if (tf_aspect_frame_id_) {
217  tf_publishers.erase(tf_aspect_frame_id_);
218  }
219  delete tf_publisher;
220  std::map<std::string, tf::TransformPublisher *>::iterator ti;
221  for (ti = tf_publishers.begin(); ti != tf_publishers.end(); ++ti) {
222  delete ti->second;
223  }
224  tf_publishers.clear();
225  tf_listener = 0;
226  tf_publisher = 0;
227  delete tf_aspect_blackboard_;
228  tf_aspect_blackboard_ = 0;
229 }
230 
231 } // end namespace fawkes
fawkes::Aspect::add_aspect
void add_aspect(const char *name)
Add an aspect to a thread.
Definition: aspect.cpp:49
fawkes::TransformAspect::tf_enable_publisher
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:145
fawkes::tf::TransformPublisher
Utility class to send transforms.
Definition: transform_publisher.h:67
fawkes::TransformAspect::finalize_TransformAspect
void finalize_TransformAspect()
Finalize transform aspect.
Definition: tf.cpp:214
fawkes::tf::Transformer
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
fawkes::BlackBoard
The BlackBoard abstract class.
Definition: blackboard.h:46
fawkes::CannotInitializeThreadException
Thread cannot be initialized.
Definition: thread_initializer.h:34
fawkes::TransformAspect::Mode
Mode
Enumeration describing the desired mode of operation.
Definition: tf.h:42
fawkes
Fawkes library namespace.
fawkes::TransformAspect::ONLY_PUBLISHER
@ ONLY_PUBLISHER
only create a transform publisher
Definition: tf.h:44
fawkes::TransformAspect::tf_add_publisher
void tf_add_publisher(const char *frame_id_format,...)
Late add of publisher.
Definition: tf.cpp:186
fawkes::TransformAspect::BOTH_DEFER_PUBLISHER
@ BOTH_DEFER_PUBLISHER
create transform listener but defer creation of publisher, cf.
Definition: tf.h:50
fawkes::TransformAspect::init_TransformAspect
void init_TransformAspect(BlackBoard *blackboard, tf::Transformer *transformer, const char *thread_name)
Init transform aspect.
Definition: tf.cpp:101
fawkes::TransformAspect::tf_listener
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
fawkes::BlackBoardWithOwnership
BlackBoard that traces interface ownership.
Definition: ownership.h:31
fawkes::TransformAspect::~TransformAspect
virtual ~TransformAspect()
Virtual empty destructor.
Definition: tf.cpp:88
fawkes::TransformAspect::tf_publisher
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
fawkes::OutOfMemoryException
System ran out of memory and desired operation could not be fulfilled.
Definition: system.h:32
fawkes::TransformAspect::ONLY_LISTENER
@ ONLY_LISTENER
only create a transform listener
Definition: tf.h:43
fawkes::TransformAspect::tf_publishers
std::map< std::string, tf::TransformPublisher * > tf_publishers
Map of transform publishers created through the aspect.
Definition: tf.h:70
fawkes::TransformAspect::BOTH
@ BOTH
create both, transform listener and publisher
Definition: tf.h:49
fawkes::TransformAspect::TransformAspect
TransformAspect(Mode mode=ONLY_LISTENER, const char *frame_id=0)
Constructor.
Definition: tf.cpp:74
fawkes::TransformAspect::DEFER_PUBLISHER
@ DEFER_PUBLISHER
Create neither listener or publisher, but allow late enabling of a publisher using tf_enable_publishe...
Definition: tf.h:45
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36