recharge

.vscode/launch.json

@@ -1,6 +1,13 @@
 {
     "version": "0.2.0",
     "configurations": [
+        {
+            "name": "Launch Chrome",
+            "request": "launch",
+            "type": "chrome",
+            "url": "http://localhost:8080",
+            "webRoot": "${workspaceFolder}"
+        },
         {
             "name": "调试轮椅状态机",
             "type": "cppdbg",

src/recharge/include/lidascan_ctrl.hpp

@@ -131,7 +131,7 @@ private:
     LaserScanSharedPtr resetLaserData(LaserScanSharedPtr msg, int b_index, int e_index);
     bool isValid(float value) const;
     int getFixedIndex(const LaserScanSharedPtr &msg, int i) const;
-    int getFixed() const { return 0; }
+    int getFixed() const { return 180; }
     Eigen::MatrixXd getPointList(const std::vector<IndexPoint> &points);
 
     // 定时器回调

src/recharge/include/wheelchair_types.hpp

@@ -57,8 +57,8 @@ struct StraightLine
 
 struct ScanFilterParam
 {
-    int min_index = 292;
-    int max_index = 382;
+    int min_index = 0;
+    int max_index = 447;
     double min_intensities = 120.0;
     double max_intensities = 220.0;
     double min_distance = 0.28;

src/recharge/src/lidascan_ctrl.cpp

@@ -48,6 +48,7 @@ void LidarScanController::initialize()
         [this](const LaserScanSharedPtr msg)
         {
             if (active_)
+                //RCLCPP_WARN(node_->get_logger(), "开始回充");
                 processLaserScan(msg);
         });
 
@@ -179,21 +180,21 @@ void LidarScanController::processLaserScan(const LaserScanSharedPtr &msg)
 {
     if (!active_)
         return;
-
     // 应用过滤算法
     filterScanModify0(msg);
     filterScanModify1(msg);
     filterScanModify2(msg);
     filterScanModify3(msg);
+    //publishFilteredScan(msg,msg->ranges);
 
     auto [begin_index, end_index] = filterScanModify4(msg);
-
+     
     if (end_index - begin_index < 2)
     {
-        RCLCPP_DEBUG(node_->get_logger(), "没有可连续性的点");
+        //RCLCPP_WARN(node_->get_logger(), "没有可连续性的点");
         return;
     }
-
+    //RCLCPP_WARN(node_->get_logger(), "开始回充");
     // 处理分段数据
     if (processSegmentData(msg, begin_index, end_index))
     {
@@ -267,7 +268,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
 
     // 重置激光数据
     auto filtered_msg = resetLaserData(msg, begin_index, end_index);
-
+    //RCLCPP_WARN(node_->get_logger(), "开始发布");
     // 发布过滤后的数据
     publishFilteredScan(filtered_msg, filtered_msg->ranges);
 
@@ -692,9 +693,10 @@ void LidarScanController::filterScanModify0(LaserScanSharedPtr msg)
 {
     int fixed = getFixed();
     double diff_yaw = msg->angle_increment * fixed;
+    // RCLCPP_WARN(node_->get_logger(), "diff_raw=%.2f",diff_yaw);
     msg->angle_min -= diff_yaw;
     msg->angle_max -= diff_yaw;
-
+{
     std::vector<float> ranges = msg->ranges;
     for (size_t i = 0; i < msg->ranges.size(); i++)
     {
@@ -703,6 +705,17 @@ void LidarScanController::filterScanModify0(LaserScanSharedPtr msg)
     }
     msg->ranges = ranges;
 }
+     {
+        //新增 intensities错位
+        std::vector<float> intensities = msg->intensities;
+        for (size_t i = 0; i < msg->intensities.size(); i++)
+        {
+            int index = getFixedIndex(msg, i);
+            intensities[index] = msg->intensities[i];
+        }
+        msg->intensities = intensities;
+    }
+}
 
 void LidarScanController::filterScanModify1(LaserScanSharedPtr msg)
 {
@@ -740,12 +753,18 @@ void LidarScanController::filterScanModify3(LaserScanSharedPtr msg)
     // 过滤强度
     for (size_t i = 0; i < msg->ranges.size(); i++)
     {
+        // RCLCPP_WARN(node_->get_logger(), "索引[%zu]: 原始距离=%.3f, 原始强度=%.2f", 
+        //              i, msg->ranges[i], msg->intensities[i]);
         if (!isValid(msg->ranges[i]))
+        {
             continue;
-
+        }
         float value = msg->intensities[i];
+        // RCLCPP_WARN(node_->get_logger(), "索引[%zu]: 当前强度值=%.2f", i, value);
         if (value < param_.min_intensities || value > param_.max_intensities)
         {
+            //  RCLCPP_WARN(node_->get_logger(), "索引[%zu]: 强度过滤 - 值=%.2f 超出范围[%.2f, %.2f], 设为inf", 
+            //              i, value, param_.min_intensities, param_.max_intensities);
             msg->ranges[i] = std::numeric_limits<float>::infinity();
         }
 
@@ -1224,7 +1243,7 @@ void LidarScanController::publishFilteredScan(
     msg->intensities = original_msg->intensities;
 
     pub_filtered_->publish(*msg);
-    RCLCPP_DEBUG(node_->get_logger(), "已发布过滤后的激光数据");
+    RCLCPP_WARN(node_->get_logger(), "已发布过滤后的激光数据");
 }
 
 LidarScanController::LaserScanSharedPtr LidarScanController::resetLaserData(