$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 #pragma once 00028 #ifndef PointCloudPlugin_H_included 00029 #define PointCloudPlugin_H_included 00030 00031 #include <srs_env_model/but_server/server_tools.h> 00032 00033 #include <message_filters/subscriber.h> 00034 #include <sensor_msgs/PointCloud2.h> 00035 #include <tf/message_filter.h> 00036 00037 #include <message_filters/sync_policies/exact_time.h> 00038 #include <message_filters/sync_policies/approximate_time.h> 00039 #include <message_filters/pass_through.h> 00040 00041 // PCL includes 00042 #include <pcl/segmentation/extract_polygonal_prism_data.h> 00043 00044 00045 namespace srs_env_model 00046 { 00047 00048 class CPointCloudPlugin : public CServerPluginBase, public COctomapCrawlerBase<tButServerOcTree::NodeType>, public CDataHolderBase< tPointCloud > 00049 { 00050 public: 00052 typedef sensor_msgs::PointCloud2 tIncommingPointCloud; 00053 00054 public: 00056 CPointCloudPlugin(const std::string & name, bool subscribe ); 00057 00059 virtual ~CPointCloudPlugin(); 00060 00062 void enable( bool enabled ) { m_publishPointCloud = enabled; } 00063 00065 virtual void init(ros::NodeHandle & node_handle); 00066 00068 virtual void init(ros::NodeHandle & node_handle, bool subscribe){ m_bSubscribe = subscribe; init(node_handle); } 00069 00071 virtual void init(ros::NodeHandle & node_handle, const std::string & topic){ m_pcSubscriberName = topic; init(node_handle); } 00072 00074 virtual void pause( bool bPause, ros::NodeHandle & node_handle ); 00075 00077 virtual bool wantsMap(); 00078 00080 void setFrameSkip(unsigned long skip){ m_use_every_nth = skip; } 00081 00083 void setUseInputColor(bool bUseInputColor) {boost::mutex::scoped_lock lock(m_lockData); m_bUseInputColor = bUseInputColor;} 00084 00086 void setDefaultColor(uint8_t r, uint8_t g, uint8_t b ){ boost::mutex::scoped_lock lock(m_lockData); m_r = r; m_g = g; m_b = b; } 00087 00088 public: 00089 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00090 00091 protected: 00093 virtual void newMapDataCB( SMapWithParameters & par ); 00094 00096 virtual bool shouldPublish(); 00097 00099 virtual void publishInternal( const ros::Time & timestamp ); 00100 00102 void handleOccupiedNode(tButServerOcTree::iterator& it, const SMapWithParameters & mp); 00103 00109 void insertCloudCallback(const tIncommingPointCloud::ConstPtr& cloud); 00110 00114 bool isRGBCloud( const tIncommingPointCloud::ConstPtr& cloud ); 00115 00117 virtual bool useFrame() { return ++m_frame_number % m_use_every_nth == 0; } 00118 00119 protected: 00121 bool m_publishPointCloud; 00122 00124 std::string m_pcPublisherName; 00125 00127 std::string m_pcSubscriberName; 00128 00130 message_filters::Subscriber<tIncommingPointCloud> *m_pcSubscriber; 00131 00133 tf::MessageFilter<tIncommingPointCloud> *m_tfPointCloudSub; 00134 00136 ros::Publisher m_pcPublisher; 00137 00139 std::string m_inputPcFrameId; 00140 00142 bool m_bSubscribe; 00143 00145 tf::TransformListener m_tfListener; 00146 00147 // 00148 bool m_latchedTopics; 00149 00151 bool m_bFilterPC; 00152 00154 double m_pointcloudMinZ; 00155 00157 double m_pointcloudMaxZ; 00158 00160 long counter; 00161 00163 bool m_bAsInput; 00164 00166 Eigen::Matrix4f m_pcOutTM; 00167 00169 tPointCloudPtr m_oldCloud; 00170 00172 tPointCloudPtr m_bufferCloud; 00173 00175 unsigned long m_frame_number; 00176 00178 unsigned long m_use_every_nth; 00179 00181 bool m_bUseInputColor; 00182 00184 uint8_t m_r, m_g, m_b; 00185 00186 00187 }; // class CPointCloudPlugin 00188 00189 00190 } // namespace srs_env_model 00191 00192 00193 // PointCloudPubPlugin_H_included 00194 #endif 00195