modify 0325熬夜

.vscode/launch.json

@@ -1,6 +1,30 @@
 {
     "version": "0.2.0",
     "configurations": [
+        {
+            "name": "(gdb) Launch-1",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "/home/zq/zkf/recharge/build/recharge/wheelchair_state_machine_node",
+            "args": [],
+            "stopAtEntry": false,
+            "cwd": "/home/zq/zkf/recharge",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "gdb",
+            "setupCommands": [
+                {
+                    "description": "Enable pretty-printing for gdb",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                },
+                {
+                    "description": "Set Disassembly Flavor to Intel",
+                    "text": "-gdb-set disassembly-flavor intel",
+                    "ignoreFailures": true
+                }
+            ]
+        },
         {
             "name": "Launch Chrome",
             "request": "launch",
@@ -9,57 +33,24 @@
             "webRoot": "${workspaceFolder}"
         },
         {
-            "name": "调试轮椅状态机",
+            "name": "recharge debug",
             "type": "cppdbg",
             "request": "launch",
-            "program": "${workspaceFolder}/build/wheelchair_state_machine/wheelchair_state_machine_node",
+            "program": "${workspaceFolder}/install/recharge/lib/recharge/wheelchair_state_machine_node",
             "args": [],
             "stopAtEntry": false,
             "cwd": "${workspaceFolder}",
             "environment": [],
             "externalConsole": false,
             "MIMode": "gdb",
-            "miDebuggerPath": "/usr/bin/gdb",
+            "miDebuggerPath": "gdb",
             "setupCommands": [
                 {
                     "description": "启用GDB整齐打印",
                     "text": "-enable-pretty-printing",
                     "ignoreFailures": true
-                },
-                {
-                    "description": "设置GDB在进入没有调试信息的函数时自动跳过",
-                    "text": "set step-mode off",
-                    "ignoreFailures": true
-                },
-                {
-                    "description": "禁用源文件查找错误提示",
-                    "text": "set source open off",
-                    "ignoreFailures": true
-                },
-                {
-                    "description": "设置反汇编风格",
-                    "text": "-gdb-set disassembly-flavor intel",
-                    "ignoreFailures": true
-                },
-                {
-                    "description": "设置当找不到源文件时继续执行",
-                    "text": "set print source-filename off",
-                    "ignoreFailures": true
                 }
-            ],
-            "preLaunchTask": "编译轮椅状态机",
-            "postDebugTask": "",
-            "logging": {
-                "engineLogging": false,
-                "programOutput": true,
-                "exceptions": true,
-                "moduleLoad": false,
-                "trace": false
-            },
-            "sourceFileMap": {
-                "/workspace": "${workspaceFolder}",
-                "/home/ros2_ws/src": "${workspaceFolder}"
-            }
+            ]
         },
         {
             "name": "带参数调试",

rosbag2_2026_03_24-17_25_44/metadata.yaml

@@ -1,38 +0,0 @@
-rosbag2_bagfile_information:
-  version: 5
-  storage_identifier: sqlite3
-  duration:
-    nanoseconds: 0
-  starting_time:
-    nanoseconds_since_epoch: 9223372036854775807
-  message_count: 0
-  topics_with_message_count:
-    - topic_metadata:
-        name: /scan/back
-        type: sensor_msgs/msg/LaserScan
-        serialization_format: cdr
-        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 2\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
-      message_count: 0
-    - topic_metadata:
-        name: /scan/out
-        type: sensor_msgs/msg/LaserScan
-        serialization_format: cdr
-        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
-      message_count: 0
-    - topic_metadata:
-        name: /nav/control
-        type: geometry_msgs/msg/Twist
-        serialization_format: cdr
-        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
-      message_count: 0
-  compression_format: ""
-  compression_mode: ""
-  relative_file_paths:
-    - rosbag2_2026_03_24-17_25_44_0.db3
-  files:
-    - path: rosbag2_2026_03_24-17_25_44_0.db3
-      starting_time:
-        nanoseconds_since_epoch: 9223372036854775807
-      duration:
-        nanoseconds: 0
-      message_count: 0

rosbag2_2026_03_24-17_37_59/metadata.yaml

@@ -1,32 +0,0 @@
-rosbag2_bagfile_information:
-  version: 5
-  storage_identifier: sqlite3
-  duration:
-    nanoseconds: 171504
-  starting_time:
-    nanoseconds_since_epoch: 1774345138077710357
-  message_count: 2
-  topics_with_message_count:
-    - topic_metadata:
-        name: /nav/control
-        type: geometry_msgs/msg/Twist
-        serialization_format: cdr
-        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
-      message_count: 2
-    - topic_metadata:
-        name: /scan/out
-        type: sensor_msgs/msg/LaserScan
-        serialization_format: cdr
-        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
-      message_count: 0
-  compression_format: ""
-  compression_mode: ""
-  relative_file_paths:
-    - rosbag2_2026_03_24-17_37_59_0.db3
-  files:
-    - path: rosbag2_2026_03_24-17_37_59_0.db3
-      starting_time:
-        nanoseconds_since_epoch: 1774345138077710357
-      duration:
-        nanoseconds: 171504
-      message_count: 2

src/recharge/include/lidascan_ctrl.hpp

@@ -93,7 +93,7 @@ private:
 
     // 激光数据处理
     void processLaserScan(const LaserScanSharedPtr &msg);
-    bool processSegmentData(const LaserScanSharedPtr &msg, int begin_index, int end_index);
+    bool processSegmentData(LaserScanSharedPtr msg, int begin_index, int end_index);
     void procData(RechargeResult &result);
 
     // 运动控制算法
@@ -129,7 +129,7 @@ private:
     // ========== 辅助函数 ==========
     void publishFilteredScan(const LaserScanSharedPtr &original_msg,
                              const std::vector<float> &filtered_ranges);
-    LaserScanSharedPtr resetLaserData(const LaserScanSharedPtr & msg, int b_index, int e_index);
+    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 180; }

src/recharge/include/wheelchair_types.hpp

@@ -66,6 +66,7 @@ struct ScanFilterParam
     int min_count = 4;
     double dock_length = 0.42;
     int baselink_back_index = 337;
+    double dy_diff = 0.125;
 };
 
 struct RechargeResult

