Fawkes API  Fawkes Development Version
laserscan_thread.h
00001 
00002 /***************************************************************************
00003  *  laserscan_thread.h - Thread to exchange laser scans
00004  *
00005  *  Created: Tue May 29 19:32:39 2012
00006  *  Copyright  2011-2012  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_LASERSCAN_THREAD_H_
00023 #define __PLUGINS_ROS_LASERSCAN_THREAD_H_
00024 
00025 #include <core/threading/thread.h>
00026 #include <aspect/blocked_timing.h>
00027 #include <aspect/configurable.h>
00028 #include <aspect/logging.h>
00029 #include <aspect/blackboard.h>
00030 #include <plugins/ros/aspect/ros.h>
00031 #include <blackboard/interface_listener.h>
00032 #include <blackboard/interface_observer.h>
00033 #include <interfaces/Laser360Interface.h>
00034 #include <interfaces/Laser720Interface.h>
00035 #include <core/threading/mutex.h>
00036 #include <utils/time/time.h>
00037 
00038 #include <list>
00039 #include <queue>
00040 
00041 #include <ros/node_handle.h>
00042 #include <sensor_msgs/LaserScan.h>
00043 
00044 class RosLaserScanThread
00045 : public fawkes::Thread,
00046   public fawkes::LoggingAspect,
00047   public fawkes::ConfigurableAspect,
00048   public fawkes::BlockedTimingAspect,
00049   public fawkes::BlackBoardAspect,
00050   public fawkes::ROSAspect,
00051   public fawkes::BlackBoardInterfaceObserver,
00052   public fawkes::BlackBoardInterfaceListener
00053 {
00054  public:
00055   RosLaserScanThread();
00056   virtual ~RosLaserScanThread();
00057 
00058   virtual void init();
00059   virtual void loop();
00060   virtual void finalize();
00061 
00062   // for BlackBoardInterfaceObserver
00063   virtual void bb_interface_created(const char *type, const char *id) throw();
00064 
00065   // for BlackBoardInterfaceListener
00066   virtual void bb_interface_data_changed(fawkes::Interface *interface) throw();
00067   virtual void bb_interface_writer_removed(fawkes::Interface *interface,
00068                                            unsigned int instance_serial) throw();
00069   virtual void bb_interface_reader_removed(fawkes::Interface *interface,
00070                                            unsigned int instance_serial) throw();
00071 
00072  private:
00073   void laser_scan_message_cb(const sensor_msgs::LaserScan::ConstPtr &msg);
00074   void conditional_close(fawkes::Interface *interface) throw();
00075   std::string topic_name(const char *if_id, const char *suffix);
00076 
00077  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
00078  protected: virtual void run() { Thread::run(); }
00079 
00080  private:
00081   std::list<fawkes::Laser360Interface *> __ls360_ifs;
00082   std::list<fawkes::Laser720Interface *> __ls720_ifs;
00083 
00084   ros::Subscriber __sub_ls;
00085 
00086   /// @cond INTERNALS
00087   typedef struct {
00088     ros::Publisher           pub;
00089     sensor_msgs::LaserScan   msg;
00090   } PublisherInfo;
00091   /// @endcond
00092   std::map<std::string, PublisherInfo> __pubs;
00093 
00094   fawkes::Mutex *__ls_msg_queue_mutex;
00095   unsigned int __active_queue;
00096   std::queue<sensor_msgs::LaserScan::ConstPtr>   __ls_msg_queues[2];
00097 
00098   std::map<std::string, fawkes::Laser360Interface *> __ls360_wifs;
00099 
00100   fawkes::Mutex *__seq_num_mutex;
00101   unsigned int   __seq_num;
00102 
00103 };
00104 
00105 #endif