使能绕障

src/autonomy/fu/config/fu_params.yaml

@@ -4,7 +4,7 @@ FU:
     enable_obstacle_stop: True
 
     # 绕障功能 False or True
-    enable_obstacle_avoid: False  
+    enable_obstacle_avoid: True  
 
     # 可视化栅格 False or True
     enable_visualize_grid: False  

src/autonomy/fu/src/fu_node.cpp

@@ -13,7 +13,6 @@
 #include "logger/logger.h"
 #include "rclcpp/rclcpp.hpp"
 #include "sweeper_interfaces/msg/can_frame.hpp"
-#include "sweeper_interfaces/msg/detect_line.hpp"
 #include "sweeper_interfaces/msg/fu.hpp"
 #include "sweeper_interfaces/msg/mc_ctrl.hpp"
 #include "sweeper_interfaces/msg/pl.hpp"
@@ -71,8 +70,6 @@ class fu_node : public rclcpp::Node
 
         msg_subscribe_pl = this->create_subscription<sweeper_interfaces::msg::Pl>(
             "pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1));
-        msg_subscribe_DetectLine = this->create_subscription<sweeper_interfaces::msg::DetectLine>(
-            "airy_message", 10, std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1));
 
         // 发布控制指令
         pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);
@@ -291,7 +288,6 @@ class fu_node : public rclcpp::Node
     // 声明订阅者和发布者
     rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscription_grid;
     rclcpp::Subscription<sweeper_interfaces::msg::Pl>::SharedPtr msg_subscribe_pl;
-    rclcpp::Subscription<sweeper_interfaces::msg::DetectLine>::SharedPtr msg_subscribe_DetectLine;
     rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
 
     rclcpp::Publisher<sweeper_interfaces::msg::McCtrl>::SharedPtr pub_mc;
@@ -754,12 +750,6 @@ class fu_node : public rclcpp::Node
         LOG_INFO("目标点位置： x -- %.2f , y -- %.2f", x, y);
     }
 
