v4r_laser_filter: v4r_laser_corner_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 #include "ros/ros.h"
00031 #include <sensor_msgs/LaserScan.h>
00032 #include <geometry_msgs/PoseArray.h>
00033 #include <visualization_msgs/Marker.h>
00034 #include <sstream>
00035 
00036 #include <dynamic_reconfigure/server.h>
00037 #include <v4r_laser_filter/CornerFilterConfig.h>
00038 #include <v4r_laser_filter/v4r_laser_filter_utils.h>
00039 
00040 #ifndef V4R_LASER_CORNER_FILTER_NODE
00041 #define V4R_LASER_CORNER_FILTER_NODE
00042 
00043 
00045 class LaserCornerFilterNode {
00046 public:
00047     struct Parameters {
00048         Parameters()
00049             : publish_marker(true)
00050             , min_line_length(0.1)
00051             , min_points_on_line(10)
00052             , normal_angle_threshold(0.1)
00053             , max_corner_line_differnce(1.2)
00054             , max_line_error(0.5)
00055             , max_corner_error(0.5)
00056             , redefine_corners(true)
00057             , remove_double_detections(true)
00058             , remove_double_detections_threshold(0.1)
00059             , write_scan(false)
00060             , read_scan(false)
00061             , scan_filename("/tmp/scan.bin")
00062             {};
00063         bool publish_marker;
00064         float min_line_length;
00065         int min_points_on_line;
00066         float normal_angle_threshold;
00067         float max_corner_line_differnce;
00068         float max_line_error;
00069         float max_corner_error;
00070         bool redefine_corners;
00071         bool remove_double_detections;
00072         float remove_double_detections_threshold;
00073         bool write_scan;
00074         bool read_scan;
00075         std::string scan_filename;
00076     };
00077     LaserCornerFilterNode ( ros::NodeHandle &n );
00078     void callback (const sensor_msgs::LaserScan::ConstPtr& msg);
00079     void callbackParameters ( v4r_laser_filter::CornerFilterConfig &config, uint32_t level );
00080 private:
00081     void init_marker_visualization();
00082     void publish_marker_visualization();
00083     void publish_marker_posearray();
00084 private: // variables
00085     ros::NodeHandle n_;
00086     ros::NodeHandle n_param_;
00087     Parameters param_;
00088     ros::Subscriber sub_;
00089     ros::Publisher pub_marker_visualization_;
00090     ros::Publisher pub_marker_posearray_;
00091     unsigned callbackCount;
00092     visualization_msgs::Marker msg_corner_pose_;
00093     visualization_msgs::Marker msg_corner_pose_estimation_;
00094     visualization_msgs::Marker msg_corner_l0_;
00095     visualization_msgs::Marker msg_corner_l0_estimation_;
00096     visualization_msgs::Marker msg_corner_l1_;
00097     visualization_msgs::Marker msg_corner_l1_estimation_;
00098     visualization_msgs::Marker msg_corner_text_;
00099     geometry_msgs::PoseArray msg_marker_posearray_;
00100     dynamic_reconfigure::Server<v4r_laser_filter::CornerFilterConfig> reconfigureServer_;
00101     dynamic_reconfigure::Server<v4r_laser_filter::CornerFilterConfig>::CallbackType reconfigureFnc_;
00102     sensor_msgs::LaserScan msg_scan_;
00103     std::vector<LaserFilter::Measurment> measurments_;
00104     std::vector<LaserFilter::Beam> beams_;
00105     std::vector<LaserFilter::LineSegment> lines_;
00106     std::vector<LaserFilter::Corner> corners_;
00107 
00108 };
00109 
00110 #endif // V4R_LASER_CORNER_FILTER_NODE