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>
76 add_aspect(
"TransformAspect");
77 if (((mode == ONLY_PUBLISHER) || (mode == BOTH) || (mode == BOTH_DEFER_PUBLISHER)
78 || (mode == DEFER_PUBLISHER))
80 tf_aspect_frame_id_ = strdup(frame_id);
82 tf_aspect_frame_id_ = 0;
84 tf_aspect_blackboard_ = 0;
90 if (tf_aspect_frame_id_)
91 free(tf_aspect_frame_id_);
103 const char * thread_name)
106 && (tf_aspect_frame_id_ == NULL)) {
108 "in mode %s but BB interface ID"
110 (tf_aspect_mode_ ==
BOTH) ?
"BOTH" :
"ONLY_PUBLISHER");
123 tf_publisher =
new tf::TransformPublisher(tf_aspect_blackboard_, tf_aspect_frame_id_);
148 throw Exception(
"Publisher can only be enabled later in (BOTH_)DEFER_PUBLISHER mode");
151 if (tf_aspect_frame_id_) {
152 throw Exception(
"Cannot overwrite frame_id '%s' with '%s' in tf_enable_publisher",
156 tf_aspect_frame_id_ = strdup(frame_id);
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",
163 :
"BOTH_DEFER_PUBLISHER");
167 tf_publisher =
new tf::TransformPublisher(tf_aspect_blackboard_, tf_aspect_frame_id_);
189 throw Exception(
"Publisher can only be enabled later in (BOTH_)DEFER_PUBLISHER mode");
193 va_start(arg, frame_id_format);
196 if (vasprintf(&msg, frame_id_format, arg) == -1) {
200 std::string frame_id = msg;
204 throw Exception(
"Publisher for %s has already been added", frame_id.c_str());
207 tf_publishers[frame_id] =
new tf::TransformPublisher(tf_aspect_blackboard_, frame_id.c_str());
216 if (tf_aspect_frame_id_) {
220 std::map<std::string, tf::TransformPublisher *>::iterator ti;
227 delete tf_aspect_blackboard_;
228 tf_aspect_blackboard_ = 0;