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 */
82{
83public:
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
105 public fawkes::ClockAspect,
111{
112public:
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) noexcept;
121
122 // InterfaceListener
123 virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept;
124 virtual void bb_interface_writer_removed(fawkes::Interface *interface,
125 fawkes::Uuid instance_serial) noexcept;
126 virtual void bb_interface_reader_removed(fawkes::Interface *interface,
127 fawkes::Uuid instance_serial) noexcept;
128
129private:
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) noexcept;
136
137private:
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
Thread to publish the robot's transforms.
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.
std::string tip
The name of the child joint.
KDL::Segment segment
The segment of the joint pair.
SegmentPair(const KDL::Segment &p_segment, const std::string &p_root, const std::string &p_tip)
Constructor.
std::string root
The name of the parent joint.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
BlackBoard interface listener.
BlackBoard interface observer.
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
Thread aspect to log output.
Definition: logging.h:33
Thread class encapsulation of pthreads.
Definition: thread.h:46
Thread aspect to access the transform system.
Definition: tf.h:39
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29