update

ros2/TrackingRos2.cpp

@@ -179,6 +179,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
   // jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
   // visualization_msgs::MarkerArray markerArray = {};
   // sendBoxes.header.frame_id = "/rslidar";
+  std::map<int,int> sameObj;
   for (auto& iter : trackers) {
     autoware_auto_perception_msgs::msg::TrackedObject obj = {};
     wit_perception_msgs::msg::DetectedObjectWithFeatureEx input_obj = {};
@@ -339,16 +340,14 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         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.position.z = poss.z + dimensionss.z/2 + 0.5;
         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.scale.x = 3;
+        markerText.scale.y = 3;
+        markerText.scale.z = 3;
         markerText.color.r = 0.0f;
         markerText.color.g = 1.0f;
         markerText.color.b = 0.0f;
@@ -358,52 +357,15 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         markerText.text = str;        
         makerTextArray.markers.emplace_back(markerText);        
         sendObjs.objects.emplace_back(obj);
+        if(iter.second->m_obj->cameraId > 0 && iter.second->m_obj->trackingId > 0)
+        {
+            if(sameObj.find(iter.second->m_obj->trackingId) == sameObj.end())
+            {
+                sameObj[iter.second->m_obj->trackingId] = 0;
+            }
+            sameObj[iter.second->m_obj->trackingId]++;
+        }
       }
-      // jsk_recognition_msgs::BoundingBox box = {};
-      // box.label = obj.type;
-      // box.value = obj.obj_id;
-      // box.header.frame_id = "/rslidar";
-      // box.dimensions.x = obj.l;
-      // box.dimensions.y = obj.w;
-      // box.dimensions.z = obj.h;
-      // box.pose.position.x = obj.x;
-      // box.pose.position.y = obj.y;
-      // box.pose.position.z = obj.z;
-      // Eigen::AngleAxisd V3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
-      // Eigen::Quaterniond R_quat(V3);
-      // box.pose.orientation.x = R_quat.x();
-      // box.pose.orientation.y = R_quat.y();
-      // 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 = 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[256] = {};
-      // int angle_i = int(rot_y_angle / _PI_ * 180);
-      // sprintf(str, "id:%llu,t:%d,yaw:%d", obj.obj_id, obj.type, angle_i);
-      // t_marker.text = str;
-
-      // markerArray.markers.emplace_back(t_marker);
-      // sendBoxes.boxes.emplace_back(box);
     }
   }
   for (int i = 0; i < lostId.size(); i++) {
@@ -450,8 +412,6 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
     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);