22 #ifndef __PLUGINS_ROS_TF_THREAD_H_ 23 #define __PLUGINS_ROS_TF_THREAD_H_ 25 #include <core/threading/thread.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/blackboard.h> 31 #include <aspect/tf.h> 32 #include <plugins/ros/aspect/ros.h> 33 #include <blackboard/interface_listener.h> 34 #include <blackboard/interface_observer.h> 35 #include <interfaces/TransformInterface.h> 36 #include <core/threading/mutex.h> 42 #include <ros/common.h> 43 #include <ros/node_handle.h> 44 #include <tf/tfMessage.h> 46 # include <tf2_msgs/TFMessage.h> 75 unsigned int instance_serial)
throw();
77 unsigned int instance_serial)
throw();
80 void tf_message_cb(
const ros::MessageEvent<::tf::tfMessage const> &msg_evt);
82 void tf_message_cb_dynamic(
const ros::MessageEvent<tf2_msgs::TFMessage const> &msg_evt);
83 void tf_message_cb_static(
const ros::MessageEvent<tf2_msgs::TFMessage const> &msg_evt);
84 void tf_message_cb(
const ros::MessageEvent<tf2_msgs::TFMessage const> &msg_evt,
bool static_tf);
88 void publish_static_transforms_to_ros();
89 void publish_transform_to_fawkes(
const geometry_msgs::TransformStamped &ts,
90 bool static_tf =
false);
95 protected:
virtual void run() { Thread::run(); }
99 float __cfg_update_interval;
102 std::list<std::string> __ros_frames;
103 std::list<fawkes::TransformInterface *> __tfifs;
105 ros::Subscriber __sub_tf;
106 ros::Subscriber __sub_static_tf;
107 ros::Publisher __pub_tf;
108 ros::Publisher __pub_static_tf;
111 unsigned int __active_queue;
112 std::queue<::tf::tfMessage::ConstPtr> __tf_msg_queues[2];
114 std::queue<std::pair<bool, tf2_msgs::TFMessage::ConstPtr> > __tf2_msg_queues[2];
118 unsigned int __seq_num;
Thread aspect to access to BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
Thread aspect to get access to a ROS node handle.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
Thread to exchange transforms between Fawkes and ROS.
A class for handling time.
Thread class encapsulation of pthreads.
Base class for all Fawkes BlackBoard interfaces.
Thread aspect to use blocked timing.
virtual void finalize()
Finalize the thread.
Thread aspect to log output.
BlackBoard interface observer.
Thread aspect to access configuration data.
RosTfThread()
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
Mutex mutual exclusion lock.
virtual void init()
Initialize the thread.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
virtual ~RosTfThread()
Destructor.
BlackBoard interface listener.
virtual void run()
Stub to see name in backtrace for easier debugging.