Fawkes API Fawkes Development Version
transform_listener.cpp
1/***************************************************************************
2 * transform_listener.cpp - Fawkes transform listener (based on ROS tf)
3 *
4 * Created: Mon Oct 24 18:47:00 2011
5 * Copyright 2011 Tim Niemueller [www.niemueller.de]
6 ****************************************************************************/
7
8/* This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version. A runtime exception applies to
12 * this software (see LICENSE.GPL_WRE file mentioned below for details).
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
20 */
21
22/* This code is based on ROS tf with the following copyright and license:
23 *
24 * Copyright (c) 2008, Willow Garage, Inc.
25 * All rights reserved.
26 *
27 * Redistribution and use in source and binary forms, with or without
28 * modification, are permitted provided that the following conditions are met:
29 *
30 * * Redistributions of source code must retain the above copyright
31 * notice, this list of conditions and the following disclaimer.
32 * * Redistributions in binary form must reproduce the above copyright
33 * notice, this list of conditions and the following disclaimer in the
34 * documentation and/or other materials provided with the distribution.
35 * * Neither the name of the Willow Garage, Inc. nor the names of its
36 * contributors may be used to endorse or promote products derived from
37 * this software without specific prior written permission.
38 *
39 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
40 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
41 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
42 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
43 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
44 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
45 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
46 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
47 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49 * POSSIBILITY OF SUCH DAMAGE.
50 */
51
52#include <blackboard/blackboard.h>
53#include <interfaces/TransformInterface.h>
54#include <tf/transform_listener.h>
55#include <tf/transformer.h>
56
57#include <cstring>
58
59namespace fawkes {
60namespace tf {
61
62/** @class TransformListener <tf/transform_listener.h>
63 * Receive transforms and answer queries.
64 * This class connects to the blackboard and listens to all interfaces
65 * publishing transforms. It opens all interfaces of type
66 * TransformInterface with a TF prefix. The data is internally
67 * cached. Queries are then resolved based on the received
68 * information.
69 * @author Tim Niemueller
70 */
71
72/** Constructor.
73 * @param bb blackboard to listen to
74 * @param tf_transformer transformer to add transforms to
75 * @param bb_is_remote must be true if the blackboard is a RemoteBlackboard
76 */
77TransformListener::TransformListener(BlackBoard *bb, Transformer *tf_transformer, bool bb_is_remote)
78: BlackBoardInterfaceListener("TransformListener"),
79 bb_(bb),
80 tf_transformer_(tf_transformer),
81 bb_is_remote_(bb_is_remote)
82{
83 if (bb_) {
84 tfifs_ = bb_->open_multiple_for_reading<TransformInterface>("/tf*");
85
86 std::list<TransformInterface *>::iterator i;
87 for (i = tfifs_.begin(); i != tfifs_.end(); ++i) {
89 // update data once we
91 }
92 bb_->register_listener(this);
93
94 bbio_add_observed_create("TransformInterface", "/tf*");
95 bb_->register_observer(this);
96 tf_transformer->set_enabled(true);
97 } else {
98 tf_transformer->set_enabled(false);
99 }
100}
101
102/** Destructor. */
104{
105 if (bb_) {
106 bb_->unregister_listener(this);
107 bb_->unregister_observer(this);
108
109 std::list<TransformInterface *>::iterator i;
110 for (i = tfifs_.begin(); i != tfifs_.end(); ++i) {
111 bb_->close(*i);
112 }
113 tfifs_.clear();
114 }
115}
116
117void
118TransformListener::bb_interface_created(const char *type, const char *id) noexcept
119{
120 if (strncmp(type, "TransformInterface", INTERFACE_TYPE_SIZE_) != 0)
121 return;
122
123 TransformInterface *tfif;
124 try {
125 tfif = bb_->open_for_reading<TransformInterface>(id, "TF-Listener");
126 } catch (Exception &e) {
127 // ignored
128 return;
129 }
130
131 bb_interface_data_refreshed(tfif);
132
133 try {
134 bbil_add_data_interface(tfif);
135 bb_->update_listener(this);
136 tfifs_.push_back(tfif);
137 } catch (Exception &e) {
138 bb_->close(tfif);
139 return;
140 }
141}
142
143void
145{
146 conditional_close(interface);
147}
148
149void
151{
152 conditional_close(interface);
153}
154
155void
156TransformListener::conditional_close(Interface *interface) noexcept
157{
158 if (bb_is_remote_) {
159 return;
160 }
161 // Verify it's a TransformInterface
162 TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
163 if (!tfif)
164 return;
165
166 std::list<TransformInterface *>::iterator i;
167 for (i = tfifs_.begin(); i != tfifs_.end(); ++i) {
168 if (*interface == **i) {
169 if (!interface->has_writer() && (interface->num_readers() == 1)) {
170 // It's only us
171 bbil_remove_data_interface(*i);
172 bb_->update_listener(this);
173 bb_->close(*i);
174 tfifs_.erase(i);
175 break;
176 }
177 }
178 }
179}
180
181void
183{
184 TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
185 if (!tfif)
186 return;
187
188 tfif->read();
189
190 std::string authority;
191 if (bb_is_remote_) {
192 authority = "remote";
193 } else {
194 std::string authority = tfif->writer();
195 }
196
197 double * translation = tfif->translation();
198 double * rotation = tfif->rotation();
199 const Time * time = tfif->timestamp();
200 const std::string frame_id = tfif->frame();
201 const std::string child_frame_id = tfif->child_frame();
202
203 try {
204 Vector3 t(translation[0], translation[1], translation[2]);
205 Quaternion r(rotation[0], rotation[1], rotation[2], rotation[3]);
206 assert_quaternion_valid(r);
207 Transform tr(r, t);
208
209 StampedTransform str(tr, *time, frame_id, child_frame_id);
210
211 tf_transformer_->set_transform(str, authority, tfif->is_static_transform());
212 } catch (InvalidArgumentException &e) {
213 // ignore invalid, might just be not initialized, yet.
214 }
215}
216
217} // end namespace tf
218} // end namespace fawkes
BlackBoard interface listener.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
The BlackBoard abstract class.
Definition: blackboard.h:46
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
std::string writer() const
Get owner name of writing interface instance.
Definition: interface.cpp:886
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:714
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
A class for handling time.
Definition: time.h:93
TransformInterface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
char * child_frame() const
Get child_frame value.
bool is_static_transform() const
Get static_transform value.
double * rotation() const
Get rotation value.
double * translation() const
Get translation value.
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29
Passed argument was invalid.
Definition: exceptions.h:55
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
virtual void bb_interface_writer_removed(Interface *interface, Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
TransformListener(BlackBoard *bb, Transformer *tf_transformer, bool bb_is_remote=false)
Constructor.
virtual void bb_interface_reader_removed(Interface *interface, Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
virtual ~TransformListener()
Destructor.
virtual void bb_interface_data_refreshed(Interface *interface) noexcept
BlackBoard data refreshed notification.
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void set_enabled(bool enabled)
Set enabled status of transformer.
Fawkes library namespace.