-    void msg_callback_DetectLine(const sweeper_interfaces::msg::DetectLine::SharedPtr msg)
-    {
-        nl_adjust = 0 - msg->adjust + remain_adjust;
-        detected_line = msg->detected_line;
-    }
-
     void timer_callback()
     {
         if (speed == 0)

src/perception/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake

@@ -19,8 +19,8 @@ if(${ENABLE_TRANSFORM})
   add_definitions("-DENABLE_TRANSFORM")
 endif(${ENABLE_TRANSFORM})
 
-set(rs_driver_INCLUDE_DIRS "/home/nvidia/sweeper_200/src/perception/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
-set(RS_DRIVER_INCLUDE_DIRS "/home/nvidia/sweeper_200/src/perception/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
+set(rs_driver_INCLUDE_DIRS "/home/nvidia/Downloads/sweeper_200/src/perception/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
+set(RS_DRIVER_INCLUDE_DIRS "/home/nvidia/Downloads/sweeper_200/src/perception/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
 
 set(rs_driver_LIBRARIES "pthread;pcap")
 set(RS_DRIVER_LIBRARIES "pthread;pcap")

src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp

@@ -22,10 +22,11 @@
 using sensor_msgs::msg::PointCloud2;
 using std::placeholders::_1;
 
-// 点云帧结构体
+// 点云帧结构体（新增processed_cloud存储处理后的点云）
 struct CloudFrame
 {
-    std::shared_ptr<PointCloud2> cloud;
+    std::shared_ptr<PointCloud2> cloud;            // 原始点云
+    std::shared_ptr<PointCloud2> processed_cloud;  // 处理后的点云
     rclcpp::Time stamp;
     rclcpp::Time received_time;  // 添加接收时间用于延时分析
     std::string source;
@@ -193,7 +194,7 @@ class LidarMerger : public rclcpp::Node
     /* ---------- 同步处理点云 ---------- */
     void processCloudSync(const PointCloud2::SharedPtr msg, const std::string& source, const rclcpp::Time& receive_time)
     {
-        // 立即进行坐标变换
+        // 1. 立即进行坐标变换（原始点云）
         auto transformed_cloud = transformCloud(*msg);
         if (!transformed_cloud)
         {
@@ -201,11 +202,19 @@ class LidarMerger : public rclcpp::Node
             return;
         }
 
-        // 创建CloudFrame
-        auto cloud_frame = std::make_shared<CloudFrame>(
-            CloudFrame{transformed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
+        // 2. 对变换后的原始点云进行处理（过滤、降采样、离群点去除）
+        auto processed_cloud = processSinglePointCloud(transformed_cloud);
+        if (!processed_cloud)
+        {
+            RCLCPP_WARN(get_logger(), "Failed to process %s cloud", source.c_str());
+            return;
+        }
 
-        // 添加到缓存并立即尝试合并
+        // 3. 创建CloudFrame（存储原始+处理后的点云）
+        auto cloud_frame = std::make_shared<CloudFrame>(
+            CloudFrame{transformed_cloud, processed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
+
+        // 4. 添加到缓存并立即尝试合并
         {
             std::lock_guard<std::mutex> lock(cache_mutex_);
             addToCache(cloud_frame);
@@ -218,6 +227,47 @@ class LidarMerger : public rclcpp::Node
         printStats();
     }
 
+    /* ---------- 单雷达点云处理（独立处理前后雷达点云） ---------- */
+    std::shared_ptr<PointCloud2> processSinglePointCloud(const std::shared_ptr<PointCloud2>& cloud_msg)
+    {
+        if (!cloud_msg || cloud_msg->data.empty()) return nullptr;
+
+        // 将ROS点云消息转换为PCL点云
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pcl(new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::fromROSMsg(*cloud_msg, *cloud_pcl);
+
+        // 1. 条件过滤（含车身/主刷过滤）
+        auto filtered = applyConditionalFiltering(cloud_pcl);
+        if (filtered->empty())
+        {
+            RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Filtered cloud is empty!");
+            return nullptr;
+        }
+
+        // 2. 降采样
+        auto downsampled = applyVoxelGridFiltering(filtered);
+        if (downsampled->empty())
+        {
+            RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Downsampled cloud is empty!");
+            return nullptr;
+        }
+
+        // 3. 离群点去除
+        auto outlier_removed = applyStatisticalOutlierRemoval(downsampled);
+        if (outlier_removed->empty())
+        {
+            RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Outlier-removed cloud is empty!");
+            return nullptr;
+        }
+
+        // 转换回ROS消息
+        auto processed_msg = cloud_pool_.acquire();
+        pcl::toROSMsg(*outlier_removed, *processed_msg);
+        processed_msg->header = cloud_msg->header;
+
+        return processed_msg;
+    }
+
     /* ---------- 添加到缓存 ---------- */
     void addToCache(std::shared_ptr<CloudFrame> frame)
     {
@@ -235,11 +285,12 @@ class LidarMerger : public rclcpp::Node
         while (cache.size() > cache_size_)
         {
             cloud_pool_.release(cache.back()->cloud);
+            cloud_pool_.release(cache.back()->processed_cloud);  // 释放处理后的点云
             cache.pop_back();
         }
     }
 
-    /* ---------- 尝试合并 - 触发式 ---------- */
+    /* ---------- 尝试合并 - 触发式（合并处理后的点云） ---------- */
     void tryMerge()
     {
         if (front_clouds_.empty() || rear_clouds_.empty()) return;
@@ -287,9 +338,9 @@ class LidarMerger : public rclcpp::Node
             return;
         }
 
-        // 合并点云
+        // 合并处理后的点云（核心修改：不再合并原始点云）
         auto merge_start = std::chrono::high_resolution_clock::now();
-        auto merged = mergeClouds(*front_frame, *rear_frame);
+        auto merged = mergeProcessedClouds(*front_frame, *rear_frame);
         auto merge_end = std::chrono::high_resolution_clock::now();
 
         if (!merged) return;
@@ -301,115 +352,80 @@ class LidarMerger : public rclcpp::Node
         merged->header.frame_id = target_frame_;
 
         // 发布合并后的点云
-        // pub_->publish(*merged);
+        pub_->publish(*merged);
         merged_count_++;
 
-        // 将ROS点云消息转换为PCL点云
+        // ========================= 栅格化 =========================
+        // 将合并后的点云转换为PCL格式用于栅格化
         pcl::PointCloud<pcl::PointXYZ>::Ptr merged_pcl(new pcl::PointCloud<pcl::PointXYZ>);
         pcl::fromROSMsg(*merged, *merged_pcl);
 
-        // ========================= 点云 =========================
-        auto processed_pcl = processPointCloud(merged_pcl);
-        if (!processed_pcl) return;
-
-        // 将处理后的PCL点云转回ROS消息
-        auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>();
-        pcl::toROSMsg(*processed_pcl, *processed);
-        processed->header = merged->header;
-
-        // 发布处理后的点云
-        pub_->publish(*processed);
-        // ========================= 点云 =========================
-        // ========================= 栅格 =========================
-        auto grid = processPointCloud_grid(merged_pcl);
-        if (grid.empty()) return;
-
-        // 可视化栅格
-        if (enable_print_) visualizeGridInTerminal(grid);
-
-        // 发布栅格到ROS话题
-        publishGrid(grid);
-        // ========================= 栅格 =========================
+        // 生成栅格（无需重复处理，直接基于合并后的处理点云）
+        auto grid = generateOccupancyGrid(merged_pcl);
+        if (!grid.empty())
+        {
+            // 可视化栅格
+            if (enable_print_) visualizeGridInTerminal(grid);
+            // 发布栅格
+            publishGrid(grid);
+        }
 
         // 延时分析（降低频率）
         if (enable_debug_ && merged_count_ % 10 == 0)  // 每10次输出一次
         {
-            auto total_delay = (now_time - processed->header.stamp).seconds();
+            auto total_delay = (now_time - merged->header.stamp).seconds();
             auto process_time =
                 std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - merge_start).count();
 
             RCLCPP_INFO(
                 get_logger(),
-                "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs",
-                merged_count_, merged->data.size() / merged->point_step, processed->data.size() / processed->point_step,
-                total_delay, process_time * 1000, adaptive_threshold);
+                "Merged #%d: merged_points=%zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs",
+                merged_count_, merged->data.size() / merged->point_step, total_delay, process_time * 1000,
+                adaptive_threshold);
         }
 
         // 清理已使用的点云
         removeUsedClouds(front_frame, rear_frame);
     }
 
-    /* ---------- 点云处理流程 ---------- */
-    pcl::PointCloud<pcl::PointXYZ>::Ptr processPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
+    /* ---------- 合并处理后的点云 ---------- */
+    std::shared_ptr<PointCloud2> mergeProcessedClouds(const CloudFrame& front, const CloudFrame& rear)
     {
-        if (!cloud || cloud->empty()) return nullptr;
-
-        // 1. 条件过滤
-        auto filtered = applyConditionalFiltering(cloud);
-        if (filtered->empty())
+        if (!front.processed_cloud || !rear.processed_cloud)
         {
-            RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!");
-            return cloud;  // 返回原始云而不是nullptr
+            RCLCPP_ERROR(get_logger(), "Processed cloud is null!");
+            return nullptr;
         }
 
-        // 2. 降采样
-        auto downsampled = applyVoxelGridFiltering(filtered);
-        if (downsampled->empty())
+        size_t front_points = front.processed_cloud->data.size() / front.processed_cloud->point_step;
+        size_t rear_points = rear.processed_cloud->data.size() / rear.processed_cloud->point_step;
+
+        if (front.processed_cloud->fields != rear.processed_cloud->fields ||
+            front.processed_cloud->point_step != rear.processed_cloud->point_step ||
+            front.processed_cloud->is_bigendian != rear.processed_cloud->is_bigendian)
         {
-            RCLCPP_WARN(get_logger(), "[processPointCloud] Downsampled cloud is empty!");
-            return filtered;
+            RCLCPP_ERROR(get_logger(), "Processed PointCloud2 formats do not match!");
+            return nullptr;
         }
 
-        // 3. 离群点去除
-        auto outlier_removed = applyStatisticalOutlierRemoval(downsampled);
-        if (outlier_removed->empty())
-        {
-            RCLCPP_WARN(get_logger(), "[processPointCloud] Outlier-removed cloud is empty!");
-            return downsampled;
-        }
+        auto merged = cloud_pool_.acquire();
 
-        return outlier_removed;
-    }
-    std::vector<std::vector<int>> processPointCloud_grid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
-    {
-        if (!cloud || cloud->empty()) return {};  // 返回空栅格
+        // 复制元数据
+        merged->header = front.processed_cloud->header;
+        merged->fields = front.processed_cloud->fields;
+        merged->is_bigendian = front.processed_cloud->is_bigendian;
+        merged->point_step = front.processed_cloud->point_step;
+        merged->height = 1;
+        merged->width = front_points + rear_points;
 
-        // 1. 条件过滤
-        auto filtered = applyConditionalFiltering(cloud);
-        if (filtered->empty())
-        {
-            RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Filtered cloud is empty!");
-            return {};
-        }
+        // 高效合并处理后的点云数据
+        merged->data.clear();
+        merged->data.reserve(front.processed_cloud->data.size() + rear.processed_cloud->data.size());
+        merged->data.insert(merged->data.end(), front.processed_cloud->data.begin(), front.processed_cloud->data.end());
+        merged->data.insert(merged->data.end(), rear.processed_cloud->data.begin(), rear.processed_cloud->data.end());
+        merged->row_step = merged->data.size();
 
-        // 2. 降采样
-        auto downsampled = applyVoxelGridFiltering(filtered);
-        if (downsampled->empty())
-        {
-            RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Downsampled cloud is empty!");
-            return {};
-        }
-
-        // 3. 离群点去除
-        auto outlier_removed = applyStatisticalOutlierRemoval(downsampled);
-        if (outlier_removed->empty())
-        {
-            RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Outlier-removed cloud is empty!");
-            return {};
-        }
-
-        // 4. 栅格化处理并返回栅格结果
-        return generateOccupancyGrid(outlier_removed);
+        return merged;
     }
 
     /* ---------- 条件过滤 ---------- */
@@ -433,7 +449,6 @@ class LidarMerger : public rclcpp::Node
 
         // 不启用车身过滤，直接返回
         if (!filter_car_) return filtered;
-        // RCLCPP_INFO(get_logger(), "启用车身过滤");
 
         // 使用CropBox移除车身区域
         pcl::CropBox<pcl::PointXYZ> crop;
@@ -453,8 +468,6 @@ class LidarMerger : public rclcpp::Node
         pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
         crop.filter(*output);
 
-        // return output;
-
         // 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65)
         pcl::CropBox<pcl::PointXYZ> crop1;
         crop1.setInputCloud(output);
@@ -630,6 +643,7 @@ class LidarMerger : public rclcpp::Node
         // 发布消息
         grid_pub_->publish(msg);
     }
+
     /* ---------- 优化的匹配算法 ---------- */
     std::pair<std::shared_ptr<CloudFrame>, std::shared_ptr<CloudFrame>> findBestMatchOptimized()
     {
@@ -685,6 +699,7 @@ class LidarMerger : public rclcpp::Node
         if (front_it != front_clouds_.end())
         {
             cloud_pool_.release((*front_it)->cloud);
+            cloud_pool_.release((*front_it)->processed_cloud);
             front_clouds_.erase(front_it);
         }
 
@@ -693,6 +708,7 @@ class LidarMerger : public rclcpp::Node
         if (rear_it != rear_clouds_.end())
         {
             cloud_pool_.release((*rear_it)->cloud);
+            cloud_pool_.release((*rear_it)->processed_cloud);
             rear_clouds_.erase(rear_it);
         }
 
@@ -717,6 +733,7 @@ class LidarMerger : public rclcpp::Node
                     RCLCPP_DEBUG(get_logger(), "Cleaning up old front cloud, age=%.3fs", age);
                 }
                 cloud_pool_.release(front_clouds_.back()->cloud);
+                cloud_pool_.release(front_clouds_.back()->processed_cloud);
                 front_clouds_.pop_back();
             }
             else
@@ -736,6 +753,7 @@ class LidarMerger : public rclcpp::Node
                     RCLCPP_DEBUG(get_logger(), "Cleaning up old rear cloud, age=%.3fs", age);
                 }
                 cloud_pool_.release(rear_clouds_.back()->cloud);
+                cloud_pool_.release(rear_clouds_.back()->processed_cloud);
                 rear_clouds_.pop_back();
             }
             else
@@ -745,39 +763,6 @@ class LidarMerger : public rclcpp::Node
         }
     }
 
-    /* ---------- 合并点云 ---------- */
-    std::shared_ptr<PointCloud2> mergeClouds(const CloudFrame& front, const CloudFrame& rear)
-    {
-        size_t front_points = front.cloud->data.size() / front.cloud->point_step;
-        size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step;
-
-        if (front.cloud->fields != rear.cloud->fields || front.cloud->point_step != rear.cloud->point_step ||
-            front.cloud->is_bigendian != rear.cloud->is_bigendian)
-        {
-            RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!");
-            return nullptr;
-        }
-
-        auto merged = cloud_pool_.acquire();
-
-        // 复制元数据
-        merged->header = front.cloud->header;
-        merged->fields = front.cloud->fields;
-        merged->is_bigendian = front.cloud->is_bigendian;
-        merged->point_step = front.cloud->point_step;
-        merged->height = 1;
-        merged->width = front_points + rear_points;
-
-        // 高效合并数据
-        merged->data.clear();
-        merged->data.reserve(front.cloud->data.size() + rear.cloud->data.size());
-        merged->data.insert(merged->data.end(), front.cloud->data.begin(), front.cloud->data.end());
-        merged->data.insert(merged->data.end(), rear.cloud->data.begin(), rear.cloud->data.end());
-        merged->row_step = merged->data.size();
-
-        return merged;
-    }
-
     /* ---------- 坐标变换 - 简化版 ---------- */
     std::shared_ptr<PointCloud2> transformCloud(const PointCloud2& in)
     {