Fawkes API  Fawkes Development Version
tf_thread.h
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