00001 /****************************************************************************** 00002 * * 00003 * pcdfilter_pa_pa_node.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_NODE_H 00047 #define __PCD_FILTER_PA_NODE_H 00048 00049 // local headers 00050 #include "pcdfilter_pa/pcdfilter_pa_ros.h" 00051 #include "pcdfilter_pa/pcdfilter_pa_node_parameter.h" 00052 #include "pcdfilter_pa/PcdFilterPaFilter.h" 00053 #include "pcdfilter_pa/PcdFilterPaCloud.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 #include <std_srvs/Empty.h> 00062 00063 #include <tf/transform_listener.h> 00064 00065 // standard headers 00066 #include <string> 00067 #include <vector> 00068 00069 //**************************[main]********************************************* 00070 int main(int argc, char **argv); 00071 00072 //**************************[cPcdFilterPaNode]********************************* 00073 class cPcdFilterPaNode : public cPcdFilterPaRos { 00074 public: 00076 cPcdFilterPaNode(); 00077 00079 ~cPcdFilterPaNode(); 00080 00081 protected: 00083 cPcdFilterPaNodeParameter nodeparams_; 00084 00086 ros::NodeHandle nh_; 00087 00089 tf::TransformListener tf_listener_; 00090 00092 ros::Subscriber sub_pcd_; 00094 ros::Subscriber sub_pcd_old_; 00096 ros::Subscriber sub_laser_; 00097 00099 ros::Publisher pub_pcd_; 00100 00102 ros::ServiceServer ser_filter_; 00103 00105 ros::ServiceServer ser_add_filters_; 00107 ros::ServiceServer ser_change_filters_; 00108 00110 ros::ServiceServer ser_enable_; 00112 ros::ServiceServer ser_disable_; 00113 00115 void setCloudCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg); 00117 void setCloudOldCallbackSub(const sensor_msgs::PointCloudConstPtr &msg); 00119 void setCloudLaserCallbackSub(const sensor_msgs::LaserScanConstPtr &msg); 00120 00122 bool filterCallbackSrv( 00123 pcdfilter_pa::PcdFilterPaCloud::Request &req, 00124 pcdfilter_pa::PcdFilterPaCloud::Response &res); 00125 00127 bool addFiltersCallbackSrv( 00128 pcdfilter_pa::PcdFilterPaFilter::Request &req, 00129 pcdfilter_pa::PcdFilterPaFilter::Response &res); 00131 bool changeFiltersCallbackSrv( 00132 pcdfilter_pa::PcdFilterPaFilter::Request &req, 00133 pcdfilter_pa::PcdFilterPaFilter::Response &res); 00134 00136 bool enableCallbackSrv(std_srvs::Empty::Request &req, 00137 std_srvs::Empty::Response &res); 00139 bool disableCallbackSrv(std_srvs::Empty::Request &req, 00140 std_srvs::Empty::Response &res); 00141 00143 void addFilters(const std::vector<std::string> &new_filters); 00144 00145 private: 00147 void enable(void); 00149 void disable(void); 00150 }; 00151 00152 #endif // __PCD_FILTER_PA_NODE_H