Fawkes API Fawkes Development Version
transform_publisher.cpp
1/***************************************************************************
2 * transform_publisher.cpp - Fawkes transform publisher (based on ROS tf)
3 *
4 * Created: Mon Oct 24 17:13:20 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 <core/threading/mutex.h>
54#include <core/threading/mutex_locker.h>
55#include <interfaces/TransformInterface.h>
56#include <tf/transform_publisher.h>
57
58namespace fawkes {
59namespace tf {
60
61/** @class TransformPublisher <tf/transform_publisher.h>
62 * Utility class to send transforms.
63 * The transform publisher opens an instance of TransformInterface on
64 * the blackboard for writing and publishes every transform through
65 * that interface. Assuming that the event-based listener is used
66 * it will catch all updates even though we might send them in quick
67 * succession.
68 * @author Tim Niemueller
69 *
70 * @fn void TransformPublisher::send_transform(const Transform &transform, const fawkes::Time &time, const std::string frame, const std::string child_frame, const bool is_static = false)
71 * Convenience wrapper to send a transform.
72 * This simply calls send_transform() with a StampedTransform created
73 * from the data pased into this method.
74 * @param transform transform to publish
75 * @param time time of the transform to publish
76 * @param frame reference frame ID
77 * @param child_frame child frame ID
78 * @param is_static true if the transform is static, i.e., it does not
79 * change over time, false otherwise
80 */
81
82/** Constructor.
83 * @param bb blackboard to open transform interface on, if 0 the
84 * publisher will be disabled. Trying to send a transform will
85 * result in a DisabledException being thrown.
86 * @param bb_iface_id the blackboard interface ID to be used for the
87 * opened TransformInterface. Note that the name is prefixed with "/tf/".
88 */
90: bb_(bb), mutex_(new Mutex())
91{
92 if (bb_) {
93 std::string bbid = (bb_iface_id[0] == '/') ? bb_iface_id : std::string("/tf/") + bb_iface_id;
94 tfif_ = bb_->open_for_writing<TransformInterface>(bbid.c_str());
95 tfif_->set_auto_timestamping(false);
96 }
97}
98
99/** Destructor.
100 * Closes TransformInterface, hence BlackBoard must still be alive and
101 * valid.
102 */
104{
105 if (bb_)
106 bb_->close(tfif_);
107 delete mutex_;
108}
109
110/** Publish transform.
111 * @param transform transform to publish
112 * @param is_static true to mark transform as static, false otherwise
113 */
114void
116{
117 if (!bb_) {
118 throw DisabledException("TransformPublisher is disabled");
119 }
120
121 MutexLocker lock(mutex_);
122
123 tfif_->set_timestamp(&transform.stamp);
124 tfif_->set_frame(transform.frame_id.c_str());
125 tfif_->set_child_frame(transform.child_frame_id.c_str());
126 tfif_->set_static_transform(is_static);
127 double translation[3], rotation[4];
128 const Vector3 &t = transform.getOrigin();
129 translation[0] = t.x();
130 translation[1] = t.y();
131 translation[2] = t.z();
132 Quaternion r = transform.getRotation();
133 assert_quaternion_valid(r);
134 rotation[0] = r.x();
135 rotation[1] = r.y();
136 rotation[2] = r.z();
137 rotation[3] = r.w();
138 tfif_->set_translation(translation);
139 tfif_->set_rotation(rotation);
140 tfif_->write();
141}
142
143} // end namespace tf
144} // end namespace fawkes
The BlackBoard abstract class.
Definition: blackboard.h:46
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:755
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
void set_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:724
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
TransformInterface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_static_transform(const bool new_static_transform)
Set static_transform value.
void set_child_frame(const char *new_child_frame)
Set child_frame value.
The requested feature is disabled.
Definition: exceptions.h:61
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:95
std::string frame_id
Parent/reference frame ID.
Definition: types.h:97
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:100
TransformPublisher(BlackBoard *bb, const char *bb_iface_id)
Constructor.
virtual ~TransformPublisher()
Destructor.
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
Fawkes library namespace.