加入距离过滤物体的类型，在距离范围内的做type统计然后一直使用那个时候的类型

BaseTracker/Track3D.cpp

@@ -552,3 +552,34 @@ float Track3D::GetRotY(float rot_y)
     while(rot_y_real >= _PI_ * 2) rot_y_real -= _PI_ * 2;
     return rot_y_real;
 }
+int Track3D::UpdateTypeNameInfo(int type, std::string name)
+{
+    if(type == 0)
+        return -1;
+    if(m_typeNameList.find(type) == m_typeNameList.end())
+    {
+        TypeNameInfo info = {};
+        info.num = 0;
+        info.name = name;
+    }
+    m_typeNameList[type].num++;
+    return 0;    
+}
+int Track3D::GetTypeNameInfo(int& type,std::string& name)
+{
+    if(m_typeNameList.empty())
+    {
+        return -1;
+    }
+    int max = 0;
+    for(auto& iter : m_typeNameList)
+    {
+        if(iter.second.num > max)
+        {
+            max = iter.second.num;
+            type = iter.first;
+            name = iter.second.name;
+        }
+    }    
+    return 0;
+}
\ No newline at end of file

BaseTracker/Track3D.h

@@ -74,6 +74,15 @@ public:
     int UpdateColorInfo(std::string color);
     int GetColorInfo(std::string& color);
     float GetRotY(float rot_y);
+
+    int UpdateTypeNameInfo(int type, std::string name);
+    int GetTypeNameInfo(int& type,std::string& name);
+    struct TypeNameInfo
+    {
+        int num = 0;
+        std::string name;
+    };
+    std::map<int,TypeNameInfo> m_typeNameList;
 };
 
 double correct_angle(std::vector<point2d>& points);

TrackingRos.cpp

@@ -424,6 +424,17 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     if(config["cloud_leafsize"])
 	m_cloud_leafsize = config["cloud_leafsize"].as<double>();
 
+    m_filter_distance_type = 0;
+    if(config["filter_distance_type"])
+        m_filter_distance_type = config["filter_distance_type"].as<int>();
+    if(m_filter_distance_type == 1)
+    {
+        if(config["filter_distance_min"])
+            m_filter_distance_min = config["filter_distance_min"].as<float>();
+        if(config["filter_distance_max"])
+            m_filter_distance_max = config["filter_distance_max"].as<float>();            
+    }
+
     m_objsQueue.set_max_num_items(2);
 
     m_isTrackingRun = true;
@@ -755,9 +766,37 @@ void TrackingRos::ThreadTrackingProcess()
                         //SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f", obj.Lat, obj.Long);
                     }
                 }
+                //加入物体type的距离过滤逻辑
+                if(m_filter_distance_type == 1 && m_cross_center_lon != 0 && m_clear_distance != 0)
+                {
+                    double s12, azi1, azi2;
+                    geod.Inverse(obj.Lat, obj.Long, m_cross_center_lat, m_cross_center_lon, s12, azi1, azi2);
+                    if(s12 >= m_filter_distance_min && s12 < m_filter_distance_max)
+                    {
+                        iter.second->UpdateTypeNameInfo(obj.type,obj.name);
+                        int filter_type = 0;
+                        std::string filter_name;
+                        int f_ret = iter.second->GetTypeNameInfo(filter_type,filter_name);
+                        if(f_ret == 0)
+                        {
+                            obj.type = filter_type;
+                            obj.name = filter_name;
+                        }
+                    }
+                    else
+                    {
+                        int filter_type = 0;
+                        std::string filter_name;
+                        int f_ret = iter.second->GetTypeNameInfo(filter_type,filter_name);
+                        if(f_ret == 0)
+                        {
+                            obj.type = filter_type;
+                            obj.name = filter_name;
+                        }                        
+                    }
+                }
                 if (is_need_send == 1)
                 {
-		    /* for test 
                     if(m_cross_center_lon != 0 && m_clear_distance != 0)
                     {
                         double s12, azi1, azi2;
@@ -765,7 +804,6 @@ void TrackingRos::ThreadTrackingProcess()
                         if(s12 > m_clear_distance)
                             continue;
                     }
-		    */
                     //暂存rot_y
                     float rot_y_angle_temp = obj.rot_y;
 

TrackingRos.h

@@ -74,4 +74,8 @@ public:
     double m_clear_distance = 0;
     int m_cloud_filter = 0;
     double m_cloud_leafsize = 0.2;
+
+    int m_filter_distance_type = 0;//是否打开距离更新type的逻辑,1是打开，0是关闭
+    float m_filter_distance_min = 0;//过滤距离最小值，单位m
+    float m_filter_distance_max = 300;//过滤距离最大值，单位m
 };