LidarMeasure/test/wj_lidar_test.cpp

// #include <pcl/visualization/cloud_viewer.h>
#include <fstream>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/stat.h>
#include <dirent.h>

#include "../wj_lidar/wj_lidar_encapsulation.h"
#include "../wj_lidar/region_detect.h"


#include <thread>
#include <mutex>
#include "../tool/s7_plc.h"

#define DB 4
#define BLOCK_SIZE 6
#define CMD_ 0
#define RNDX_ 1
#define RNDB_ 2
#define STATUS_ 3
#define CURRX_ 4
#define CURRB_ 5
typedef std::chrono::duration<int, std::ratio<1, 1000>> milli_type;

std::atomic<bool> g_measure_cmd, g_measure_completed;
// 范围: 
// x -2400 0
// b -20 20
// plc(0,0)->laser(4.315, 84.46)
// x 0 2400
// ->laser(1.915, 84.46)

class Temp_plc
{
public:
    Temp_plc(std::string ip)
    {
        mb_exit = false;
        mb_ready = false;
        mb_device_responsing = false;
        m_ip_str = ip;
        memset(m_data, 0, sizeof(short) * BLOCK_SIZE);
        mb_ready = m_plc.connect(ip);

        mp_update_thread = new std::thread(Temp_plc::update_thread, this);
        mp_update_thread->detach();
    }
    ~Temp_plc()
    {
        mb_exit = true;
        mb_ready = false;
        if(mp_update_thread!=nullptr)
        {
            // mp_update_thread->join();
            delete (mp_update_thread);
            mp_update_thread = nullptr;
        }
    }


    bool write_data(short data, int index)
    {
        if(mb_exit || ! mb_ready)
        {
            std::cout << "plc not ready." << std::endl;
            return false;
        }
        // std::cout << "???" << std::endl;
        std::lock_guard<std::mutex> lck(m_mutex);
        // std::cout << "111" << std::endl;
        mb_ready = m_plc.WriteShorts(DB, index, 1, &data);
        // std::cout << "222" << std::endl;
        return true;
    }

    bool write_cmd(short rndx, short rndb)
    {
        // std::cout << "???" << std::endl;
        bool result = write_data(rndx, RNDX_);
        result &= write_data(rndb, RNDB_);
        result &= write_data((short)1, CMD_);
        return result;
    }

    short get_status()
    {
        // 1空闲,2正忙,3错误
        return m_data[STATUS_];
    }

    short get_curr_x()
    {
        return m_data[CURRX_];
    }

    short get_curr_b()
    {
        return m_data[CURRB_];
    }

    short get_last_x()
    {
        return m_last_data[CURRX_];
    }

    short get_last_b()
    {
        return m_last_data[CURRB_];
    }

    bool get_data(short *data)
    {
        if(mb_exit || ! mb_ready)
        {
            std::cout << "plc not ready." << std::endl;
            return false;
        }
        if(data == nullptr)
        {
            std::cout << "data nullptr" << std::endl;
            return false;
        }
        std::lock_guard<std::mutex> lck(m_mutex);
        memcpy(data, m_data, BLOCK_SIZE * sizeof(short));
        disp_data();

        return true;
    }

private:
    // cmd rndx rndb status x b
    void disp_data()
    {
        char buf[512];
        memset(buf, 0, 512);
        sprintf(buf, "cmd: %d, rndx: %d, rndb: %d, status: %d, x: %d, b: %d",m_data[0],m_data[1],m_data[2],m_data[3],m_data[4],m_data[5]);
        std::cout << buf << std::endl;
    }

