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