Fawkes API  Fawkes Development Version
robot_state_publisher_thread.h
1 /***************************************************************************
2  * robot_state_publisher_thread.h - 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 #ifndef __PLUGINS_ROBOTSTATEPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_
59 #define __PLUGINS_ROBOTSTATEPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_
60 
61 #include <core/threading/thread.h>
62 #include <aspect/logging.h>
63 #include <aspect/blocked_timing.h>
64 #include <aspect/clock.h>
65 #include <aspect/tf.h>
66 #include <aspect/configurable.h>
67 #include <aspect/blackboard.h>
68 #include <blackboard/interface_listener.h>
69 #include <blackboard/interface_observer.h>
70 
71 #include <interfaces/JointInterface.h>
72 
73 #include <kdl/kdl.hpp>
74 #include <kdl/frames.hpp>
75 #include <kdl/segment.hpp>
76 #include <kdl/tree.hpp>
77 
78 #include <map>
79 #include <list>
80 
81 /** @class SegmentPair
82  * This class represents the segment between a parent and a child joint
83  */
85 {
86 public:
87  /** Constructor.
88  * @param p_segment The Segment of the joint pair
89  * @param p_root The name of the parent joint
90  * @param p_tip The name of the child joint
91  */
92  SegmentPair(const KDL::Segment& p_segment, const std::string& p_root, const std::string& p_tip):
93  segment(p_segment), root(p_root), tip(p_tip){}
94 
95  /** The segment of the joint pair */
96  KDL::Segment segment;
97  /** The name of the parent joint */
98  std::string root;
99  /** The name of the child joint */
100  std::string tip;
101 };
102 
104 : public fawkes::Thread,
105  public fawkes::LoggingAspect,
107  public fawkes::ClockAspect,
113 {
114 public:
116 
117  virtual void init();
118  virtual void loop();
119  virtual void finalize();
120 
121  // InterfaceObserver
122  virtual void bb_interface_created(const char *type, const char *id) throw();
123 
124  // InterfaceListener
125  virtual void bb_interface_data_changed(fawkes::Interface *interface) throw();
126  virtual void bb_interface_writer_removed(fawkes::Interface *interface,
127  unsigned int instance_serial) throw();
128  virtual void bb_interface_reader_removed(fawkes::Interface *interface,
129  unsigned int instance_serial) throw();
130 
131 private:
132  void publish_fixed_transforms();
133 
134  void add_children(const KDL::SegmentMap::const_iterator segment);
135  void transform_kdl_to_tf(const KDL::Frame &k, fawkes::tf::Transform &t);
136  bool joint_is_in_model(const char *id);
137  void conditional_close(fawkes::Interface *interface) throw();
138 
139 private:
140 
141  std::map<std::string, SegmentPair> segments_, segments_fixed_;
142  KDL::Tree tree_;
143  std::string cfg_urdf_path_;
144  float cfg_postdate_to_future_;
145 
146  std::list<fawkes::JointInterface *> ifs_;
147 };
148 
149 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
SegmentPair(const KDL::Segment &p_segment, const std::string &p_root, const std::string &p_tip)
Constructor.
KDL::Segment segment
The segment of the joint pair.
This class represents the segment between a parent and a child joint.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Thread aspect to access the transform system.
Definition: tf.h:42
Thread aspect to use blocked timing.
Thread aspect to log output.
Definition: logging.h:35
Thread to publish the robot&#39;s transforms.
BlackBoard interface observer.
Thread aspect to access configuration data.
Definition: configurable.h:35
std::string tip
The name of the child joint.
std::string root
The name of the parent joint.
BlackBoard interface listener.