|
|
@@ -295,129 +295,114 @@ void Terminor_Command_Executor::set_save_root_path(std::string root)
|
|
|
m_save_root_path=root;
|
|
|
}
|
|
|
|
|
|
-Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
-{
|
|
|
+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();
|
|
|
+ Error_manager code;
|
|
|
+ LOG(INFO) << ">>>>>>>>>>>> Enter terminator ID:" << m_terminor_parameter.id();
|
|
|
//计时起点
|
|
|
- auto t_start_point=std::chrono::system_clock::now();
|
|
|
+ auto t_start_point = std::chrono::system_clock::now();
|
|
|
|
|
|
//准备目录
|
|
|
time_t tt;
|
|
|
- time( &tt );
|
|
|
- tt = tt + 8*3600; // transform the time zone
|
|
|
- tm* t= gmtime( &tt );
|
|
|
- char path[255]={0};
|
|
|
- sprintf(path,"%s/%4d/%02d/%02d",m_save_root_path.c_str(),
|
|
|
+ time(&tt);
|
|
|
+ tt = tt + 8 * 3600; // transform the time zone
|
|
|
+ tm *t = gmtime(&tt);
|
|
|
+ char path[255] = {0};
|
|
|
+ sprintf(path, "%s/%4d/%02d/%02d", m_save_root_path.c_str(),
|
|
|
t->tm_year + 1900,
|
|
|
t->tm_mon + 1,
|
|
|
t->tm_mday);
|
|
|
PathCreator path_creator;
|
|
|
path_creator.CreateDatePath(path);
|
|
|
- std::string project_path=path_creator.GetCurPath();
|
|
|
+ std::string project_path = path_creator.GetCurPath();
|
|
|
+
|
|
|
|
|
|
- Error_manager code;
|
|
|
//第一步,启动雷达扫描点云,切换当前状态为扫描中
|
|
|
//根据配置筛选雷达
|
|
|
pcl::PointCloud<pcl::PointXYZ> scan_cloud;
|
|
|
std::mutex cloud_lock;
|
|
|
- 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)
|
|
|
- {
|
|
|
- int laser_id=m_terminor_parameter.laser_parameter(i).id();
|
|
|
- int frame_count=m_terminor_parameter.laser_parameter(i).frame_count();
|
|
|
- if(frame_count<=0)
|
|
|
- {
|
|
|
- LOG(WARNING)<<"terminal parameter laser["<<laser_id<<"] frame count is <0";
|
|
|
+ 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) {
|
|
|
+ int laser_id = m_terminor_parameter.laser_parameter(i).id();
|
|
|
+ int frame_count = m_terminor_parameter.laser_parameter(i).frame_count();
|
|
|
+ if (frame_count <= 0) {
|
|
|
+ LOG(WARNING) << "terminal parameter laser[" << laser_id << "] frame count is <0";
|
|
|
continue;
|
|
|
}
|
|
|
- if(laser_id>=0&&laser_id<mp_laser_vector.size())
|
|
|
- {
|
|
|
+ if (laser_id >= 0 && laser_id < mp_laser_vector.size()) {
|
|
|
//判断该id的雷达是否已经添加进去, 放置配置中出现重复的雷达编号
|
|
|
- bool tb_repeat=false;
|
|
|
- for(int j=0;j<tp_lasers.size();++j)
|
|
|
- {
|
|
|
- if(tp_lasers[j]==mp_laser_vector[laser_id])
|
|
|
- {
|
|
|
- tb_repeat=true;
|
|
|
+ bool tb_repeat = false;
|
|
|
+ for (int j = 0; j < tp_lasers.size(); ++j) {
|
|
|
+ if (tp_lasers[j] == mp_laser_vector[laser_id]) {
|
|
|
+ tb_repeat = true;
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
- if(tb_repeat==false)
|
|
|
- {
|
|
|
+ if (tb_repeat == false) {
|
|
|
tp_lasers.push_back(mp_laser_vector[laser_id]);
|
|
|
//创建扫描任务,
|
|
|
- Laser_task* laser_task=new Laser_task();
|
|
|
+ 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);
|
|
|
//发送任务单给雷达
|
|
|
- code=tp_lasers[i]->execute_task(laser_task);
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
+ code = tp_lasers[i]->execute_task(laser_task);
|
|
|
+ if (code != SUCCESS) {
|
|
|
return code;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
- if(tp_lasers.size()==0)
|
|
|
- {
|
|
|
- return Error_manager(TERMINOR_NOT_CONTAINS_LASER,NORMAL,"terminal not contains laser");
|
|
|
+ if (tp_lasers.size() == 0) {
|
|
|
+ return Error_manager(TERMINOR_NOT_CONTAINS_LASER, NORMAL, "terminal not contains laser");
|
|
|
}
|
|
|
- m_terminor_statu=TERMINOR_SCANNING;
|
|
|
+ m_terminor_statu = TERMINOR_SCANNING;
|
|
|
|
|
|
//等待雷达完成任务单
|
|
|
- while(1)
|
|
|
- {
|
|
|
+ while (1) {
|
|
|
//判断是否强制退出
|
|
|
- if(mb_force_quit==true)
|
|
|
- {
|
|
|
- char description[255]={0};
|
|
|
- sprintf(description,"ternimal is force quit terminal id : %d",m_terminor_parameter.id());
|
|
|
- return Error_manager(TERMINOR_FORCE_QUIT,NORMAL,description);
|
|
|
+ if (mb_force_quit == true) {
|
|
|
+ char description[255] = {0};
|
|
|
+ sprintf(description, "ternimal is force quit terminal id : %d", m_terminor_parameter.id());
|
|
|
+ return Error_manager(TERMINOR_FORCE_QUIT, NORMAL, description);
|
|
|
}
|
|
|
//判断雷达任务单是否全部完成
|
|
|
- bool tb_laser_complete=true;
|
|
|
- for(int i=0;i<laser_task_vector.size();++i)
|
|
|
- {
|
|
|
- tb_laser_complete &=(TASK_OVER==laser_task_vector[i]->get_statu());
|
|
|
+ bool tb_laser_complete = true;
|
|
|
+ for (int i = 0; i < laser_task_vector.size(); ++i) {
|
|
|
+ tb_laser_complete &= (TASK_OVER == laser_task_vector[i]->get_statu());
|
|
|
}
|
|
|
- if(tb_laser_complete)
|
|
|
- {
|
|
|
+ if (tb_laser_complete) {
|
|
|
break;
|
|
|
}
|
|
|
//计算扫描时间,若超时,并且没有点,则返回错误.
|
|
|
- auto t_end_point=std::chrono::system_clock::now();
|
|
|
- auto duration=std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
|
|
|
- double second=double(duration.count()) *
|
|
|
- std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
|
|
|
- if(second>m_timeout_second)
|
|
|
- {
|
|
|
+ auto t_end_point = std::chrono::system_clock::now();
|
|
|
+ auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
|
|
|
+ double second = double(duration.count()) *
|
|
|
+ std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
|
|
|
+ if (second > m_timeout_second) {
|
|
|
//扫描超时,点云中没有点,则返回错误
|
|
|
- if(scan_cloud.size()==0)
|
|
|
- {
|
|
|
- return Error_manager(TERMINOR_LASER_TIMEOUT,MAJOR_ERROR,"laser scanning timeout");
|
|
|
+ if (scan_cloud.size() == 0) {
|
|
|
+ return Error_manager(TERMINOR_LASER_TIMEOUT, MAJOR_ERROR, "laser scanning timeout");
|
|
|
}
|
|
|
|
|
|
|
|
|
}
|
|
|
- usleep(1000*100);
|
|
|
+ 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)
|
|
|
- {
|
|
|
- if(laser_task_vector[i]!=0)
|
|
|
- {
|
|
|
+ for (int i = 0; i < laser_task_vector.size(); ++i) {
|
|
|
+ if (laser_task_vector[i] != 0) {
|
|
|
delete laser_task_vector[i];
|
|
|
- laser_task_vector[i]=NULL;
|
|
|
+ laser_task_vector[i] = NULL;
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
//第二步,根据区域筛选点云
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
pcl::PassThrough<pcl::PointXYZ> passX;
|
|
|
@@ -425,121 +410,124 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
|
|
|
pcl::PassThrough<pcl::PointXYZ> passZ;
|
|
|
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.setFilterLimits(m_terminor_parameter.area_3d().min_x(), m_terminor_parameter.area_3d().max_x());
|
|
|
passX.filter(*locate_cloud);
|
|
|
|
|
|
passY.setInputCloud(locate_cloud);
|
|
|
passY.setFilterFieldName("y");
|
|
|
- passY.setFilterLimits(m_terminor_parameter.area_3d().min_y(),m_terminor_parameter.area_3d().max_y());
|
|
|
+ passY.setFilterLimits(m_terminor_parameter.area_3d().min_y(), m_terminor_parameter.area_3d().max_y());
|
|
|
passY.filter(*locate_cloud);
|
|
|
|
|
|
passY.setInputCloud(locate_cloud);
|
|
|
passY.setFilterFieldName("z");
|
|
|
- passY.setFilterLimits(m_terminor_parameter.area_3d().min_z(),m_terminor_parameter.area_3d().max_z());
|
|
|
+ passY.setFilterLimits(m_terminor_parameter.area_3d().min_z(), m_terminor_parameter.area_3d().max_z());
|
|
|
passY.filter(*locate_cloud);
|
|
|
- LOG(INFO)<<"筛选点云点数 : "<<locate_cloud->size();
|
|
|
+ LOG(INFO) << "筛选点云点数 : " << locate_cloud->size();
|
|
|
+
|
|
|
+
|
|
|
|
|
|
//第三步,创建测量任务, 进入测量中
|
|
|
- Locate_task* locate_task=new Locate_task();
|
|
|
+ Locate_task *locate_task = new Locate_task();
|
|
|
locate_task->set_locate_cloud(locate_cloud);
|
|
|
locate_task->set_save_path(project_path);
|
|
|
- m_terminor_statu=TERMINOR_MEASURING;
|
|
|
- code=mp_locater->execute_task(locate_task);
|
|
|
- m_measure_information.locate_correct=false;
|
|
|
+ m_terminor_statu = TERMINOR_MEASURING;
|
|
|
+ code = mp_locater->execute_task(locate_task);
|
|
|
+ m_measure_information.locate_correct = false;
|
|
|
//获取测量结果
|
|
|
- Locate_information dj_locate_information=locate_task->get_locate_information();
|
|
|
+ Locate_information dj_locate_information = locate_task->get_locate_information();
|
|
|
//释放locate task
|
|
|
- if(locate_task!=NULL)
|
|
|
- {
|
|
|
+ if (locate_task != NULL) {
|
|
|
delete locate_task;
|
|
|
- locate_task=NULL;
|
|
|
+ locate_task = NULL;
|
|
|
}
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
+ if (code != SUCCESS) {
|
|
|
return code;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
//进入万集雷达流程--------
|
|
|
- Wj_lidar_Task* wj_task=new Wj_lidar_Task();
|
|
|
- code=wj_task->init();
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
+ Wj_lidar_Task *wj_task = new Wj_lidar_Task();
|
|
|
+ code = wj_task->init();
|
|
|
+ if (code != SUCCESS) {
|
|
|
delete wj_task;
|
|
|
return code;
|
|
|
}
|
|
|
wj_command t_wj_command;
|
|
|
- t_wj_command.terminal_id=m_terminor_parameter.id();
|
|
|
- t_wj_command.command_time=std::chrono::steady_clock::now();
|
|
|
- t_wj_command.timeout_in_milliseconds=2000;
|
|
|
+ t_wj_command.terminal_id = m_terminor_parameter.id();
|
|
|
+ t_wj_command.command_time = std::chrono::steady_clock::now();
|
|
|
+ t_wj_command.timeout_in_milliseconds = 2000;
|
|
|
wj_task->set_command(t_wj_command);
|
|
|
- code=mp_wj_lidar->execute_task(wj_task);
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
+ code = mp_wj_lidar->execute_task(wj_task);
|
|
|
+ if (code != SUCCESS) {
|
|
|
delete wj_task;
|
|
|
return code;
|
|
|
}
|
|
|
///万集测量正确,对比两数据
|
|
|
wj_measure_result wj_result;
|
|
|
- code=wj_task->get_result(wj_result);
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
+ code = wj_task->get_result(wj_result);
|
|
|
+ if (code != SUCCESS) {
|
|
|
delete wj_task;
|
|
|
return code;
|
|
|
}
|
|
|
Locate_information wj_locate_information;
|
|
|
- wj_locate_information.locate_x=wj_result.x;
|
|
|
- wj_locate_information.locate_y=wj_result.y;
|
|
|
- wj_locate_information.locate_theta=wj_result.angle;
|
|
|
- wj_locate_information.locate_wheel_base=wj_result.wheel_base;
|
|
|
- wj_locate_information.locate_width=wj_result.width;
|
|
|
+ wj_locate_information.locate_x = wj_result.x;
|
|
|
+ wj_locate_information.locate_y = wj_result.y;
|
|
|
+ wj_locate_information.locate_theta = wj_result.angle;
|
|
|
+ wj_locate_information.locate_wheel_base = wj_result.wheel_base;
|
|
|
+ wj_locate_information.locate_width = wj_result.width;
|
|
|
//对比两个数据
|
|
|
- float offset_x=fabs(dj_locate_information.locate_x-wj_locate_information.locate_x);
|
|
|
- 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_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>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 ");
|
|
|
+ float offset_x = fabs(dj_locate_information.locate_x - wj_locate_information.locate_x);
|
|
|
+ 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_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>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;
|
|
|
- if(offset_angle<2)
|
|
|
- m_measure_information.locate_theta=0.5*wj_locate_information.locate_theta+0.5*dj_locate_information.locate_theta;
|
|
|
+ m_measure_information.locate_x = wj_locate_information.locate_x;
|
|
|
+ m_measure_information.locate_y = wj_locate_information.locate_y;
|
|
|
+ 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);
|
|
|
- float weight_wj=dj_distance/(dj_distance+wj_distance);
|
|
|
- m_measure_information.locate_wheel_base=weight_dj*dj_locate_information.locate_wheel_base+
|
|
|
- weight_wj*wj_locate_information.locate_wheel_base;
|
|
|
- m_measure_information.locate_length=dj_locate_information.locate_length;
|
|
|
- m_measure_information.locate_width=wj_locate_information.locate_width;
|
|
|
- m_measure_information.locate_hight=dj_locate_information.locate_hight;
|
|
|
- m_measure_information.locate_correct=true;
|
|
|
+ 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);
|
|
|
+ float weight_wj = dj_distance / (dj_distance + wj_distance);
|
|
|
+ m_measure_information.locate_wheel_base = weight_dj * dj_locate_information.locate_wheel_base +
|
|
|
+ weight_wj * wj_locate_information.locate_wheel_base;
|
|
|
+ m_measure_information.locate_length = dj_locate_information.locate_length;
|
|
|
+ m_measure_information.locate_width = wj_locate_information.locate_width;
|
|
|
+ m_measure_information.locate_hight = dj_locate_information.locate_hight;
|
|
|
+ m_measure_information.locate_correct = true;
|
|
|
////如果测量正确,检验结果
|
|
|
- if(mp_verify_tool!=NULL) {
|
|
|
+ if (mp_verify_tool != NULL) {
|
|
|
|
|
|
cv::RotatedRect rotated_rect;
|
|
|
rotated_rect.center = cv::Point2f(m_measure_information.locate_x, m_measure_information.locate_y);
|
|
|
rotated_rect.angle = m_measure_information.locate_theta;
|
|
|
- float ext_length=720;
|
|
|
- float robot_width=2650.0;
|
|
|
- float new_length=m_measure_information.locate_length+ext_length*2.0;
|
|
|
- rotated_rect=create_rotate_rect(new_length,robot_width,m_measure_information.locate_theta,
|
|
|
- m_measure_information.locate_x, m_measure_information.locate_y);
|
|
|
- code = mp_verify_tool->verify(rotated_rect,m_measure_information.locate_hight, false);
|
|
|
+ float ext_length = 720;
|
|
|
+ float robot_width = 2650.0;
|
|
|
+ float new_length = m_measure_information.locate_length + ext_length * 2.0;
|
|
|
+ rotated_rect = create_rotate_rect(new_length, robot_width, m_measure_information.locate_theta,
|
|
|
+ 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;
|
|
|
|
|
|
+ } else {
|
|
|
+ delete wj_task;
|
|
|
}
|
|
|
- delete wj_task;
|
|
|
+
|
|
|
return code;
|
|
|
}
|
|
|
|
