提交修改

TrackingRos.cpp

@@ -77,7 +77,7 @@ void TrackingRos::Init(ros::NodeHandle& nh)
 
 
     ADD_DEF_LOG(1, "tracking.csv");
-    SDK_IDX_LOG(1, SDK_INFO, "date,frame,id,input_radian,input_angle,predict_radian,predict_angle,output_radian,output_angle,input_x,input_y,input_z,input_l,input_w,input_h,predict_x,predict_y,predict_z,predict_l,predict_w,preditc_h,output_x,output_y,output_z,output_l,output_w,output_h,output_rot_y,v_x,v_y,v_z,lat,lon");
+    SDK_IDX_LOG(1, SDK_INFO, "date,frameNum,time,id,input_radian,input_angle,predict_radian,predict_angle,output_radian,output_angle,input_x,input_y,input_z,input_l,input_w,input_h,predict_x,predict_y,predict_z,predict_l,predict_w,preditc_h,output_x,output_y,output_z,output_l,output_w,output_h,output_rot_y,v_x,v_y,v_z,lat,lon");
     //m_coordinate.Init(value[0][0], value[0][1], value[0][2], value[0][3], value[1][0], value[1][1]);
 
     m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>("/tracking_res", 100);
@@ -159,6 +159,7 @@ void TrackingRos::ThreadTrackingProcess()
         bool ret = m_objsQueue.timeout_pop(objsPtr, 2000);
         if (ret)
         {
+            m_frameNum++;
             //SDK_LOG(SDK_INFO, "get objs size = %d",objsPtr->array.size());
             std::vector< std::vector<float> > input;
             for (int i = 0; i < objsPtr->array.size(); i++)
@@ -283,13 +284,13 @@ void TrackingRos::ThreadTrackingProcess()
                 if (iter.second->GetPredictData(predict) == 0)
                 {
                     //SDK_LOG(SDK_INFO, "frame = %llu, id = %llu, input(%f,%f,%f,%f,%f,%f,%f) predict(%f,%f,%f,%f,%f,%f,%f) output(%f,%f,%f,%f,%f,%f,%f) prob = %f",
-                    //    obj.frame, iter.first, objsPtr->array[updateId[iter.first]].x, objsPtr->array[updateId[iter.first]].y, objsPtr->array[updateId[iter.first]].z,
+                    //    obj.frame, iter.first, objsPtr->array[updateId[iter.first]].x, obj    sPtr->array[updateId[iter.first]].y, objsPtr->array[updateId[iter.first]].z,
                     //    objsPtr->array[updateId[iter.first]].l, objsPtr->array[updateId[iter.first]].w, objsPtr->array[updateId[iter.first]].h, objsPtr->array[updateId[iter.first]].rot_y,
                     //    predict[0], predict[1], predict[2], predict[4], predict[5], predict[6], predict[3],
                     //    obj.x, obj.y, obj.z, obj.l, obj.w, obj.h, obj.rot_y, iter.second->GetProb());
                     //date,frame,id,input_radian,input_angle,predict_radian,predict_angle,output_radian,output_angle,input_x,input_y,input_z,input_l,input_w,input_h,predict_x,predict_y,predict_z,predict_l,predict_w,preditc_h,output_x,output_y,output_z,output_l,output_w,output_h,output_rot_y,v_x,v_y,v_z,lat,lon
-                    SDK_IDX_LOG(1, SDK_INFO, "%s,%llu,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.10f,%.10f",
-                        GetTimeStr(obj.frame).c_str(), obj.frame, iter.first, input_obj.rot_y, input_obj.rot_y*180/_PI_, predict[3], predict[3]*180/_PI_, rot_y_angle, rot_y_angle*180/_PI_,
+                    SDK_IDX_LOG(1, SDK_INFO, "%s,%d,%llu,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.10f,%.10f",
+                        GetTimeStr(obj.frame).c_str(),m_frameNum, obj.frame, iter.first, input_obj.rot_y, input_obj.rot_y*180/_PI_, predict[3], predict[3]*180/_PI_, rot_y_angle, rot_y_angle*180/_PI_,
                         input_obj.x, input_obj.y, input_obj.z, input_obj.l, input_obj.w, input_obj.h,
                         predict[0], predict[1], predict[2], predict[4], predict[5], predict[6],
                         obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,obj.rot_y,obj.v_x,obj.v_y,obj.v_z,obj.Lat,obj.Long);

TrackingRos.h

@@ -45,5 +45,7 @@ public:
 
     std::vector<std::vector<float>> m_trans;
 
+    int m_frameNum = 0;
+
     float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
 };