add acc to sendmsg

BaseTracker/Track3D.cpp

@@ -455,3 +455,28 @@ void Track3D::MeasureIouData(const std::vector<float>& input, std::vector<float>
     out.push_back(input[4]);
 }
 
+double Track3D::get_acceleration_x(double time_interval){
+    if(time_interval == 0 ){
+        return 0;
+    }
+    int n = m_points.size();
+    if(n < 3){
+        return 0;
+    }
+    double v_0 = (m_points[n-1].x - m_points[n-2].x) / time_interval;
+    double v_1 = (m_points[n-2].x - m_points[n-3].x) / time_interval;
+    return (v_0 - v_1) / time_interval;
+}
+
+double Track3D::get_acceleration_y(double time_interval){
+    if(time_interval == 0 ){
+        return 0;
+    }
+    int n = m_points.size();
+    if(n < 3){
+        return 0;
+    }
+    double v_0 = (m_points[n-1].y - m_points[n-2].y) / time_interval;
+    double v_1 = (m_points[n-2].y - m_points[n-3].y) / time_interval;
+    return (v_0 - v_1) / time_interval;
+}
\ No newline at end of file

BaseTracker/Track3D.h

@@ -53,6 +53,9 @@ public:
     virtual int GetIouData(std::vector<float>& data, int& obj_type);
     static void MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type);
 
+    double get_acceleration_x(double time_interval);
+    double get_acceleration_y(double time_interval);
+
     trackOjbPtr m_obj = nullptr;
 
     std::vector<point2d> m_points;//保存最近的5个移动的点位

TrackingRos.cpp

@@ -554,6 +554,8 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.v_y = data[8];
                         obj.v_z = data[9];
                         obj.obj_id = iter.first;
+                        obj.acc_x = iter.second->get_acceleration_x(0.1);
+                        obj.acc_y = iter.second->get_acceleration_y(0.1);
 
 #ifdef _ABUZHABI_
                         double lon = 0.0f;
@@ -623,6 +625,8 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.v_y = data[8];
                         obj.v_z = data[9];
                         obj.obj_id = iter.first;
+                        obj.acc_x = iter.second->get_acceleration_x(0.1);
+                        obj.acc_y = iter.second->get_acceleration_y(0.1);
 #ifdef _ABUZHABI_
 						double lon = 0.0f;
                         double lat = 0.0f;