提交代码

BaseTracker/BaseTracker.h

@@ -16,7 +16,7 @@ class BaseTracker
 public:
 	BaseTracker() {}
 
-	int Run(const std::vector<std::vector<float> >& detections,std::vector<uint64_t>& detectionsId,std::map<uint64_t,int>& addId,std::vector<uint64_t>& lostId);
+	int Run(const std::vector<std::vector<float> >& detections,std::vector<uint64_t>& detectionsId,std::map<uint64_t,int>& updateId,std::vector<uint64_t>& lostId);
 
 	std::map<uint64_t, std::shared_ptr<T> >& GetStates();
 
@@ -38,7 +38,7 @@ public:
 
 
 template<class T>
-int BaseTracker<T>::Run(const std::vector<std::vector<float> >& detections, std::vector<uint64_t>& detectionsId, std::map<uint64_t, int>& addId, std::vector<uint64_t>& lostId)
+int BaseTracker<T>::Run(const std::vector<std::vector<float> >& detections, std::vector<uint64_t>& detectionsId, std::map<uint64_t, int>& updateId, std::vector<uint64_t>& lostId)
 {
     /*** Predict internal tracks from previous frame ***/
     for (auto& track : m_tracker)
@@ -64,6 +64,7 @@ int BaseTracker<T>::Run(const std::vector<std::vector<float> >& detections, std:
         const auto& id = match.first;
         m_tracker[id]->Update(detections[match.second]);
         detectionsId[match.second] = id;
+        updateId[id] = match.second;
     }
 
     /*** Create new tracks for unmatched detections ***/
@@ -74,7 +75,8 @@ int BaseTracker<T>::Run(const std::vector<std::vector<float> >& detections, std:
         // Create new track and generate new ID
         uint64_t newId = ++m_countId;
         m_tracker[newId] = trackPtr;
-        addId[newId] = det;
+        detectionsId[det] = newId;
+        updateId[newId] = det;
     }
 
     /*** Delete lose tracked tracks ***/

BaseTracker/Track3D.h

@@ -3,7 +3,9 @@
 #include <vector>
 #include "BaseTrack.h"
 #include "kalman_filter.h"
+#include "jfx_common_msgs/det_tracking.h"
 
+using trackOjbPtr = std::shared_ptr< jfx_common_msgs::det_tracking>;
 
 class Track3D : public BaseTrack
 {
@@ -14,4 +16,6 @@ public:
 
     virtual float CalculateIou(std::vector<float>& data);
 
+
+    trackOjbPtr m_obj = nullptr;
 };

TrackingRos.cpp

+#include "TrackingRos.h"
+#include "Component.h"
+#include <sensor_msgs/PointCloud2.h>
+
+TrackingRos::~TrackingRos()
+{
+    if (m_isTrackingRun == true)
+    {
+        m_isTrackingRun = false;
+        if (m_trackingThread.joinable()) {
+            m_trackingThread.join();
+        }
+    }
+}
+
+void TrackingRos::Init(ros::NodeHandle& nh)
+{
+    m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>("/tracking_res", 100);
+    
+    m_objsQueue.set_max_num_items(2);
+
+    m_isTrackingRun = true;
+    m_trackingThread = std::thread(&TrackingRos::ThreadTrackingProcess, this);
+}
+
+void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg)//接受ros发过来的消息
+{
+    objTrackListPtr objsPtr = std::make_shared<jfx_common_msgs::det_tracking_array>();
+    objsPtr->array = msg->array;
+    objsPtr->cloud = msg->cloud;
+    //for (auto& item : msg->array)
+    //{
+    //    objTrackPtr obj = std::make_shared<objTrack>();
+    //    obj->time = item.frame;//时间戳
+    //    obj->prob = item.score;//置信度
+    //    obj->name = item.name;//类型名称，其中包括car truck bus 自行车，行人
+    //    obj->type = item.type;//类型值，基本上与上面的类型对应
+    //    obj->licensePlateNumber = item.license_plate_number;//车牌号，没有就是：NONE
+    //    obj->vehicleColor = item.color_name;//车颜色，没有就是：NONE
+    //    obj->objH = item.h;//检测出车的3D框高
+    //    obj->objW = item.w;//检测出车的3D框宽
+    //    obj->objL = item.l;//检测出车的3D框长
+    //    obj->nowX = item.x;//检测出车的3D框中心点X
+    //    obj->nowY = item.y;//检测出车的3D框中心点Y
+    //    obj->nowZ = item.z;//检测出车的3D框中心点Z
+    //    obj->lat = item.Lat;//车所在位置的纬度
+    //    obj->lon = item.Long;//车所在位置的经度
+    //    obj->heading = item.rot_y;//车运行的朝向，北为0，顺时针0-360
+    //    obj->id = item.obj_id;//物体id
+    //    objsPtr->objs.emplace_back(obj);
+    //}
+    //objsPtr->cloud = msg->cloud;
+    m_objsQueue.push(objsPtr);
+}
+
+void TrackingRos::ThreadTrackingProcess()
+{
+    SDK_LOG(SDK_INFO, "TrackingRos::ThreadTrackingProcess begin");
+    uint64_t beginT = GetCurTime();
+    while (m_isTrackingRun)
+    {
+        objTrackListPtr objsPtr;
+        bool ret = m_objsQueue.timeout_pop(objsPtr, 2000);
+        if (ret)
+        {
+            std::vector< std::vector<float> > input;
+            for (int i = 0; i < objsPtr->array.size(); i++)
+            {
+                jfx_common_msgs::det_tracking& obj = objsPtr->array[i];
+                std::vector<float> data;
+                data.push_back(obj.x);
+                data.push_back(obj.y);
+                data.push_back(obj.z);
+                data.push_back(obj.w);
+                data.push_back(obj.l);
+                data.push_back(obj.h);
+                double angle = obj.rot_y / 180 * 3.1415926;
+                data.push_back(angle);
+                input.emplace_back(data);
+            }
+            std::vector<uint64_t> detectionsId;
+            std::map<uint64_t, int> updateId;
+            std::vector<uint64_t> lostId;
+            m_tracker.run(input, detectionsId, updateId, lostId);
+
+            std::map<uint64_t, std::shared_ptr<Track3D> >& trakcers = m_tracker.GetStates();
+
+            jfx_common_msgs::det_tracking_array sendObjs = {};
+            for (auto& iter : trakcers)
+            {
+                jfx_common_msgs::det_tracking obj = {};
+                if (updateId.find(iter.first) != updateId.end())
+                {
+                    obj = objsPtr->array[updateId[iter.first]];
+                    std::vector<float> data;
+                    if (iter.second->GetStateData(data) == 0)
+                    {
+                        obj.x = data[0];
+                        obj.y = data[1];
+                        obj.z = data[2];
+                        obj.w = data[3];
+                        obj.l = data[4];
+                        obj.h = data[5];
+                        obj.rot_y = data[6] / 3.1415926 * 180;
+                        obj.v_x = data[7];
+                        obj.v_y = data[8];
+                        obj.v_z = data[9];
+                    }
+                    iter.second->m_obj = std::make_shared<jfx_common_msgs::det_tracking>(obj);
+                }
+                else
+                {
+                    if(iter.second->m_obj)
+                        obj = *(iter.second->m_obj);
+                }
+                sendObjs.array.emplace_back(obj);
+            }
+            sendObjs.cloud = objsPtr->cloud;
+
+            m_pub.publish(sendObjs);
+        }
+
+    }
+}
\ No newline at end of file

