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