提交发送marker的消息

TrackingRos.cpp

@@ -2,6 +2,8 @@
 #include "Component.h"
 #include <sensor_msgs/PointCloud2.h>
 #include <jsk_recognition_msgs/BoundingBoxArray.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <math.h>
 #include "LogBase.h"
 #include <yaml-cpp/yaml.h>
 #include "matrix.h"
@@ -253,6 +255,7 @@ 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_pubMarkerArray = nh.advertise<visualization_msgs::MarkerArray>("/tracking_loc", 100);
     
 
     m_objsQueue.set_max_num_items(2);
@@ -417,7 +420,9 @@ 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 sendMarkers = {};
             sendBoxes.header.frame_id = "Pandar64";
+            int i = 0;
             for (auto& iter : trackers)
             {
                 jfx_common_msgs::det_tracking obj = {};
@@ -535,6 +540,60 @@ void TrackingRos::ThreadTrackingProcess()
                     box.pose.orientation.w = R_quat.w();
                     sendBoxes.boxes.emplace_back(box);
                     sendObjs.array.emplace_back(obj);
+
+                    if (obj.type != 4)
+                    {
+                        visualization_msgs::Marker marker = {};
+                        marker.id = i;
+                        marker.header.frame_id = "/Pandar64";
+                        marker.action = visualization_msgs::Marker::ADD;
+                        marker.type = visualization_msgs::Marker::SPHERE;
+                        marker.pose.position.x = obj.x;
+                        marker.pose.position.y = obj.y;
+                        marker.pose.position.z = obj.z + obj.h / 2;
+                        marker.pose.orientation.x = 0.0;
+                        marker.pose.orientation.y = 0.0;
+                        marker.pose.orientation.z = 0.0;
+                        marker.pose.orientation.w = 1.0;
+                        marker.scale.x = 0.5;
+                        marker.scale.y = 0.5;
+                        marker.scale.z = 0.5;
+                        marker.color.a = 1.0;
+                        marker.color.r = 1.0;
+                        marker.color.g = 1.0;
+                        marker.color.b = 1.0;
+                        marker.lifetime = ros::Duration(0.5);
+                        sendMarkers.markers.append(marker);
+                        i += 1;
+
+                        visualization_msgs::Marker t_marker = {};
+                        t_marker.id = i;
+                        t_marker.header.frame_id = "/Pandar64";
+                        t_marker.action = visualization_msgs::Marker::ADD;
+                        t_marker.type = visualization_msgs::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 = ros::Duration(0.5);
+                        char str[512] = {};
+                        sprintf(str, "id:{%llu},type:{%d},type_name:{%s},v:{%.2f}m/s,No:{%s}\ncolor_name:{%s},timestamp:{%llu}",
+                            obj.obj_id, obj.type, obj.name.c_str(), sqrt(obj.v_x* obj.v_x + obj.v_y * obj.v_y) * 10, obj.license_plate_number.c_str(), obj.color_name.c_str(), obj.frame);
+                        t_marker.text = str;
+                        //t_marker.text = 'id:{0},type:{1},type_name:{2},v:{3:.2f}m/s,No:{4}\ncolor_name:{5},timestamp:{6}'.format(msg.obj_id, msg.type, msg.name, math.sqrt(msg.v_x * msg.v_x + msg.v_y * msg.v_y) * 10, msg.license_plate_number, msg.color_name, msg.frame)
+                        sendMarkers.markers.append(t_marker);
+                        i += 1
+                    }
                 }
             }
             //sendObjs.cloud = objsPtr->cloud;
@@ -542,6 +601,7 @@ void TrackingRos::ThreadTrackingProcess()
 
             m_pub.publish(sendObjs);
             m_pubBoundingBoxes.publish(sendBoxes);
+            m_pubMarkerArray.publish(sendMarkers);
             m_pubCloud.publish(objsPtr->cloud);
 #ifdef _USING_NSIGHT_
             nvtxRangePop();

TrackingRos.h

@@ -45,6 +45,8 @@ public:
     ros::Publisher m_pub;//发送所有物体信息的publisher
     ros::Publisher m_pubCloud;//发送点云数据
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息
+    ros::Publisher m_pubMarkerArray;//发送marker框信息
+    
 
     std::vector<std::vector<double>> m_trans;
     std::vector<std::vector<double>> m_kitti2origin;