clamp_safety/test.cpp

#include "test.hpp"

#define DEBUG(...) \
if (0) { \
    printf(__VA_ARGS__); \
}

Test::Test(Clamp* clamp):m_clamp(clamp) {
    m_snap_wite.m_id = 1;
    m_snap_read.m_id = 1;
    m_snap_wite.m_start_index = 34;
    m_snap_read.m_start_index = 4;
    m_snap_wite.m_communication_mode = Snap7_buf::Communication_mode::LOOP_COMMUNICATION;
    m_snap_read.m_communication_mode = Snap7_buf::Communication_mode::LOOP_COMMUNICATION;
    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);
}

void Test::setWriteSnap(int id, int index) {
    m_snap_wite.m_id = id;
    m_snap_wite.m_start_index = index;
}

void Test::setReadSnap(int id, int index) {
    m_snap_read.m_id = id;
    m_snap_read.m_start_index = index;
}

bool Test::GetAlg(float &x) {
    float sumx = 0,sumy = 0, w = 0, sumw = 0;

    for(int i=0; i < m_cloud->size(); i++) {
        w = float(1/pow(cos(atan(fabs(m_cloud->points[i].x/ m_cloud->points[i].y))), 2));
        // w = float(sqrt((36.0 - w))/6.0);
        sumw += w;
        sumx += w * m_cloud->points[i].x;
    }
    x = sumx/sumw;
    // return x > WARNING_ALG_VALUE;
    return true;
}

void Test::exportTxtFile() {
    // 文件记录算法坐标、实际坐标、记录id、
    // 文件夹下存放每一次的雷达点云数据，文件名为记录id、算法坐标、实际坐标
    m_record_data.id++;
    std::fstream txtFile, lidarFile;
    if(NULL == opendir(lidarPath.c_str())) {
        mkdir(lidarPath.c_str(), 0777);
    }
    txtFile.open(filePath + fileName, std::fstream::out | std::fstream::app);

    if (GetAlg(m_record_data.alg_x)) {
        lidarFile.open(lidarPath + std::to_string(m_record_data.id) + "_" + std::to_string(m_record_data.alg_x) + "_" + std::to_string(m_record_data.real_x - DEFAULT_WHEEL_AXIS) + ".txt", std::fstream::out | std::fstream::app);
        for (int i = 0; i < m_cloud->size(); i++) {
            lidarFile << m_cloud->points[i].x << " " << m_cloud->points[i].y << std::endl;
        }
    }

    txtFile << float(m_record_data.id) << " " << float(m_record_data.alg_x*1000) << " " << float(m_record_data.real_x - DEFAULT_WHEEL_AXIS ) << std::endl;
    
    lidarFile.close();
    txtFile.close();
}

void Test::commnuition() {
    float point_x;
    bool isMoved = true;
    int16_t old_move = 0, now_move = 0, plc_move = 0;
    std::fstream pointsFile;
    pointsFile.open("../record/big_point_water_measure.txt");
    char cloudbuf[256];
    pcl::PointXYZ cloud_point;
    DEBUG("commnuition\n")
    while (true)
    {
        if(isMoved) {
            // 模拟生成随机下一次实际车轮坐标
            srand(time(NULL));
            point_x = rand()%201 + DEFAULT_WHEEL_AXIS - 100;
            // point_x = float(DEFAULT_WHEEL_AXIS);
            m_record_data.real_x = point_x;
            
            // 发送给plc目标坐标值
            if(!snap7_write(point_x)) {
                // continue;
            }
            isMoved = false;
        }
        
        // 读plc数据，等待plc到达设置的车轮坐标
        if(snap7_read(plc_move)) {
            old_move = now_move;
            now_move = plc_move;
        } else {
            // continue;
        }

        // srand(time(NULL));
        // now_move = rand()%201 > 100;
        old_move = now_move = 1;
        if (old_move == 1 && now_move == 1) {
            m_record_data.real_x = point_x;
#ifdef lidar
            update_clould();
#else
            pointsFile.getline(cloudbuf, sizeof(cloudbuf));
            std::string cloudstr = cloudbuf;
            if(std::string::npos == cloudstr.find(" ")) {
                DEBUG("%s\n", cloudbuf);
                m_cloud->clear();
                for (int i = atoi(cloudbuf); i > 0; i--) {
                    pointsFile.getline(cloudbuf, sizeof(cloudbuf));
                    DEBUG("%s\n", cloudbuf);
                    
                    std::istringstream iss;
                    iss.str(cloudbuf);
                    float value;
                    float data[2]={0};
                    for(int i=0;i<2;++i)
                    {
                        if(iss>>value)
                        {
                            data[i]=value;
                        }
                    }
                    cloud_point.x=data[0];
                    cloud_point.y=data[1];
                    DEBUG("%.4f %.4f\n", cloud_point.x, cloud_point.y);
                    m_cloud->push_back(cloud_point);
                }
            }
#endif
            exportTxtFile();
            usleep(100*1000);
        }
        if (old_move == 0 && now_move == 1) {
            isMoved = true;
        } else {
            isMoved = false;
        }
    }
    
}

void Test::update_clould() {
    m_clamp->mutex.lock();
    m_cloud->clear();
    m_clamp->lidar.get_last_cloud(m_cloud, std::chrono::system_clock::now());
    m_clamp->mutex.unlock();
}

float Test::SWAP_float(float x) {
    int b;
    float value = 0;
    memcpy(&b,&x,sizeof(float));
    int t = ((((b) & 0xff000000) >> 24) | (((b) & 0x00ff0000) >>  8) |(((b) & 0x0000ff00) <<  8) | (((b) & 0x000000ff) << 24));
    memcpy(&value,&t,sizeof(float));
    return value;
}

bool Test::snap7_write(float &data) {
    DEBUG("---Debug write data : %f\n", data);
    data = SWAP_float(data);
    m_snap_wite.m_size = sizeof(data);
    m_snap_wite.mp_buf_reverse = &data;
#ifdef plc
    if(m_plc->write_data_buf(m_snap_wite) != SUCCESS) {
        // while(m_plc->get_status() == Snap7_communication_base::SNAP7_COMMUNICATION_DISCONNECT) {}
        return false;
    }
#endif
    DEBUG("---Debug write data end : %f\n", data);
    return true;
};

bool Test::snap7_read(float &data) {
    m_snap_read.m_size = sizeof(data);
    m_snap_read.mp_buf_reverse = &data;
#ifdef plc
    if(m_plc->read_data_buf(m_snap_read) == SUCCESS) {
        // while(m_plc->get_status() == Snap7_communication_base::SNAP7_COMMUNICATION_DISCONNECT) {}
        return false;
    }
#endif
    data = SWAP_float(data);
    return true;
}

bool Test::snap7_write(int16_t &data) {
    data = bswap_16(data);
    m_snap_wite.m_size = sizeof(data);
    m_snap_wite.mp_buf_reverse = &data;
#ifdef plc
    if(m_plc->write_data_buf(m_snap_wite) != SUCCESS) {
        // while(m_plc->get_status() == Snap7_communication_base::SNAP7_COMMUNICATION_DISCONNECT) {}
        return false;
    }
#endif
    return true;
};

bool Test::snap7_read(int16_t &data) {
    m_snap_read.m_size = sizeof(data);
    m_snap_read.mp_buf_reverse = &data;
#ifdef plc
    if(m_plc->read_data_buf(m_snap_read) == SUCCESS) {
        // while(m_plc->get_status() == Snap7_communication_base::SNAP7_COMMUNICATION_DISCONNECT) {}
        return false;
    }
#endif
    data = bswap_16(data);
    return true;
}