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 <tf/transform_listener.h>
53 #include <tf/utils.h>
54 
55 #include <blackboard/blackboard.h>
56 #include <interfaces/TransformInterface.h>
57 
58 #include <cstring>
59 
60 namespace fawkes {
61  namespace tf {
62 #if 0 /* just to make Emacs auto-indent happy */
63  }
64 }
65 #endif
66 
67 /** @class TransformListener <tf/transform_listener.h>
68  * Receive transforms and answer queries.
69  * This class connects to the blackboard and listens to all interfaces
70  * publishing transforms. It opens all interfaces of type
71  * TransformInterface with a TF prefix. The data is internally
72  * cached. Queries are then resolved based on the received
73  * information.
74  * @author Tim Niemueller
75  */
76 
77 /** Constructor.
78  * @param bb blackboard to listen to
79  */
80 TransformListener::TransformListener(BlackBoard *bb)
81  : BlackBoardInterfaceListener("TransformListener"),
82  __bb(bb)
83 {
84  if (__bb) {
85  __tfifs = __bb->open_multiple_for_reading<TransformInterface>("TF *");
86 
87  std::list<TransformInterface *>::iterator i;
88  for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
90  }
91  __bb->register_listener(this);
92 
93  bbio_add_observed_create("TransformInterface", "TF *");
94  __bb->register_observer(this);
95  set_enabled(true);
96  } else {
97  set_enabled(false);
98  }
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 
117 
118 /** Resolve transform name.
119  * @param frame_name frame name
120  * @return resolved frame name
121  */
122 std::string
123 TransformListener::resolve(const std::string& frame_name)
124 {
125  return fawkes::tf::resolve("", frame_name);
126 }
127 
128 void
129 TransformListener::bb_interface_created(const char *type, const char *id) throw()
130 {
131  if (strncmp(type, "TransformInterface", __INTERFACE_TYPE_SIZE) != 0) return;
132 
133  TransformInterface *tfif;
134  try {
135  tfif = __bb->open_for_reading<TransformInterface>(id);
136  } catch (Exception &e) {
137  // ignored
138  return;
139  }
140 
141  try {
142  bbil_add_data_interface(tfif);
143  __bb->update_listener(this);
144  __tfifs.push_back(tfif);
145  } catch (Exception &e) {
146  __bb->close(tfif);
147  return;
148  }
149 }
150 
151 void
153  unsigned int instance_serial)
154  throw()
155 {
156  conditional_close(interface);
157 }
158 
159 
160 void
162  unsigned int instance_serial)
163  throw()
164 {
165  conditional_close(interface);
166 }
167 
168 
169 void
170 TransformListener::conditional_close(Interface *interface) throw()
171 {
172  // Verify it's a TransformInterface
173  TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
174  if (! tfif) return;
175 
176  std::list<TransformInterface *>::iterator i;
177  for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
178  if (*interface == **i) {
179  if (! interface->has_writer() && (interface->num_readers() == 1)) {
180  // It's only us
181  bbil_remove_data_interface(*i);
182  __bb->update_listener(this);
183  __bb->close(*i);
184  __tfifs.erase(i);
185  break;
186  }
187  }
188  }
189 }
190 
191 
192 void
194 {
195  TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
196  if (! tfif) return;
197 
198  tfif->read();
199 
200  double *translation = tfif->translation();
201  double *rotation = tfif->rotation();
202  const Time *time = tfif->timestamp();
203  const std::string frame_id = tfif->frame();
204  const std::string child_frame_id = tfif->child_frame();
205 
206  Vector3 t(translation[0], translation[1], translation[2]);
207  Quaternion r(rotation[0], rotation[1], rotation[2], rotation[3]);
208  Transform tr(r, t);
209 
210  StampedTransform str(tr, *time, frame_id, child_frame_id);
211 
212  set_transform(str);
213 }
214 
215 } // end namespace tf
216 } // end namespace fawkes
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:211
virtual void bb_interface_data_changed(Interface *interface)
BlackBoard data changed notification.
double * translation() const
Get translation value.
Fawkes library namespace.
void set_enabled(bool enabled)
Set transformer enabled or disabled.
A class for handling time.
Definition: time.h:91
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:200
TransformInterface Fawkes BlackBoard Interface.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
double * rotation() const
Get rotation value.
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:691
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:174
virtual void bb_interface_writer_removed(Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:224
std::string resolve(const std::string &frame_name)
Resolve transform name.
char * frame() const
Get frame value.
virtual void bb_interface_reader_removed(Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
The BlackBoard abstract class.
Definition: blackboard.h:49
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*")=0
Open multiple interfaces for reading.
char * child_frame() const
Get child_frame value.
virtual ~TransformListener()
Destructor.
BlackBoard interface listener.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual void close(Interface *interface)=0
Close interface.