TrackingRos.h

+#pragma once
+
+#include <vector>
+#include "BaseTracker.h"
+#include "Track3D.h"
+#include "jfx_common_msgs/det_tracking_array.h"
+#include "SafeQueue.hpp"
+//#include "TrackingMsg.h"
+#include "ros/ros.h"
+
+
+using objTrackListPtr = std::shared_ptr<jfx_common_msgs::det_tracking_array>;
+
+
+class TrackingRos
+{
+public:
+    // Constructor
+    TrackingRos() {}
+    ~TrackingRos();
+
+    void TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg);//接受ros发过来的消息
+
+    void Init(ros::NodeHandle& nh);
+
+    void ThreadTrackingProcess();
+
+
+public:
+
+    SafeQueue<objTrackListPtr> m_objsQueue;
+
+    BaseTracker<Track3D> m_tracker;
+
+    bool m_isTrackingRun = false;//记录是否在运行
+    std::thread m_trackingThread;//运行事件线程
+    ros::Publisher m_pub;//发送所有物体信息的publisher
+};

common/Component.cpp

+
+#include "Component.h"
+#include <sys/time.h>
+#include <unistd.h>
+
+uint64_t GetCurTime()
+{
+    struct timeval time;
+    gettimeofday(&time, NULL);
+    uint64_t seconds = time.tv_sec;
+    uint64_t ttt = seconds * 1000 * 1000 + time.tv_usec;
+    return ttt;
+}
\ No newline at end of file

