提交修改

TrackingRos.cpp

@@ -655,10 +655,10 @@ void TrackingRos::ThreadTrackingProcess()
                         t_marker.scale.x = 1.5;
                         t_marker.scale.y = 1.5;
                         t_marker.scale.z = 1.5;
-                        t_marker.color.a = 1.0;
-                        t_marker.color.r = 0;
-                        t_marker.color.g = 1.0;
-                        t_marker.color.b = 0;
+                        t_marker.color.a = 1;
+                        t_marker.color.r = 1.0;
+                        t_marker.color.g = 0.0;
+                        t_marker.color.b = 1.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}",

ros2/TrackingRos2.cpp

@@ -36,6 +36,8 @@ TrackingRos2::TrackingRos2(const rclcpp::NodeOptions& node_options)
       autoware_auto_perception_msgs::msg::TrackedObjects>(pub_topic, 10);
   m_pubMarker = this->create_publisher<visualization_msgs::msg::MarkerArray>(
       "output/sensors_fusion_obj_markers", 10);
+  m_pubMarkerText = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "output/sensors_fusion_text_markers", 10);
   //获取yaml文件
   std::string root_dir =
       this->declare_parameter<std::string>("root_dir", std::string(""));
@@ -87,6 +89,7 @@ void TrackingRos2::ReceiveFusionResMsg(
 void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
   if (!objsPtr) return;
   visualization_msgs::msg::MarkerArray makerArray = {};
+  visualization_msgs::msg::MarkerArray makerTextArray = {};
   autoware_auto_perception_msgs::msg::TrackedObjects sendObjs = {};
   sendObjs.header = objsPtr->header;
   sendObjs.header.frame_id = "top_lidar";
@@ -324,6 +327,33 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         marker.color.b = 0.0f;
         marker.color.a = 0.5;
         makerArray.markers.emplace_back(marker);
+
+        visualization_msgs::msg::Marker markerText = {};
+        markerText.lifetime = rclcpp::Duration(1);
+        markerText.header.frame_id = "top_lidar";
+        markerText.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+        markerText.action = visualization_msgs::msg::Marker::ADD;
+        markerText.id = iter.first;
+        markerText.pose.position.x = poss.x;
+        markerText.pose.position.y = poss.y;
+        markerText.pose.position.z = poss.z + dimensionss.z/2 + 0.3m;
+        Eigen::AngleAxisd V3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
+        Eigen::Quaterniond R_quat(V3);
+        markerText.pose.orientation.x = 0.0f;
+        markerText.pose.orientation.y = 0.0f;
+        markerText.pose.orientation.z = 0.0f;
+        markerText.pose.orientation.w = 1.0f;
+        markerText.scale.x = 1.5;
+        markerText.scale.y = 1.5;
+        markerText.scale.z = 1,5;
+        markerText.color.r = 0.0f;
+        markerText.color.g = 1.0f;
+        markerText.color.b = 0.0f;
+        markerText.color.a = 1.0;
+        char str[512] = {};
+        sprintf(str, "%f",poss.x);
+        markerText.text = str;        
+        makerTextArray.markers.emplace_back(markerText);        
         sendObjs.objects.emplace_back(obj);
       }
       // jsk_recognition_msgs::BoundingBox box = {};
@@ -395,12 +425,34 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
     marker.color.b = 0.0f;
     marker.color.a = 0.5;
     makerArray.markers.emplace_back(marker);
+    visualization_msgs::msg::Marker markerText = {};
+    markerText.lifetime = rclcpp::Duration(1);
+    markerText.header.frame_id = "top_lidar";
+    markerText.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    markerText.action = visualization_msgs::msg::Marker::DELETE;
+    markerText.id = lostId[i];
+    markerText.pose.position.x = 0;
+    markerText.pose.position.y = 0;
+    markerText.pose.position.z = 0;
+    markerText.pose.orientation.x = 0;
+    markerText.pose.orientation.y = 0;
+    markerText.pose.orientation.z = 0;
+    markerText.pose.orientation.w = 0;
+    markerText.scale.x = 0;
+    markerText.scale.y = 0;
+    markerText.scale.z = 0;
+    markerText.color.r = 1.0f;
+    markerText.color.g = 0.0f;
+    markerText.color.b = 0.0f;
+    markerText.color.a = 0.5;
+    makerTextArray.markers.emplace_back(marker);
   }
   // sendObjs.cloud = objsPtr->cloud;
   // SDK_LOG(SDK_INFO, "calculate objs size = %d", sendObjs.array.size());
 
   m_tracking_res->publish(sendObjs);
   m_pubMarker->publish(makerArray);
+  m_pubMarkerText->publish(makerTextArray);
   // m_pubBoundingBoxes.publish(sendBoxes);
   // objsPtr->cloud.header.frame_id = "/rslidar";
   // m_pubCloud.publish(objsPtr->cloud);

ros2/TrackingRos2.h

@@ -36,6 +36,7 @@ class TrackingRos2 : public rclcpp::Node {
     rclcpp::Subscription<wit_perception_msgs::msg::DetectedObjectsWithFeatureEx>::SharedPtr m_fusion_res;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr m_tracking_res;
     rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr m_pubMarker;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr m_pubMarkerText;
 
     void ReceiveFusionResMsg(const wit_perception_msgs::msg::DetectedObjectsWithFeatureEx & msg);