add lidar2vechicle mutex

README.md

@@ -2,7 +2,7 @@
  * @Author: yangxue && xue.yang@waytous.com
  * @Date: 2025-04-07 09:26:17
  * @LastEditors: yangxue xue.yang@waytous.com
- * @LastEditTime: 2025-04-08 06:24:07
+ * @LastEditTime: 2025-04-10 09:45:19
  * @FilePath: /ubuntu/projects/ros_camera_bev/README.md
  * @Description: 
  * 
@@ -58,5 +58,5 @@ rviz
 ```
 # 压缩部署关键文件
 ```
-zip -r ros_camera_bev.zip ros_camera_bev/ -x "ros_camera_bev/.vscode/*" -x "ros_camera_bev/devel/*" -x "ros_camera_bev/build/*" -x "ros_camera_bev/sdk/*" -x "ros_camera_bev/src/camera_back_warn/*" -x "*.engine" -x "*.bag" -x "ros_camera_bev/.git/*"
+zip -r ros_camera_bev.zip ros_camera_bev/ -x "ros_camera_bev/.vscode/*" -x "ros_camera_bev/devel/*" -x "ros_camera_bev/build/*" -x "ros_camera_bev/sdk/*" -x "ros_camera_bev/src/camera_back_warn/*" -x "*.engine" -x "*.bag" -x "ros_camera_bev/.git/*" -x "*.gitignore"
 ```
\ No newline at end of file

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-04-10 06:31:09
+ * @LastEditTime: 2025-04-17 03:47:34
  */
 
 
@@ -171,7 +171,11 @@ void sensorManager::publishDetection()
 
         if(point_cloud_ptr != nullptr){
             Cloud::Ptr pcl_cloud(new Cloud);
-            pcl::fromROSMsg(*point_cloud_ptr, *pcl_cloud);
+            {
+                std::lock_guard<std::mutex> lock_(pointcloud_mutex);
+                pcl::fromROSMsg(*point_cloud_ptr, *pcl_cloud);
+            }
+            
             // 2d->bev，结果保存到vGrid中 (frame->objs_有track_id, vGrid->vInsObjs没有)
             // vision_converter_->Process_objects(frame_ptr->img_src_, pcl_cloud, frame_ptr->objs_, *vGrid);
             vision_converter_->Process_objects(frame_ptr->img_src_, frame_ptr->depth_, pcl_cloud, frame_ptr->objs_, *vGrid);
@@ -229,9 +233,11 @@ void sensorManager::publishDetection()
                                     * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitZ());
             ::Eigen::Quaterniond orient;  
             orient = matR;
-
-            ob.pose.position.x = pos.x + lidar2vehicle_(0,3);
-            ob.pose.position.y = pos.y + lidar2vehicle_(1,3);
+            {
+                std::lock_guard<std::mutex> lock_(tf_mutex);
+                ob.pose.position.x = pos.x + lidar2vehicle_(0,3);
+                ob.pose.position.y = pos.y + lidar2vehicle_(1,3);
+            }
             ob.pose.position.z = 0;
             ob.pose.orientation.x = orient.x();
             ob.pose.orientation.y = orient.y();

src/camera_bev_infer/src/sensor_manager.h

@@ -5,7 +5,7 @@
  * @Date: 2023-09-03 01:53:20
  * @email: xin.wang@waytous.com
  * @LastEditors: yangxue xue.yang@waytous.com
- * @LastEditTime: 2025-04-10 06:33:42
+ * @LastEditTime: 2025-04-17 03:46:30
  */
 
 #ifndef SENSORMANAGER_H
@@ -148,31 +148,22 @@ public:
         {
 
             std::lock_guard<std::mutex> lock_(pointcloud_mutex);
-            pc_timestamp_sys_ = GetMillSecondTimeStamp();           
-           
-            ros::Time input_time = pdata->header.stamp;
-            std::string cloud_frame = pdata->header.frame_id;
-            current_pointcloud_ = pdata;            
-
-            // // 初始化 current_pointcloud_
-            // current_pointcloud_ = boost::make_shared<sensor_msgs::PointCloud2>();
+            pc_timestamp_sys_ = GetMillSecondTimeStamp();
+            current_pointcloud_ = pdata; 
             
-            // // std::cout << "get lidar2vehicle & transform pc to vehicle frame: base_frame:" << base_frame_ << ",  lidar frame:" << cloud_frame << std::endl;
-            tran_->waitForTransform(base_frame_, cloud_frame, input_time, ros::Duration(10.0));  // destination_frame, original_frame, cant tf tree receive
-            tran_->lookupTransform(base_frame_, cloud_frame, ros::Time(0), lidar2vehicle_tf_);            
-            // pcl_ros::transformPointCloud(base_frame_, *pdata, *current_pointcloud_, *tran_);
-            // std::cout << "current_pointcloud_ header: " << current_pointcloud_->header.stamp << std::endl;
-            // std::cout << "current_pointcloud_ frame_id: " << current_pointcloud_->header.frame_id << std::endl;
-            
-            // std::cout << "update lidar2camera  with lidar2vehicle_: " << std::endl;
-            // update lidar2camera  with lidar2vehicle_
-            lidar2vehicle_ = transformTFToEigen(lidar2vehicle_tf_);
-
-            // Eigen::Matrix4d camera2lidar = infer_param.cameraParam_->camera2lidar_extrinsic;
-            // Eigen::Matrix4d camera2lidar_new = lidar2vehicle * camera2lidar; // camera2vehicle actually!
-            // std::cout << "camera2lidar: \n" << camera2lidar << std::endl;
-            // std::cout << "lidar2vehicle: \n" << lidar2vehicle_ << std::endl;
-            // vision_converter_->setLidar2Camera(camera2lidar_new.inverse());
+            if(!is_lidar2vehicle_) {
+                std::lock_guard<std::mutex> lock_(tf_mutex);
+                std::cout << "get lidar2vehicle transform" << std::endl;
+                ros::Time input_time = pdata->header.stamp;
+                std::string cloud_frame = pdata->header.frame_id;                  
+    
+                tran_->waitForTransform(base_frame_, cloud_frame, input_time, ros::Duration(10.0));  // destination_frame, original_frame, cant tf tree receive
+                tran_->lookupTransform(base_frame_, cloud_frame, ros::Time(0), lidar2vehicle_tf_);          
+    
+                // update lidar2camera  with lidar2vehicle_
+                lidar2vehicle_ = transformTFToEigen(lidar2vehicle_tf_);
+                is_lidar2vehicle_ = true;
+            }        
         }
     };
 
@@ -192,7 +183,11 @@ public:
             if(std::abs(tm_now - pc_timestamp_sys_) > pc_timevalid_interval_){
                 current_pointcloud_ = nullptr;
             }
-            std::cout << "getCurPointcloud: " << temp->header.stamp << std::endl;
+            if(temp == nullptr) {
+                std::cout << "getCurPointcloud: nullptr" << std::endl;
+            } else {
+                std::cout << "getCurPointcloud: " << temp->header.stamp << std::endl;
+            }
             return temp;
         }
     }
@@ -223,7 +218,8 @@ private:
     tf::TransformListener *tran_;         // tf监听器
     std::string base_frame_ = "vehicle_link";              // 目标坐标系
     tf::StampedTransform lidar2vehicle_tf_; // lidar-vehicle transform
-    Eigen::Matrix4d lidar2vehicle_;    
+    Eigen::Matrix4d lidar2vehicle_;
+    bool is_lidar2vehicle_ = false; // 是否有lidar-vehicle transform
 
     ros::Publisher obj_publisher_; // 交通障碍物发布者
     ros::Subscriber image_subscriber_; // 图像订阅者
@@ -233,6 +229,7 @@ private:
     std::mutex image_mutex; // 管理图像信息的互斥量
     std::mutex pointcloud_mutex; // 管理点云信息的互斥量
     std::mutex ts_mutex; // 管理时间戳的互斥量
+    std::mutex tf_mutex; // 管理时间戳的互斥量
 
     int64_t img_timestamp_sys_ = 0;  //接收到图像消息的系统时间
     int64_t pc_timestamp_sys_ = 0;  //接收到点云消息的系统时间