Skip to content

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

提交时间队列的修改

parent 972fbffa
Hide whitespace changes
Inline Side-by-side
Showing with 139 additions and 88 deletions
TimeQueueStruct.h 0 → 100644
View file @ 3af93ee4
#pragma once
#include "jfxrosperceiver/det_tracking_array.h"
#include "jfxrosperceiver/InferReses.h"
#define jfx_common_msgs jfxrosperceiver
enum class QueueDataEnum //数据类型的枚举
{
QD_NONE = -1,//默认值，无类型
QD_DET_TRACKING_ARRAY = 0,//对应jfx_common_msgs::det_tracking_array
QD_INFERRESES = 1,//对应jfx_common_msgs::InferReses
QD_ENUM_NUM
};
struct TimeQueueData
{
uint64_t timestamp = 0;//排序的时间
QueueDataEnum dataType = QueueDataEnum::QD_NONE;//数据类型
union {
jfx_common_msgs::det_tracking_array detArray;
jfx_common_msgs::InferReses infRes;
} data;
};
TrackingRosEx.cpp
View file @ 3af93ee4
......@@ -341,6 +341,10 @@ void TrackingRos::Init(ros::NodeHandle& nh)
m_isTrackingRun = true;
m_trackingThread = std::thread(&TrackingRos::ThreadTrackingProcess, this);
m_timeQueue.SetCallbackByEnum(20, QueueDataEnum::QD_INFERRESES, 500,[=](std::vector< TimeQueueDataPtr>& outs) {
TimeQueueProcess(outs);
});
//Eigen::MatrixXf mat(2,2);
//mat(0, 0) = 0.1f;
//mat(0, 1) = 0.2f;
......@@ -388,65 +392,18 @@ void TrackingRos::Init(ros::NodeHandle& nh)
//angle = correct_angle(tests);
//SDK_LOG(SDK_INFO, "test4 correct_angle = %f", angle);
}
void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg)//接受ros发过来的消息
void TrackingRos::TimeQueueProcess(std::vector< TimeQueueDataPtr>& outs)
{
objTrackListPtr objsPtr = std::make_shared<jfx_common_msgs::det_tracking_array>();
objsPtr->array = msg->array;
objsPtr->cloud = msg->cloud;
unsigned long long timestamp = (unsigned long long)msg->cloud.header.stamp.sec * 1000000 + (unsigned long long)msg->cloud.header.stamp.nsec * 1e-3;
//SDK_LOG(SDK_INFO, "TrackingCallBackFunc msg->header.stamp.sec = %d,msg->header.stamp.nsec = %d time = %llu", msg->cloud.header.stamp.sec, msg->cloud.header.stamp.nsec, timestamp);
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 lisar msg count = %d, average rate = %f", countR, (float)countR/3);
countBeginR = GetCurTime();
countR = 0;
}
jfx_common_msgs::det_tracking_array* cloudPtr = nullptr;
jfx_common_msgs::InferReses* msg = nullptr;
uint64_t timestamp = 0;
for (auto iter : outs)
{
std::lock_guard<std::mutex> lock(m_mtx);
if (m_objsCloudQueue.size() >= 20)
{
m_objsCloudQueue.erase(m_objsCloudQueue.begin());
}
m_objsCloudQueue.push_back(objsPtr);
if (iter->dataType == QueueDataEnum::QD_DET_TRACKING_ARRAY)
cloudPtr = &(iter->data.detArray);
if (iter->dataType == QueueDataEnum::QD_INFERRESES)
msg = &(iter->data.infRes);
}
//m_objsQueue.push(objsPtr);
//m_pubCloud.publish(msg->cloud);
}
inline int _GET_VALID_VALUE(unsigned char position, int bits)
{
unsigned int the_mask = (((unsigned int)1) << position);
return (the_mask &= bits) >> position;
}
void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesConstPtr& msg)
{
static int countC = 0;
countC++;
static uint64_t countBeginC = 0;
if (countBeginC == 0)
countBeginC = GetCurTime();
if (GetCurTime() - countBeginC > 3000 * 1000)
{
SDK_LOG(SDK_INFO, "recv camera msg count = %d, average rate = %f", countC, (float)countC / 3);
countBeginC = GetCurTime();
countC = 0;
}
unsigned long long timestamp = (unsigned long long)msg->header.stamp.sec * 1000 + (unsigned long long)msg->header.stamp.nsec * 1e-6;
//SDK_LOG(SDK_INFO, "msg msg->header.stamp.sec = %d,msg->header.stamp.nsec = %d time = %llu", msg->header.stamp.sec, msg->header.stamp.nsec, timestamp);
objTrackListPtr cloudPtr = GetNearestCloudMsg(timestamp);
if (cloudPtr)
{
timestamp = (unsigned long long)cloudPtr->cloud.header.stamp.sec * 1000000 + (unsigned long long)cloudPtr->cloud.header.stamp.nsec * 1e-3;
......@@ -509,12 +466,12 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
float y = 0.0f;
if (m_send_camera_image == 1 && msg->Images.size() == 3)
{
cv::Rect rect(x1,y1, x2 - x1, y2 - y1);
cv::Rect rect(x1, y1, x2 - x1, y2 - y1);
cv::rectangle(sendImage[i], rect, cv::Scalar(0, 0, 255), 5, cv::LINE_8, 0);
}
if (i == 0)
{
if(m_camera_one_center_type == 1 || m_camera_one_center_type == 2)
if (m_camera_one_center_type == 1 || m_camera_one_center_type == 2)
bottom_center_x = x1 + (bottom_center_x / 1920) * (x2 - x1);
if (m_camera_one_center_type == 2)
bottom_center_y = (float(y1) + float(y2)) / 2;
......@@ -537,7 +494,7 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
if (area < MAXAREA)
continue;
if (m_camera_one_center_type == 1 || m_camera_one_center_type == 2)
bottom_center_x = x2 - ( (1280 - bottom_center_x) / 1280) * (x2 - x1) * 0.5;
bottom_center_x = x2 - ((1280 - bottom_center_x) / 1280) * (x2 - x1) * 0.5;
//bottom_center_x = x1;
//bottom_center_y = y2;
get_camera_left_tolidar_loc(bottom_center_x, bottom_center_y, x, y);
......@@ -606,7 +563,7 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
{
if (m_front_camera_len != 0)
{
if ( x*x + y*y < m_front_camera_len* m_front_camera_len && objMsg.type != 4 && objMsg.type != 5)
if (x * x + y * y < m_front_camera_len * m_front_camera_len && objMsg.type != 4 && objMsg.type != 5)
continue;
}
}
......@@ -615,7 +572,7 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
cv::Point2f point(bottom_center_x, bottom_center_y);
cv::circle(sendImage[i], point, (int)5, cv::Scalar(0, 255, 0), 5);
}
if(i == 0 && m_camera_one_center_type != 2)
if (i == 0 && m_camera_one_center_type != 2)
objMsg.x = objMsg.x + (objMsg.l / 2);
int duplicate = 0;
if (_GET_VALID_VALUE((unsigned char)0, m_recvMsgBit))
......@@ -647,7 +604,7 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
{
if (i == 0 || i == 1 || i == 2)
{
if(m_camera_debug_type == 1)
if (m_camera_debug_type == 1)
objMsg.type = 7;
//SDK_LOG(SDK_INFO, "camera 1,2 x = %f,y = %f", objMsg.x, objMsg.y);
}
......@@ -680,6 +637,71 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
m_objsQueue.push(objsPtr);
//if(cloudPtr)
// m_pubCloud.publish(cloudPtr->cloud);
}
void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg)//接受ros发过来的消息
{
TimeQueueDataPtr tqdPtr = std::make_shared<TimeQueueData>();
//objTrackListPtr objsPtr = std::make_shared<jfx_common_msgs::det_tracking_array>();
tqdPtr->dataType = QueueDataEnum::QD_DET_TRACKING_ARRAY;
objsPtr->data.detArray.array = msg->array;
objsPtr->data.detArray.cloud = msg->cloud;
tqPtr->timestamp = (unsigned long long)msg->cloud.header.stamp.sec * 1000000 + (unsigned long long)msg->cloud.header.stamp.nsec * 1e-3;
//SDK_LOG(SDK_INFO, "TrackingCallBackFunc msg->header.stamp.sec = %d,msg->header.stamp.nsec = %d time = %llu", msg->cloud.header.stamp.sec, msg->cloud.header.stamp.nsec, timestamp);
m_timeQueue.PushQueueData(tqdPtr);
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 lisar msg count = %d, average rate = %f", countR, (float)countR/3);
countBeginR = GetCurTime();
countR = 0;
}
//{
// std::lock_guard<std::mutex> lock(m_mtx);
// if (m_objsCloudQueue.size() >= 20)
// {
// m_objsCloudQueue.erase(m_objsCloudQueue.begin());
// }
// m_objsCloudQueue.push_back(objsPtr);
//}
//m_objsQueue.push(objsPtr);
//m_pubCloud.publish(msg->cloud);
}
inline int _GET_VALID_VALUE(unsigned char position, int bits)
{
unsigned int the_mask = (((unsigned int)1) << position);
return (the_mask &= bits) >> position;
}
void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesConstPtr& msg)
{
static int countC = 0;
countC++;
static uint64_t countBeginC = 0;
if (countBeginC == 0)
countBeginC = GetCurTime();
if (GetCurTime() - countBeginC > 3000 * 1000)
{
SDK_LOG(SDK_INFO, "recv camera msg count = %d, average rate = %f", countC, (float)countC / 3);
countBeginC = GetCurTime();
countC = 0;
}
TimeQueueDataPtr tqdPtr = std::make_shared<TimeQueueData>();
tqdPtr->dataType = QueueDataEnum::QD_INFERRESES;
tqdPtr->timestamp = (unsigned long long)msg->header.stamp.sec * 1000 + (unsigned long long)msg->header.stamp.nsec * 1e-6;
tqdPtr->data.infRes = *msg;
m_timeQueue.PushQueueData(tqdPtr);
//SDK_LOG(SDK_INFO, "msg msg->header.stamp.sec = %d,msg->header.stamp.nsec = %d time = %llu", msg->header.stamp.sec, msg->header.stamp.nsec, timestamp);
}
objTrackListPtr TrackingRos::GetNearestCloudMsg(unsigned long long timestamp)
......
TrackingRosEx.h
View file @ 3af93ee4
......@@ -15,6 +15,7 @@
//#include "TrackingMsg.h"
#include "ros/ros.h"
#include "coordinate_convert.h"
#include "TimeQueueObj.h"
using objTrackListPtr = std::shared_ptr<jfx_common_msgs::det_tracking_array>;
......@@ -72,4 +73,8 @@ public:
int m_right_left_camera_y_len = 7;//左右相机的过滤宽度米数
int m_front_camera_len = 0;//-前相机过滤的距离米数,0是不过滤，70就是过滤70米，但不过滤行人和机动车
int m_send_camera_image = 0;//是否发送相机图像，0是不发送，1是发送
void TimeQueueProcess(std::vector< TimeQueueDataPtr>& outs);
TimeQueueObj m_timeQueue;
};
common/TimeQueueObj.cpp
View file @ 3af93ee4
......@@ -10,10 +10,11 @@ TimeQueueObj::TimeQueueObj()
}
int TimeQueueObj::SetCallbackByEnum(int max_num, QueueDataEnum base, TimeQueueDataProcessCallback cb)
int TimeQueueObj::SetCallbackByEnum(int max_num, QueueDataEnum base, uint64_t interval, TimeQueueDataProcessCallback cb)
{
m_max_num = max_num;
m_baseEnum = base;
m_baseInterval = interval;
m_cb = cb;
m_isStop = false;
......@@ -45,6 +46,8 @@ void TimeQueueObj::PushQueueData(TimeQueueDataPtr& data)
}
m_totelCount++;
m_enumCount[data->dataType]++;
if (data->dataType == m_baseEnum)
m_baseTimestmap = data->timestamp;//更新基准类型的最新时间戳，为了判断时间差用
if (m_max_num > 0 && m_totelCount > m_max_num)
{
......@@ -79,22 +82,31 @@ int TimeQueueObj::GetQueueData(std::vector<TimeQueueDataPtr>& outs)
});
outs.clear();
int num = 0;
if (m_enumCount[m_baseEnum] <= 0)
{
return -1;
}
std::list< TimeQueueDataPtr>::iterator pos = nullptr;
TimeQueueDataPtr merge[static_cast<int>(QueueDataEnum::QD_ENUM_NUM)];
uint64_t detaT[static_cast<int>(QueueDataEnum::QD_ENUM_NUM)];
for (int i = 0; i < static_cast<int>(QueueDataEnum::QD_ENUM_NUM); i++)
{
merge[i] = nullptr;
detaT[i] = 1000;
}
for (auto iter = m_timeQueue.begin(); iter != m_timeQueue.end(); iter++)
{
if (merge[(*iter)->dataType] == nullptr)
num++;
merge[(*iter)->dataType] = *iter;
if ((*iter)->dataType == m_baseEnum && num = static_cast<int>(QueueDataEnum::QD_ENUM_NUM))
if (labs(m_baseTimestmap - (*iter)->timestamp) < std::min(m_baseInterval, detaT[(*iter)->dataType]))
{
if(merge[(*iter)->dataType] == nullptr)
num++;
detaT[(*iter)->dataType] = labs(m_baseTimestmap - (*iter)->timestamp);
merge[(*iter)->dataType] = *iter;
pos = iter;
break;
}
}
if (pos)
if (num == static_cast<int>(QueueDataEnum::QD_ENUM_NUM))
{
for (int i = 0; i < static_cast<int>(QueueDataEnum::QD_ENUM_NUM); i++)
outs.push_back(merge[i]);
......
common/TimeQueueObj.h
View file @ 3af93ee4
......@@ -9,23 +9,7 @@
#include <functional>
#ifdef _CAR_SENSING_
enum class QueueDataEnum //数据类型的枚举
{
QD_NONE = -1,//默认值，无类型
QD_DET_TRACKING_ARRAY = 0,//对应jfx_common_msgs::det_tracking_array
QD_INFERRESES = 1,//对应jfx_common_msgs::InferReses
QD_ENUM_NUM
};
struct TimeQueueData
{
uint64_t timestamp = 0;//排序的时间
QueueDataEnum dataType = QueueDataEnum::QD_NONE;//数据类型
union {
jfx_common_msgs::det_tracking_array detArray;
jfx_common_msgs::InferReses infRes;
} data;
};
#include "TimeQueueStruct.h"
#else
enum class QueueDataEnum //数据类型的枚举
{
......@@ -37,7 +21,7 @@ enum class QueueDataEnum //数据类型的枚举
struct TimeQueueData
{
uint64_t timestamp = 0;//排序的时间
uint64_t timestamp = 0;//排序的时间,以毫秒为单位
QueueDataEnum dataType = QueueDataEnum::QD_NONE;//数据类型
union {
uint64_t value;
......@@ -58,7 +42,7 @@ public:
TimeQueueObj();
~TimeQueueObj() {}
int SetCallbackByEnum(int max_num,QueueDataEnum base, TimeQueueDataProcessCallback cb);
int SetCallbackByEnum(int max_num,QueueDataEnum base,uint64_t interval, TimeQueueDataProcessCallback cb);
void PushQueueData(TimeQueueDataPtr& data);
......@@ -81,4 +65,6 @@ public:
int m_enumCount[static_cast<int>(QueueDataEnum::QD_ENUM_NUM)];//记录每个类型的数量
int m_totelCount = 0;//记录总数量
uint64_t m_baseTimestmap = 0;//base数据最新的时间戳
uint64_t m_baseInterval = 0;//base数据判断的时间间隔
};
\ No newline at end of file
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