recharge

src/recharge/include/lidascan_ctrl.hpp

@@ -50,6 +50,7 @@ public:
 
 private:
     rclcpp::Node *node_;
+    std::mutex data_mutex_;
 
     // 新增：存储当前角平分线信息
     double current_angle_bisector_k_;
@@ -128,7 +129,7 @@ private:
     // ========== 辅助函数 ==========
     void publishFilteredScan(const LaserScanSharedPtr &original_msg,
                              const std::vector<float> &filtered_ranges);
-    LaserScanSharedPtr resetLaserData(LaserScanSharedPtr msg, int b_index, int e_index);
+    LaserScanSharedPtr resetLaserData(const 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/report.hpp

@@ -17,7 +17,7 @@
 #include <interface/srv/scene_controls.hpp>
 #include "ipad_manager.hpp"
 #include <memory>
-
+class IpadManager; 
 
 class ReportManager
 {
@@ -25,8 +25,7 @@ public:
     using StatusReportCallback = std::function<void(const std::string &)>;
     using ErrorCallback = std::function<void(const ErrorInfo &)>;
 
-    ReportManager(rclcpp::Node *node,IpadManager* ipad);
-
+    ReportManager(rclcpp::Node *node, IpadManager* ipad);
     void handleRechargeService(const std::shared_ptr<interface::srv::SceneControls::Request> request, std::shared_ptr<interface::srv::SceneControls::Response> response);
     // 报告生成接口
     std::string generateSystemStatusReport(

src/recharge/include/wheelchair_types.hpp

@@ -59,7 +59,7 @@ struct ScanFilterParam
 {
     int min_index = 0;
     int max_index = 447;
-    double min_intensities = 120.0;
+    double min_intensities = 30.0;
     double max_intensities = 220.0;
     double min_distance = 0.28;
     double max_distance = 3.5;

src/recharge/src/lidascan_ctrl.cpp

@@ -164,6 +164,7 @@ void LidarScanController::stopMotion()
 
 void LidarScanController::navigationTimerCallback()
 {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     if (!active_)
         return;
 
@@ -178,34 +179,48 @@ void LidarScanController::navigationTimerCallback()
 
 void LidarScanController::processLaserScan(const LaserScanSharedPtr &msg)
 {
+    // std::lock_guard<std::mutex> lock(data_mutex_);
     if (!active_)
         return;
     // 应用过滤算法
+    // printf("debug %d\n", __LINE__);
     filterScanModify0(msg);
+    // printf("debug %d\n", __LINE__);
     filterScanModify1(msg);
+    // printf("debug %d\n", __LINE__);
     filterScanModify2(msg);
+    // printf("debug %d\n", __LINE__);
     filterScanModify3(msg);
-    //publishFilteredScan(msg,msg->ranges);
-
+    // printf("debug %d\n", __LINE__);
+    // publishFilteredScan(msg,msg->ranges);
     auto [begin_index, end_index] = filterScanModify4(msg);
+    // printf("debug %d\n", __LINE__);
+    
      
     if (end_index - begin_index < 2)
     {
         //RCLCPP_WARN(node_->get_logger(), "没有可连续性的点");
         return;
     }
+    // printf("debug %d\n", __LINE__);
     //RCLCPP_WARN(node_->get_logger(), "开始回充");
     // 处理分段数据
+    // return;
     if (processSegmentData(msg, begin_index, end_index))
     {
+    // printf("debug %d\n", __LINE__);
         detect_station_time_ = node_->now();
     }
+    // printf("debug %d\n", __LINE__);
+    // usleep(10*1000*1000);
+    // printf("debug %d\n", __LINE__);
 }
 
 bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
                                              int begin_index, int end_index)
 {
     // 转换到XY坐标系
+    printf("debug %d\n", __LINE__);
     auto xy_list = laserScanToXY1(begin_index, end_index, msg);
     if (xy_list.size() < 2)
     {
@@ -213,6 +228,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         return false;
     }
 
+    printf("debug %d\n", __LINE__);
     // 获取分段
     auto sections = getSections(begin_index, end_index, msg, xy_list);
     if (sections.empty())
@@ -220,6 +236,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         // RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
         return false;
     }
+    printf("debug %d\n", __LINE__);
 
     // 选择最近的分段
     int nearest_index = -1;
@@ -258,6 +275,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         }
     }
 
+    printf("debug %d\n", __LINE__);
     if (nearest_index < 0)
     {
         return false;
@@ -266,12 +284,14 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
     begin_index = sections[nearest_index].first;
     end_index = sections[nearest_index].second;
 
+    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__);
     // 进一步处理
     auto xy_list_full = laserScanToXY(filtered_msg);
     if (xy_list_full.size() < 2)
@@ -280,9 +300,12 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         return false;
     }
 
+    printf("debug %d\n", __LINE__);
     // 获取分段
     auto turn_segments = getSegments(xy_list_full);
+    printf("debug %d\n", __LINE__);
     auto filtered_segments = filterSegments(turn_segments);
+    printf("debug %d\n", __LINE__);
 
     if (filtered_segments.empty())
     {
@@ -290,6 +313,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         return false;
     }
 
+    printf("debug %d\n", __LINE__);
     // 创建结果对象
     RechargeResult result;
     result.frameid = frame_index_++;
@@ -326,12 +350,16 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
             break; // 只需要两个分段
     }
 
+    printf("debug %d\n", __LINE__);
     // 如果成功获取左右点，进行控制
     if (!result.left_points.empty() && !result.right_points.empty())
     {
+    printf("debug %d\n", __LINE__);
         procData(result);
+    printf("debug %d\n", __LINE__);
         return true;
     }
+    printf("debug %d\n", __LINE__);
     // publishStationDetected(false);
     return false;
 }
@@ -763,8 +791,8 @@ void LidarScanController::filterScanModify3(LaserScanSharedPtr msg)
         // 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);
+        //      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();
         }
 
@@ -787,18 +815,23 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
     // 顺序找起点
     for (int i = param_.min_index + 1; i < param_.max_index; i++)
     {
+        // RCLCPP_WARN(node_->get_logger(), "顺序查找 i=%d, ranges[i]=%f, isValid(ranges[i])=%d", 
+        //              i, ranges[i], isValid(ranges[i]));
         if (!isValid(ranges[i]))
             continue;
         if (!isValid(ranges[i - 1]))
         {
             begin_index = i;
+            // RCLCPP_WARN(node_->get_logger(), "找到起点 begin_index=%d", begin_index);
             break;
         }
     }
 
     if (static_cast<int>(msg->ranges.size()) - begin_index < 10)
     {
-        RCLCPP_ERROR(node_->get_logger(), "数据太少无法继续处理");
+        // RCLCPP_ERROR(node_->get_logger(), "数据太少无法继续处理");
+        RCLCPP_ERROR(node_->get_logger(), "数据太少无法继续处理, begin_index=%d, size=%zu", 
+                     begin_index, msg->ranges.size());
         return {0, 0};
     }
 
@@ -1247,9 +1280,24 @@ void LidarScanController::publishFilteredScan(
 }
 
 LidarScanController::LaserScanSharedPtr LidarScanController::resetLaserData(
-    LaserScanSharedPtr msg,
+    const LaserScanSharedPtr & input_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;
+
+    
+    auto& msg = output_msg;
     std::vector<float> filtered_ranges = msg->ranges;
     std::vector<float> intensities = msg->intensities;