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 00028 #pragma once 00029 #ifndef OCTOMAP_FILTER_GROUND_H_ 00030 #define OCTOMAP_FILTER_GROUND_H_ 00031 00032 #include "octomap_filter_base.h" 00033 00034 namespace srs_env_model 00035 { 00039 class COcFilterGround : public COcTreeFilterBase 00040 { 00041 public: 00043 COcFilterGround(const std::string & octree_frame_id, ERunMode mode = FILTER_ALLWAYS); 00044 00046 virtual void writeLastRunInfo() 00047 { 00048 std::cerr << "COcFilterGround: Ground cloud size: " << m_groundPc->points.size()<< std::endl; 00049 std::cerr << "COcFilterGround: Non ground cloud size: " << m_nongroundPc->points.size()<< std::endl; 00050 } 00051 00053 virtual void init(ros::NodeHandle & node_handle); 00054 00056 void setCloud(const tPointCloud * cloud); 00057 00059 tPointCloud * getGroundPc() { return m_groundPc; } 00060 00062 tPointCloud * getNongroundPc() { return m_nongroundPc; } 00063 00064 protected: 00066 virtual void filterInternal(tButServerOcTree & tree); 00067 00068 protected: 00070 long m_numSpecRemoved; 00071 00073 const tPointCloud * m_inputPc; 00074 00076 tPointCloud * m_groundPc; 00077 00079 tPointCloud * m_nongroundPc; 00080 00081 double m_groundFilterDistance; 00082 double m_groundFilterAngle; 00083 double m_groundFilterPlaneDistance; 00084 }; 00085 00086 } // namespace srs_env_model 00087 00088 00089 #endif /* OCTOMAP_FILTER_GROUND_H_ */