clamp_safety/detect/clamp_safety_detect.h

//
// Created by zx on 22-8-30.
//

#ifndef SHUTTER_VERIFY_DETECT_CLAMP_SAFETY_DETECT_H_
#define SHUTTER_VERIFY_DETECT_CLAMP_SAFETY_DETECT_H_
#define PCL_NO_PRECOMPILE
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/point_representation.h>


#if __VIEW__PCL
#include <pcl/visualization/pcl_visualizer.h>
#endif
class Safety_status
{
 public:
    Safety_status(){
        clamp_completed=false;
        wheel_exist=false;
        cx=0;
        gap=0;
        time_point=std::chrono::steady_clock::now();
        timeout_s=0.5;
    };
    ~Safety_status(){}
    Safety_status(const Safety_status& other)
    {
        clamp_completed=other.clamp_completed;
        wheel_exist=other.wheel_exist;
        cx=other.cx;
        gap=other.gap;
        time_point=other.time_point;
        timeout_s=other.timeout_s;
    }
    Safety_status& operator=(const Safety_status& other)
    {
        clamp_completed=other.clamp_completed;
        wheel_exist=other.wheel_exist;
        cx=other.cx;
        gap=other.gap;
        time_point=other.time_point;
        timeout_s=other.timeout_s;
        return *this;
    }
    void set_timeout(float s){
        timeout_s=s;
    }
    bool is_timeout(){
        auto t2 = std::chrono::steady_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - time_point);

        double time = double(duration.count()) * std::chrono::microseconds::period::num /
                std::chrono::microseconds::period::den;
        return time>timeout_s;
    }

    bool clamp_completed;
    bool wheel_exist;          //点云直线是否是轮子
    float cx;
    float gap;      //轮子距离雷达
    std::chrono::steady_clock::time_point time_point;
    float timeout_s;
    std::mutex mutex_;

};



//ax+by+c=0
typedef struct {
    float a;
    float b;
    float c;
    float cx;
    float cy;
    float theta_by_x;
    float cov;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
}Line;

class clamp_safety_detect
{
 public:

    clamp_safety_detect();

    ~clamp_safety_detect();

#if __VIEW__PCL
    void set_viewer(pcl::visualization::PCLVisualizer* m_viewer,int port);
#endif

     bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Safety_status& safety);
     bool fit_line(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Line& line);
     bool check_wheel_line(const Line& line,Safety_status& safety);
 protected:
     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> classify_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
    //  bool check_wheel_line(const Line& line,Safety_status& safety);
     bool check_clamp_lines(const Line& line1,const Line& line2,Safety_status& safety);

#if __VIEW__PCL
    void view_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string name,int r,int g,int b);
     pcl::visualization::PCLVisualizer* m_viewer;
     int m_port;
#endif
};


#endif //SHUTTER_VERIFY_DETECT_CLAMP_SAFETY_DETECT_H_