Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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 }