$search
00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2010 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob_driver 00012 * ROS package name: cob_sick_s300 00013 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de 00018 * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de 00019 * 00020 * Date of creation: Jan 2010 00021 * ToDo: 00022 * 00023 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00024 * 00025 * Redistribution and use in source and binary forms, with or without 00026 * modification, are permitted provided that the following conditions are met: 00027 * 00028 * * Redistributions of source code must retain the above copyright 00029 * notice, this list of conditions and the following disclaimer. 00030 * * Redistributions in binary form must reproduce the above copyright 00031 * notice, this list of conditions and the following disclaimer in the 00032 * documentation and/or other materials provided with the distribution. 00033 * * Neither the name of the Fraunhofer Institute for Manufacturing 00034 * Engineering and Automation (IPA) nor the names of its 00035 * contributors may be used to endorse or promote products derived from 00036 * this software without specific prior written permission. 00037 * 00038 * This program is free software: you can redistribute it and/or modify 00039 * it under the terms of the GNU Lesser General Public License LGPL as 00040 * published by the Free Software Foundation, either version 3 of the 00041 * License, or (at your option) any later version. 00042 * 00043 * This program is distributed in the hope that it will be useful, 00044 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00045 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00046 * GNU Lesser General Public License LGPL for more details. 00047 * 00048 * You should have received a copy of the GNU Lesser General Public 00049 * License LGPL along with this program. 00050 * If not, see <http://www.gnu.org/licenses/>. 00051 * 00052 ****************************************************************/ 00053 00054 //################## 00055 //#### includes #### 00056 00057 // standard includes 00058 #include <vector> 00059 #include <algorithm> 00060 00061 // ROS includes 00062 #include <ros/ros.h> 00063 #include <XmlRpc.h> 00064 00065 // ROS message includes 00066 #include <sensor_msgs/LaserScan.h> 00067 00068 00069 //#################### 00070 //#### node class #### 00071 class NodeClass 00072 { 00073 public: 00074 std::vector<std::vector<double> > scan_intervals; 00075 00076 ros::NodeHandle nh; 00077 // topics to publish 00078 ros::Subscriber topicSub_laser_scan_raw; 00079 ros::Publisher topicPub_laser_scan; 00080 00081 NodeClass() { 00082 // loading config 00083 scan_intervals = loadScanRanges(); 00084 00085 // implementation of topics to publish 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 //if no filter intervals specified 00092 if(scan_intervals.size()==0) { 00093 topicPub_laser_scan.publish(*msg); 00094 return; 00095 } 00096 00097 00098 // use hole received message, later only clear some ranges 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; //laser_scan.range_min; 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; //laser_scan.range_min; 00136 } 00137 00138 // publish message 00139 topicPub_laser_scan.publish(laser_scan); 00140 } 00141 00142 std::vector<std::vector<double> > loadScanRanges(); 00143 }; 00144 00145 //####################### 00146 //#### main programm #### 00147 int main(int argc, char** argv) 00148 { 00149 // initialize ROS, spezify name of node 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 //grab the range-list from the parameter server if possible 00168 XmlRpc::XmlRpcValue intervals_list; 00169 if(nh.hasParam(scan_intervals_param)){ 00170 nh.getParam(scan_intervals_param, intervals_list); 00171 //make sure we have a list of lists 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 //make sure we have a list of lists of size 2 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 //make sure that the value we're looking at is either a double or an int 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 //make sure that the value we're looking at is either a double or an int 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 //basic checking validity 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 //throw std::runtime_error("Found a scan interval < -PI!"); 00206 } 00207 //basic checking validity 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 //throw std::runtime_error("Found a scan interval > PI!"); 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 //now we want to sort the intervals and check for overlapping 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 /* DEBUG out: 00238 for(unsigned int i = 0; i<vd_interval_set.size(); i++) { 00239 std::cout << "Interval " << i << " is " << vd_interval_set.at(i).at(0) << " | " << vd_interval_set.at(i).at(1) << std::endl; 00240 } */ 00241 00242 return vd_interval_set; 00243 } 00244