Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * pcl_thread.cpp - Thread to exchange point clouds 00004 * 00005 * Created: Mon Nov 07 02:26:35 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_PCL_THREAD_H_ 00023 #define __PLUGINS_ROS_PCL_THREAD_H_ 00024 00025 #include "pcl_adapter.h" 00026 00027 #include <core/threading/thread.h> 00028 #include <aspect/blocked_timing.h> 00029 #include <aspect/clock.h> 00030 #include <aspect/configurable.h> 00031 #include <aspect/logging.h> 00032 #include <aspect/pointcloud.h> 00033 #include <plugins/ros/aspect/ros.h> 00034 #include <blackboard/interface_listener.h> 00035 #include <blackboard/interface_observer.h> 00036 #include <interfaces/TransformInterface.h> 00037 #include <core/threading/mutex.h> 00038 00039 #include <list> 00040 #include <queue> 00041 00042 #include <ros/node_handle.h> 00043 #include <sensor_msgs/PointCloud2.h> 00044 00045 class RosPointCloudThread 00046 : public fawkes::Thread, 00047 public fawkes::ClockAspect, 00048 public fawkes::LoggingAspect, 00049 public fawkes::ConfigurableAspect, 00050 public fawkes::BlockedTimingAspect, 00051 public fawkes::PointCloudAspect, 00052 public fawkes::ROSAspect 00053 { 00054 public: 00055 RosPointCloudThread(); 00056 virtual ~RosPointCloudThread(); 00057 00058 virtual void init(); 00059 virtual void loop(); 00060 virtual void finalize(); 00061 00062 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00063 protected: virtual void run() { Thread::run(); } 00064 00065 private: 00066 RosPointCloudAdapter *__adapter; 00067 00068 /// @cond INTERNALS 00069 typedef struct { 00070 ros::Publisher pub; 00071 sensor_msgs::PointCloud2 msg; 00072 fawkes::Time last_sent; 00073 } PublisherInfo; 00074 /// @endcond 00075 std::map<std::string, PublisherInfo> __pubs; 00076 00077 }; 00078 00079 #endif