Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * transform_publisher.cpp - Fawkes transform publisher (based on ROS tf) 00003 * 00004 * Created: Mon Oct 24 17:13:20 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_publisher.h> 00053 00054 #include <blackboard/blackboard.h> 00055 #include <interfaces/TransformInterface.h> 00056 00057 #include <core/threading/mutex.h> 00058 #include <core/threading/mutex_locker.h> 00059 00060 namespace fawkes { 00061 namespace tf { 00062 #if 0 /* just to make Emacs auto-indent happy */ 00063 } 00064 } 00065 #endif 00066 00067 /** @class TransformPublisher <tf/transform_publisher.h> 00068 * Utility class to send transforms. 00069 * The transform publisher opens an instance of TransformInterface on 00070 * the blackboard for writing and publishes every transform through 00071 * that interface. Assuming that the event-based listener is used 00072 * it will catch all updates even though we might send them in quick 00073 * succession. 00074 * @author Tim Niemueller 00075 * 00076 * @fn void TransformPublisher::send_transform(const Transform &transform, const fawkes::Time &time, const std::string frame, const std::string child_frame) 00077 * Convenience wrapper to send a transform. 00078 * This simply calls send_transform() with a StampedTransform created 00079 * from the data pased into this method. 00080 * @param transform transform to publish 00081 * @param time time of the transform to publish 00082 * @param frame reference frame ID 00083 * @param child_frame child frame ID 00084 */ 00085 00086 /** Constructor. 00087 * @param bb blackboard to open transform interface on, if 0 the 00088 * publisher will be disabled. Trying to send a transform will 00089 * result in a DisabledException being thrown. 00090 * @param bb_iface_id the blackboard interface ID to be used for the 00091 * opened TransformInterface. Note that the name is prefixed with "TF ". 00092 */ 00093 TransformPublisher::TransformPublisher(BlackBoard *bb, 00094 const char *bb_iface_id) 00095 : __bb(bb), __mutex(new Mutex()) 00096 { 00097 if (__bb) { 00098 std::string bbid = std::string("TF ") + bb_iface_id; 00099 __tfif = __bb->open_for_writing<TransformInterface>(bbid.c_str()); 00100 __tfif->set_auto_timestamping(false); 00101 } 00102 } 00103 00104 00105 /** Destructor. 00106 * Closes TransformInterface, hence BlackBoard must still be alive and 00107 * valid. 00108 */ 00109 TransformPublisher::~TransformPublisher() 00110 { 00111 if (__bb) __bb->close(__tfif); 00112 delete __mutex; 00113 } 00114 00115 00116 /** Publish transform. 00117 * @param transform transform to publish 00118 */ 00119 void 00120 TransformPublisher::send_transform(const StampedTransform &transform) 00121 { 00122 if (! __bb) { 00123 throw DisabledException("TransformPublisher is disabled"); 00124 } 00125 00126 MutexLocker lock(__mutex); 00127 00128 __tfif->set_timestamp(&transform.stamp); 00129 __tfif->set_frame(transform.frame_id.c_str()); 00130 __tfif->set_child_frame(transform.child_frame_id.c_str()); 00131 double translation[3], rotation[4]; 00132 const Vector3 &t = transform.getOrigin(); 00133 translation[0] = t.x(); translation[1] = t.y(); translation[2] = t.z(); 00134 Quaternion r = transform.getRotation(); 00135 assert_quaternion_valid(r); 00136 rotation[0] = r.x(); rotation[1] = r.y(); 00137 rotation[2] = r.z(); rotation[3] = r.w(); 00138 __tfif->set_translation(translation); 00139 __tfif->set_rotation(rotation); 00140 __tfif->write(); 00141 } 00142 00143 00144 } // end namespace tf 00145 } // end namespace fawkes