opt arrow publish

TrackingRos.cpp

@@ -364,6 +364,7 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     std::string cloud_topic = "/tracking_cloud";
     std::string tracking_box_topic = "/tracking_bbox";
     std::string tracking_marker_topic = "/tracking_loc";
+    std::string tracking_marker_arrow = "/tracking_arrow";
     if(config["fusion_topic_name"])
         fusion_topic = config["fusion_topic_name"].as<std::string>();
     if(config["tracking_topic_name"])
@@ -373,12 +374,15 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     if(config["tracking_box_topic_name"])
         tracking_box_topic = config["tracking_box_topic_name"].as<std::string>();
     if(config["tracking_marker_topic_name"])
-        tracking_marker_topic = config["tracking_marker_topic_name"].as<std::string>();           
+        tracking_marker_topic = config["tracking_marker_topic_name"].as<std::string>();         
+    f(config["tracking_marker_arrow_name"])
+        tracking_marker_arrow = config["tracking_marker_arrow_name"].as<std::string>();    
     m_subFusionRes = nh.subscribe<jfx_common_msgs::det_tracking_array>(fusion_topic.c_str(), 1000, &TrackingRos::TrackingCallBackFunc, this);
     m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>(tracking_topic.c_str(), 100);
     m_pubCloud = nh.advertise<sensor_msgs::PointCloud2>(cloud_topic.c_str(), 100);
     m_pubBoundingBoxes = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>(tracking_box_topic.c_str(), 100);
     m_pubMarkerArray = nh.advertise<visualization_msgs::MarkerArray>(tracking_marker_topic.c_str(), 100);
+    m_pubMarkerArrow = nh.advertise<visualization_msgs::MarkerArray>(tracking_marker_arrow.c_str(), 100);
 #ifdef _SEND_ABUZHABI_
     m_pubAbuzhabi = nh.advertise<std_msgs::String>("/abuzhabi_msg",100);
 #endif
@@ -556,6 +560,7 @@ void TrackingRos::ThreadTrackingProcess()
             jfx_common_msgs::det_tracking_array sendObjs = {};
             jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
             visualization_msgs::MarkerArray sendMarkers = {};
+            visualization_msgs::MarkerArray sendArrows = {};
             sendBoxes.header.frame_id = "Pandar64";
 #ifdef _SEND_ABUZHABI_
             V2xReport send_abu = {};
@@ -806,18 +811,13 @@ void TrackingRos::ThreadTrackingProcess()
                     if (obj.type != 4)
                     {
                         visualization_msgs::Marker marker = {};
-                        marker.id = i;
+                        marker.id = obj.obj_id;
                         marker.header.frame_id = "/Pandar64";
                         marker.action = visualization_msgs::Marker::ADD;
                         marker.type = visualization_msgs::Marker::ARROW;
                         marker.pose.position.x = obj.x;
                         marker.pose.position.y = obj.y;
-                        //marker.pose.position.z = obj.z + obj.h / 2;
                         marker.pose.position.z = obj.z;
-                        //marker.pose.orientation.x = 0.0;
-                        //marker.pose.orientation.y = 0.0;
-                        //marker.pose.orientation.z = 0.0;
-                        //marker.pose.orientation.w = 1.0;
                         tf2::Quaternion myquater;
                         myquater.setRPY( 0, 0, rot_y_angle_temp );
                         //myquater.normalize();
@@ -833,11 +833,11 @@ void TrackingRos::ThreadTrackingProcess()
                         marker.color.g = 0;
                         marker.color.b = 0;
                         marker.lifetime = ros::Duration(0.5);
-                        sendMarkers.markers.emplace_back(marker);
+                        sendArrows.markers.emplace_back(marker);
                         i += 1;
 
                         visualization_msgs::Marker t_marker = {};
-                        t_marker.id = i;
+                        t_marker.id = obj.obj_id;
                         t_marker.header.frame_id = "/Pandar64";
                         t_marker.action = visualization_msgs::Marker::ADD;
                         t_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
@@ -879,6 +879,7 @@ void TrackingRos::ThreadTrackingProcess()
             m_pub.publish(sendObjs);
             m_pubBoundingBoxes.publish(sendBoxes);
             m_pubMarkerArray.publish(sendMarkers);
+            m_pubMarkerArrow.publish(sendArrows);
             m_pubCloud.publish(objsPtr->cloud);
 #ifdef _SEND_ABUZHABI_
             static int sendNum = 0;

TrackingRos.h

@@ -46,6 +46,7 @@ public:
     ros::Publisher m_pubCloud;//发送点云数据
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息
     ros::Publisher m_pubMarkerArray;//发送marker框信息
+    ros::Publisher m_pubMarkerArrow;//发送marker框信息
     ros::Subscriber m_subFusionRes;
 #ifdef _SEND_ABUZHABI_
     ros::Publisher m_pubAbuzhabi;//阿布扎比发送的数据