提交代码

TrackingRos.cpp

@@ -20,15 +20,18 @@ float to360(float rot_y,float lidar_x_angle)
     {
         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;
     tmp_angle = lidar_x_angle - car_angle;
+    SDK_LOG(SDK_INFO, "car_angle = %f, tmp_angle = %f,lidar_x_angle = %d", car_angle, tmp_angle, lidar_x_angle);
     tmp_angle += 360;
     while (tmp_angle > 360)
     {
         tmp_angle -= 360;
     }
     angle = tmp_angle;
+    SDK_LOG(SDK_INFO, "angle = %f,angle_y = %f", angle, angle/180*3.1415926);
     return angle;
 }
 
@@ -107,6 +110,9 @@ void TrackingRos::Init(ros::NodeHandle& nh)
 
     SDK_LOG(SDK_INFO, "data = %f,%f,%f,%f", m[0], m[1], m[2], m[3]);
 
+    float rot_y = 1.427666;
+    float rot_y_a = to360(rot_y, m_lidar_x_angle);
+
 }
 
 void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg)//接受ros发过来的消息