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 #include "ros/ros.h"
00032 #include <sensor_msgs/LaserScan.h>
00033 #include <sstream>
00034
00035 #define MX_LASER_RANGE_FILTER_INF_TO_MAX true
00036 #define MX_LASER_RANGE_FILTER_NAN_TO_MIN false
00037 #define MX_LASER_RANGE_FILTER_MAX_OFFSET -0.01
00038 #define MX_LASER_RANGE_FILTER_MIN_OFFSET 0.01
00039 #define MX_LASER_RANGE_FILTER_MAX -1
00040 #define MX_LASER_RANGE_FILTER_MIN -1
00041
00043 class LaserRangeFilterNode {
00044 public:
00045 LaserRangeFilterNode ( ros::NodeHandle &n )
00046 :n_ ( n ), n_param_ ( "~" ) {
00047 sub_ = n_.subscribe("scan", 1000, &LaserRangeFilterNode::callback, this);
00048 pub_ = n_.advertise<sensor_msgs::LaserScan>("scan_filtered", 1000);
00049
00050 n_param_.param<bool>("inf2max", inf2max_, MX_LASER_RANGE_FILTER_INF_TO_MAX);
00051 ROS_INFO("%s: max_range: %s", n_param_.getNamespace().c_str(), ((inf2max_) ? "true" : "false"));
00052 n_param_.param<bool>("nan2min", nan2min_, MX_LASER_RANGE_FILTER_NAN_TO_MIN);
00053 ROS_INFO("%s: min_range: %s", n_param_.getNamespace().c_str(), ((nan2min_) ? "true" : "false"));
00054 n_param_.param<double>("max_offset", max_offset_, MX_LASER_RANGE_FILTER_MAX_OFFSET);
00055 ROS_INFO("%s: max_offset: %4.3f", n_param_.getNamespace().c_str(), max_offset_);
00056 n_param_.param<double>("min_offset", min_offset_, MX_LASER_RANGE_FILTER_MIN_OFFSET);
00057 ROS_INFO("%s: min_offset: %4.3f", n_param_.getNamespace().c_str(), min_offset_);
00058 n_param_.param<double>("max", max_, MX_LASER_RANGE_FILTER_MAX);
00059 ROS_INFO("%s: max: %4.3f", n_param_.getNamespace().c_str(), max_);
00060 n_param_.param<double>("min", min_, MX_LASER_RANGE_FILTER_MIN);
00061 ROS_INFO("%s: min: %4.3f", n_param_.getNamespace().c_str(), min_);
00062
00063 }
00064 void callback (const sensor_msgs::LaserScan::ConstPtr& _msg) {
00065 if(pub_.getNumSubscribers() == 0) return;
00066 sensor_msgs::LaserScan msg = *_msg;
00067 unsigned int nrOfRanges = msg.ranges.size();
00068 unsigned int nrOfIntensities = msg.intensities.size();
00069 for (int i = 0; i < nrOfRanges; i++) {
00070 float &range = msg.ranges[i];
00071 if (nan2min_ && isnan(range)) {
00072 range = msg.range_min + min_offset_;
00073 }
00074 if (inf2max_ && isinf(range)) {
00075 range = msg.range_max + max_offset_;
00076 }
00077 if ((max_ > 0.) && range > max_) {
00078 range = max_;
00079 }
00080 if ((min_ > 0.) && range < min_) {
00081 range = min_;
00082 }
00083 }
00084 pub_.publish(msg);
00085 }
00086 private:
00087 ros::Publisher pub_;
00088 ros::Subscriber sub_;
00089 private:
00090 ros::NodeHandle n_;
00091 ros::NodeHandle n_param_;
00092 bool inf2max_;
00093 bool nan2min_;
00094 double max_offset_;
00095 double min_offset_;
00096 double max_;
00097 double min_;
00098 };
00099
00100 int main(int argc, char **argv) {
00101
00102 ros::init(argc, argv, "LaserFilter");
00103 ros::NodeHandle n;
00104 LaserRangeFilterNode my_node(n);
00105 ros::spin();
00106 return 0;
00107 }