提交阿布扎比更新

TrackingRos.cpp

@@ -12,6 +12,9 @@
 #include <nvToolsExt.h>
 #endif
 #include "yaml_parser.h"
+#ifdef _SEND_ABUZHABI_
+#include "SendMsg.h"
+#endif
 
 
 float to360(float rot_y,float lidar_x_angle)
@@ -256,7 +259,9 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     m_pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/tracking_cloud", 100);
     m_pubBoundingBoxes = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("/tracking_bbox", 100);
     m_pubMarkerArray = nh.advertise<visualization_msgs::MarkerArray>("/tracking_loc", 100);
-    
+#ifdef _SEND_ABUZHABI_
+    m_pub = nh.advertise<std::string>("/abuzhabi_msg", 100);
+#endif
 
     m_objsQueue.set_max_num_items(2);
 
@@ -425,6 +430,15 @@ void TrackingRos::ThreadTrackingProcess()
             jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
             visualization_msgs::MarkerArray sendMarkers = {};
             sendBoxes.header.frame_id = "Pandar64";
+#ifdef _SEND_ABUZHABI_
+            V2xReport send_abu = {};
+            send_abu.transID = std::to_string(m_frameNum);
+            send_abu.ID = 1000;
+            send_abu.messageName = "V2xReport";
+            send_abu.detection_timestamp = cur_timestamp;
+            send_abu.timestamp = GetCurTime() / 1000;
+            send_abu.deviceID = "juefx";
+#endif
             int i = 0;
             for (auto& iter : trackers)
             {
@@ -544,6 +558,22 @@ void TrackingRos::ThreadTrackingProcess()
                     box.pose.orientation.w = R_quat.w();
                     sendBoxes.boxes.emplace_back(box);
                     sendObjs.array.emplace_back(obj);
+#ifdef _SEND_ABUZHABI_
+                    Targets target = {};
+                    if (obj.type == 1 || obj.type == 2 || obj.type == 3)
+                        target.reportType = "AT";
+                    else if (obj.type == 4)
+                        target.reportType = "Pedestrian";
+                    else
+                        target.reportType = "Other_Truck";
+                    target.vehicleNum = "0";
+                    target.confidence = obj.score * 100;
+                    target.location.x = obj.Lat;
+                    target.location.y = obj.Long;
+                    target.location.heading = obj.rot_y;
+                    target.location.velocity = sqrt(obj.v_x * obj.v_x + obj.v_y * obj.v_y);
+                    send_abu.targets.emplace_back(target);
+#endif
 
                     if (obj.type != 4)
                     {
@@ -612,6 +642,12 @@ void TrackingRos::ThreadTrackingProcess()
             m_pubBoundingBoxes.publish(sendBoxes);
             m_pubMarkerArray.publish(sendMarkers);
             m_pubCloud.publish(objsPtr->cloud);
+#ifdef _SEND_ABUZHABI_
+            std::stringstream ss;
+            jf_ajson::save_to(ss, send_abu);
+            std::string body = std::move(ss.str());
+            m_pubAbuzhabi.publish(body);
+#endif
 #ifdef _USING_NSIGHT_
             nvtxRangePop();
 #endif

TrackingRos.h

@@ -46,7 +46,9 @@ public:
     ros::Publisher m_pubCloud;//发送点云数据
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息
     ros::Publisher m_pubMarkerArray;//发送marker框信息
-    
+#ifdef _SEND_ABUZHABI_
+    ros::Publisher m_pubAbuzhabi;//阿布扎比发送的数据
+#endif
 
     std::vector<std::vector<double>> m_trans;
     std::vector<std::vector<double>> m_kitti2origin;

abuzhabi/SendMsg.h

+#pragma once
+
+#include <cstdint>
+#include <string>
+#include <memory>
+#include "ajson_c.hpp"
+
+
+typedef struct _Location {
+    double x;
+    double y;
+    double utm_x;
+    double utm_y;
+    float heading;
+    float velocity;
+} Location;
+//using TrkObjPtr = std::shared_ptr<TrkObj>;
+
+AJSON(Location,
+    x,
+    y,
+    utm_x,
+    utm_y,
+    heading,
+    velocity
+)
+
+typedef struct _Targets {
+    std::string reportType;
+    std::string vehicleNum;
+    int confidence;
+    std::vector<Location> location;
+}Targets;
+
+AJSON(Targets,
+    reportType,
+    vehicleNum,
+    confidence,
+    location
+)
+
+typedef struct _V2xReport {
+    std::string transID;
+    int64_t ID;
+    std::string messageName; 
+    int64_t detection_timestamp; 
+    int64_t timestamp;
+    std::string deviceID;
+    std::vector<Targets> targets;
+}V2xReport;
+//using TrkObjsPtr = std::shared_ptr<TrkObjs>;
+
+AJSON(V2xReport,
+    transID,
+    ID,
+    messageName,
+    detection_timestamp,
+    timestamp,
+    deviceID,
+    targets
+)
\ No newline at end of file