    static void update_thread(Temp_plc *p)
    {
        if(p == nullptr)
        {
            std::cout << "plc nullptr" << std::endl;
            return;
        }

        std::chrono::steady_clock::time_point current_time = std::chrono::steady_clock::now();
        std::chrono::time_point<std::chrono::steady_clock, milli_type> current_time_in_milliseconds = std::chrono::time_point_cast<milli_type>(current_time);
        int now_in_milliseconds = current_time_in_milliseconds.time_since_epoch().count();
        uint64 seed = uint64(now_in_milliseconds);
        cv::RNG rnd = cv::RNG(seed);
        while (!p->mb_exit)
        {
            // std::cout << "cycling" << std::endl;
            if (p->m_plc.getConnection() && p->mb_ready)
            {
                // 自动读
                {
                    std::lock_guard<std::mutex> lck(p->m_mutex);
                    p->mb_ready = p->m_plc.ReadShorts(DB, 0, BLOCK_SIZE, p->m_data);
                    memcpy(p->m_last_data, p->m_data, BLOCK_SIZE * sizeof(short));
                }
                p->disp_data();
                // 查询状态
                if(p->get_status() == 1)
                {
                    if (!p->mb_device_responsing)
                    {
                        // 设备空闲,启动测量
                        g_measure_cmd = true;
                        // 等待测量结束
                        while (!g_measure_completed)
                        {
                            usleep(1000 * 100);
                            {
                                std::lock_guard<std::mutex> lck(p->m_mutex);
                                p->mb_ready = p->m_plc.ReadShorts(DB, 0, BLOCK_SIZE, p->m_data);
                                memcpy(p->m_last_data, p->m_data, BLOCK_SIZE * sizeof(short));
                            }
                            std::cout << "wating for measurement." << std::endl;
                        }
                        g_measure_completed = false;
                        std::cout << "measure finished." << std::endl;

                        // 写指令
                        p->mb_device_responsing = true;
                        short rndx, rndb;
                        do{
                            rndx = rnd.uniform(0, 8);
                            rndb = 0;
                            // rnd.uniform(-20, 20);
                        } while (abs(rndx*300 - p->get_curr_x()) < 300);// || abs(rndb - p->get_curr_b()) < 3);
                        std::cout << "write cmd: "<< rndx*300<<", "<<rndb << std::endl;
                        p->mb_ready = p->write_cmd(rndx*300, rndb);
                    }
                    else{
                        std::cout << "waiting for status switch." << std::endl;
                    }
                }
                // 设备接收到指令,状态已切换到正忙,等待完成
                if(p->get_status() == 2 && p->mb_device_responsing)
                {
                    p->mb_device_responsing = false;
                    // 清除指令
                    p->mb_ready = p->write_data((short)0, CMD_);
                }else{
                    // p->write_data(1, 3);
                }

            }else{
                std::lock_guard<std::mutex> lck(p->m_mutex);
                p->m_plc.disconnect();
                if (p->m_ip_str != "")
                {
                    p->mb_ready = p->m_plc.connect(p->m_ip_str);
                    // LOG(WARNING) << "find plc connection error, trying to connect";
                    if (p->mb_ready)
                    {
                        std::cout << "successfully reconnect." << std::endl;
                    }
                    else
                    {
                        std::cout << "failed to connect plc." << std::endl;
                    }
                }
            }

            usleep(1000*p->m_update_interval_milli);
        }
    }

private:
    std::string m_ip_str;
    bool mb_exit;
    bool mb_ready;
    std::mutex m_mutex;
    std::thread *mp_update_thread;           // plc更新线程
    S7PLC m_plc;                             // S7协议句柄
    const int m_update_interval_milli = 100; // plc更新频率
    short m_data[BLOCK_SIZE];
    short m_last_data[BLOCK_SIZE];
    bool mb_device_responsing; // 设备状态切换中

    bool mb_device_working; // 设备工作中
};

void init_glog()
{
    FLAGS_max_log_size = 100;
    FLAGS_logbufsecs = 0;
    google::InitGoogleLogging("LidarMeasurement_test");
    google::SetStderrLogging(google::INFO);
    google::InstallFailureSignalHandler();
    FLAGS_colorlogtostderr = true;        // Set log color
    FLAGS_logbufsecs = 0;                // Set log output speed(s)
    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
    FLAGS_stop_logging_if_full_disk = true;
}

void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
{
    std::ofstream os;
    os.open(txt, std::ios::out);
    for (int i = 0; i < pCloud->points.size(); i++)
    {
        pcl::PointXYZ point = pCloud->points[i];
        char buf[255];
        memset(buf, 0, 255);
        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
        os.write(buf, strlen(buf));
    }
    os.close();
}

