提交修改

TrackingRos.cpp

@@ -12,14 +12,6 @@
 float to360(float rot_y,float lidar_x_angle)
 {
     float angle = rot_y;
-    while (angle >= _PI_)
-    {
-        angle -= _PI_ * 2;
-    }
-    while (angle < -_PI_)
-    {
-        angle += _PI_ * 2;
-    }
     //SDK_LOG(SDK_INFO, "rot_y = %f, angle = %f", rot_y, angle);
     float tmp_angle = 0.0f;
     float car_angle = angle / _PI_ * 180.0f;
@@ -276,6 +268,14 @@ void TrackingRos::ThreadTrackingProcess()
                     }
                 }
                 //修正rot_y值,变为角度
+                while (obj.rot_y >= _PI_)
+                {
+                    obj.rot_y -= _PI_ * 2;
+                }
+                while (obj.rot_y < -_PI_)
+                {
+                    obj.rot_y += _PI_ * 2;
+                }
                 float rot_y_angle = obj.rot_y;
                 obj.rot_y = to360(obj.rot_y, m_lidar_x_angle);
                 //SDK_LOG(SDK_INFO, "rot_y = %f, rot_y_angle = %f", rot_y_angle, rot_y_a);