123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434 |
- #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;
- }
- ////////////////////////////////
|