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
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 #include <vector>
00059 #include <algorithm>
00060
00061
00062 #include <ros/ros.h>
00063 #include <XmlRpc.h>
00064
00065
00066 #include <sensor_msgs/LaserScan.h>
00067
00068
00069
00070
00071 class NodeClass
00072 {
00073 public:
00074 std::vector<std::vector<double> > scan_intervals;
00075
00076 ros::NodeHandle nh;
00077
00078 ros::Subscriber topicSub_laser_scan_raw;
00079 ros::Publisher topicPub_laser_scan;
00080
00081 NodeClass() {
00082
00083 scan_intervals = loadScanRanges();
00084
00085
00086 topicPub_laser_scan = nh.advertise<sensor_msgs::LaserScan>("scan_filtered", 1);
00087 topicSub_laser_scan_raw = nh.subscribe("scan", 1, &NodeClass::scanCallback, this);
00088 }
00089
00090 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
00091
00092 if(scan_intervals.size()==0) {
00093 topicPub_laser_scan.publish(*msg);
00094 return;
00095 }
00096
00097
00098
00099 sensor_msgs::LaserScan laser_scan = * msg;
00100
00101
00102 int start_scan, stop_scan, num_scans;
00103 num_scans = laser_scan.ranges.size();
00104
00105 stop_scan = 0;
00106 for ( unsigned int i=0; i<scan_intervals.size(); i++) {
00107 std::vector<double> * it = & scan_intervals.at(i);
00108
00109 if( it->at(1) <= laser_scan.angle_min ) {
00110 ROS_WARN("Found an interval that lies below min scan range, skip!");
00111 continue;
00112 }
00113 if( it->at(0) >= laser_scan.angle_max ) {
00114 ROS_WARN("Found an interval that lies beyond max scan range, skip!");
00115 continue;
00116 }
00117
00118 if( it->at(0) <= laser_scan.angle_min ) start_scan = 0;
00119 else {
00120 start_scan = (int)( (it->at(0) - laser_scan.angle_min) / laser_scan.angle_increment);
00121 }
00122
00123 for(int u = stop_scan; u<start_scan; u++) {
00124 laser_scan.ranges.at(u) = 0.0;
00125 }
00126
00127 if( it->at(1) >= laser_scan.angle_max ) stop_scan = num_scans-1;
00128 else {
00129 stop_scan = (int)( (it->at(1) - laser_scan.angle_min) / laser_scan.angle_increment);
00130 }
00131
00132 }
00133
00134 for(unsigned int u = stop_scan; u<laser_scan.ranges.size(); u++) {
00135 laser_scan.ranges.at(u) = 0.0;
00136 }
00137
00138
00139 topicPub_laser_scan.publish(laser_scan);
00140 }
00141
00142 std::vector<std::vector<double> > loadScanRanges();
00143 };
00144
00145
00146
00147 int main(int argc, char** argv)
00148 {
00149
00150 ros::init(argc, argv, "scanner_filter");
00151
00152 NodeClass nc;
00153
00154 ros::spin();
00155 return 0;
00156 }
00157
00158 bool compareIntervals(std::vector<double> a, std::vector<double> b) {
00159 return a.at(0) < b.at(0);
00160 }
00161
00162 std::vector<std::vector<double> > NodeClass::loadScanRanges() {
00163 std::string scan_intervals_param = "scan_intervals";
00164 std::vector<std::vector<double> > vd_interval_set;
00165 std::vector<double> vd_interval;
00166
00167
00168 XmlRpc::XmlRpcValue intervals_list;
00169 if(nh.hasParam(scan_intervals_param)){
00170 nh.getParam(scan_intervals_param, intervals_list);
00171
00172 if(!(intervals_list.getType() == XmlRpc::XmlRpcValue::TypeArray)){
00173 ROS_FATAL("The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
00174 throw std::runtime_error("The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
00175 }
00176
00177 for(int i = 0; i < intervals_list.size(); ++i){
00178 vd_interval.clear();
00179
00180
00181 XmlRpc::XmlRpcValue interval = intervals_list[i];
00182 if(!(interval.getType() == XmlRpc::XmlRpcValue::TypeArray && interval.size() == 2)){
00183 ROS_FATAL("The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
00184 throw std::runtime_error("The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
00185 }
00186
00187
00188 if(!(interval[0].getType() == XmlRpc::XmlRpcValue::TypeInt || interval[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
00189 ROS_FATAL("Values in the scan intervals specification must be numbers");
00190 throw std::runtime_error("Values in the scan intervals specification must be numbers");
00191 }
00192 vd_interval.push_back( interval[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(interval[0]) : (double)(interval[0]) );
00193
00194
00195 if(!(interval[1].getType() == XmlRpc::XmlRpcValue::TypeInt || interval[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
00196 ROS_FATAL("Values in the scan intervals specification must be numbers");
00197 throw std::runtime_error("Values in the scan intervals specification must be numbers");
00198 }
00199 vd_interval.push_back( interval[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(interval[1]) : (double)(interval[1]) );
00200
00201
00202 if(vd_interval.at(0)< -M_PI || vd_interval.at(1)< -M_PI) {
00203 ROS_WARN("Found a scan interval < -PI, skip!");
00204 continue;
00205
00206 }
00207
00208 if(vd_interval.at(0)>M_PI || vd_interval.at(1)>M_PI) {
00209 ROS_WARN("Found a scan interval > PI, skip!");
00210 continue;
00211
00212 }
00213
00214
00215 if(vd_interval.at(0) >= vd_interval.at(1)) {
00216 ROS_WARN("Found a scan interval with i1 > i2, switched order!");
00217 vd_interval[1] = vd_interval[0];
00218 vd_interval[0] = ( interval[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(interval[1]) : (double)(interval[1]) );
00219 }
00220
00221 vd_interval_set.push_back(vd_interval);
00222 }
00223 } else ROS_WARN("Scan filter has not found any scan interval parameters.");
00224
00225
00226 sort(vd_interval_set.begin(), vd_interval_set.end(), compareIntervals);
00227
00228 for(unsigned int i = 0; i<vd_interval_set.size(); i++) {
00229 for(unsigned int u = i+1; u<vd_interval_set.size(); u++) {
00230 if( vd_interval_set.at(i).at(1) > vd_interval_set.at(u).at(0)) {
00231 ROS_FATAL("The scan intervals you specified are overlapping!");
00232 throw std::runtime_error("The scan intervals you specified are overlapping!");
00233 }
00234 }
00235 }
00236
00237
00238
00239
00240
00241
00242 return vd_interval_set;
00243 }
00244