LaserMeasure_linux/src/measuretask.cpp

#include "measuretask.h"
#include "pathcreator.h"
std::string Error_string(ERROR_CODE code)
{
    std::string StrError;
    switch (code)
    {
    case eSucc:StrError = "测量正确";
        break;
    case eArea:StrError = "跨车位区域或者车位区域无车";
        break;
    case eLidar:
        StrError = "两雷达数据不重合";
        break;
    case eLimitL:
        StrError = "左边界超界";
        break;
    case eLimitR:
        StrError = "右边界超界";
        break;
    case eLimitT:
        StrError = "上边界超界";
        break;
    case eLimitB:
        StrError = "下边界超界";
        break;
    case eNoCar:
        StrError = "测量失败， 场景内未找到车辆";
        break;
    case eMulCar:
        StrError = "车位区域内存在多辆车";
        break;
    case eDistance:
        StrError = "车辆间距离过进，机械臂无法下降";
        break;
    case eCloud:
        StrError = "雷达断线，无点云数据";
        break;
    default:
        break;
    }
    return StrError;
}

bool RegionInRegion(std::vector<cv::Point2f>& mini_poly, std::vector<cv::Point2f>& large_poly)
{
    for (int i = 0; i < mini_poly.size(); ++i)
    {
        if (cv::pointPolygonTest(large_poly, mini_poly[i], false) < 0)
            return false;
    }
    return true;
}

double distance_polys(std::vector<cv::Point2f> poly1, std::vector<cv::Point2f> poly2)
{
    std::vector<cv::Point2f>& minpoly = poly1.size() >= poly2.size() ? poly2 : poly1;
    std::vector<cv::Point2f>& maxpoly = poly1.size() >= poly2.size() ? poly1 : poly2;
    double mindis = (numeric_limits<double>::max)();
    for (int i = 0; i < minpoly.size(); ++i)
    {
        double dis = -cv::pointPolygonTest(maxpoly, minpoly[i], true);
        if (dis < mindis)
        {
            mindis = dis;
        }
    }
    return mindis;
}

std::mutex* MeasureTask::g_mutex = new std::mutex();

MeasureTask::MeasureTask(std::vector<CLaser*> lasers, void* locater,int positon_id,
    modbus::CPLCMonitor* plc,Automatic::stCalibParam param)
{
    m_lasers = lasers;
    m_locater = locater;
    m_plc = plc;
    m_exit = false;
    m_calib_param = param;
    m_position_id = positon_id;
    m_ptr = 0;
    m_post_result_func=0;
}

ERROR_CODE MeasureTask::SelectResult(int plate_index, std::vector<CarPosition> results, CarPosition& result)
{
    //一两车不考虑区域问题
    if (results.size() == 1)
    {
        result = results[0];
        return eSucc;
    }

    LOG(INFO) << "选择号牌机对应车辆，号牌编号：" << plate_index;
    if (plate_index >= m_calib_param.plate_area_size() || plate_index < 0)
    {
        LOG(ERROR) << "当前车位没有车辆,车位ID：" << plate_index;
        return eArea;
    }

    std::vector<cv::Point2f> area;
    for (int i = 0; i < m_calib_param.plate_area(plate_index).point_size(); ++i)
    {
        Automatic::stPoint stP = m_calib_param.plate_area(plate_index).point(i);
        cv::Point2f point;
        point.x = stP.x();
        point.y = stP.y();
        area.push_back(point);
    }
    if (area.size() < 3)
    {
        LOG(ERROR) << " plate aera points size<3  size=" << area.size();
        return eArea;
    }
    double max_distance = -1.0;

    int num = 0;
    int id = -1;
    for (int i = 0; i < results.size(); ++i)
    {
        cv::Point2f vtx[4];
        results[i].rrect.points(vtx);
        std::vector<cv::Point2f> poly(vtx, vtx + 4);
        if (RegionInRegion(poly, area))
        {
            result = results[i];
            LOG(INFO) << vtx[0] << " " << vtx[1] << " " << vtx[2] << " " << vtx[3] << " ";
            num++;
            id = i;
        }
    }
    if (num == 1)		//找到符合要求车辆，rs[id]
    {
        cv::Point2f vtx[4];
        results[id].rrect.points(vtx);
        std::vector<cv::Point2f> poly(vtx, vtx + 4);
        for (int i = 0; i < results.size(); ++i)
        {
            if (i != id)
            {
                cv::Point2f vtx1[4];
                results[i].rrect.points(vtx1);
                std::vector<cv::Point2f> poly1(vtx1, vtx1 + 4);
                if (distance_polys(poly, poly1) < 400.)
                    return eDistance;
            }
        }
        return eSucc;
    }
    if (num > 1)
        return eMulCar;
    return eArea;
}

