use ObjectTrack

src/camera_bev_infer/src/common_maps.h

@@ -5,7 +5,7 @@
  * @Date: 2023-09-03 03:14:28
  * @email: xin.wang@waytous.com
  * @LastEditors: yangxue xue.yang@waytous.com
- * @LastEditTime: 2025-03-31 01:16:25
+ * @LastEditTime: 2025-04-07 03:49:19
  */
 
 #ifndef COMMON_MAP_H_
@@ -27,9 +27,10 @@ static const cv::Scalar Perception_Default_Color = cv::Scalar(255, 255, 255);
 static const int Perception_Default_Maxsize = 100; // m
 
 static std::map<std::string, int> name2type = {
+    {"unknown", waytous_perception_msgs::Object::TYPE_UNKNOWN},
     {"pedestrian", waytous_perception_msgs::Object::TYPE_PEDESTRIAN},
-    {"vehicle", waytous_perception_msgs::Object::TYPE_TRUCK},
-    {"stone", waytous_perception_msgs::Object::TYPE_UNKNOWN}
+    {"vehicle", waytous_perception_msgs::Object::TYPE_CAR},
+    {"stone", waytous_perception_msgs::Object::TYPE_TRUCK}
     // {"two_wheel", waytous_perception_msgs::Object::TYPE_BICYCLE},
     // {"car", waytous_perception_msgs::Object::TYPE_VEHICLE},
     // {"truck", waytous_perception_msgs::Object::TYPE_VEHICLE},

src/camera_bev_infer/src/sensor_manager.cpp

@@ -5,7 +5,7 @@
  * @Date: 2023-09-03 02:18:07
  * @email: xin.wang@waytous.com
  * @LastEditors: yangxue xue.yang@waytous.com
- * @LastEditTime: 2025-03-28 09:27:22
+ * @LastEditTime: 2025-04-07 03:55:10
  */
 
 
@@ -52,7 +52,7 @@ sensorManager::sensorManager(ros::NodeHandle& nodehandle, ImageInferNodeParam& p
     // 初始化订阅、发布话题
     image_subscriber_ = nodehandle.subscribe<sensor_msgs::Image>(param.rgb_sub_topic_name, 1, boost::bind(&sensorManager::updateImage, this, _1));
     pointcloud_subscriber_ = nodehandle.subscribe<sensor_msgs::PointCloud2>(param.pointcloud_sub_topic_name, 1, boost::bind(&sensorManager::updatePointcloud, this, _1));
-    obj_publisher_ = nodehandle.advertise<waytous_perception_msgs::ObjectArray>(param.detection_pub_topic_name, 1);
+    obj_publisher_ = nodehandle.advertise<waytous_perception_msgs::ObjectTrackArray>(param.detection_pub_topic_name, 1);
 
     pub_thread_ = new boost::thread(boost::bind(&sensorManager::publishDetection, this));
 };
@@ -179,6 +179,8 @@ void sensorManager::publishDetection()
         std::cout << "BEV objects: " << vGrid->vInsObjs.size() << std::endl;
 
         waytous_perception_msgs::ObjectArray obFrame;
+        waytous_perception_msgs::ObjectTrackArray obTrackFrame;
+
         int8_t obj_id = 0;
         for(auto& obj: vGrid->vInsObjs)
         {
@@ -187,10 +189,14 @@ void sensorManager::publishDetection()
                 continue;
             }
             waytous_perception_msgs::Object ob;
+            waytous_perception_msgs::ObjectTrack obTrack;
+
             ob.sensor_type = waytous_perception_msgs::Object::SENSOR_CAMERA;
+            obTrack.msgname = "camera";
 
             //int8 id = 1;   
             ob.id = obj_id++;
+            obTrack.track_id = ob.id;
 
             //int8 property = 2;
 
@@ -228,6 +234,10 @@ void sensorManager::publishDetection()
             ob.dimensions.y = (obj.rrc.size.height*BEV_GRID_RESOLUTION);
             ob.dimensions.z = 1;
 
+            // jsk_recognition_msgs/BoundingBox
+            obTrack.bounding_box.pose = ob.pose;
+            obTrack.bounding_box.dimensions = ob.dimensions;
+            
             //geometry_msgs/Polygon convex_hull = 7
             cv::Point2f vertices[4];
             obj.rrc.points(vertices);
@@ -257,6 +267,7 @@ void sensorManager::publishDetection()
 
             // int8  label_type = 9
             ob.label_type = (name2type.find(obj.names) == name2type.end() ? Perception_Default_Classification : name2type[obj.names]);
+            obTrack.track_type = (name2type.find(obj.names) == name2type.end() ? Perception_Default_Classification : name2type[obj.names]);
 
             //float32[]  type_probs = 10
             // std::vector<float> probs(waytous_perception_msgs::Object::TYPE_UNKNOWN_STATIC + 1, 0); // main-type length
@@ -280,8 +291,9 @@ void sensorManager::publishDetection()
             // geometry_msgs/Vector3[]                         trajectory              # optional
             // FleetObject                                     fleet_object            # optional    
             obFrame.foreground_objects.emplace_back(ob);
+            obTrackFrame.foreground_objects.emplace_back(obTrack);
         }
-        obj_publisher_.publish(obFrame);
+        obj_publisher_.publish(obTrackFrame); // obFrame or obTrackFrame
         std::cout<<" onRun :  "<<double(clock()-start_)/CLOCKS_PER_SEC<<"s.  "<< "Detected Objects: " 
             << obFrame.foreground_objects.size() <<std::endl;  //输出时间（单位：ｓ）