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 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
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 file in the doc directory.
20  */
21 
22 /* This code is based on ROS robot_state_publisher and ROS geometry
23  * with the following copyright and license:
24  * Software License Agreement (BSD License)
25  *
26  * Copyright (c) 2008, Willow Garage, Inc.
27  * All rights reserved.
28  *
29  * Redistribution and use in source and binary forms, with or without
30  * modification, are permitted provided that the following conditions
31  * are met:
32  *
33  * * Redistributions of source code must retain the above copyright
34  * notice, this list of conditions and the following disclaimer.
35  * * Redistributions in binary form must reproduce the above
36  * copyright notice, this list of conditions and the following
37  * disclaimer in the documentation and/or other materials provided
38  * with the distribution.
39  * * Neither the name of the Willow Garage nor the names of its
40  * contributors may be used to endorse or promote products derived
41  * from this software without specific prior written permission.
42  *
43  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
44  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
45  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
46  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
47  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
48  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
49  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
50  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
51  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
52  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
53  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
54  * POSSIBILITY OF SUCH DAMAGE.
55  *********************************************************************/
56 
57 
58 #include "robot_state_publisher_thread.h"
59 #include <kdl/frames_io.hpp>
60 #include <kdl_parser/kdl_parser.h>
61 
62 #include <fstream>
63 #include <list>
64 
65 #define CFG_PREFIX "/robot_state_publisher/"
66 
67 using namespace fawkes;
68 using namespace std;
69 
70 /** @class RobotStatePublisherThread "robot_state_publisher_thread.h"
71  * Thread to publish the robot's transforms
72  * @author Till Hofmann
73  */
74 
75 /** Constructor. */
77 : Thread("RobotStatePublisherThread", Thread::OPMODE_WAITFORWAKEUP),
78  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
79  TransformAspect(TransformAspect::ONLY_PUBLISHER, "robot_state_transforms"),
80  BlackBoardInterfaceListener("RobotStatePublisher")
81 {
82 }
83 
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;
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  }
128  else {
129  blackboard->close(*it);
130  }
131  }
132  for (map<string, SegmentPair>::const_iterator it = unknown_segments.begin();
133  it != unknown_segments.end(); 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 
145 {
148  for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); it++) {
149  blackboard->close(*it);
150  }
151 }
152 
154 {
155  publish_fixed_transforms();
156 }
157 
158 
159 // add children to correct maps
160 void RobotStatePublisherThread::add_children(const KDL::SegmentMap::const_iterator segment)
161 {
162  const std::string& root = segment->second.segment.getName();
163 
164  const std::vector<KDL::SegmentMap::const_iterator>& children = segment->second.children;
165  for (unsigned int i=0; i<children.size(); i++){
166  const KDL::Segment& child = children[i]->second.segment;
167  SegmentPair s(children[i]->second.segment, root, child.getName());
168  if (child.getJoint().getType() == KDL::Joint::None){
169  segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
170  logger->log_debug(name(), "Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
171  }
172  else{
173  segments_.insert(make_pair(child.getJoint().getName(), s));
174  logger->log_debug(name(), "Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
175  }
176  add_children(children[i]);
177  }
178 }
179 
180 // publish fixed transforms
181 void RobotStatePublisherThread::publish_fixed_transforms()
182 {
183  std::vector<tf::StampedTransform> tf_transforms;
184  tf::StampedTransform tf_transform;
185  fawkes::Time now(clock);
186  tf_transform.stamp = now + cfg_postdate_to_future_; // future publish
187 
188  // loop over all fixed segments
189  for (map<string, SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++){
190  transform_kdl_to_tf(seg->second.segment.pose(0), tf_transform);
191  tf_transform.frame_id = seg->second.root;
192  tf_transform.child_frame_id = seg->second.tip;
193  tf_transforms.push_back(tf_transform);
194  }
195  for (std::vector<tf::StampedTransform>::const_iterator it = tf_transforms.begin();
196  it != tf_transforms.end(); it++) {
198  }
199 }
200 
201 void RobotStatePublisherThread::transform_kdl_to_tf(const KDL::Frame &k, fawkes::tf::Transform &t)
202  {
203  t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
204  t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
205  k.M.data[3], k.M.data[4], k.M.data[5],
206  k.M.data[6], k.M.data[7], k.M.data[8]));
207  }
208 
209 
210 /**
211  * @return true if the joint (represented by the interface) is part of our robot model
212  */
213 bool RobotStatePublisherThread::joint_is_in_model(const char *id) {
214  return (segments_.find(id) != segments_.end());
215 }
216 
217 // InterfaceObserver
218 void
219 RobotStatePublisherThread::bb_interface_created(const char *type, const char *id) throw()
220 {
221  if (strncmp(type, "JointInterface", __INTERFACE_TYPE_SIZE) != 0) return;
222  if (!joint_is_in_model(id)) return;
223  JointInterface *interface;
224  try {
225  interface = blackboard->open_for_reading<JointInterface>(id);
226  } catch (Exception &e) {
227  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
228  return;
229  }
230  logger->log_debug(name(), "Found joint information for %s", interface->id());
231  try {
232  ifs_.push_back(interface);
233  bbil_add_data_interface(interface);
234  bbil_add_reader_interface(interface);
235  bbil_add_writer_interface(interface);
237  } catch (Exception &e) {
238  // remove from all watch lists, then close
239  bbil_remove_data_interface(interface);
240  bbil_remove_reader_interface(interface);
241  bbil_remove_writer_interface(interface);
243  blackboard->close(interface);
244  logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
245  return;
246  }
247 }
248 
249 void
251  unsigned int instance_serial)
252  throw()
253 {
254  conditional_close(interface);
255 }
256 
257 
258 void
260  unsigned int instance_serial)
261  throw()
262 {
263  conditional_close(interface);
264 }
265 
266 void
267 RobotStatePublisherThread::conditional_close(Interface *interface) throw()
268 {
269  // Verify it's a JointInterface
270  JointInterface *jiface = dynamic_cast<JointInterface *>(interface);
271  if (! jiface) return;
272 
273  std::list<JointInterface *>::iterator it;
274  for (it = ifs_.begin(); it != ifs_.end(); ++it) {
275  if (*interface == **it) {
276  if (! interface->has_writer() && (interface->num_readers() == 1)) {
277  // It's only us
282  blackboard->close(*it);
283  ifs_.erase(it);
284  break;
285  }
286  }
287  }
288 }
289 
290 void
292 {
293  JointInterface *jiface = dynamic_cast<JointInterface *>(interface);
294  if (!jiface) return;
295  jiface->read();
296  std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jiface->id());
297  if (seg == segments_.end()) return;
298  tf::StampedTransform transform;
299  transform.stamp = fawkes::Time(clock);
300  transform.frame_id = seg->second.root;
301  transform.child_frame_id = seg->second.tip;
302  transform_kdl_to_tf(seg->second.segment.pose(jiface->position()), transform);
303  tf_publisher->send_transform(transform);
304 
305 }
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:230
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:71
float position() const
Get position value.
Fawkes library namespace.
This class represents the segment between a parent and a child joint.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:100
STL namespace.
A class for handling time.
Definition: time.h:91
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:218
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:203
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
void bbil_remove_writer_interface(Interface *interface)
Remove an interface to the writer addition/removal watch list.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
Thread aspect to access the transform system.
Definition: tf.h:42
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
Definition: model.cpp:69
virtual void loop()
Code to execute in the thread.
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:244
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
const char * name() const
Get name of thread.
Definition: thread.h:95
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.
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:105
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.
void bbil_remove_data_interface(Interface *interface)
Remove an interface to the data modification watch list.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
virtual void init()
Initialize the thread.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
JointInterface Fawkes BlackBoard Interface.
std::string frame_id
Parent/reference frame ID.
Definition: types.h:102
unsigned int num_readers() const
Get the number of readers.
Definition: interface.cpp:863
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void bbil_remove_reader_interface(Interface *interface)
Remove an interface to the reader addition/removal watch list.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual void close(Interface *interface)=0
Close interface.