bool ReadProtoParam(std::string path, wj::wjLidarParams &params)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &params);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

bool ReadProtoParam(std::string path, wj::wjManagerParams &params)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &params);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
    // std::lock_guard<std::mutex> lck(m_cloud_mutex);
    pointcloud->clear();
    std::ifstream in(filename);
    if (!in.is_open())
    {
        std::cout << "failed to open file " << filename << std::endl;
        return false;
    }
    while (!in.eof())
    {
        std::string t_linestr;
        if (getline(in, t_linestr))
        {
            std::vector<std::string> str_vector;
            std::stringstream ss(t_linestr);
            std::string num_str;
            while (getline(ss, num_str, ' '))
            {
                str_vector.push_back(num_str);
            }
            if (str_vector.size() != 3)
            {
                std::cout << "unsupported point cloud / cannot find point x y z value: " << filename << std::endl;
                return false;
            }
            pcl::PointXYZ t_cloud;
            t_cloud.x = stod(str_vector[0]);
            t_cloud.y = stod(str_vector[1]);
            t_cloud.z = stod(str_vector[2]);
            pointcloud->push_back(t_cloud);
        }
    }
    in.close();
    // std::cout << "file read finished with points " << pointcloud->size() << std::endl;
    return true;
}

void list_dir( const char * dir_name,std::vector<std::string>& files)
{
    if( 0 == dir_name )
    {
        std::cout<<" dir_name is null ! "<<std::endl;
        return;
    }

    struct stat s;
    lstat( dir_name , &s );
    if( ! S_ISDIR( s.st_mode ) )
    {
        return;
    }
    struct dirent * filename;
    DIR * dir;
    dir = opendir( dir_name );
    if( NULL == dir )
    {
        return;
    }

    int iName=0;
    while( ( filename = readdir(dir) ) != NULL )
    {
        if( strcmp( filename->d_name , "." ) == 0 ||
            strcmp( filename->d_name , "..") == 0)
            continue;

        char wholePath[256] = {0};
        sprintf(wholePath, "%s", (std::string(dir_name)+std::string("/")+filename->d_name).c_str());
        // std::cout << "??? "<<wholePath << std::endl;
        struct stat t_s;
        lstat(wholePath, &t_s);
        if (S_ISDIR(t_s.st_mode))
        {
            list_dir(wholePath, files);
        }else{
            files.push_back(wholePath);
        }
    }
}

void lidar_test()
{
    Wj_lidar_encapsulation *lidar = new Wj_lidar_encapsulation();
    wj::wjLidarParams params;
    bool result = ReadProtoParam("../src/test/wj_lidar_settings.prototxt", params);
    LOG(INFO) << "读配置结果" << result;
    // params.mutable_net_config()->set_ip_address("192.168.0.2");
    // params.mutable_net_config()->set_port(2110);
    if (lidar != 0)
    {
        Error_manager code = lidar->initialize(params);
        LOG(INFO) << code.to_string();
        // usleep(1000 * 1);
        if (code == SUCCESS)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            for (int i = 0; i < 10; i++)
            {
                LOG(INFO) << "connect status: " << lidar->get_connection_status();
                code = lidar->get_cloud(cloud, std::chrono::steady_clock::now(), 3000);
                if (code == SUCCESS)
                {
                    char temp[100];
                    memset(temp, 0, 100);
                    sprintf(temp, "cloud_%d.txt", i);
                    if (cloud->size() > 0)
                    {
                        save_cloud_txt(temp, cloud);
                        LOG(INFO) << "cloud saved to " << std::string(temp);
                    }
                }
                else
                {
                    LOG(WARNING) << code.to_string();
                }
                usleep(1000 * 30);
                LOG(INFO) << "end of cycle";
            }
        }
    }
    delete lidar;
}

