Fawkes API Fawkes Development Version
robot_state_publisher_thread.cpp
1/***************************************************************************
2 * robot_state_publisher_thread.cpp - Robot State Publisher Plugin
3 *
4 * Created on Thu Aug 22 11:19:00 2013
5 * Copyright (C) 2013 by Till Hofmann, AllemaniACs RoboCup Team
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.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Library General Public License for more details.
17 *
18 * Read the full text in the LICENSE.GPL file in the doc directory.
19 */
20
21/* This code is based on ROS robot_state_publisher and ROS geometry
22 * with the following copyright and license:
23 * Software License Agreement (BSD License)
24 *
25 * Copyright (c) 2008, Willow Garage, Inc.
26 * All rights reserved.
27 *
28 * Redistribution and use in source and binary forms, with or without
29 * modification, are permitted provided that the following conditions
30 * are met:
31 *
32 * * Redistributions of source code must retain the above copyright
33 * notice, this list of conditions and the following disclaimer.
34 * * Redistributions in binary form must reproduce the above
35 * copyright notice, this list of conditions and the following
36 * disclaimer in the documentation and/or other materials provided
37 * with the distribution.
38 * * Neither the name of the Willow Garage nor the names of its
39 * contributors may be used to endorse or promote products derived
40 * from this software without specific prior written permission.
41 *
42 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
43 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
44 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
45 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
46 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
47 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
48 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
49 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
50 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
51 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
52 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
53 * POSSIBILITY OF SUCH DAMAGE.
54 *********************************************************************/
55
56#include "robot_state_publisher_thread.h"
57
58#include <kdl_parser/kdl_parser.h>
59
60#include <fstream>
61#include <kdl/frames_io.hpp>
62#include <list>
63
64#define CFG_PREFIX "/robot_state_publisher/"
65
66using namespace fawkes;
67using namespace std;
68
69/** @class RobotStatePublisherThread "robot_state_publisher_thread.h"
70 * Thread to publish the robot's transforms
71 * @author Till Hofmann
72 */
73
74/** Constructor. */
76: Thread("RobotStatePublisherThread", Thread::OPMODE_WAITFORWAKEUP),
77 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
78 TransformAspect(TransformAspect::ONLY_PUBLISHER, "robot_state_transforms"),
79 BlackBoardInterfaceListener("RobotStatePublisher")
80{
81}
82
83void
85{
86 cfg_urdf_path_ = config->get_string(CFG_PREFIX "urdf_file");
87 try {
88 cfg_postdate_to_future_ = config->get_float(CFG_PREFIX "postdate_to_future");
89 } catch (const Exception &e) {
90 cfg_postdate_to_future_ = 0.f;
91 }
92
93 string urdf;
94 string line;
95 if (cfg_urdf_path_.substr(0, 1) != "/") {
96 // relative path, add prefix RESDIR/urdf/
97 cfg_urdf_path_.insert(0, RESDIR "/urdf/");
98 }
99 ifstream urdf_file(cfg_urdf_path_);
100 if (!urdf_file.is_open()) {
101 throw Exception("Failed to open URDF File %s", cfg_urdf_path_.c_str());
102 }
103 while (getline(urdf_file, line)) {
104 urdf += line;
105 }
106 urdf_file.close();
107
108 if (!kdl_parser::tree_from_string(urdf, tree_)) {
109 logger->log_error(name(), "failed to parse urdf description to tree");
110 throw Exception("Failed to parse URDF description");
111 }
112 // walk the tree and add segments to segments_
113 add_children(tree_.getRootSegment());
114
115 std::map<std::string, SegmentPair> unknown_segments = segments_;
116
117 // check for open JointInterfaces
118 std::list<fawkes::JointInterface *> ifs = blackboard->open_multiple_for_reading<JointInterface>();
119 for (std::list<JointInterface *>::iterator it = ifs.begin(); it != ifs.end(); ++it) {
120 if (joint_is_in_model((*it)->id())) {
121 logger->log_debug(name(), "Found joint information for %s", (*it)->id());
122 unknown_segments.erase((*it)->id());
123 ifs_.push_back(*it);
127 } else {
128 blackboard->close(*it);
129 }
130 }
131 for (map<string, SegmentPair>::const_iterator it = unknown_segments.begin();
132 it != unknown_segments.end();
133 ++it) {
134 logger->log_warn(name(), "No information for joint %s available", it->first.c_str());
135 }
136 // watch for creation of new JointInterfaces
137 bbio_add_observed_create("JointInterface");
138
139 // register to blackboard
142}
143
144void
146{
149 for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); ++it) {
150 blackboard->close(*it);
151 }
152}
153
154void
156{
157 publish_fixed_transforms();
158}
159
160// add children to correct maps
161void
162RobotStatePublisherThread::add_children(const KDL::SegmentMap::const_iterator segment)
163{
164 const std::string &root = segment->second.segment.getName();
165
166 const std::vector<KDL::SegmentMap::const_iterator> &children = segment->second.children;
167 for (unsigned int i = 0; i < children.size(); ++i) {
168 const KDL::Segment &child = children[i]->second.segment;
169 SegmentPair s(children[i]->second.segment, root, child.getName());
170 if (child.getJoint().getType() == KDL::Joint::None) {
171 segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
173 "Adding fixed segment from %s to %s",
174 root.c_str(),
175 child.getName().c_str());
176 } else {
177 segments_.insert(make_pair(child.getJoint().getName(), s));
179 "Adding moving segment from %s to %s",
180 root.c_str(),
181 child.getName().c_str());
182 }
183 add_children(children[i]);
184 }
185}
186
187// publish fixed transforms
188void
189RobotStatePublisherThread::publish_fixed_transforms()
190{
191 std::vector<tf::StampedTransform> tf_transforms;
192 tf::StampedTransform tf_transform;
193 fawkes::Time now(clock);
194 tf_transform.stamp = now + cfg_postdate_to_future_; // future publish
195
196 // loop over all fixed segments
197 for (map<string, SegmentPair>::const_iterator seg = segments_fixed_.begin();
198 seg != segments_fixed_.end();
199 ++seg) {
200 transform_kdl_to_tf(seg->second.segment.pose(0), tf_transform);
201 tf_transform.frame_id = seg->second.root;
202 tf_transform.child_frame_id = seg->second.tip;
203 tf_transforms.push_back(tf_transform);
204 }
205 for (std::vector<tf::StampedTransform>::const_iterator it = tf_transforms.begin();
206 it != tf_transforms.end();
207 ++it) {
209 }
210}
211
212void
213RobotStatePublisherThread::transform_kdl_to_tf(const KDL::Frame &k, fawkes::tf::Transform &t)
214{
215 t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
216 t.setBasis(tf::Matrix3x3(k.M.data[0],
217 k.M.data[1],
218 k.M.data[2],
219 k.M.data[3],
220 k.M.data[4],
221 k.M.data[5],
222 k.M.data[6],
223 k.M.data[7],
224 k.M.data[8]));
225}
226
227/**
228 * @return true if the joint (represented by the interface) is part of our robot model
229 */
230bool
231RobotStatePublisherThread::joint_is_in_model(const char *id)
232{
233 return (segments_.find(id) != segments_.end());
234}
235
236// InterfaceObserver
237void
238RobotStatePublisherThread::bb_interface_created(const char *type, const char *id) noexcept
239{
240 if (strncmp(type, "JointInterface", INTERFACE_TYPE_SIZE_) != 0)
241 return;
242 if (!joint_is_in_model(id))
243 return;
244 JointInterface *interface;
245 try {
246 interface = blackboard->open_for_reading<JointInterface>(id);
247 } catch (Exception &e) {
248 logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
249 return;
250 }
251 logger->log_debug(name(), "Found joint information for %s", interface->id());
252 try {
253 ifs_.push_back(interface);
254 bbil_add_data_interface(interface);
255 bbil_add_reader_interface(interface);
256 bbil_add_writer_interface(interface);
257 blackboard->update_listener(this);
258 } catch (Exception &e) {
259 // remove from all watch lists, then close
260 bbil_remove_data_interface(interface);
261 bbil_remove_reader_interface(interface);
262 bbil_remove_writer_interface(interface);
263 blackboard->update_listener(this);
264 blackboard->close(interface);
265 logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
266 return;
267 }
268}
269
270void
272 Uuid instance_serial) noexcept
273{
274 conditional_close(interface);
275}
276
277void
279 Uuid instance_serial) noexcept
280{
281 conditional_close(interface);
282}
283
284void
285RobotStatePublisherThread::conditional_close(Interface *interface) noexcept
286{
287 // Verify it's a JointInterface
288 JointInterface *jiface = dynamic_cast<JointInterface *>(interface);
289 if (!jiface)
290 return;
291
292 std::list<JointInterface *>::iterator it;
293 for (it = ifs_.begin(); it != ifs_.end(); ++it) {
294 if (*interface == **it) {
295 if (!interface->has_writer() && (interface->num_readers() == 1)) {
296 // It's only us
297 bbil_remove_data_interface(*it);
298 bbil_remove_reader_interface(*it);
299 bbil_remove_writer_interface(*it);
300 blackboard->update_listener(this);
301 blackboard->close(*it);
302 ifs_.erase(it);
303 break;
304 }
305 }
306 }
307}
308
309void
311{
312 JointInterface *jiface = dynamic_cast<JointInterface *>(interface);
313 if (!jiface)
314 return;
315 jiface->read();
316 std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jiface->id());
317 if (seg == segments_.end())
318 return;
319 tf::StampedTransform transform;
320 transform.stamp = fawkes::Time(clock);
321 transform.frame_id = seg->second.root;
322 transform.child_frame_id = seg->second.tip;
323 transform_kdl_to_tf(seg->second.segment.pose(jiface->position()), transform);
324 tf_publisher->send_transform(transform);
325}
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
virtual void init()
Initialize the thread.
This class represents the segment between a parent and a child joint.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:197
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
JointInterface Fawkes BlackBoard Interface.
float position() const
Get position value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
Definition: multi.cpp:174
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29
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
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
Fawkes library namespace.