提交boundingbox代码

TrackingRos.cpp

 #include "TrackingRos.h"
 #include "Component.h"
 #include <sensor_msgs/PointCloud2.h>
+#include <jsk_recognition_msgs/BoundingBoxArray.h>
 #include "LogBase.h"
 #include <yaml-cpp/yaml.h>
 #include "matrix.h"
@@ -56,6 +57,8 @@ void TrackingRos::Init(ros::NodeHandle& nh)
 
     m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>("/tracking_res", 100);
     m_pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/tracking_cloud", 100);
+    m_pubBoundingBoxes = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("/tracking_bbox", 100);
+    
 
     m_objsQueue.set_max_num_items(2);
 
@@ -164,6 +167,8 @@ void TrackingRos::ThreadTrackingProcess()
             
             SDK_LOG(SDK_INFO, "run before size = %d, after size = %d,",objsPtr->array.size(), trackers.size());
             jfx_common_msgs::det_tracking_array sendObjs = {};
+            jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
+            sendBoxes.header.frame_id = "Pandar64";
             for (auto& iter : trackers)
             {
                 jfx_common_msgs::det_tracking obj = {};
@@ -229,12 +234,30 @@ void TrackingRos::ThreadTrackingProcess()
                         //SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f", obj.Lat, obj.Long);
                     }
                 }
+                jsk_recognition_msgs::BoundingBox box = {};
+                box.label = obj.obj_id;
+                box.value = 2;
+                box.header.frame_id = "Pandar64";
+                box.dimensions.x = obj.l;
+                box.dimensions.y = obj.w;
+                box.dimensions.z = obj.h;
+                box.pose.position.x = obj.x;
+                box.pose.position.y = obj.y;
+                box.pose.position.z = obj.z;
+                AngleAxisd V3(obj.rot_y/180*3.1415926., Vector3d(0, 0, 1));
+                Quaterniond R_quat(V3);
+                box.pose.orientation.x = R_quat.x();
+                box.pose.orientation.y = R_quat.y();
+                box.pose.orientation.z = R_quat.z();
+                box.pose.orientation.w = R_quat.w();
+                sendBoxes.boxes.emplace_back(box);
                 sendObjs.array.emplace_back(obj);
             }
             //sendObjs.cloud = objsPtr->cloud;
             SDK_LOG(SDK_INFO, "calculate objs size = %d", sendObjs.array.size());
 
             m_pub.publish(sendObjs);
+            m_pubBoundingBoxes.publish(sendBoxes);
         }
 
     }

TrackingRos.h

@@ -36,6 +36,7 @@ public:
     std::thread m_trackingThread;//运行事件线程
     ros::Publisher m_pub;//发送所有物体信息的publisher
     ros::Publisher m_pubCloud;//发送点云数据
+    ros::Publisher m_pubBoundingBoxes;//发送3D框信息
 
     std::vector<std::vector<float>> m_trans;
 };