void sample_detect(std::string base_path)
{
    // 读取配置
    wj::wjManagerParams params;
    if (!ReadProtoParam("/home/youchen/Documents/measure/MainStructure/LidarMeasure/build/setting/wj_manager_settings.prototxt", params))
    {
        std::cout << "read proto failed" << std::endl;
        return;
    }

    // 初始化各模块
    std::vector<Region_detector *> t_detectors;
    for (size_t i = 0; i < params.regions_size(); i++)
    {
        t_detectors.push_back(new Region_detector(i, params.regions(i)));
    }
    std::cout << "init detector: "<< t_detectors.size() << std::endl;

    // 遍历点云,除去整车库点云与失败的点云
    std::vector<std::__cxx11::string> files;
    pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    list_dir(base_path.c_str(), files);
    pcl::PointXYZ t_point;
    int success_count=0, failed_count=0;
    for (size_t i = 0; i < files.size(); i++)
    {
        // 除去失败点云
        if(files[i].find("failed", 0) < files[i].size())
            continue;
        if (read_pointcloud(files[i], tp_cloud))
        {
            // 除去完整点云
            if(tp_cloud->size() > 400 || tp_cloud->size() < 100)
                continue;
            // 找到中心点,判断所属区域
            t_point = tp_cloud->points[tp_cloud->size() - 5];
            int index = static_cast<int>(t_point.x / 3.3);
            // std::cout << "detect index: " << index << std::endl;
            tp_cloud->resize(tp_cloud->size() - 5);

            detect_wheel_ceres::Detect_result t_result;
            if(t_detectors.size() <= index)
            {
                std::cout << "fatal, region detector size error : "<< t_detectors.size()<<", "<<index << std::endl;
                return;
            }
            std::string t_info;
            Error_manager ec = t_detectors[index]->detect(tp_cloud, t_result.cx, t_result.cy, t_result.theta,t_result.front_theta, t_result.wheel_base, t_result.width, t_info, false);
            if(ec == SUCCESS)
            {
                success_count++;
            }
            else
            {
                failed_count++;
                std::cout << "detect failed" << std::endl;
            }
        }
        else
        {
            std::cout << "read failed" << std::endl;
        }
        // usleep(1000 * 1000);
    }

    std::cout << "detect finished with (success, failed): " << success_count << ", " << failed_count << std::endl;
}

void plc_test_only()
{
    // 初始化一个plc类用于读写
    Temp_plc t_plc("192.168.0.1");
    int count = 500;
    while(count-->0)
    {
        std::cout << "cycle " << 500 - count << std::endl;
        if (g_measure_cmd)
        {
            g_measure_cmd = false;
            usleep(1000 * 150);
            std::cout << "measured..." << std::endl;
            g_measure_completed = true;
        }
        usleep(1000 * 1000);
    }
}

