|
@@ -7,6 +7,8 @@
|
|
|
#include "terminor_msg.pb.h"
|
|
|
#include "MeasureRequest.h"
|
|
|
|
|
|
+std::mutex Terminor_Command_Executor::ms_mutex_scan_measure;
|
|
|
+
|
|
|
//////以下两个函数用于测试,读取指定点云
|
|
|
bool string2point(std::string str,pcl::PointXYZ& point)
|
|
|
{
|
|
@@ -295,6 +297,8 @@ void Terminor_Command_Executor::set_save_root_path(std::string root)
|
|
|
|
|
|
Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
{
|
|
|
+ std::lock_guard<std::mutex> lock(ms_mutex_scan_measure);
|
|
|
+ LOG(INFO)<<">>>>>>>>>>>> Enter terminator ID:"<<m_terminor_parameter.id();
|
|
|
//计时起点
|
|
|
auto t_start_point=std::chrono::system_clock::now();
|
|
|
|
|
@@ -315,9 +319,9 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
Error_manager code;
|
|
|
//第一步,启动雷达扫描点云,切换当前状态为扫描中
|
|
|
//根据配置筛选雷达
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> scan_cloud;
|
|
|
std::mutex cloud_lock;
|
|
|
- std::vector<Task_Base*> laser_task_vector;
|
|
|
+ std::vector<Laser_task*> laser_task_vector;
|
|
|
|
|
|
std::vector<Laser_base*> tp_lasers;
|
|
|
for(int i=0;i<m_terminor_parameter.laser_parameter_size();++i)
|
|
@@ -347,7 +351,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
//创建扫描任务,
|
|
|
Laser_task* laser_task=new Laser_task();
|
|
|
//
|
|
|
- laser_task->task_init(TASK_CREATED,scan_cloud,&cloud_lock);
|
|
|
+ laser_task->task_init(TASK_CREATED,&scan_cloud,&cloud_lock);
|
|
|
laser_task->set_task_frame_maxnum(frame_count);
|
|
|
laser_task->set_save_path(project_path);
|
|
|
laser_task_vector.push_back(laser_task);
|
|
@@ -394,7 +398,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
if(second>m_timeout_second)
|
|
|
{
|
|
|
//扫描超时,点云中没有点,则返回错误
|
|
|
- if(scan_cloud->size()==0)
|
|
|
+ if(scan_cloud.size()==0)
|
|
|
{
|
|
|
return Error_manager(TERMINOR_LASER_TIMEOUT,MAJOR_ERROR,"laser scanning timeout");
|
|
|
}
|
|
@@ -404,7 +408,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
usleep(1000*100);
|
|
|
}
|
|
|
//检查雷达任务完成情况,是否是正常完成
|
|
|
- LOG(INFO)<<" laser scanning over cloud size:"<<scan_cloud->size();
|
|
|
+ LOG(INFO)<<" laser scanning over cloud size:"<<scan_cloud.size();
|
|
|
//释放扫描任务单
|
|
|
for(int i=0;i<laser_task_vector.size();++i)
|
|
|
{
|
|
@@ -419,7 +423,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
pcl::PassThrough<pcl::PointXYZ> passX;
|
|
|
pcl::PassThrough<pcl::PointXYZ> passY;
|
|
|
pcl::PassThrough<pcl::PointXYZ> passZ;
|
|
|
- passX.setInputCloud(scan_cloud);
|
|
|
+ passX.setInputCloud(scan_cloud.makeShared());
|
|
|
passX.setFilterFieldName("x");
|
|
|
passX.setFilterLimits(m_terminor_parameter.area_3d().min_x(),m_terminor_parameter.area_3d().max_x());
|
|
|
passX.filter(*locate_cloud);
|
|
@@ -460,6 +464,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
code=wj_task->init();
|
|
|
if(code!=SUCCESS)
|
|
|
{
|
|
|
+ delete wj_task;
|
|
|
return code;
|
|
|
}
|
|
|
wj_command t_wj_command;
|
|
@@ -470,6 +475,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
code=mp_wj_lidar->execute_task(wj_task);
|
|
|
if(code!=SUCCESS)
|
|
|
{
|
|
|
+ delete wj_task;
|
|
|
return code;
|
|
|
}
|
|
|
///万集测量正确,对比两数据
|
|
@@ -477,6 +483,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
code=wj_task->get_result(wj_result);
|
|
|
if(code!=SUCCESS)
|
|
|
{
|
|
|
+ delete wj_task;
|
|
|
return code;
|
|
|
}
|
|
|
Locate_information wj_locate_information;
|
|
@@ -490,17 +497,21 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
float offset_y=fabs(dj_locate_information.locate_y-wj_locate_information.locate_y);
|
|
|
float offset_angle=fabs(dj_locate_information.locate_theta-wj_locate_information.locate_theta);
|
|
|
float offset_width=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
|
|
|
- float offset_wheel_base=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
|
|
|
- if(offset_x>100 ||offset_y>100 ||offset_angle>2 || offset_wheel_base>200 ||offset_width>150)
|
|
|
+ float offset_wheel_base=fabs(dj_locate_information.locate_wheel_base-wj_locate_information.locate_wheel_base);
|
|
|
+ if(offset_x>100 ||offset_y>100 ||offset_angle>4 || offset_wheel_base>200 ||offset_width>200)
|
|
|
{
|
|
|
- LOG(WARNING)<<"chekc failed. (offset x>100, y>100, angle>2, wheel_base>200, width>100): "<<offset_x<<" "<<offset_y<<" "<<offset_angle<<" "<<offset_wheel_base<<" "<<offset_width;
|
|
|
+ LOG(WARNING)<<"chekc failed. (offset x>100, y>100, angle>4, wheel_base>200, width>200): "<<offset_x<<" "<<offset_y<<" "<<offset_angle<<" "<<offset_wheel_base<<" "<<offset_width;
|
|
|
return Error_manager(TERMINOR_CHECK_RESULTS_ERROR,NORMAL,"check results failed ");
|
|
|
}
|
|
|
///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以2750为基准作卡尔曼滤波
|
|
|
///车长以大疆雷达为准,车宽以万集雷达为准,高以大疆雷达为准
|
|
|
m_measure_information.locate_x=wj_locate_information.locate_x;
|
|
|
m_measure_information.locate_y=wj_locate_information.locate_y;
|
|
|
- m_measure_information.locate_theta=0.5*(wj_locate_information.locate_theta+dj_locate_information.locate_theta);
|
|
|
+ if(offset_angle<2)
|
|
|
+ m_measure_information.locate_theta=0.5*wj_locate_information.locate_theta+0.5*dj_locate_information.locate_theta;
|
|
|
+ else
|
|
|
+ m_measure_information.locate_theta=0.1*wj_locate_information.locate_theta+0.9*dj_locate_information.locate_theta;
|
|
|
+
|
|
|
float dj_distance=fabs(dj_locate_information.locate_wheel_base-2750);
|
|
|
float wj_distance=fabs(wj_locate_information.locate_wheel_base-2750);
|
|
|
float weight_dj=wj_distance/(dj_distance+wj_distance);
|
|
@@ -524,10 +535,11 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
m_measure_information.locate_x, m_measure_information.locate_y);
|
|
|
code = mp_verify_tool->verify(rotated_rect,m_measure_information.locate_hight, false);
|
|
|
m_measure_information.locate_correct = (code == SUCCESS);
|
|
|
+ delete wj_task;
|
|
|
return code;
|
|
|
|
|
|
}
|
|
|
-
|
|
|
+ delete wj_task;
|
|
|
return code;
|
|
|
}
|
|
|
|