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 
59 namespace fawkes {
60 namespace 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  */
77 TransformListener::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 
117 void
118 TransformListener::bb_interface_created(const char *type, const char *id) throw()
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_changed(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 
143 void
145  unsigned int instance_serial) throw()
146 {
147  conditional_close(interface);
148 }
149 
150 void
152  unsigned int instance_serial) throw()
153 {
154  conditional_close(interface);
155 }
156 
157 void
158 TransformListener::conditional_close(Interface *interface) throw()
159 {
160  if (bb_is_remote_) {
161  return;
162  }
163  // Verify it's a TransformInterface
164  TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
165  if (!tfif)
166  return;
167 
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)) {
172  // It's only us
173  bbil_remove_data_interface(*i);
174  bb_->update_listener(this);
175  bb_->close(*i);
176  tfifs_.erase(i);
177  break;
178  }
179  }
180  }
181 }
182 
183 void
185 {
186  TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
187  if (!tfif)
188  return;
189 
190  tfif->read();
191 
192  std::string authority;
193  if (bb_is_remote_) {
194  authority = "remote";
195  } else {
196  std::string authority = tfif->writer();
197  }
198 
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();
204 
205  try {
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);
209  Transform tr(r, t);
210 
211  StampedTransform str(tr, *time, frame_id, child_frame_id);
212 
213  tf_transformer_->set_transform(str, authority, tfif->is_static_transform());
214  } catch (InvalidArgumentException &e) {
215  // ignore invalid, might just be not initialized, yet.
216  }
217 }
218 
219 } // end namespace tf
220 } // end namespace fawkes
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
fawkes::TransformInterface::child_frame
char * child_frame() const
Get child_frame value.
Definition: TransformInterface.cpp:119
fawkes::TransformInterface::translation
double * translation() const
Get translation value.
Definition: TransformInterface.cpp:192
fawkes::TransformInterface::is_static_transform
bool is_static_transform() const
Get static_transform value.
Definition: TransformInterface.cpp:155
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
fawkes::Interface::timestamp
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:705
fawkes::tf::TransformListener::bb_interface_data_changed
virtual void bb_interface_data_changed(Interface *interface)
BlackBoard data changed notification.
Definition: transform_listener.cpp:184
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
fawkes::tf::Transformer
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
fawkes::BlackBoardInterfaceListener
BlackBoard interface listener.
Definition: interface_listener.h:42
fawkes::BlackBoard
The BlackBoard abstract class.
Definition: blackboard.h:46
fawkes::TransformInterface::rotation
double * rotation() const
Get rotation value.
Definition: TransformInterface.cpp:264
fawkes::TransformInterface::frame
char * frame() const
Get frame value.
Definition: TransformInterface.cpp:83
fawkes::tf::Transformer::set_enabled
void set_enabled(bool enabled)
Set enabled status of transformer.
Definition: transformer.cpp:110
fawkes::BlackBoardInterfaceObserver::bbio_add_observed_create
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
Definition: interface_observer.cpp:119
fawkes::Interface::writer
std::string writer() const
Get owner name of writing interface instance.
Definition: interface.cpp:855
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes
Fawkes library namespace.
fawkes::tf::TransformListener::bb_interface_writer_removed
virtual void bb_interface_writer_removed(Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
Definition: transform_listener.cpp:144
fawkes::tf::TransformListener::TransformListener
TransformListener(BlackBoard *bb, Transformer *tf_transformer, bool bb_is_remote=false)
Constructor.
Definition: transform_listener.cpp:77
fawkes::BlackBoard::register_observer
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::BlackBoard::unregister_observer
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
fawkes::tf::TransformListener::~TransformListener
virtual ~TransformListener()
Destructor.
Definition: transform_listener.cpp:103
fawkes::TransformInterface
TransformInterface Fawkes BlackBoard Interface.
Definition: TransformInterface.h:34
fawkes::BlackBoard::open_multiple_for_reading
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.
fawkes::tf::InvalidArgumentException
Passed argument was invalid.
Definition: exceptions.h:55
fawkes::BlackBoardInterfaceListener::bbil_add_data_interface
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
Definition: interface_listener.cpp:232
fawkes::tf::TransformListener::bb_interface_created
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
Definition: transform_listener.cpp:118
fawkes::tf::TransformListener::bb_interface_reader_removed
virtual void bb_interface_reader_removed(Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
Definition: transform_listener.cpp:151
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36