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