提交距离检查逻辑，距离太远的就不发送了。

TrackingRos.cpp

@@ -20,7 +20,9 @@
 #include "SendMsg.h"
 #include "std_msgs/String.h"
 #endif
+#include <GeographicLib/Geodesic.hpp>
 
+using namespace GeographicLib;
 
 float to360(float rot_y,float lidar_x_angle)
 {
@@ -390,6 +392,12 @@ void TrackingRos::Init(ros::NodeHandle& nh)
 #ifdef _SEND_ABUZHABI_
     m_pubAbuzhabi = nh.advertise<std_msgs::String>("/abuzhabi_msg",100);
 #endif
+    if(config["CROSS_CENTER_LON"])
+        m_cross_center_lon = config["CROSS_CENTER_LON"].as<double>();
+    if(config["CROSS_CENTER_LAT"])
+        m_cross_center_lat = config["CROSS_CENTER_LAT"].as<double>();
+    if(config["CLEAR_DISTANCE"])
+        m_clear_distance = config["CLEAR_DISTANCE"].as<double>();
 
     m_objsQueue.set_max_num_items(2);
 
@@ -493,6 +501,7 @@ void TrackingRos::ThreadTrackingProcess()
 {
     SDK_LOG(SDK_INFO, "TrackingRos::ThreadTrackingProcess begin");
     uint64_t beginT = GetCurTime();
+    Geodesic geod = Geodesic::WGS84();    
     while (m_isTrackingRun)
     {
         objTrackListPtr objsPtr;
@@ -723,6 +732,13 @@ void TrackingRos::ThreadTrackingProcess()
                 }
                 if (is_need_send == 1)
                 {
+                    if(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_clear_distance)
+                            continue;
+                    }
                     //暂存rot_y
                     float rot_y_angle_temp = obj.rot_y;
 

TrackingRos.h

@@ -68,4 +68,8 @@ public:
     std::string m_nodeName;//点位信息，现在就是10-1，1-1等信息
     YAML::Node m_config;//配置的yaml文件
     int mark_type_text = 0;//0-type,1-object_id,2-object-type,3-text...
+
+    double m_cross_center_lon = 0;
+    double m_cross_center_lat = 0;
+    double m_clear_distance = 0;
 };