Fawkes API  Fawkes Development Version
transform_listener.cpp
00001 /***************************************************************************
00002  *  transform_listener.cpp - Fawkes transform listener (based on ROS tf)
00003  *
00004  *  Created: Mon Oct 24 18:47:00 2011
00005  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version. A runtime exception applies to
00012  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00020  */
00021 
00022 /* This code is based on ROS tf with the following copyright and license:
00023  *
00024  * Copyright (c) 2008, Willow Garage, Inc.
00025  * All rights reserved.
00026  * 
00027  * Redistribution and use in source and binary forms, with or without
00028  * modification, are permitted provided that the following conditions are met:
00029  * 
00030  *     * Redistributions of source code must retain the above copyright
00031  *       notice, this list of conditions and the following disclaimer.
00032  *     * Redistributions in binary form must reproduce the above copyright
00033  *       notice, this list of conditions and the following disclaimer in the
00034  *       documentation and/or other materials provided with the distribution.
00035  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00036  *       contributors may be used to endorse or promote products derived from
00037  *       this software without specific prior written permission.
00038  * 
00039  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00040  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00041  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00042  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00043  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00044  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00045  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00046  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00047  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00048  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00049  * POSSIBILITY OF SUCH DAMAGE.
00050  */
00051 
00052 #include <tf/transform_listener.h>
00053 #include <tf/utils.h>
00054 
00055 #include <blackboard/blackboard.h>
00056 #include <interfaces/TransformInterface.h>
00057 
00058 #include <cstring>
00059 
00060 namespace fawkes {
00061   namespace tf {
00062 #if 0 /* just to make Emacs auto-indent happy */
00063   }
00064 }
00065 #endif
00066 
00067 /** @class TransformListener <tf/transform_listener.h>
00068  * Receive transforms and answer queries.
00069  * This class connects to the blackboard and listens to all interfaces
00070  * publishing transforms. It opens all interfaces of type
00071  * TransformInterface with a TF prefix. The data is internally
00072  * cached. Queries are then resolved based on the received
00073  * information.
00074  * @author Tim Niemueller
00075  */
00076 
00077 /** Constructor.
00078  * @param bb blackboard to listen to
00079  */
00080 TransformListener::TransformListener(BlackBoard *bb)
00081   : BlackBoardInterfaceListener("TransformListener"),
00082     __bb(bb)
00083 {
00084   if (__bb) {
00085     __tfifs = __bb->open_multiple_for_reading<TransformInterface>("TF *");
00086 
00087     std::list<TransformInterface *>::iterator i;
00088     for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
00089       bbil_add_data_interface(*i);
00090     }
00091     __bb->register_listener(this);
00092 
00093     bbio_add_observed_create("TransformInterface", "TF *");
00094     __bb->register_observer(this);
00095     set_enabled(true);
00096   } else {
00097     set_enabled(false);
00098   }
00099 }
00100 
00101 
00102 /** Destructor. */
00103 TransformListener::~TransformListener()
00104 {
00105   if (__bb) {
00106     __bb->unregister_listener(this);
00107     __bb->unregister_observer(this);
00108 
00109     std::list<TransformInterface *>::iterator i;
00110     for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
00111       __bb->close(*i);
00112     }
00113     __tfifs.clear();
00114   }
00115 }
00116 
00117 
00118 /** Resolve transform name.
00119  * @param frame_name frame name
00120  * @return resolved frame name
00121  */
00122 std::string
00123 TransformListener::resolve(const std::string& frame_name)
00124 {
00125   return fawkes::tf::resolve("", frame_name);
00126 }
00127 
00128 void
00129 TransformListener::bb_interface_created(const char *type, const char *id) throw()
00130 {
00131   if (strncmp(type, "TransformInterface", __INTERFACE_TYPE_SIZE) != 0)  return;
00132 
00133   TransformInterface *tfif;
00134   try {
00135     tfif = __bb->open_for_reading<TransformInterface>(id);
00136   } catch (Exception &e) {
00137     // ignored
00138     return;
00139   }
00140 
00141   try {
00142     bbil_add_data_interface(tfif);
00143     __bb->update_listener(this);
00144     __tfifs.push_back(tfif);
00145   } catch (Exception &e) {
00146     __bb->close(tfif);
00147     return;
00148   }
00149 }
00150 
00151 void
00152 TransformListener::bb_interface_writer_removed(Interface *interface,
00153                                                unsigned int instance_serial)
00154   throw()
00155 {
00156   conditional_close(interface);
00157 }
00158 
00159 
00160 void
00161 TransformListener::bb_interface_reader_removed(Interface *interface,
00162                                                unsigned int instance_serial)
00163   throw()
00164 {
00165   conditional_close(interface);
00166 }
00167 
00168 
00169 void
00170 TransformListener::conditional_close(Interface *interface) throw()
00171 {
00172   // Verify it's a TransformInterface
00173   TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
00174   if (! tfif) return;
00175   
00176   std::list<TransformInterface *>::iterator i;
00177   for (i = __tfifs.begin(); i != __tfifs.end(); ++i) {
00178     if (*interface == **i) {
00179       if (! interface->has_writer() && (interface->num_readers() == 1)) {
00180         // It's only us
00181         bbil_remove_data_interface(*i);
00182         __bb->update_listener(this);
00183         __bb->close(*i);
00184         __tfifs.erase(i);
00185         break;
00186       }
00187     }
00188   }
00189 }
00190 
00191 
00192 void
00193 TransformListener::bb_interface_data_changed(Interface *interface) throw()
00194 {
00195   TransformInterface *tfif = dynamic_cast<TransformInterface *>(interface);
00196   if (! tfif) return;
00197 
00198   tfif->read();
00199 
00200   double *translation = tfif->translation();
00201   double *rotation = tfif->rotation();
00202   const Time *time = tfif->timestamp();
00203   const std::string frame_id = tfif->frame();
00204   const std::string child_frame_id = tfif->child_frame();
00205 
00206   Vector3 t(translation[0], translation[1], translation[2]);
00207   Quaternion r(rotation[0], rotation[1], rotation[2], rotation[3]);
00208   Transform tr(r, t);
00209 
00210   StampedTransform str(tr, *time, frame_id, child_frame_id);
00211 
00212   set_transform(str);
00213 }
00214 
00215 } // end namespace tf
00216 } // end namespace fawkes