30 #include <sensor_msgs/LaserScan.h> 50 topicPub_laser_scan = nh.
advertise<sensor_msgs::LaserScan>(
"scan_out", 1);
56 if(scan_intervals.size()==0) {
57 topicPub_laser_scan.
publish(*msg);
63 sensor_msgs::LaserScan laser_scan = * msg;
66 int start_scan, stop_scan, num_scans;
67 num_scans = laser_scan.ranges.size();
70 for (
unsigned int i=0; i<scan_intervals.size(); i++) {
71 std::vector<double> * it = & scan_intervals.at(i);
73 if( it->at(1) <= laser_scan.angle_min ) {
74 ROS_WARN(
"Found an interval that lies below min scan range, skip!");
77 if( it->at(0) >= laser_scan.angle_max ) {
78 ROS_WARN(
"Found an interval that lies beyond max scan range, skip!");
82 if( it->at(0) <= laser_scan.angle_min ) start_scan = 0;
84 start_scan = (int)( (it->at(0) - laser_scan.angle_min) / laser_scan.angle_increment);
87 for(
int u = stop_scan; u<start_scan; u++) {
88 laser_scan.ranges.at(u) = 0.0;
91 if( it->at(1) >= laser_scan.angle_max ) stop_scan = num_scans-1;
93 stop_scan = (int)( (it->at(1) - laser_scan.angle_min) / laser_scan.angle_increment);
98 for(
unsigned int u = stop_scan; u<laser_scan.ranges.size(); u++) {
99 laser_scan.ranges.at(u) = 0.0;
103 topicPub_laser_scan.
publish(laser_scan);
111 int main(
int argc,
char** argv)
123 return a.at(0) < b.at(0);
127 std::string scan_intervals_param =
"scan_intervals";
128 std::vector<std::vector<double> > vd_interval_set;
129 std::vector<double> vd_interval;
134 nh.
getParam(scan_intervals_param, intervals_list);
137 ROS_FATAL(
"The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
138 throw std::runtime_error(
"The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
141 for(
int i = 0; i < intervals_list.
size(); ++i){
147 ROS_FATAL(
"The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
148 throw std::runtime_error(
"The scan intervals must be specified as a list of lists [[x1, y1], [x2, y2], ..., [xn, yn]]");
153 ROS_FATAL(
"Values in the scan intervals specification must be numbers");
154 throw std::runtime_error(
"Values in the scan intervals specification must be numbers");
160 ROS_FATAL(
"Values in the scan intervals specification must be numbers");
161 throw std::runtime_error(
"Values in the scan intervals specification must be numbers");
166 if(vd_interval.at(0)< -M_PI || vd_interval.at(1)< -M_PI) {
167 ROS_WARN(
"Found a scan interval < -PI, skip!");
172 if(vd_interval.at(0)>M_PI || vd_interval.at(1)>M_PI) {
173 ROS_WARN(
"Found a scan interval > PI, skip!");
179 if(vd_interval.at(0) >= vd_interval.at(1)) {
180 ROS_WARN(
"Found a scan interval with i1 > i2, switched order!");
181 vd_interval[1] = vd_interval[0];
185 vd_interval_set.push_back(vd_interval);
187 }
else ROS_WARN(
"Scan filter has not found any scan interval parameters.");
192 for(
unsigned int i = 0; i<vd_interval_set.size(); i++) {
193 for(
unsigned int u = i+1; u<vd_interval_set.size(); u++) {
194 if( vd_interval_set.at(i).at(1) > vd_interval_set.at(u).at(0)) {
195 ROS_FATAL(
"The scan intervals you specified are overlapping!");
196 throw std::runtime_error(
"The scan intervals you specified are overlapping!");
206 return vd_interval_set;
ros::Publisher topicPub_laser_scan
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< std::vector< double > > scan_intervals
void scanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
Type const & getType() const
ros::Subscriber topicSub_laser_scan_raw
std::vector< std::vector< double > > loadScanRanges()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
bool getParam(const std::string &key, std::string &s) const
bool hasParam(const std::string &key) const
bool compareIntervals(std::vector< double > a, std::vector< double > b)