#include "pcl/io/pcd_io.h"
void accuracy_validation()
{
    g_measure_cmd = false;
    g_measure_completed = false;

    time_t tt = time(0);
    struct tm *tc = localtime(&tt);
    char buf[255] = {0};
    memset(buf, 0, 255);
    sprintf(buf, "./result_%d-%02d-%02d.txt", tc->tm_year + 1900,
            tc->tm_mon + 1, tc->tm_mday);

    std::ofstream out;
    out.open(buf, std::ios::app);
    if(!out.is_open())
    {
        std::cout << "open file failed, "<< buf << std::endl;
        return;
    }

    // 读取配置
    wj::wjManagerParams params;
    if (!ReadProtoParam("/home/youchen/Documents/measure/MainStructure/LidarMeasure/build/setting/wj_manager_settings_gedian.prototxt", params))
    {
        std::cout << "read proto failed" << std::endl;
        return;
    }

    // 初始化各模块
    // 初始化激光
    std::vector<Wj_lidar_encapsulation *> t_lidars;
    for (int i = 0; i < params.wj_lidar_size(); ++i)
    {
        t_lidars.push_back(new Wj_lidar_encapsulation());
        Error_manager code = t_lidars[i]->initialize(params.wj_lidar(i));
        // 错误码信息已在内部打印
        if (code != SUCCESS)
        {
            std::cout << "万集雷达连接失败 "<< i << std::endl;
            return;
        }
    }
    std::cout << "init lidar: "<< t_lidars.size() << std::endl;

    // 初始化测量
    std::vector<Region_detector *> t_detectors;
    for (size_t i = 0; i < params.regions_size(); i++)
    {
        t_detectors.push_back(new Region_detector(i, params.regions(i)));
    }
    std::cout << "init detector: "<< t_detectors.size() << std::endl;

    // 初始化一个plc类用于读写
    Temp_plc t_plc("192.168.0.100");
    short t_plc_data[BLOCK_SIZE];
    // g_measure_cmd = true;
    char ch = 'x';
    int mcount = 0;
    while (ch != 'q')
    {
        std::cout << "please input cmd (m for measure, q for quit):" << std::endl;
        std::cin >> ch;

        if (ch == 'm')
        {
            std::cout << "start measure process." << std::endl;

            while (true)
            {
                if(g_measure_cmd){
                    mcount++;
                    g_measure_cmd = false;

                    int repeated_count = 6;
                    while(repeated_count-->0){
                        // 获取点云
                        pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
                        for (int i = 0; i < t_lidars.size(); ++i)
                        {
                            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
                            if (t_lidars[i]->get_cloud(cloud, std::chrono::steady_clock::now()) == SUCCESS)
                            {
                                total_cloud->operator+=(*cloud);
                            }
                        }
                        std::cout << "get cloud size: "
                                << total_cloud->size() << std::endl;
                        // pcl::PCDWriter t_writer;
                        // t_writer.write(std::string("./cloud/")+std::to_string(mcount)+std::to_string(repeated_count) + ".pcd", *total_cloud);

                        // 测量获得结果,测试时通常只存在一个区域测量块
                        detect_wheel_ceres::Detect_result t_result;
                        std::string t_info;
                        Error_manager ec = t_detectors[0]->detect(total_cloud, t_result.cx, t_result.cy, t_result.theta, t_result.front_theta, t_result.wheel_base, t_result.width, t_info, false);
                        // Error_manager ec = t_detectors[0]->find_circle(total_cloud, t_result.cx, t_result.cy);

                        // 从plc获取编码值
                        short cx, cy, theta;
                        cx = t_plc.get_last_x();
                        cy = 0;
                        theta = t_plc.get_last_b();

                        // 测量结果与plc读取数据写入文件
                        char line_buf[512];
                        memset(line_buf, 0, 512);
                        if (ec == SUCCESS)
                        {
                            sprintf(line_buf, "success,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,plc,%d,%d,%d",
                                    t_result.cx, t_result.cy, t_result.theta, t_result.front_theta, t_result.wheel_base, t_result.width,
                                    cx, cy, theta);
                        }
                        else
                        {
                            sprintf(line_buf, "failed,0.0,0.0,0.0,0.0,0.0,0.0,plc,%d,%d,%d",
                                    cx, cy, theta);
                            std::cout << "detect failed: "<< ec.to_string()<<" "<<t_info.c_str() << std::endl;
                        }
                        out << line_buf << std::endl;
                        std::cout << line_buf << ", " << strlen(line_buf) << std::endl;
                    }
                    out << "seperation line" << std::endl;
                    g_measure_completed = true;
                    usleep(1000 * 50);
                }
            }
        }
    }
    if(out.is_open())
    {
        out.close();
    }
}

int main(int argc, char *argv[])
{
    init_glog();
    // lidar_test();
    // usleep(1000 * 1000);

    // lidar_test();
    // usleep(1000 * 1000);

    // lidar_test();
    // usleep(1000 * 1000);

    // std::chrono::steady_clock::time_point current_time = std::chrono::steady_clock::now();
    // std::chrono::time_point<std::chrono::steady_clock, milli_type> current_time_in_milliseconds = std::chrono::time_point_cast<milli_type>(current_time);
    // int now_in_milliseconds = current_time_in_milliseconds.time_since_epoch().count();
    // uint64 seed = uint64(now_in_milliseconds);
    // cv::RNG rnd = cv::RNG(seed);
    // for (size_t i = 0; i < 100; i++)
    // {
    //     short ttx = (short)rnd.uniform(-2400, 0);
    //     std::cout << ttx<< std::endl;
    // }

    // wj test
    // sample_detect("/home/youchen/Documents/measure/MainStructure/wj_data_records");
    // plc_test_only();

    accuracy_validation();


    // LOG(INFO) << "end";
    getchar();
    return 0;
}