update

src/guangpo_yolo_lidar/src/main.cpp

@@ -126,6 +126,9 @@ public:
         pub_box3d_markers_topic, 10);
     std::string send_image_topic =
         yaml_config["pub_image_topic"].as<std::string>();
+    std::string send_image_raw_topic =
+        yaml_config["pub_image_raw_topic"].as<std::string>();
+    image_raw_pub_ = nh.advertise<sensor_msgs::Image>(send_image_raw_topic, 1);
     image_pub_ = nh.advertise<sensor_msgs::Image>(send_image_topic, 1);
     std::string send_lidar_topic =
         yaml_config["pub_lidar_topic"].as<std::string>();
@@ -192,6 +195,16 @@ public:
     image_pub_.publish(msg);
     ROS_INFO("Image published!");
   }
+  void SendImageRaw(const cv::Mat &image) {
+    // 将cv::Mat图像转换为sensor_msgs::Image消息
+    sensor_msgs::ImagePtr msg =
+        cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
+    msg->header.stamp = ros::Time::now();
+
+    // 发布图像消息
+    image_raw_pub_.publish(msg);
+    ROS_INFO("Image Raw published!");
+  }
 
   // 输入的点云是激光坐标系下的,发布的是车辆坐标系
   void SendLidar(ros::Publisher &pub,
@@ -291,12 +304,12 @@ public:
     visualization_msgs::MarkerArray marker_array;
 
     // 首先发布一个DELETEALL命令,删除上次发布的所有障碍物标记
-    visualization_msgs::Marker delete_marker;
-    delete_marker.header.frame_id = "map";
-    delete_marker.action = visualization_msgs::Marker::LINE_STRIP;
-    marker_array.markers.push_back(delete_marker);
-    pub_3dbox_.publish(marker_array);
-    marker_array.markers.clear(); // 清空marker_array
+    // visualization_msgs::Marker delete_marker;
+    // delete_marker.header.frame_id = "map";
+    // delete_marker.action = visualization_msgs::Marker::LINE_STRIP;
+    // marker_array.markers.push_back(delete_marker);
+    // pub_3dbox_.publish(marker_array);
+    // marker_array.markers.clear(); // 清空marker_array
 
     for (size_t i = 0; i < obstacle_polygons.size(); ++i) {
       visualization_msgs::Marker marker;
@@ -318,6 +331,7 @@ public:
         p.y = point.y;
         p.z = 0.0; // 2D 矩形,高度设为0
         marker.points.push_back(p);
+        gDebugWarn() << VAR(p.x,p.y);
       }
       p.x = obstacle_polygons[i][0].x;
       p.y = obstacle_polygons[i][0].y;
@@ -399,6 +413,8 @@ public:
     if (img_distort_enable) {
       help.UndistortImage(image);
     }
+    // send image raw
+    SendImageRaw(image);
     // 对点云进行降采样
     if (leaf_size > 0) {
       // 创建 VoxelGrid 滤波器
@@ -690,6 +706,7 @@ private:
   ros::Publisher pub_obstacles_;
 
   ros::Publisher image_pub_;
+  ros::Publisher image_raw_pub_;
   ros::Publisher lidar_pub1_;
   ros::Publisher lidar_pub2_;
   ros::Publisher pub_cloud_noground_;
@@ -726,4 +743,4 @@ int main(int argc, char **argv) {
   ros::spin();
 
   return 0;
-}
+}

src/guangpo_yolo_lidar/yaml_config/yaml_config.yaml

@@ -2,9 +2,10 @@
 sub_image_topic : "/usb_cam/image_raw"
 sub_pointcloud_topic : "/livox/lidar_192_168_20_105"
 pub_image_topic : "detect_img"
+pub_image_raw_topic : "image_raw"
 pub_lidar_topic : "detect_lidar"
 pub_cloud_noground_topic : "cloud_noground"
-pub_box3d_markers_topic : "box3d_markers"
+pub_box3d_markers_topic : "/box3d_markers"
 
 # 使用仿真还是激光雷达
 # 使用仿真就是图像数据从功能包接收
@@ -34,14 +35,14 @@ max_iterations : 400
 # 图像是否去畸变,会增加约12ms耗时
 img_distort_enable : true
 # 点云降采样大小,单位 m,小于等于0就是不降采样
-leaf_size : 0.05
+leaf_size : 0.02
 
 # 点云dbscan聚类
 cluster :
   octreeResolution : 1
   minPtsAux : 20
-  epsilon : 0.03
-  minPts : 10
+  epsilon : 0.08
+  minPts : 5
   # 欧式聚类
   cluster_tolerance : 0.08
   min_cluster_size : 10
@@ -75,7 +76,13 @@ convert_lidar2car:
 # 相机到激光雷达坐标系是使用matrix还是用欧拉角
 if_use_matrix : true
 # 相机到激光雷达坐标系变换
-convert_carmera2lidar_matrix: [1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16]
+convert_carmera2lidar_matrix: [ 9.8926083033897869e-02, 9.9507023727951061e-01,
+       6.9894904012135772e-03, -4.2821240081363088e-01,
+       -2.5562146328092408e-02, -4.4804609003203655e-03,
+       9.9966319435358919e-01, 6.4979552549797051e-02,
+       9.9476640814347539e-01, -9.9071430546949363e-02,
+       2.4992896565013255e-02, 1.2746014762805993e-01, 0., 0., 0., 1. ]
+
 
 # 相机到激光雷达坐标系变换
 # convert_carmera2lidar:
@@ -111,4 +118,4 @@ camera_in_param:
 # 启动时warmup并生成测试
 warmup: false
 # 生成debug图片
-# write_img: false
+# write_img: false