提交代码

TrackingRosEx.h

+#pragma once
+
+#include <vector>
+#include <mutex>
+#include "BaseTracker.h"
+#include "Track3D.h"
+#ifdef _QICHECHENG_
+#include "jfxrosperceiver/det_tracking_array.h"
+#define jfx_common_msgs jfxrosperceiver
+#else
+#include "jfx_common_msgs/det_tracking_array.h"
+#endif
+#include "SafeQueue.hpp"
+//#include "TrackingMsg.h"
+#include "ros/ros.h"
+#include "coordinate_convert.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 TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesConstPtr& msg);//接受camera发过来的消息
+
+    void Init(ros::NodeHandle& nh);
+
+    void ThreadTrackingProcess();
+
+
+    objTrackListPtr GetNearestCloudMsg(unsigned long long timestamp);
+
+public:
+
+    SafeQueue<objTrackListPtr> m_objsQueue;
+
+    std::mutex m_mtx;
+    std::vector<objTrackListPtr> m_objsCloudQueue;
+
+    BaseTracker<Track3D> m_tracker;
+
+    bool m_isTrackingRun = false;//记录是否在运行
+    std::thread m_trackingThread;//运行事件线程
+    ros::Publisher m_pub;//发送所有物体信息的publisher
+    ros::Publisher m_pubCloud;//发送点云数据
+    ros::Publisher m_pubBoundingBoxes;//发送3D框信息
+
+    std::vector<std::vector<double>> m_trans;
+    std::vector<std::vector<double>> m_kitti2origin;
+
+    int m_frameNum = 0;
+
+    float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
+};

jfx_tracking_ex.cpp

+#include <ros/ros.h>
+//#include "image_synchronizer.h"
+//#include <yaml-cpp/yaml.h>
+
+//#include "cv_config.h"
+#include "Track3D.h"
+#include "Track2D.h"
+#include "BaseTracker.h"
+#include "Iou.h"
+#include "jfx_log.h"
+#include "LogBase.h"
+#include "TrackingRosEx.h"
+
+//using namespace jfx_vision;
+
+//std::unique_ptr<VisionConfig>  jfx_vision::g_visionConfig;
+
+int main(int argc, char*argv[]) {
+    //ROS_INFO("START JFX_VISION_NODE");
+
+
+    InitLog(argv[0]);
+
+    SDK_LOG(SDK_INFO, "main function");
+
+
+    ros::init(argc, argv, "lidar_tracking_listener");
+    ros::NodeHandle nh("~");
+
+
+    //BaseTracker<Track3D> tracker3d;
+    //BaseTracker<Track2D> tracker2d;
+
+    //std::map<uint64_t, std::shared_ptr<Track3D> >& a = tracker3d.GetStates();
+    //std::map<uint64_t, std::shared_ptr<Track2D> >& b = tracker2d.GetStates();
+    
+
+    //std::vector<float> truth_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    //std::vector<float> truth_poses{ -0.1875, -0.798913, -1.0, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    ////std::vector<float> landmark_poses{ -0.506616, -0.796303, -1.20499, 0, 0, -0.345443, 0.22,  0.22, 0.4 };
+    //std::vector<float> landmark_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.47063, 0.375 };
+    //double iou_3d = CuboidIoU(truth_poses, landmark_poses);
+    //std::cout<<"the iou of two cuboid is: "<< iou_3d << std::endl;
+
+    //std::string config_yaml = "/data/catkin_ws_xishan/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml";
+    //nh.param<std::string>("vision_config_path", config_yaml, "");
+
+    //g_visionConfig.reset(new VisionConfig(config_yaml));
+
+    //int cam_size = g_visionConfig->globalConf.camera_size;
+    //ROS_INFO("cam_size:%d", cam_size);
+
+    // ROS_INFO("CAM[0] roi: h %d,w %d,x %d,y %d", g_visionConfig->cameraParamConfigs_map[0].roiRect.height, g_visionConfig->cameraParamConfigs_map[0].roiRect.width, 
+    // g_visionConfig->cameraParamConfigs_map[0].roiRect.x, g_visionConfig->cameraParamConfigs_map[0].roiRect.y);
+
+    //MultiCamImageSynchronizer<sensor_msgs::Image> sync(nh, cam_size); 
+
+    TrackingRos jfx_tracking;
+    jfx_tracking.Init(nh);
+    ros::Subscriber track_sub = nh.subscribe<jfx_common_msgs::det_tracking_array>("/detection/lidar_detector/objects", 1000, &TrackingRos::TrackingCallBackFunc, &jfx_tracking);
+    ros::Subscriber track_camera = nh.subscribe<jfx_common_msgs::InferReses>("/InferReses", 1000, &TrackingRos::TrackingCameraCallBackFunc, &jfx_tracking);
+    //rospy.Subscriber("/detection/lidar_detector/objects", det_tracking_array, callback)
+
+    ros::spin();
+
+    return 0;
+}