ERROR_CODE MeasureTask::Dispatch_posID(std::vector<CarPosition>& results)
{
    if (results.size() == 0)
        return eNoCar;

    if ( m_calib_param.plate_area_size()==0 )
    {
        LOG(ERROR) << "\t未配置车位信息，无法标识车位";
        return eArea;
    }

    std::vector<std::vector<cv::Point2f>> areas;
    for (int k = 0; k < m_calib_param.plate_area_size(); ++k)
    {
        std::vector<cv::Point2f> area;
        for (int i = 0; i < m_calib_param.plate_area(k).point_size(); ++i)
        {
            Automatic::stPoint stP = m_calib_param.plate_area(k).point(i);
            cv::Point2f point;
            point.x = stP.x();
            point.y = stP.y();
            area.push_back(point);
        }
        areas.push_back(area);
    }

    double max_distance = -1.0;

    int num = 0;
    for (int i = 0; i < results.size(); ++i)
    {

        //判断结果是否合法
        CarPosition rs = results[i];
        ERROR_CODE code = rs.error_code;

        if (rs.x < 0 || rs.y < 0)
        {
            code = eArea;
            continue;
        }
        //// 分配车位
        cv::Point2f center = results[i].rrect.center;
        for (int j = 0; j < areas.size(); ++j)
        {
            if (cv::pointPolygonTest(areas[j], center, false) >= 0)
            {
                results[i].pos_id = j;
                LOG(INFO) << "\tCar[" << i << "],center:" << results[i].rrect.center << ",size:" << results[i].rrect.size << ",车位ID:" << j;
                num++;
            }
        }
        /*cv::Point2f vtx[4];
        results[i].rrect.points(vtx);
        std::vector<cv::Point2f> poly(vtx, vtx + 4);
        for (int j = 0; j < areas.size(); ++j)
        {
            if (RegionInRegion(poly, areas[j]))
            {
                results[i].pos_id = j;
                LOG(INFO) << "\tCar[" << i << "],center:" << results[i].rrect.center << ",size:" << results[i].rrect.size << ",车位ID:" << j;
                num++;
            }
        }*/
        if (results[i].pos_id < 0)
            continue;
        float l = 4200;
        float w = 2630;
        cv::RotatedRect rect = rs.rrect;
        rect.center = rs.rrect.center;

        cv::Point2f pt[4];
        rect.points(pt);
        float minx = areas[results[i].pos_id][0].x;//m_calib_param.valid_region().min_x();
        float maxx = areas[results[i].pos_id][3].x;//m_calib_param.valid_region().max_x();
        float miny = areas[results[i].pos_id][1].y;//m_calib_param.valid_region().min_y();
        float maxy = areas[results[i].pos_id][0].y;//m_calib_param.valid_region().max_y();
        for (int i = 0; i < 4; ++i)
        {
            cv::Point2f point = pt[i];
            LOG(INFO) << "error pt " << i << ":" << point.x << "," << point.y;
            if (point.x < minx)
            {

                code = eLimitL;
                //break;
            }
            if (point.x > maxx)
            {

                code = eLimitR;
                //break;
            }
            if (point.y < miny)
            {

                code = eLimitB;
                //break;
            }

        }

        results[i].error_code = code;

        if (code != eSucc)
        {
            LOG(INFO) << "\tCar[" << i << "] error,info:"<<Error_string(code);
            continue;
        }

    }
    if (num >= 1)		//找到符合要求车辆，rs[id]
    {
        return eSucc;
    }
    return eArea;
}

void MeasureTask::PushPoint(CPoint3D point, void* pointer)
{
    MeasureTask* task = reinterpret_cast<MeasureTask*>(pointer);
    PointT pcl_point;
    pcl_point.x = point.x;
    pcl_point.y = point.y;
    pcl_point.z = point.z;

    float min_x = task->m_calib_param.valid_region().min_x();
    float max_x = task->m_calib_param.valid_region().max_x();
    float min_y = task->m_calib_param.valid_region().min_y();
    float max_y = task->m_calib_param.valid_region().max_y();
    float min_z = task->m_calib_param.valid_region().min_z();
    float max_z = task->m_calib_param.valid_region().max_z();

    if (pcl_point.x<min_x || pcl_point.x>max_x || pcl_point.y<min_y ||
        pcl_point.y>max_y || pcl_point.z<min_z || pcl_point.z>max_z)
        return;

    std::lock_guard<std::mutex> lk(task->m_mutex);
    task->m_cloud.push_back(pcl_point);

}
void MeasureTask::SetResultCallback(void *ptr, ResultCallback func)
{
    m_post_result_func=func;
    m_ptr=ptr;
}

