Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * tf_thread.h - Thread to exchange transforms 00004 * 00005 * Created: Wed Oct 26 00:50:12 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #ifndef __PLUGINS_ROS_TF_THREAD_H_ 00023 #define __PLUGINS_ROS_TF_THREAD_H_ 00024 00025 #include <core/threading/thread.h> 00026 #include <aspect/blocked_timing.h> 00027 #include <aspect/clock.h> 00028 #include <aspect/configurable.h> 00029 #include <aspect/logging.h> 00030 #include <aspect/blackboard.h> 00031 #include <aspect/tf.h> 00032 #include <plugins/ros/aspect/ros.h> 00033 #include <blackboard/interface_listener.h> 00034 #include <blackboard/interface_observer.h> 00035 #include <interfaces/TransformInterface.h> 00036 #include <core/threading/mutex.h> 00037 00038 #include <list> 00039 #include <queue> 00040 00041 // from ROS 00042 #include <ros/node_handle.h> 00043 #include <tf/tfMessage.h> 00044 00045 class RosTfThread 00046 : public fawkes::Thread, 00047 public fawkes::ClockAspect, 00048 public fawkes::LoggingAspect, 00049 public fawkes::ConfigurableAspect, 00050 public fawkes::BlockedTimingAspect, 00051 public fawkes::BlackBoardAspect, 00052 public fawkes::TransformAspect, 00053 public fawkes::ROSAspect, 00054 public fawkes::BlackBoardInterfaceObserver, 00055 public fawkes::BlackBoardInterfaceListener 00056 { 00057 public: 00058 RosTfThread(); 00059 virtual ~RosTfThread(); 00060 00061 virtual void init(); 00062 virtual void loop(); 00063 virtual void finalize(); 00064 00065 // for BlackBoardInterfaceObserver 00066 virtual void bb_interface_created(const char *type, const char *id) throw(); 00067 00068 // for BlackBoardInterfaceListener 00069 virtual void bb_interface_data_changed(fawkes::Interface *interface) throw(); 00070 virtual void bb_interface_writer_removed(fawkes::Interface *interface, 00071 unsigned int instance_serial) throw(); 00072 virtual void bb_interface_reader_removed(fawkes::Interface *interface, 00073 unsigned int instance_serial) throw(); 00074 00075 private: 00076 void tf_message_cb(const ::tf::tfMessage::ConstPtr &msg); 00077 void conditional_close(fawkes::Interface *interface) throw(); 00078 00079 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00080 protected: virtual void run() { Thread::run(); } 00081 00082 private: 00083 std::list<fawkes::TransformInterface *> __tfifs; 00084 00085 ros::Subscriber __sub_tf; 00086 ros::Publisher __pub_tf; 00087 00088 fawkes::Mutex *__tf_msg_queue_mutex; 00089 unsigned int __active_queue; 00090 std::queue<tf::tfMessage::ConstPtr> __tf_msg_queues[2]; 00091 00092 fawkes::Mutex *__seq_num_mutex; 00093 unsigned int __seq_num; 00094 00095 }; 00096 00097 #endif