common/Component.h

+#pragma once
+
+#include <string>
+
+
+uint64_t GetCurTime();

common/SafeQueue.hpp

+/*
+ * SafeQueue.hpp
+ * Copyright (C) 2019 Alfredo Pons Menargues <apons@linucleus.com>
+ *
+ * This program is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef SAFEQUEUE_HPP_
+#define SAFEQUEUE_HPP_
+
+#include <queue>
+#include <list>
+#include <mutex>
+#include <thread>
+#include <cstdint>
+#include <condition_variable>
+ 
+ 
+ 
+/** A thread-safe asynchronous queue */
+template <class T, class Container = std::list<T>>
+class SafeQueue
+{
+ 
+    typedef typename Container::value_type value_type;
+    typedef typename Container::size_type size_type;
+    typedef Container container_type;
+ 
+  public:
+ 
+    /*! Create safe queue. */
+    SafeQueue() = default;
+    SafeQueue (SafeQueue&& sq)
+    {
+      m_queue = std::move (sq.m_queue);
+    }
+    SafeQueue (const SafeQueue& sq)
+    {
+      std::lock_guard<std::mutex> lock (sq.m_mutex);
+      m_queue = sq.m_queue;
+    }
+ 
+    /*! Destroy safe queue. */
+    ~SafeQueue()
+    {
+      std::lock_guard<std::mutex> lock (m_mutex);
+    }
+ 
+    /**
+     * Sets the maximum number of items in the queue. Defaults is 0: No limit
+     * \param[in] item An item.
+     */
+    void set_max_num_items (unsigned int max_num_items)
+    {
+      m_max_num_items = max_num_items;
+    }
+ 
+    /**
+     *  Pushes the item into the queue.
+     * \param[in] item An item.
+     * \return true if an item was pushed into the queue
+     */
+    bool push(const value_type& item)
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+        int ret = true;
+        if (m_max_num_items > 0 && m_queue.size() > m_max_num_items)
+        {
+            m_queue.pop();
+            ret = false;
+        }
+
+        m_queue.push(item);
+        m_condition.notify_one();
+        return ret;
+    }
+
+    /**
+     *  Pushes the item into the queue.
+     * \param[in] item An item.
+     * \return true if an item was pushed into the queue
+     */
+    bool push(const value_type&& item)
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+        int ret = true;
+        if (m_max_num_items > 0 && m_queue.size() > m_max_num_items)
+        {
+            m_queue.pop();
+            ret = false;
+        }
+
+        m_queue.push(item);
+        m_condition.notify_one();
+        return ret;
+    }
+    /**
+    *  Pushes the item into the queue.
+    * \param[in] item An item.
+    * \return true if an item was pushed into the queue
+    */
+    bool push_ex(const value_type& item, value_type& out)
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+        bool  ret = true;
+        if (m_max_num_items > 0 && m_queue.size() > m_max_num_items)
+        {
+            //修改为队列里大于这个值，pop出老的，push新的
+            out = m_queue.front();
+            m_queue.pop();
+            ret = false;
+        }
+        //return false;
+
+        m_queue.push(item);
+        m_condition.notify_one();
+        return ret;
+    }
+
+    /**
+     *  Pushes the item into the queue.
+     * \param[in] item An item.
+     * \return true if an item was pushed into the queue
+     */
+    bool push_ex(const value_type&& item, value_type& out)
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+        bool  ret = true;
+        if (m_max_num_items > 0 && m_queue.size() > m_max_num_items)
+        {
+            //修改为队列里大于这个值，pop出老的，push新的
+            out = m_queue.front();
+            m_queue.pop();
+            ret = false;
+        }
+        //return false;
+
+        m_queue.push(item);
+        m_condition.notify_one();
+        return ret;
+    }
+
+    /**
+     *  Pops item from the queue. If queue is empty, this function blocks until item becomes available.
+     * \param[out] item The item.
+     */
+    void pop (value_type& item)
+    {
+      std::unique_lock<std::mutex> lock (m_mutex);
+      m_condition.wait (lock, [this]() // Lambda funct
+      {
+        return !m_queue.empty();
+      });
+      item = m_queue.front();
+      m_queue.pop();
+    }
+ 
+    /**
+     *  Pops item from the queue using the contained type's move assignment operator, if it has one..
+     *  This method is identical to the pop() method if that type has no move assignment operator.
+     *  If queue is empty, this function blocks until item becomes available.
+     * \param[out] item The item.
+     */
+    void move_pop (value_type& item)
+    {
+      std::unique_lock<std::mutex> lock (m_mutex);
+      m_condition.wait (lock, [this]() // Lambda funct
+      {
+        return !m_queue.empty();
+      });
+      item = std::move (m_queue.front());
+      m_queue.pop();
+    }
+ 
+    /**
+     *  Tries to pop item from the queue.
+     * \param[out] item The item.
+     * \return False is returned if no item is available.
+     */
+    bool try_pop (value_type& item)
+    {
+      std::unique_lock<std::mutex> lock (m_mutex);
+ 
+      if (m_queue.empty())
+        return false;
+ 
+      item = m_queue.front();
+      m_queue.pop();
+      return true;
+    }
+ 
+    /**
+     *  Tries to pop item from the queue using the contained type's move assignment operator, if it has one..
+     *  This method is identical to the try_pop() method if that type has no move assignment operator.
+     * \param[out] item The item.
+     * \return False is returned if no item is available.
+     */
+    bool try_move_pop (value_type& item)
+    {
+      std::unique_lock<std::mutex> lock (m_mutex);
+ 
+      if (m_queue.empty())
+        return false;
+ 
+      item = std::move (m_queue.front());
+      m_queue.pop();
+      return true;
+    }
+ 
+    /**
+     *  Pops item from the queue. If the queue is empty, blocks for timeout microseconds, or until item becomes available.
+     * \param[out] t An item.
+     * \param[in] timeout The number of microseconds to wait.
+     * \return true if get an item from the queue, false if no item is received before the timeout.
+     */
+    bool timeout_pop (value_type& item, std::uint64_t timeout)
+    {
+      std::unique_lock<std::mutex> lock (m_mutex);
+ 
+      while(m_queue.empty())
+        {
+          if (timeout == 0)
+            return false;
+ 
+          if (m_condition.wait_for (lock, std::chrono::milliseconds (timeout)) == std::cv_status::timeout)
+            return false;
+        }
+ 
+      item = m_queue.front();
+      m_queue.pop();
+      return true;
+    }
+ 
+    /**
+     *  Pops item from the queue using the contained type's move assignment operator, if it has one..
+     *  If the queue is empty, blocks for timeout microseconds, or until item becomes available.
+     *  This method is identical to the try_pop() method if that type has no move assignment operator.
+     * \param[out] t An item.
+     * \param[in] timeout The number of microseconds to wait.
+     * \return true if get an item from the queue, false if no item is received before the timeout.
+     */
+    bool timeout_move_pop (value_type& item, std::uint64_t timeout)
+    {
+      std::unique_lock<std::mutex> lock (m_mutex);
+ 
+      if (m_queue.empty())
+        {
+          if (timeout == 0)
+            return false;
+ 
+          if (m_condition.wait_for (lock, std::chrono::microseconds (timeout)) == std::cv_status::timeout)
+            return false;
+        }
+ 
+      item = std::move (m_queue.front());
+      m_queue.pop();
+      return true;
+    }
+ 
+    /**
+     *  Gets the number of items in the queue.
+     * \return Number of items in the queue.
+     */
+    size_type size() const
+    {
+      std::lock_guard<std::mutex> lock (m_mutex);
+      return m_queue.size();
+    }
+ 
+    /**
+     *  Check if the queue is empty.
+     * \return true if queue is empty.
+     */
+    bool empty() const
+    {
+      std::lock_guard<std::mutex> lock (m_mutex);
+      return m_queue.empty();
+    }
+ 
+    /**
+     *  Swaps the contents.
+     * \param[out] sq The SafeQueue to swap with 'this'.
+     */
+    void swap (SafeQueue& sq)
+    {
+      if (this != &sq)
+        {
+          std::lock_guard<std::mutex> lock1 (m_mutex);
+          std::lock_guard<std::mutex> lock2 (sq.m_mutex);
+          m_queue.swap (sq.m_queue);
+ 
+          if (!m_queue.empty())
+            m_condition.notify_all();
+ 
+          if (!sq.m_queue.empty())
+            sq.m_condition.notify_all();
+        }
+    }
+ 
+    /*! The copy assignment operator */
+    SafeQueue& operator= (const SafeQueue& sq)
+    {
+      if (this != &sq)
+        {
+          std::lock_guard<std::mutex> lock1 (m_mutex);
+          std::lock_guard<std::mutex> lock2 (sq.m_mutex);
+          std::queue<T, Container> temp {sq.m_queue};
+          m_queue.swap (temp);
+ 
+          if (!m_queue.empty())
+            m_condition.notify_all();
+        }
+ 
+      return *this;
+    }
+ 
+    /*! The move assignment operator */
+    SafeQueue& operator= (SafeQueue && sq)
+    {
+      std::lock_guard<std::mutex> lock (m_mutex);
+      m_queue = std::move (sq.m_queue);
+ 
+      if (!m_queue.empty())  m_condition.notify_all();
+ 
+      return *this;
+    }
+ 
+ 
+  private:
+ 
+    std::queue<T, Container> m_queue;
+    mutable std::mutex m_mutex;
+    std::condition_variable m_condition;
+    unsigned int m_max_num_items = 0;
+};
+ 
+/*! Swaps the contents of two SafeQueue objects. */
+template <class T, class Container>
+void swap (SafeQueue<T, Container>& q1, SafeQueue<T, Container>& q2)
+{
+  q1.swap (q2);
+}
+#endif /* SAFEQUEUE_HPP_ */

jfx_tracking.cpp

@@ -9,6 +9,7 @@
 #include "Iou.h"
 #include "jfx_log.h"
 #include "LogBase.h"
+#include "TrackingRos.h"
 
 //using namespace jfx_vision;
 
@@ -23,7 +24,7 @@ int main(int argc, char*argv[]) {
     SDK_LOG(SDK_INFO, "main function");
 
 
-    ros::init(argc, argv, "jfx_vision");
+    ros::init(argc, argv, "jfx_tracking");
     ros::NodeHandle nh("~");
 
 
@@ -54,6 +55,10 @@ int main(int argc, char*argv[]) {
 
     //MultiCamImageSynchronizer<sensor_msgs::Image> sync(nh, cam_size); 
 
+    TrackingRos jfx_tracking;
+    ros::Subscriber track_sub = nh.subscribe<jfx_common_msgs::det_tracking_array>("/detection/lidar_detector/objects", 1000, &TrackingRos::TrackingCallBackFunc, &jfx_tracking);
+    //rospy.Subscriber("/detection/lidar_detector/objects", det_tracking_array, callback)
+
     ros::spin();
 
     return 0;