00001 00018 #ifndef LASERSCANTHREAD_H 00019 #define LASERSCANTHREAD_H 00020 #include <QThread> 00021 #include <vector> 00022 00023 #ifndef Q_MOC_RUN 00024 #include <sensor_msgs/LaserScan.h> 00025 #include <ros/ros.h> 00026 #include <ros/init.h> 00027 #include <Eigen/Dense> 00028 #endif 00029 00030 #include <Laserscanner/abstract_laserscanner.h> 00031 #include <MathHelpers/linearApproximator.h> 00032 00033 using namespace std; 00034 class LaserScanThread : public QThread 00035 { 00036 Q_OBJECT 00037 public: 00038 explicit LaserScanThread(Abstract_LaserScanner* scanner, double segmentation_lambda, double maxVarianceFilter, double maxAngleDeviation, double variationStep, int variationStepCount); 00039 ~LaserScanThread(); 00040 unsigned int data_field_size; 00041 float angle_spread; 00042 float starting_angle; 00043 double *dx; 00044 double *data_raw; 00045 double *data_avg; 00046 double *data_filtered; 00047 double *data_segments; 00048 double *data_edges; 00049 void stop(); 00050 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00051 protected: 00052 00053 static constexpr unsigned int GAUSSIANFIELDSIZE = 20; 00054 static constexpr unsigned int AVERAGINGWINDOW = 450; 00055 static constexpr unsigned int DETECTIONWINDOW = 10; 00056 static constexpr double sigma = 1.5; 00057 //static const double sigma = 0.798; 00058 00059 unsigned int stepNumber; 00060 std::vector<float> segments; 00061 std::vector<int> segmentsizes; 00062 std::vector<int> segmentdistance; 00063 double *averagedRanges; 00064 double *rotatingAccumulationWindow[AVERAGINGWINDOW]; 00065 double *filteredRanges; 00066 Eigen::Vector2d *coordinates; 00067 std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > edgesfound; 00068 double feld[2*GAUSSIANFIELDSIZE+1]; 00069 double gaussFactor; 00070 double maxVarianceFilter = 0.15; 00071 double maxAngleDeviation; 00072 double variationStep = 0.006; 00073 int variationStepCount = 2; 00074 double segmentation_lambda = 1.5; 00075 00076 double markersize = 0.02; 00077 00078 Abstract_LaserScanner* scanner; 00079 00080 LinearApproximator * linearApproximator; 00081 00082 ros::NodeHandle nh; 00083 ros::Publisher scanDataPublisher; 00084 ros::Publisher segmentDataPublisher; 00085 ros::Publisher segmentBorderPublisher; 00086 00087 void calculateGaussianMatrix(); 00088 void launchSICK(); 00089 void run(); 00090 signals: 00091 void valueChanged(); //Is called everytime new data is available 00092 void trianglesFound(std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > *triangles); //Is called everytime a triangle is detected 00093 00094 public slots: 00095 void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& msg); //Is called everytime the laserscanner has new data 00096 }; 00097 00098 00099 #endif // LASERSCANTHREAD_H