$search
00001 //================================================================================================= 00002 // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Simulation, Systems Optimization and Robotics 00013 // group, TU Darmstadt nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 00030 #include "ros/ros.h" 00031 #include <sensor_msgs/LaserScan.h> 00032 #include <vector> 00033 00034 00035 class LaserScanFilter 00036 { 00037 public: 00038 LaserScanFilter() 00039 { 00040 ros::NodeHandle nh; 00041 00042 scan_sub_ = nh.subscribe("hokuyo_scan", 1, &LaserScanFilter::scanCallback, this); 00043 scan_filtered_pub_ = nh.advertise<sensor_msgs::LaserScan>("hokuyo_scan_filtered",1,false); 00044 00045 ros::NodeHandle pnh("~"); 00046 XmlRpc::XmlRpcValue my_list; 00047 pnh.getParam("filter_index_list", my_list); 00048 if (my_list.getType() != XmlRpc::XmlRpcValue::TypeArray) ros::shutdown(); 00049 00050 00051 for (int32_t i = 0; i < my_list.size(); ++i) 00052 { 00053 if (my_list[i].getType() != XmlRpc::XmlRpcValue::TypeArray) ros::shutdown(); 00054 00055 int min = my_list[i][0]; 00056 int max = my_list[i][1]; 00057 00058 addFilterIndices(min, max); 00059 00060 ROS_INFO("scan filter index interval %d : min: %d max: %d",i, min, max); 00061 } 00062 } 00063 00064 void scanCallback(const sensor_msgs::LaserScan& scan) 00065 { 00066 this->pubFilteredScan(scan); 00067 } 00068 00069 void pubFilteredScan(const sensor_msgs::LaserScan& scan) 00070 { 00071 filtered_scan_ = scan; 00072 00073 size_t filter_indices_size = filter_indices_.size(); 00074 00075 for (size_t i = 0; i < filter_indices_size; ++i) 00076 { 00077 filtered_scan_.ranges[filter_indices_[i]] = scan.range_max + 1.0; 00078 } 00079 00080 scan_filtered_pub_.publish(filtered_scan_); 00081 } 00082 00083 void addFilterIndices(size_t min, size_t max) 00084 { 00085 for (size_t i = min; i < max; ++i){ 00086 filter_indices_.push_back(i); 00087 } 00088 } 00089 00090 protected: 00091 ros::Subscriber scan_sub_; 00092 ros::Publisher scan_filtered_pub_; 00093 00094 sensor_msgs::LaserScan filtered_scan_; 00095 00096 std::vector<size_t> filter_indices_; 00097 }; 00098 00099 int main(int argc, char **argv) 00100 { 00101 ros::init(argc, argv, "hector_turtlebot_scan_filter"); 00102 00103 LaserScanFilter lsf; 00104 00105 ros::spin(); 00106 00107 return 0; 00108 }