Fawkes API  Fawkes Development Version
laser_pointcloud_thread.h
00001 
00002 /***************************************************************************
00003  *  laser_pointcloud_thread.h - Convert laser data to pointclouds
00004  *
00005  *  Created: Thu Nov 17 10:21:40 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_LASER_POINTCLOUDS_LASER_POINTCLOUD_THREAD_H_
00023 #define __PLUGINS_LASER_POINTCLOUDS_LASER_POINTCLOUD_THREAD_H_
00024 
00025 #include <core/threading/thread.h>
00026 #include <aspect/blocked_timing.h>
00027 #include <aspect/logging.h>
00028 #include <aspect/blackboard.h>
00029 #include <aspect/pointcloud.h>
00030 #include <blackboard/interface_listener.h>
00031 #include <blackboard/interface_observer.h>
00032 #include <core/utils/lock_list.h>
00033 
00034 #include <pcl/point_cloud.h>
00035 #include <pcl/point_types.h>
00036 
00037 namespace fawkes {
00038   class Interface;
00039   class Laser360Interface;
00040   class Laser720Interface;
00041 }
00042 
00043 class LaserPointCloudThread
00044 : public fawkes::Thread,
00045   public fawkes::LoggingAspect,
00046   public fawkes::BlackBoardAspect,
00047   public fawkes::BlockedTimingAspect,
00048   public fawkes::PointCloudAspect,
00049   public fawkes::BlackBoardInterfaceObserver,
00050   public fawkes::BlackBoardInterfaceListener
00051 {
00052  public:
00053   LaserPointCloudThread();
00054   virtual ~LaserPointCloudThread();
00055 
00056   virtual void init();
00057   virtual void loop();
00058   virtual void finalize();
00059 
00060   // for BlackBoardInterfaceObserver
00061   virtual void bb_interface_created(const char *type, const char *id) throw();
00062 
00063   // for BlackBoardInterfaceListener
00064   virtual void bb_interface_writer_removed(fawkes::Interface *interface,
00065                                            unsigned int instance_serial) throw();
00066   virtual void bb_interface_reader_removed(fawkes::Interface *interface,
00067                                            unsigned int instance_serial) throw();
00068 
00069  private:
00070   void conditional_close(fawkes::Interface *interface) throw();
00071   std::string interface_to_pcl_name(const char *interface_id);
00072 
00073  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
00074  protected: virtual void run() { Thread::run(); }
00075 
00076  private:
00077   /// @cond INTERNALS
00078   typedef struct {
00079     std::string id;
00080     bool is_360;
00081     union {
00082       fawkes::Laser360Interface *as360;
00083       fawkes::Laser720Interface *as720;
00084     } interface_typed;
00085     fawkes::Interface *interface;
00086 
00087     fawkes::RefPtr<pcl::PointCloud<pcl::PointXYZ> > cloud;
00088   } InterfaceCloudMapping;
00089   /// @endcond
00090 
00091   fawkes::LockList<InterfaceCloudMapping> __mappings;
00092 
00093   float sin_angles360[360];
00094   float cos_angles360[360];
00095   float sin_angles720[720];
00096   float cos_angles720[720];
00097 };
00098 
00099 #endif