v4r_laser_filter: v4r_laser_line_filter

Go to the documentation of this file.
00001 /***************************************************************************
00002  * Copyright (c) 2014 Markus Bader <markus.bader@tuwien.ac.at>
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 1. Redistributions of source code must retain the above copyright
00008  *    notice, this list of conditions and the following disclaimer.
00009  * 2. Redistributions in binary form must reproduce the above copyright
00010  *    notice, this list of conditions and the following disclaimer in the
00011  *    documentation and/or other materials provided with the distribution.
00012  * 3. All advertising materials mentioning features or use of this software
00013  *    must display the following acknowledgement:
00014  *    This product includes software developed by the TU-Wien.
00015  * 4. Neither the name of the TU-Wien nor the
00016  *    names of its contributors may be used to endorse or promote products
00017  *    derived from this software without specific prior written permission.
00018  * 
00019  * THIS SOFTWARE IS PROVIDED BY Markus Bader ''AS IS'' AND ANY
00020  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY
00023  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  ***************************************************************************/
00030 
00031 #include "ros/ros.h"
00032 #include <sensor_msgs/LaserScan.h>
00033 #include <geometry_msgs/PolygonStamped.h>
00034 #include <visualization_msgs/Marker.h>
00035 #include <sstream>
00036 
00037 #include <dynamic_reconfigure/server.h>
00038 #include <v4r_laser_filter/LineFilterConfig.h>
00039 #include <v4r_laser_filter/v4r_laser_filter_utils.h>
00040 
00041 #ifndef V4R_LASER_LINE_FILTER_NODE
00042 #define V4R_LASER_LINE_FILTER_NODE
00043 
00044 
00045 #define V4R_LASER_LINE_FILTER_PUBLISH_MARKER false
00046 #define V4R_LASER_LINE_FILTER_PUBLISH_LINES true
00047 #define V4R_LASER_LINE_FILTER_THRESHOLD_SPLIT 0.02
00048 #define V4R_LASER_LINE_FILTER_MIN_LENGTH 0.2
00049 #define V4R_LASER_LINE_FILTER_MIN_POINTS_PER_LINE 20
00050 #define V4R_LASER_LINE_FILTER_MIN_POINTS_PER_METER 10
00051 #define V4R_LASER_LINE_FILTER_FIT_LINES false
00052 #define V4R_LASER_LINE_FILTER_SPLIT_SCAN true
00053 
00055 class LaserLineFilterNode {
00056 public:
00057     struct Parameters {
00058         Parameters()
00059             : publish_marker(V4R_LASER_LINE_FILTER_PUBLISH_MARKER)
00060             , publish_lines(V4R_LASER_LINE_FILTER_PUBLISH_LINES)
00061             , threshold_split(V4R_LASER_LINE_FILTER_THRESHOLD_SPLIT)
00062             , threshold_split_neighbor(true)
00063             , min_length(V4R_LASER_LINE_FILTER_MIN_LENGTH)
00064             , min_points_per_line(V4R_LASER_LINE_FILTER_MIN_POINTS_PER_LINE)
00065             , min_points_per_meter(V4R_LASER_LINE_FILTER_MIN_POINTS_PER_METER)
00066             , split_scan(V4R_LASER_LINE_FILTER_SPLIT_SCAN)
00067             , fit_lines(V4R_LASER_LINE_FILTER_FIT_LINES)
00068             , write_scan(false)
00069             , read_scan(false)
00070             , scan_filename("/tmp/scan.bin") {};
00071         bool publish_marker;
00072         bool publish_lines;
00073         float threshold_split;
00074         bool threshold_split_neighbor;
00075         float min_length;
00076         int min_points_per_line;
00077         float min_points_per_meter;
00078         bool split_scan;
00079         bool fit_lines;
00080         bool write_scan;
00081         bool read_scan;
00082         std::string scan_filename;
00083     };
00084     LaserLineFilterNode ( ros::NodeHandle &n );
00085     void callback (const sensor_msgs::LaserScan::ConstPtr& msg);
00086     void callbackParameters ( v4r_laser_filter::LineFilterConfig &config, uint32_t level );
00087 private:
00088     void split(LaserFilter::LineSegment &line);
00089     void splitStart();
00090     void readScan(const std::string &filename, sensor_msgs::LaserScan &msg );
00091     void writeScan(const std::string &filename, const sensor_msgs::LaserScan &msg );
00092     void lineFitStart();
00093     void publish_marker();
00094 private: // variables
00095     ros::NodeHandle n_;
00096     ros::NodeHandle n_param_;
00097     Parameters param_;
00098     ros::Publisher pub_laser_lines_;
00099     ros::Publisher pub_laser_line_split_;
00100     ros::Publisher pub_laser_line_fit_;
00101     ros::Publisher pub_laser_input_;
00102     ros::Publisher pub_marker_;
00103     ros::Subscriber sub_;
00104     dynamic_reconfigure::Server<v4r_laser_filter::LineFilterConfig> reconfigureServer_;
00105     dynamic_reconfigure::Server<v4r_laser_filter::LineFilterConfig>::CallbackType reconfigureFnc_;
00106     sensor_msgs::LaserScan msg_scan_;
00107     geometry_msgs::PolygonStamped msg_lines;
00108     visualization_msgs::Marker msg_line_list_;
00109     std::vector<LaserFilter::Beam> beams_;
00110     std::vector<LaserFilter::Measurment> measurments_;
00111     std::vector<std::pair<unsigned int, unsigned int> > connectedMeasurments_;
00112     std::vector<LaserFilter::LineSegment> lineSegments_;
00113     std::vector<LaserFilter::Line> lines_;
00114     float  angle_increment_;
00115     float  angle_increment_sin_;
00116 
00117 };
00118 
00119 #endif // V4R_LASER_LINE_FILTER_NODE