Skip to content

J
jfx_kalman_filter_src
Commit 73371870 authored by oscar's avatar oscar
Browse files
Options

提交去除打印信息

parent 56063621
Hide whitespace changes
Inline Side-by-side
Showing with 19 additions and 9 deletions
TrackingRos.cpp
View file @ 73371870
......@@ -87,7 +87,17 @@ void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_array
objsPtr->array = msg->array;
//objsPtr->cloud = msg->cloud;
SDK_LOG(SDK_INFO, "recv msg size = %d",msg->array.size());
static int countR = 0;
countR++;
static uint64_t countBeginR = 0;
if (countBeginR == 0)
countBeginR = GetCurTime();
if (GetCurTime() - countBeginR > 3000 * 1000)
{
SDK_LOG(SDK_INFO, "recv msg count = %d, average rate = %f", countR, (float)countR/3);
countBeginR = GetCurTime();
countR = 0;
}
//for (auto& item : msg->array)
//{
// objTrackPtr obj = std::make_shared<objTrack>();
......@@ -125,7 +135,7 @@ void TrackingRos::ThreadTrackingProcess()
bool ret = m_objsQueue.timeout_pop(objsPtr, 2000);
if (ret)
{
SDK_LOG(SDK_INFO, "get objs size = %d",objsPtr->array.size());
//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++)
{
......@@ -147,7 +157,7 @@ void TrackingRos::ThreadTrackingProcess()
uint64_t begin = GetCurTime();
m_tracker.Run(input, detectionsId, updateId, lostId);
uint64_t rTime = GetCurTime() - begin;
SDK_LOG(SDK_INFO, "m_tracker.Run time = %llu", rTime);
//SDK_LOG(SDK_INFO, "m_tracker.Run time = %llu", rTime);
static int count = 0;
count++;
static uint64_t totelTime = 0;
......@@ -165,7 +175,7 @@ void TrackingRos::ThreadTrackingProcess()
std::map<uint64_t, std::shared_ptr<Track3D> >& trackers = m_tracker.GetStates();
SDK_LOG(SDK_INFO, "run before size = %d, after size = %d,",objsPtr->array.size(), trackers.size());
//SDK_LOG(SDK_INFO, "run before size = %d, after size = %d,",objsPtr->array.size(), trackers.size());
jfx_common_msgs::det_tracking_array sendObjs = {};
jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
sendBoxes.header.frame_id = "Pandar64";
......@@ -189,10 +199,10 @@ void TrackingRos::ThreadTrackingProcess()
obj.v_y = data[8];
obj.v_z = data[9];
obj.obj_id = iter.first;
SDK_LOG(SDK_INFO, "frame = %llu, id = %llu, input(%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,
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,
obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,obj.rot_y, iter.second->GetProb());
//SDK_LOG(SDK_INFO, "frame = %llu, id = %llu, input(%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,
// 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,
// obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,obj.rot_y, iter.second->GetProb());
float gx = 0.0f;
float gy = 0.0f;
CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);
......@@ -254,7 +264,7 @@ void TrackingRos::ThreadTrackingProcess()
sendObjs.array.emplace_back(obj);
}
//sendObjs.cloud = objsPtr->cloud;
SDK_LOG(SDK_INFO, "calculate objs size = %d", sendObjs.array.size());
//SDK_LOG(SDK_INFO, "calculate objs size = %d", sendObjs.array.size());
m_pub.publish(sendObjs);
m_pubBoundingBoxes.publish(sendBoxes);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment