update

ros2/TrackingRos2.cpp

@@ -299,16 +299,19 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         linear.z = 0;//data[9];
       }
     }
-    if (is_need_send == 1) {
-      geometry_msgs::msg::Quaternion& orient =
-          obj.kinematics.pose_with_covariance.pose.orientation;
-      //修正rot_y值,变为角度
-      while (orient.y >= _PI_) {
-        orient.y -= _PI_ * 2;
-      }
-      while (orient.y < -_PI_) {
-        orient.y += _PI_ * 2;
-      }
+    geometry_msgs::msg::Quaternion& orient =
+        obj.kinematics.pose_with_covariance.pose.orientation;
+    //修正rot_y值,变为角度
+    while (orient.y >= _PI_) {
+    orient.y -= _PI_ * 2;
+    }
+    while (orient.y < -_PI_) {
+    orient.y += _PI_ * 2;
+    }    
+    geometry_msgs::msg::Point& poss =
+        obj.kinematics.pose_with_covariance.pose.position;    
+    if (is_need_send == 1 && orient.y < 2 && poss.y > -15 && poss.y < 15) {
+
       float rot_y_angle = orient.y;
       Eigen::AngleAxisd v3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
       Eigen::Quaterniond rquat(v3);
@@ -324,8 +327,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
       if (iter.second->GetPredictData(predict) == 0) 
       {
       }
-      geometry_msgs::msg::Point& poss =
-          obj.kinematics.pose_with_covariance.pose.position;
+
       geometry_msgs::msg::Vector3& dimensionss = obj.shape.dimensions;
       if (1/*rot_y_angle > -2 && rot_y_angle < 2*/) {
         visualization_msgs::msg::Marker marker = {};
@@ -376,7 +378,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         markerText.color.b = 0.0f;
         markerText.color.a = 1.0;
         char str[512] = {};
-        sprintf(str, "%d:%.2f",input_obj.tracking_id == 0 ? tracking_id: input_obj.tracking_id,poss.x);
+        sprintf(str, "%.2f:%d:%.2f",obj.existence_probability,obj.classification[0].label,poss.x);
         markerText.text = str;        
         makerTextArray.markers.emplace_back(markerText);        
         sendObjs.objects.emplace_back(obj);