提交代码

TrackingRos.cpp

@@ -9,25 +9,27 @@
 
 #define _PI_ 3.1415926
 
-void to360(float& rot_y,float lidar_x_angle)
+float to360(float rot_y,float lidar_x_angle)
 {
-    while (rot_y >= _PI_)
+    float angle = rot_y;
+    while (angle >= _PI_)
     {
-        rot_y -= _PI_ * 2;
+        angle -= _PI_ * 2;
     }
-    while (rot_y < -_PI_)
+    while (angle < -_PI_)
     {
-        rot_y += _PI_ * 2;
+        angle += _PI_ * 2;
     }
     float tmp_angle = 0.0f;
-    float car_angle = rot_y / _PI_ * 180.0f;
+    float car_angle = angle / _PI_ * 180.0f;
     tmp_angle = lidar_x_angle - car_angle;
     tmp_angle += 360;
     while (tmp_angle > 360)
     {
         tmp_angle -= 360;
     }
-    rot_y = tmp_angle;
+    angle = tmp_angle;
+    return angle;
 }
 
 void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>& trans, float& gx, float& gy)
@@ -272,7 +274,9 @@ void TrackingRos::ThreadTrackingProcess()
                     }
                 }
                 //修正rot_y值,变为角度
-                to360(obj.rot_y, m_lidar_x_angle);
+                float rot_y_angle = obj.rot_y;
+                obj.rot_y = to360(obj.rot_y, m_lidar_x_angle);
+                rot_y_angle = obj.rot_y / 180 * 3.14159265;
 
                 jsk_recognition_msgs::BoundingBox box = {};
                 box.label = obj.obj_id;
@@ -284,7 +288,7 @@ void TrackingRos::ThreadTrackingProcess()
                 box.pose.position.x = obj.x;
                 box.pose.position.y = obj.y;
                 box.pose.position.z = obj.z;
-                Eigen::AngleAxisd V3(obj.rot_y/180*3.14159265, Eigen::Vector3d(0, 0, 1));
+                Eigen::AngleAxisd V3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
                 Eigen::Quaterniond R_quat(V3);
                 box.pose.orientation.x = R_quat.x();
                 box.pose.orientation.y = R_quat.y();