Fawkes API
Fawkes Development Version
|
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