00001 /****************************************************************************** 00002 * * 00003 * pcdfilter_pa_ros.h * 00004 * ================== * 00005 * * 00006 ******************************************************************************* 00007 * * 00008 * github repository * 00009 * https://github.com/peterweissig/ros_pcdfilter * 00010 * * 00011 * Chair of Automation Technology, Technische Universität Chemnitz * 00012 * https://www.tu-chemnitz.de/etit/proaut * 00013 * * 00014 ******************************************************************************* 00015 * * 00016 * New BSD License * 00017 * * 00018 * Copyright (c) 2015-2017, Peter Weissig, Technische Universität Chemnitz * 00019 * All rights reserved. * 00020 * * 00021 * Redistribution and use in source and binary forms, with or without * 00022 * modification, are permitted provided that the following conditions are met: * 00023 * * Redistributions of source code must retain the above copyright * 00024 * notice, this list of conditions and the following disclaimer. * 00025 * * Redistributions in binary form must reproduce the above copyright * 00026 * notice, this list of conditions and the following disclaimer in the * 00027 * documentation and/or other materials provided with the distribution. * 00028 * * Neither the name of the Technische Universität Chemnitz nor the * 00029 * names of its contributors may be used to endorse or promote products * 00030 * derived from this software without specific prior written permission. * 00031 * * 00032 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * 00033 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * 00034 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00035 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY * 00036 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 00037 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * 00038 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * 00039 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * 00040 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 00041 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00042 * DAMAGE. * 00043 * * 00044 ******************************************************************************/ 00045 00046 #ifndef __PCD_FILTER_PA_ROS_H 00047 #define __PCD_FILTER_PA_ROS_H 00048 00049 // local headers 00050 #include "pcdfilter_pa/pcdfilter_pa.h" 00051 #include "pcdfilter_pa/pcdfilter_pa_ros_parameter.h" 00052 #include "pcdfilter_pa/pcdfilter_pa_ros_throttle.h" 00053 #include "pcdfilter_pa/pcdfilter_pa_ros_filter.h" 00054 00055 // ros headers 00056 #include <ros/ros.h> 00057 00058 #include <sensor_msgs/PointCloud2.h> 00059 #include <sensor_msgs/PointCloud.h> 00060 #include <sensor_msgs/LaserScan.h> 00061 00062 #include <tf/transform_datatypes.h> 00063 00064 // additional libraries 00065 #include <opencv2/highgui/highgui.hpp> 00066 00067 // standard headers 00068 #include <string> 00069 00070 //**************************[cPcdFilterPaRos]********************************** 00071 class cPcdFilterPaRos : public cPcdFilterPa { 00072 public: 00073 00075 cPcdFilterPaRos(); 00076 00078 ~cPcdFilterPaRos(); 00079 00081 sensor_msgs::PointCloud2Ptr convertCloud( 00082 const sensor_msgs::PointCloudConstPtr &msg) const; 00083 00085 sensor_msgs::PointCloud2Ptr convertCloud( 00086 const sensor_msgs::LaserScanConstPtr &msg) const; 00087 00090 bool updateTf( const tf::TransformListener &tf_listener, 00091 const std::string base_frame, 00092 const ros::Time time = ros::Time::now()); 00093 00096 bool updateTf( const tf::TransformListener &tf_listener); 00097 00099 bool filterCloud(const sensor_msgs::PointCloud2ConstPtr &msg, 00100 sensor_msgs::PointCloud2Ptr &result); 00101 00103 bool filterCloud(sensor_msgs::PointCloud2Ptr &result); 00104 00109 const cv::Mat convertCloudToOpencv( 00110 const sensor_msgs::PointCloud2ConstPtr &msg, 00111 const bool force_copy = false) const; 00112 00116 sensor_msgs::PointCloud2Ptr applyMasktoCloud( 00117 const sensor_msgs::PointCloud2ConstPtr cloud, 00118 const std::vector<bool> mask) const; 00119 00120 00122 cPcdFilterPaRosThrottle input_throttle_; 00123 00125 cPcdFilterPaRosFilters filters_; 00126 00128 cPcdFilterPaRosParameter rosparams_; 00129 00132 cv::Mat pcd_buffered_points_; 00135 sensor_msgs::PointCloud2ConstPtr pcd_buffered_msg_; 00136 00137 }; 00138 00139 00140 #endif // __PCD_FILTER_PA_ROS_H