recharge

.vscode/settings.json

@@ -1,3 +1,3 @@
 {
-    "cmake.sourceDirectory": "/home/wljy/zkf/recharge/src/wheelchair_state_machine"
+    "cmake.sourceDirectory": "/home/zq/zkf/recharge/src/recharge"
 }

rosbag2_2026_03_24-17_25_44/metadata.yaml

@@ -0,0 +1,38 @@
+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

@@ -0,0 +1,32 @@
+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

@@ -29,7 +29,7 @@ public:
     void initialize();
 
     // 新增：获取当前检测到的角平分线信息
-    std::pair<double, double> getAngleBisectorInfo() const { return {current_angle_bisector_k_, current_angle_bisector_b_}; }
+    //std::pair<double, double> getAngleBisectorInfo() const { return {current_angle_bisector_k_, current_angle_bisector_b_}; }
 
     // 导航控制接口（外部调用）
     void startNavigation();
@@ -100,7 +100,7 @@ private:
     void controlRechargeMotion(const RechargeResult &result);
 
     // 过滤算法
-    void filterScanModify0(LaserScanSharedPtr msg);
+    void filterScanModify0(LaserScanSharedPtr  msg);
     void filterScanModify1(LaserScanSharedPtr msg);
     void filterScanModify2(LaserScanSharedPtr msg);
     void filterScanModify3(LaserScanSharedPtr msg);

src/recharge/src/lidascan_ctrl.cpp

@@ -177,8 +177,23 @@ void LidarScanController::navigationTimerCallback()
     }
 }
 
-void LidarScanController::processLaserScan(const LaserScanSharedPtr &msg)
+void LidarScanController::processLaserScan(const LaserScanSharedPtr &input_msg)
 {
+    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;
     // std::lock_guard<std::mutex> lock(data_mutex_);
     if (!active_)
         return;
@@ -220,7 +235,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
                                              int begin_index, int end_index)
 {
     // 转换到XY坐标系
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
     auto xy_list = laserScanToXY1(begin_index, end_index, msg);
     if (xy_list.size() < 2)
     {
@@ -228,7 +243,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         return false;
     }
 
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
     // 获取分段
     auto sections = getSections(begin_index, end_index, msg, xy_list);
     if (sections.empty())
@@ -236,7 +251,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         // RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
         return false;
     }
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
 
     // 选择最近的分段
     int nearest_index = -1;
@@ -275,7 +290,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         }
     }
 
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
     if (nearest_index < 0)
     {
         return false;
@@ -284,14 +299,13 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
     begin_index = sections[nearest_index].first;
     end_index = sections[nearest_index].second;
 
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
     // 重置激光数据
     auto filtered_msg = resetLaserData(msg, begin_index, end_index);
     //RCLCPP_WARN(node_->get_logger(), "开始发布");
     // 发布过滤后的数据
     publishFilteredScan(filtered_msg, filtered_msg->ranges);
-
-    printf("debug %d\n", __LINE__);
+    //printf("debug %d\n", __LINE__);
     // 进一步处理
     auto xy_list_full = laserScanToXY(filtered_msg);
     if (xy_list_full.size() < 2)
@@ -300,12 +314,12 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         return false;
     }
 
-    printf("debug %d\n", __LINE__);
+    //printf("debug %d\n", __LINE__);
     // 获取分段
     auto turn_segments = getSegments(xy_list_full);
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
     auto filtered_segments = filterSegments(turn_segments);
-    printf("debug %d\n", __LINE__);
+    //printf("debug %d\n", __LINE__);
 
     if (filtered_segments.empty())
     {
@@ -313,7 +327,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         return false;
     }
 
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
     // 创建结果对象
     RechargeResult result;
     result.frameid = frame_index_++;
@@ -350,16 +364,16 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
             break; // 只需要两个分段
     }
 
-    printf("debug %d\n", __LINE__);
+  //  printf("debug %d\n", __LINE__);
     // 如果成功获取左右点，进行控制
     if (!result.left_points.empty() && !result.right_points.empty())
     {
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
         procData(result);
-    printf("debug %d\n", __LINE__);
+   // printf("debug %d\n", __LINE__);
         return true;
     }
-    printf("debug %d\n", __LINE__);
+  //  printf("debug %d\n", __LINE__);
     // publishStationDetected(false);
     return false;
 }
@@ -372,7 +386,6 @@ void LidarScanController::procData(RechargeResult &result)
         has_angle_bisector_ = false;
         return;
     }
-
     // 拟合左边直线
     auto [k1, b1] = RechargeTool::simpleFitLine(result.left_points);
     RCLCPP_DEBUG(node_->get_logger(), "左直线拟合: y = %.4fx + %.4f", k1, b1);
@@ -462,6 +475,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         ctrl_twist_.linear.x = 0.0;
         ctrl_twist_.angular.z = 0.0;
         ctrl_twist_time_ = node_->now();
+        //printf("debug %d\n", __LINE__);
         stopNavigation();
         return;
     }
@@ -745,7 +759,7 @@ void LidarScanController::filterScanModify0(LaserScanSharedPtr msg)
     }
 }
 
-void LidarScanController::filterScanModify1(LaserScanSharedPtr msg)
+void LidarScanController::filterScanModify1(LaserScanSharedPtr  msg)
 {
     // 过滤角度，只接受后面90度
     int min_index = param_.min_index;
@@ -760,7 +774,7 @@ void LidarScanController::filterScanModify1(LaserScanSharedPtr msg)
     }
 }
 
-void LidarScanController::filterScanModify2(LaserScanSharedPtr msg)
+void LidarScanController::filterScanModify2(LaserScanSharedPtr  msg)
 {
     // 过滤距离
     for (size_t i = 0; i < msg->ranges.size(); i++)
@@ -811,7 +825,6 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
     int begin_index = 0;
     int end_index = 0;
     std::vector<float> ranges = msg->ranges;
-
     // 顺序找起点
     for (int i = param_.min_index + 1; i < param_.max_index; i++)
     {
@@ -826,7 +839,6 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
             break;
         }
     }
-
     if (static_cast<int>(msg->ranges.size()) - begin_index < 10)
     {
         // RCLCPP_ERROR(node_->get_logger(), "数据太少无法继续处理");
@@ -834,7 +846,7 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
                      begin_index, msg->ranges.size());
         return {0, 0};
     }
-
+    
     // 倒序找终点
     for (int i = param_.max_index - 2; i > 0; i--)
     {
@@ -851,7 +863,7 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
     {
         return {0, 0};
     }
-
+    
     // 顺序检查连续性
     for (int i = begin_index + 1; i < end_index - 1; i++)
     {
@@ -938,17 +950,24 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
             }
         }
     }
-
+    
     // 设置无效区域
     for (int i = param_.min_index; i < begin_index - 1; i++)
     {
         msg->ranges[i] = std::numeric_limits<float>::infinity();
     }
-    for (int i = end_index + 1; i <= param_.max_index; i++)
+    
+    for (int i = end_index + 1; i <= param_.max_index&&i<msg->ranges.size(); i++)
     {
+        if(i<0||i>=msg->ranges.size())
+        {
+            RCLCPP_ERROR(node_->get_logger(), "数据太少无法继续处理,i=%d,size=%ld",i,msg->ranges.size());
+            sleep(1000);
+        }
+
         msg->ranges[i] = std::numeric_limits<float>::infinity();
     }
-
+    
     return {begin_index, end_index};
 }
 
@@ -1390,4 +1409,4 @@ bool LidarScanController::getChargingVoltage() const
     // }
     // return false;
     return detect_charging_voltage_; 
-}
+}