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
34namespace 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 */
74TransformAspect::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 */
100void
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 */
144void
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 */
185void
186TransformAspect::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 */
213void
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
void add_aspect(const char *name)
Add an aspect to a thread.
Definition: aspect.cpp:49
BlackBoard that traces interface ownership.
Definition: ownership.h:31
The BlackBoard abstract class.
Definition: blackboard.h:46
Base class for exceptions in Fawkes.
Definition: exception.h:36
System ran out of memory and desired operation could not be fulfilled.
Definition: system.h:32
Mode
Enumeration describing the desired mode of operation.
Definition: tf.h:42
@ ONLY_LISTENER
only create a transform listener
Definition: tf.h:43
@ ONLY_PUBLISHER
only create a transform publisher
Definition: tf.h:44
@ BOTH
create both, transform listener and publisher
Definition: tf.h:49
@ BOTH_DEFER_PUBLISHER
create transform listener but defer creation of publisher, cf.
Definition: tf.h:50
@ DEFER_PUBLISHER
Create neither listener or publisher, but allow late enabling of a publisher using tf_enable_publishe...
Definition: tf.h:45
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:145
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
void init_TransformAspect(BlackBoard *blackboard, tf::Transformer *transformer, const char *thread_name)
Init transform aspect.
Definition: tf.cpp:101
std::map< std::string, tf::TransformPublisher * > tf_publishers
Map of transform publishers created through the aspect.
Definition: tf.h:70
TransformAspect(Mode mode=ONLY_LISTENER, const char *frame_id=0)
Constructor.
Definition: tf.cpp:74
virtual ~TransformAspect()
Virtual empty destructor.
Definition: tf.cpp:88
void tf_add_publisher(const char *frame_id_format,...)
Late add of publisher.
Definition: tf.cpp:186
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
void finalize_TransformAspect()
Finalize transform aspect.
Definition: tf.cpp:214
Utility class to send transforms.
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
Fawkes library namespace.