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 #ifndef _PLUGINS_ROBOTSTATEPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_
58 #define _PLUGINS_ROBOTSTATEPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_
59 
60 #include <aspect/blackboard.h>
61 #include <aspect/blocked_timing.h>
62 #include <aspect/clock.h>
63 #include <aspect/configurable.h>
64 #include <aspect/logging.h>
65 #include <aspect/tf.h>
66 #include <blackboard/interface_listener.h>
67 #include <blackboard/interface_observer.h>
68 #include <core/threading/thread.h>
69 #include <interfaces/JointInterface.h>
70 
71 #include <kdl/frames.hpp>
72 #include <kdl/kdl.hpp>
73 #include <kdl/segment.hpp>
74 #include <kdl/tree.hpp>
75 #include <list>
76 #include <map>
77 
78 /** @class SegmentPair
79  * This class represents the segment between a parent and a child joint
80  */
81 class SegmentPair
82 {
83 public:
84  /** Constructor.
85  * @param p_segment The Segment of the joint pair
86  * @param p_root The name of the parent joint
87  * @param p_tip The name of the child joint
88  */
89  SegmentPair(const KDL::Segment &p_segment, const std::string &p_root, const std::string &p_tip)
90  : segment(p_segment), root(p_root), tip(p_tip)
91  {
92  }
93 
94  /** The segment of the joint pair */
95  KDL::Segment segment;
96  /** The name of the parent joint */
97  std::string root;
98  /** The name of the child joint */
99  std::string tip;
100 };
101 
103  public fawkes::LoggingAspect,
105  public fawkes::ClockAspect,
111 {
112 public:
114 
115  virtual void init();
116  virtual void loop();
117  virtual void finalize();
118 
119  // InterfaceObserver
120  virtual void bb_interface_created(const char *type, const char *id) throw();
121 
122  // InterfaceListener
123  virtual void bb_interface_data_changed(fawkes::Interface *interface) throw();
124  virtual void bb_interface_writer_removed(fawkes::Interface *interface,
125  unsigned int instance_serial) throw();
126  virtual void bb_interface_reader_removed(fawkes::Interface *interface,
127  unsigned int instance_serial) throw();
128 
129 private:
130  void publish_fixed_transforms();
131 
132  void add_children(const KDL::SegmentMap::const_iterator segment);
133  void transform_kdl_to_tf(const KDL::Frame &k, fawkes::tf::Transform &t);
134  bool joint_is_in_model(const char *id);
135  void conditional_close(fawkes::Interface *interface) throw();
136 
137 private:
138  std::map<std::string, SegmentPair> segments_, segments_fixed_;
139  KDL::Tree tree_;
140  std::string cfg_urdf_path_;
141  float cfg_postdate_to_future_;
142 
143  std::list<fawkes::JointInterface *> ifs_;
144 };
145 
146 #endif
SegmentPair
Definition: robot_state_publisher_thread.h:80
SegmentPair::segment
KDL::Segment segment
The segment of the joint pair.
Definition: robot_state_publisher_thread.h:94
SegmentPair::SegmentPair
SegmentPair(const KDL::Segment &p_segment, const std::string &p_root, const std::string &p_tip)
Constructor.
Definition: robot_state_publisher_thread.h:88
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:277
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:45
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
RobotStatePublisherThread::finalize
virtual void finalize()
Finalize the thread.
Definition: robot_state_publisher_thread.cpp:144
RobotStatePublisherThread
Definition: robot_state_publisher_thread.h:101
RobotStatePublisherThread::init
virtual void init()
Initialize the thread.
Definition: robot_state_publisher_thread.cpp:83
fawkes::BlackBoardAspect
Definition: blackboard.h:36
fawkes::LoggingAspect
Definition: logging.h:36
fawkes::TransformAspect
Definition: tf.h:41
fawkes::BlackBoardInterfaceObserver
Definition: interface_observer.h:40
fawkes::Interface
Definition: interface.h:77
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:270
RobotStatePublisherThread::bb_interface_data_changed
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
Definition: robot_state_publisher_thread.cpp:309
RobotStatePublisherThread::RobotStatePublisherThread
RobotStatePublisherThread()
Constructor.
Definition: robot_state_publisher_thread.cpp:74
fawkes::Thread
Definition: thread.h:44
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:237
fawkes::ConfigurableAspect
Definition: configurable.h:36
SegmentPair::tip
std::string tip
The name of the child joint.
Definition: robot_state_publisher_thread.h:98
SegmentPair::root
std::string root
The name of the parent joint.
Definition: robot_state_publisher_thread.h:96
RobotStatePublisherThread::loop
virtual void loop()
Code to execute in the thread.
Definition: robot_state_publisher_thread.cpp:154
fawkes::ClockAspect
Definition: clock.h:38