提交更新

TrackingRos.cpp

@@ -2,6 +2,8 @@
 #include "Component.h"
 #include <sensor_msgs/PointCloud2.h>
 #include "LogBase.h"
+#include <yaml-cpp/yaml.h>
+
 
 TrackingRos::~TrackingRos()
 {
@@ -16,6 +18,12 @@ TrackingRos::~TrackingRos()
 
 void TrackingRos::Init(ros::NodeHandle& nh)
 {
+    YAML::Node config = YAML::LoadFile("7-1.yaml");
+
+    auto value = config["TRACKING"]["TRANS"].as< vector<vector<float>> >();
+
+    m_coordinate.Init(value[0][0], value[0][1], value[0][2], value[0][3], value[1][0], value[1][1]);
+
     m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>("/tracking_res", 100);
     m_pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/tracking_cloud", 100);
 
@@ -115,6 +123,11 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.v_x = data[7];
                         obj.v_y = data[8];
                         obj.v_z = data[9];
+                        ekf::Array pos = { obj.x,obj.y };
+                        ekf::Array out;
+                        m_coordinate.Dev2WGS84(pos, out, false);
+                        obj.Lat = out[0];
+                        obj.Long = out[1];
                     }
                     iter.second->m_obj = std::make_shared<jfx_common_msgs::det_tracking>(obj);
                 }

TrackingRos.h

@@ -7,7 +7,7 @@
 #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>;
 
@@ -36,4 +36,6 @@ public:
     std::thread m_trackingThread;//运行事件线程
     ros::Publisher m_pub;//发送所有物体信息的publisher
     ros::Publisher m_pubCloud;//发送点云数据
+
+    CoordinateConvert m_coordinate;
 };