soft_obstacle_detection.h
Go to the documentation of this file.
00001 #ifndef _HECTOR_SOFT_OBSTACLE_DETECTION_H_
00002 #define _HECTOR_SOFT_OBSTACLE_DETECTION_H_
00003 
00004 #include <ros/ros.h>
00005 #include <tf/tf.h>
00006 #include <tf/transform_listener.h>
00007 
00008 #include <Eigen/Core>
00009 #include <Eigen/Geometry>
00010 
00011 #include <opencv/cv.h>
00012 #include <cv_bridge/cv_bridge.h>
00013 #include <opencv2/opencv.hpp>
00014 #include <opencv2/highgui/highgui.hpp>
00015 
00016 #include <sensor_msgs/LaserScan.h>
00017 
00018 #include <dynamic_reconfigure/server.h>
00019 #include <hector_soft_obstacle_detection/SoftObstacleDetectionConfig.h>
00020 
00021 //#define DEBUG
00022 
00023 using hector_soft_obstacle_detection::SoftObstacleDetectionConfig;
00024 
00025 class SoftObstacleDetection
00026 {
00027 public:
00028   SoftObstacleDetection();
00029   ~SoftObstacleDetection();
00030 
00031 private:
00032   void transformScanToImage(const sensor_msgs::LaserScanConstPtr scan, cv::Mat& img, cv::Point2i& scan_center) const;
00033   void houghTransform(const cv::Mat& img, std::vector<cv::Vec4i>& lines) const;
00034   uchar getMaxAtLine(const cv::Mat& img, const cv::Point& p1, const cv::Point& p2) const;
00035   void getLine(const cv::Mat& img, const cv::Vec4i& line, cv::Mat& out) const;
00036   void edgeDetection(const cv::Mat& signal, std::vector<double>& edges, std::vector<double>& centers) const;
00037 
00038   bool checkSegmentsMatching(const std::vector<double>& edges, const std::vector<double>& centers, double veil_segment_size, double min_segments, double max_segments) const;
00039 
00040   double evalModel(const std::vector<double>& edges, const std::vector<double>& centers, double lambda) const;
00041   bool checkFrequencyMatching(const std::vector<double>& edges, const std::vector<double>& centers, double lambda, double min_segments, double max_segments) const;
00042 
00043   void lineToPointCloud(const cv::Vec4i& line, const cv::Point2i& scan_center, pcl::PointCloud<pcl::PointXYZ>& out) const;
00044 
00045   void update(const ros::TimerEvent& event);
00046 
00047   void laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan);
00048 
00049   void dynRecParamCallback(SoftObstacleDetectionConfig& config, uint32_t level);
00050 
00051   ros::Timer update_timer;
00052 
00053   sensor_msgs::LaserScanConstPtr last_scan;
00054 
00055   double unit_scale;
00056   int border_size;
00057 
00058   tf::TransformListener tf_listener;
00059 
00060   // subscriber
00061   ros::Subscriber laser_scan_sub_;
00062 
00063   // publisher
00064   ros::Publisher veil_percept_pub_;
00065   ros::Publisher veil_percept_pose_pub_;
00066   ros::Publisher veil_point_cloud_pub_;
00067 
00068   dynamic_reconfigure::Server<SoftObstacleDetectionConfig> dyn_rec_server_;
00069 
00070   // dynamic reconfigure params
00071   unsigned int min_hole_size_;
00072   double max_curtain_length_sq_;
00073   double min_frequency_;
00074   double max_frequency_;
00075   double veil_segment_size_;
00076   int min_segments_;
00077   int max_segments_;
00078   double max_segment_size_mse_;
00079   double max_segment_size_var_;
00080   double max_segment_dist_var_;
00081   double size_dist_ratio_;
00082   double max_frequency_mse_;
00083   std::string percept_class_id_;
00084 };
00085 
00086 #endif


hector_soft_obstacle_detection
Author(s): Alexander Stumpf
autogenerated on Wed Sep 16 2015 10:20:00