src/recharge/src/lidascan_ctrl.cpp

@@ -231,7 +231,7 @@ void LidarScanController::processLaserScan(const LaserScanSharedPtr &input_msg)
     // printf("debug %d\n", __LINE__);
 }
 
-bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
+bool LidarScanController::processSegmentData(LaserScanSharedPtr msg,
                                              int begin_index, int end_index)
 {
     // 转换到XY坐标系
@@ -409,6 +409,8 @@ void LidarScanController::procData(RechargeResult &result)
     // 计算交点
     double x = (b2 - b1) / (k1 - k2);
     double y = k1 * x + b1;
+    x = std::abs(x);
+    y = -1.0 * y;
 
     // 更新中间点
     result.middle_point = IndexPoint(x, y, 0);
@@ -487,7 +489,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
     RCLCPP_DEBUG(node_->get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);
 
     // 调整 dy
-    dy = dy - 0.10;
+    dy = dy - param_.dy_diff;
     RCLCPP_DEBUG(node_->get_logger(), "调整后 dy: %.4f", dy);
 
     // 计算偏航角误差
@@ -696,7 +698,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         stopNavigation();
 
         // 模拟检测到充电电压
-        setChargingVoltage(true);
+        // setChargingVoltage(true);
     }
 }
 
@@ -793,7 +795,8 @@ void LidarScanController::filterScanModify2(LaserScanSharedPtr  msg)
 void LidarScanController::filterScanModify3(LaserScanSharedPtr msg)
 {
     // 过滤强度
-    for (size_t i = 0; i < msg->ranges.size(); i++)
+    int max_index = static_cast<int>(std::min(msg->ranges.size(), msg->intensities.size()));
+    for (size_t i = 0; i < max_index; i++)
     {
         // RCLCPP_WARN(node_->get_logger(), "索引[%zu]: 原始距离=%.3f, 原始强度=%.2f", 
         //              i, msg->ranges[i], msg->intensities[i]);
@@ -1299,34 +1302,36 @@ void LidarScanController::publishFilteredScan(
 }
 
 LidarScanController::LaserScanSharedPtr LidarScanController::resetLaserData(
-    const LaserScanSharedPtr & input_msg,
+    LaserScanSharedPtr msg,
     int b_index, int e_index)
 {
-    LaserScanSharedPtr output_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
-    //拷貝input_msg 到 output_msg;
-    output_msg->header = input_msg->header;
-    output_msg->angle_min = input_msg->angle_min;
-    output_msg->angle_max = input_msg->angle_max;
-    output_msg->angle_increment = input_msg->angle_increment;
-    output_msg->time_increment = input_msg->time_increment;
-    output_msg->scan_time = input_msg->scan_time;
-    output_msg->range_min = input_msg->range_min;
-    output_msg->range_max = input_msg->range_max;
-    output_msg->ranges = input_msg->ranges;
-    output_msg->intensities = input_msg->intensities;
+    // LaserScanSharedPtr output_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
+    // //拷貝input_msg 到 output_msg;
+    // output_msg->header = input_msg->header;
+    // output_msg->angle_min = input_msg->angle_min;
+    // output_msg->angle_max = input_msg->angle_max;
+    // output_msg->angle_increment = input_msg->angle_increment;
+    // output_msg->time_increment = input_msg->time_increment;
+    // output_msg->scan_time = input_msg->scan_time;
+    // output_msg->range_min = input_msg->range_min;
+    // output_msg->range_max = input_msg->range_max;
+    // output_msg->ranges = input_msg->ranges;
+    // output_msg->intensities = input_msg->intensities;
 
     
-    auto& msg = output_msg;
+    // auto& msg = output_msg;
     std::vector<float> filtered_ranges = msg->ranges;
     std::vector<float> intensities = msg->intensities;
 
-    for (int i = 0; i < b_index; i++)
+    int max_index = static_cast<int>(std::min(msg->ranges.size(), msg->intensities.size()));
+
+    for (int i = 0; i < b_index && i < max_index; i++)
     {
         filtered_ranges[i] = std::numeric_limits<float>::infinity();
         intensities[i] = 0.0;
     }
 
-    for (int i = e_index; i < static_cast<int>(msg->ranges.size()); i++)
+    for (int i = e_index; i < max_index; i++)
     {
         filtered_ranges[i] = std::numeric_limits<float>::infinity();
         intensities[i] = 0.0;