提交新消息发送

TrackingRosEx.cpp

@@ -2,6 +2,7 @@
 #include "Component.h"
 #include <sensor_msgs/PointCloud2.h>
 #include <jsk_recognition_msgs/BoundingBoxArray.h>
+#include <visualization_msgs/MarkerArray.h>
 #include "LogBase.h"
 #include <yaml-cpp/yaml.h>
 #include "matrix.h"
@@ -649,10 +650,6 @@ void TrackingRos::ThreadTrackingProcess()
 #ifdef _USING_NSIGHT_
             nvtxRangePop();
 #endif
-            if (lostId.size() > 0)
-            {
-                
-            }
             uint64_t rTime = GetCurTime() - begin;
             //SDK_LOG(SDK_INFO, "m_tracker.Run time = %llu", rTime);
             std::map<uint64_t, std::shared_ptr<Track3D> >& trackers = m_tracker.GetStates();
@@ -676,6 +673,7 @@ 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 = {};
+            visualization_msgs::MarkerArray markerArray = {};
             sendBoxes.header.frame_id = "/rslidar";
             for (auto& iter : trackers)
             {
@@ -794,6 +792,32 @@ void TrackingRos::ThreadTrackingProcess()
                     box.pose.orientation.z = R_quat.z();
                     //SDK_LOG(SDK_INFO, "rot_y_angle = %f", rot_y_angle);
                     box.pose.orientation.w = R_quat.w();
+
+                    visualization_msgs::Marker t_marker = {};
+                    t_marker.id = obj.obj_id;
+                    t_marker.header.frame_id = "/rslidar";
+                    t_marker.action = Marker::ADD;
+                    t_marker.type = Marker::TEXT_VIEW_FACING;
+                    t_marker.pose.position.x = obj.x;
+                    t_marker.pose.position.y = obj.y;
+                    t_marker.pose.position.z = obj.z + obj.h / 2 + 0.3;
+                    t_marker.pose.orientation.x = 0.0;
+                    t_marker.pose.orientation.y = 0.0;
+                    t_marker.pose.orientation.z = 0.0;
+                    t_marker.pose.orientation.w = 1.0;
+                    t_marker.scale.x = 1.2;
+                    t_marker.scale.y = 1.2;
+                    t_marker.scale.z = 1.2;
+                    t_marker.color.a = 1.0;
+                    t_marker.color.r = 0;
+                    t_marker.color.g = 1.0;
+                    t_marker.color.b = 0;
+                    //t_marker.lifetime = rospy.Duration(0.5)
+                    char str[256] = {};
+                    snprintf(str, "id:%llu,type:%d", mobj.obj_id, obj.type);
+                    t_marker.text = str;
+
+                    markerArray.markers.emplace_back(t_marker);
                     sendBoxes.boxes.emplace_back(box);
                     sendObjs.array.emplace_back(obj);
                 }