Fawkes API Fawkes Development Version
tf_thread.h
1
2/***************************************************************************
3 * tf_thread.h - Thread to exchange transforms
4 *
5 * Created: Wed Oct 26 00:50:12 2011
6 * Copyright 2011 Tim Niemueller [www.niemueller.de]
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#ifndef _PLUGINS_ROS_TF_THREAD_H_
23#define _PLUGINS_ROS_TF_THREAD_H_
24
25#include <aspect/blackboard.h>
26#include <aspect/blocked_timing.h>
27#include <aspect/clock.h>
28#include <aspect/configurable.h>
29#include <aspect/logging.h>
30#include <aspect/tf.h>
31#include <blackboard/interface_listener.h>
32#include <blackboard/interface_observer.h>
33#include <core/threading/mutex.h>
34#include <core/threading/thread.h>
35#include <interfaces/TransformInterface.h>
36#include <plugins/ros/aspect/ros.h>
37
38#include <list>
39#include <queue>
40
41// from ROS
42#include <ros/common.h>
43#include <ros/node_handle.h>
44#include <tf/tfMessage.h>
45#ifdef HAVE_TF2_MSGS
46# include <tf2_msgs/TFMessage.h>
47#endif
48
56 public fawkes::ROSAspect,
59{
60public:
62 virtual ~RosTfThread();
63
64 virtual void init();
65 virtual void loop();
66 virtual void finalize();
67
68 // for BlackBoardInterfaceObserver
69 virtual void bb_interface_created(const char *type, const char *id) noexcept;
70
71 // for BlackBoardInterfaceListener
72 virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept;
73 virtual void bb_interface_writer_removed(fawkes::Interface *interface,
74 fawkes::Uuid instance_serial) noexcept;
75 virtual void bb_interface_reader_removed(fawkes::Interface *interface,
76 fawkes::Uuid instance_serial) noexcept;
77
78private:
79 void tf_message_cb(const ros::MessageEvent<::tf::tfMessage const> &msg_evt);
80#ifdef HAVE_TF2_MSGS
81 void tf_message_cb_dynamic(const ros::MessageEvent<tf2_msgs::TFMessage const> &msg_evt);
82 void tf_message_cb_static(const ros::MessageEvent<tf2_msgs::TFMessage const> &msg_evt);
83 void tf_message_cb(const ros::MessageEvent<tf2_msgs::TFMessage const> &msg_evt, bool static_tf);
84#endif
85
86 void conditional_close(fawkes::Interface *interface) noexcept;
87 void publish_static_transforms_to_ros();
88 void publish_transform_to_fawkes(const geometry_msgs::TransformStamped &ts,
89 bool static_tf = false);
90 geometry_msgs::TransformStamped create_transform_stamped(fawkes::TransformInterface *tfif,
91 const fawkes::Time * time = NULL);
92
93 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
94protected:
95 virtual void
97 {
98 Thread::run();
99 }
100
101private:
102 bool cfg_use_tf2_;
103 float cfg_update_interval_;
104
105 std::list<std::string> ros_frames_;
106 std::list<fawkes::TransformInterface *> tfifs_;
107
108 ros::Subscriber sub_tf_;
109 ros::Subscriber sub_static_tf_;
110 ros::Publisher pub_tf_;
111 ros::Publisher pub_static_tf_;
112
113 fawkes::Mutex * tf_msg_queue_mutex_;
114 unsigned int active_queue_;
115 std::queue<::tf::tfMessage::ConstPtr> tf_msg_queues_[2];
116#ifdef HAVE_TF2_MSGS
117 std::queue<std::pair<bool, tf2_msgs::TFMessage::ConstPtr>> tf2_msg_queues_[2];
118#endif
119
120 fawkes::Mutex *seq_num_mutex_;
121 unsigned int seq_num_;
122
123 fawkes::Time *last_update_;
124};
125
126#endif
Thread to exchange transforms between Fawkes and ROS.
Definition: tf_thread.h:59
virtual void bb_interface_reader_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
Definition: tf_thread.cpp:242
RosTfThread()
Constructor.
Definition: tf_thread.cpp:38
virtual void loop()
Code to execute in the thread.
Definition: tf_thread.cpp:120
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: tf_thread.h:96
virtual void init()
Initialize the thread.
Definition: tf_thread.cpp:58
virtual void bb_interface_writer_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
Definition: tf_thread.cpp:235
virtual void finalize()
Finalize the thread.
Definition: tf_thread.cpp:104
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
Definition: tf_thread.cpp:200
virtual ~RosTfThread()
Destructor.
Definition: tf_thread.cpp:50
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
Definition: tf_thread.cpp:166
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
Mutex mutual exclusion lock.
Definition: mutex.h:33
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
TransformInterface Fawkes BlackBoard Interface.
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29