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