void  MeasureTask::Main()
{
    ////等待雷达空闲资源
    PathCreator pathCreator;
    while (!m_exit)
    {
        g_mutex->lock();
        if (IsLaserReady())
        {

            pathCreator.CreateDatePath(m_calib_param.project_dir());
            LOG(INFO) << "\tTask dir:" << pathCreator.GetCurPath();
            //启动摆扫
            for (int i = 0; i < m_lasers.size(); ++i)
            {
                m_lasers[i]->SetSaveDir(pathCreator.GetCurPath());
                m_lasers[i]->SetPointCallBack(PushPoint, this);
                m_lasers[i]->Start();
            }
            g_mutex->unlock();
            break;
        }
        g_mutex->unlock();
        usleep(100*1000);
    }

    while (!m_exit && !IsLaserReady()) { usleep(1000); }	//等待摆扫结束

    std::vector<CarPosition> results;
    ERROR_CODE code = eSucc;
    if (m_locater)
    {
        //m_locater->Locate(m_cloud.makeShared(), results, strPath.GetBuffer(), code)
        if (m_cloud.size() == 0)
        {
            code = eCloud;
        }
        else if (0)
        {
            code = Dispatch_posID(results);
            if (code == eSucc)
            {
                /////写入结果到plc
                for (int i = 0; i < results.size(); ++i)
                {
                    CarPosition				rs = results[i];
                    rs.pos_id = 0;
                    whiskboom_laser_value value;
                    //value.a = int((rs.a+4.1) * 100);
                    value.a = int((rs.a) * 100);
                    value.corrected = (rs.error_code == eSucc);
                    value.h = rs.h;
                    value.l = rs.l;
                    value.w = rs.w;
                    value.x = rs.x ;
                    value.y = rs.y;

                    // 正常返回状态3
                    LOG(INFO) << "\t post：" << rs.a << "°" << ", " << " center:(" <<
                        rs.x << "," << rs.y << "), size:" << "(" << rs.l << ", " << rs.w << ", " << rs.h << ")"<<",rs.code:"<<Error_string(rs.error_code);
                    if (m_plc)
                        m_plc->setMeasureResult(m_calib_param.plate_area(rs.pos_id).plc_addr(), &value);
                }
            }
        }
        else
        {
            LOG(ERROR) << "\t测量失败 : " << Error_string(code);
        }
    }
    ///写入测量完成信号
    if (m_plc)
        m_plc->MeasureComplete(code==eSucc);


    /////回调函数，窗口显示结果
    QtMessageData msg;
    msg.msg_type=eMeasure;

    if(results.size()>0)
    {
        msg.OK=1;

        msg.x=results[0].x;
        msg.y=results[0].y;
        msg.c=results[0].a;
        msg.l=std::max(results[0].rrect.size.width,results[0].rrect.size.height);
        msg.w=results[0].w;
        msg.h=results[0].h;
        msg.d=results[0].l;
    }
    else
    {
        msg.OK=0;
    }
    ///// id  time
    short id=0;
    for(int i=0;i<m_lasers.size();++i)
    {
        id =id | (0x01<<m_lasers[i]->ID());
    }
    msg.lasers_id=id;
    time_t tt = time(0);//时间cuo
    tm* t = localtime(&tt);
    memset(msg.time_str,0,255);
    sprintf(msg.time_str, "%d-%02d-%02d %02d:%02d:%02d", t->tm_year + 1900, t->tm_mon + 1,
        t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec);

    memset(msg.project_dir,0,255);
    sprintf(msg.project_dir,"%s",pathCreator.GetCurPath().c_str());

    if(m_post_result_func)
        m_post_result_func(msg,m_ptr);

}
void  MeasureTask::Cancel()
{
    m_exit = true;
}

bool MeasureTask::IsLaserReady()
{
    bool ready = true;
    for (int i = 0; i < m_lasers.size(); ++i)
    {
        ready = (ready && m_lasers[i]->IsReady());
    }

    return ready;
}

////////////////////////////////