修改合并bug

terminor/terminal_command_executor.cpp

@@ -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);