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 
66 using namespace fawkes;
67 using 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 
83 void
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 
144 void
146 {
149  for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); ++it) {
150  blackboard->close(*it);
151  }
152 }
153 
154 void
156 {
157  publish_fixed_transforms();
158 }
159 
160 // add children to correct maps
161 void
162 RobotStatePublisherThread::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));
172  logger->log_debug(name(),
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));
178  logger->log_debug(name(),
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
188 void
189 RobotStatePublisherThread::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 
212 void
213 RobotStatePublisherThread::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  */
230 bool
231 RobotStatePublisherThread::joint_is_in_model(const char *id)
232 {
233  return (segments_.find(id) != segments_.end());
234 }
235 
236 // InterfaceObserver
237 void
238 RobotStatePublisherThread::bb_interface_created(const char *type, const char *id) throw()
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 
270 void
272  unsigned int instance_serial) throw()
273 {
274  conditional_close(interface);
275 }
276 
277 void
279  unsigned int instance_serial) throw()
280 {
281  conditional_close(interface);
282 }
283 
284 void
285 RobotStatePublisherThread::conditional_close(Interface *interface) throw()
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 
309 void
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 }
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
SegmentPair
This class represents the segment between a parent and a child joint.
Definition: robot_state_publisher_thread.h:82
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
RobotStatePublisherThread::bb_interface_reader_removed
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
Definition: robot_state_publisher_thread.cpp:278
fawkes::tf::StampedTransform::stamp
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:95
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
fawkes::BlackBoardInterfaceListener
BlackBoard interface listener.
Definition: interface_listener.h:42
fawkes::tf::StampedTransform::child_frame_id
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:100
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::Interface::id
const char * id() const
Get identifier of interface.
Definition: interface.cpp:652
fawkes::ClockAspect::clock
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
fawkes::BlackBoardInterfaceObserver::bbio_add_observed_create
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
Definition: interface_observer.cpp:119
fawkes::MultiLogger::log_debug
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
Definition: multi.cpp:174
fawkes::MultiLogger::log_warn
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
RobotStatePublisherThread::finalize
virtual void finalize()
Finalize the thread.
Definition: robot_state_publisher_thread.cpp:145
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
RobotStatePublisherThread::init
virtual void init()
Initialize the thread.
Definition: robot_state_publisher_thread.cpp:84
fawkes
Fawkes library namespace.
fawkes::tf::TransformPublisher::send_transform
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
Definition: transform_publisher.cpp:115
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::BlackBoard::register_observer
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
fawkes::tf::StampedTransform::frame_id
std::string frame_id
Parent/reference frame ID.
Definition: types.h:97
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::JointInterface
JointInterface Fawkes BlackBoard Interface.
Definition: JointInterface.h:34
RobotStatePublisherThread::bb_interface_writer_removed
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
Definition: robot_state_publisher_thread.cpp:271
fawkes::Time
A class for handling time.
Definition: time.h:93
RobotStatePublisherThread::bb_interface_data_changed
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
Definition: robot_state_publisher_thread.cpp:310
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
RobotStatePublisherThread::RobotStatePublisherThread
RobotStatePublisherThread()
Constructor.
Definition: robot_state_publisher_thread.cpp:75
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoard::unregister_observer
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
fawkes::TransformAspect::tf_publisher
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
RobotStatePublisherThread::bb_interface_created
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
Definition: robot_state_publisher_thread.cpp:238
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::BlackBoardInterfaceListener::bbil_add_reader_interface
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
Definition: interface_listener.cpp:258
fawkes::BlackBoard::open_multiple_for_reading
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.
fawkes::JointInterface::position
float position() const
Get position value.
Definition: JointInterface.cpp:72
fawkes::BlackBoardInterfaceListener::bbil_add_data_interface
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
Definition: interface_listener.cpp:232
RobotStatePublisherThread::loop
virtual void loop()
Code to execute in the thread.
Definition: robot_state_publisher_thread.cpp:155
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
fawkes::BlackBoard::update_listener
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:197
fawkes::BlackBoardInterfaceListener::bbil_add_writer_interface
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
Definition: interface_listener.cpp:270
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36