52 #include <blackboard/blackboard.h>
53 #include <interfaces/TransformInterface.h>
54 #include <tf/transform_listener.h>
55 #include <tf/transformer.h>
78 : BlackBoardInterfaceListener(
"TransformListener"),
80 tf_transformer_(tf_transformer),
81 bb_is_remote_(bb_is_remote)
84 tfifs_ = bb_->open_multiple_for_reading<TransformInterface>(
"/tf*");
86 std::list<TransformInterface *>::iterator i;
87 for (i = tfifs_.begin(); i != tfifs_.end(); ++i) {
88 bbil_add_data_interface(*i);
90 bb_interface_data_changed(*i);
92 bb_->register_listener(
this);
94 bbio_add_observed_create(
"TransformInterface",
"/tf*");
95 bb_->register_observer(
this);
109 std::list<TransformInterface *>::iterator i;
110 for (i = tfifs_.begin(); i != tfifs_.end(); ++i) {
120 if (strncmp(type,
"TransformInterface", INTERFACE_TYPE_SIZE_) != 0)
126 }
catch (Exception &e) {
131 bb_interface_data_changed(tfif);
134 bbil_add_data_interface(tfif);
135 bb_->update_listener(
this);
136 tfifs_.push_back(tfif);
145 unsigned int instance_serial)
throw()
147 conditional_close(interface);
152 unsigned int instance_serial)
throw()
154 conditional_close(interface);
158 TransformListener::conditional_close(
Interface *interface)
throw()
168 std::list<TransformInterface *>::iterator i;
169 for (i = tfifs_.begin(); i != tfifs_.end(); ++i) {
170 if (*interface == **i) {
171 if (!interface->has_writer() && (interface->num_readers() == 1)) {
173 bbil_remove_data_interface(*i);
174 bb_->update_listener(
this);
186 TransformInterface *tfif =
dynamic_cast<TransformInterface *
>(interface);
192 std::string authority;
194 authority =
"remote";
196 std::string authority = tfif->writer();
199 double * translation = tfif->translation();
200 double * rotation = tfif->rotation();
201 const Time * time = tfif->timestamp();
202 const std::string frame_id = tfif->frame();
203 const std::string child_frame_id = tfif->child_frame();
206 Vector3 t(translation[0], translation[1], translation[2]);
207 Quaternion r(rotation[0], rotation[1], rotation[2], rotation[3]);
208 assert_quaternion_valid(r);
211 StampedTransform str(tr, *time, frame_id, child_frame_id);
213 tf_transformer_->set_transform(str, authority, tfif->is_static_transform());
214 }
catch (InvalidArgumentException &e) {