|
|
@@ -482,21 +482,20 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
|
|
|
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 > 2 || offset_wheel_base > 200 || offset_width > 200) {
|
|
|
- LOG(WARNING) << "chekc failed. (offset x>100, y>100, angle>2, wheel_base>200, width>200): " << offset_x
|
|
|
+ if (offset_x > 100 || offset_y > 150 || offset_angle > 5 || offset_wheel_base > 250 || offset_width > 250) {
|
|
|
+ LOG(WARNING) << "chekc failed. (offset x>100, y>150, angle>5, wheel_base>250, width>250): " << offset_x
|
|
|
<< " " << offset_y << " " << offset_angle << " " << offset_wheel_base << " " << offset_width;
|
|
|
return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
|
|
|
}
|
|
|
+ ///根据sigmod函数计算角度权重
|
|
|
+ double angle_weight=1.0-1.0/(1.0+exp(-offset_angle/1.8));
|
|
|
///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以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;
|
|
|
- else
|
|
|
- m_measure_information.locate_theta =
|
|
|
- 0.1 * wj_locate_information.locate_theta + 0.9 * dj_locate_information.locate_theta;
|
|
|
+
|
|
|
+ m_measure_information.locate_theta =angle_weight * wj_locate_information.locate_theta +
|
|
|
+ (1.0-angle_weight) * 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);
|
