回充调优 0325

src/recharge/include/wheelchair_types.hpp

@@ -55,18 +55,19 @@ struct StraightLine
     StraightLine(double k = 0.0, double b = 0.0) : k(k), b(b) {}
 };
 
-struct ScanFilterParam
+
+struct ScanFilterParam //新车参数
 {
-    int min_index = 0;
-    int max_index = 447;
-    double min_intensities = 30.0;
-    double max_intensities = 220.0;
+    int min_index = 40;
+    int max_index = 140;
+    double min_intensities = 48.0;
+    double max_intensities = 69.0;
     double min_distance = 0.28;
     double max_distance = 3.5;
     int min_count = 4;
     double dock_length = 0.42;
     int baselink_back_index = 337;
-    double dy_diff = 0.125;
+    double dy_diff = 0.125 + 0.07;
 };
 
 struct RechargeResult

src/recharge/src/lidascan_ctrl.cpp

@@ -47,8 +47,6 @@ void LidarScanController::initialize()
         "/scan/back", qos_profile,
         [this](const LaserScanSharedPtr msg)
         {
-            if (active_)
-                //RCLCPP_WARN(node_->get_logger(), "开始回充");
                 processLaserScan(msg);
         });
 
@@ -179,6 +177,8 @@ void LidarScanController::navigationTimerCallback()
 
 void LidarScanController::processLaserScan(const LaserScanSharedPtr &input_msg)
 {
+    // if (!active_)
+    return;
     LaserScanSharedPtr output_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
     //拷貝input_msg 到 output_msg;
     output_msg->header = input_msg->header;
@@ -194,9 +194,6 @@ void LidarScanController::processLaserScan(const LaserScanSharedPtr &input_msg)
 
     
     auto& msg = output_msg;
-    // std::lock_guard<std::mutex> lock(data_mutex_);
-    if (!active_)
-        return;
     // 应用过滤算法
     // printf("debug %d\n", __LINE__);
     filterScanModify0(msg);
@@ -441,12 +438,10 @@ void LidarScanController::procData(RechargeResult &result)
     result.middle_line = StraightLine(k3, b3);
     result.left_line = StraightLine(k1, b1);
     result.right_line = StraightLine(k2, b2);
-
-    // 运动控制
-    controlRechargeMotion(result);
-
     // 可视化
     publishRechargeVisualization(result);
+    // 运动控制
+    controlRechargeMotion(result);
 }
 
 void LidarScanController::controlRechargeMotion(const RechargeResult &result)
@@ -483,13 +478,14 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
     }
 
     double dx = result.middle_point.x;
-    double dy = result.middle_point.y;
+    double dy_center = result.middle_point.y;
     double k = result.middle_line.k;
 
-    RCLCPP_DEBUG(node_->get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);
+    //RCLCPP_DEBUG(node_->get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);
 
     // 调整 dy
-    dy = dy - param_.dy_diff;
+    double dy_jipian =dy_center-param_.dy_diff;
+    double& dy = dy_jipian;
     RCLCPP_DEBUG(node_->get_logger(), "调整后 dy: %.4f", dy);
 
     // 计算偏航角误差
@@ -516,7 +512,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         double adjust_angular = 0.0;
         std::string adjust_type;
 
-        const double ANGLE_8DEG = 5.0 * M_PI / 180.0; // 5度阈值
+        const double ANGLE_8DEG = 8.0 * M_PI / 180.0; // 5度阈值
                                                       // const double DY_THRESHOLD = 0.05;             // 5cm阈值
 
         // 优先调整角度
@@ -570,7 +566,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
             }
 
             // 5. 向前走一小段距离（新增功能）
-            const double FORWARD_DISTANCE = 0.80; // 向前走150cm
+            const double FORWARD_DISTANCE = 0.60; // 向前走150cm
             const double FORWARD_SPEED = 0.09;    // 前进速度 0.08 m/s
 
             // 计算前进所需时间
@@ -705,8 +701,8 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
 bool LidarScanController::checkStationAlignment(double dx, double dy, double yaw_diff)
 {
     bool isstop = false;
-    const double ALIGNMENT_DISTANCE_THRESHOLD = 0.80;            // 80cm
-    const double ALIGNMENT_ANGLE_THRESHOLD = 5.0 * M_PI / 180.0; // 5度
+    const double ALIGNMENT_DISTANCE_THRESHOLD = 0.70;            // 70cm
+    const double ALIGNMENT_ANGLE_THRESHOLD = 8.0 * M_PI / 180.0; // 8度
 
     if (dx <= ALIGNMENT_DISTANCE_THRESHOLD)
     {
@@ -714,7 +710,7 @@ bool LidarScanController::checkStationAlignment(double dx, double dy, double yaw
                     dx, yaw_diff * 180.0 / M_PI);
 
         // 判断是否对准（夹角小于10度）
-        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD)
+        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD && std::fabs(dy) <= 0.08)
         {
             RCLCPP_INFO(node_->get_logger(), "✓ 已对准基站，继续行驶 - 夹角: %.2f°,dx=%.2f,dy=%.2f",
                         yaw_diff * 180.0 / M_PI, dx, dy);
@@ -723,7 +719,7 @@ bool LidarScanController::checkStationAlignment(double dx, double dy, double yaw
         }
         else
         {
-            RCLCPP_WARN(node_->get_logger(), "✗ 未对准基站，停止行驶 - 夹角: %.2f° > 5°,dx=%.2f,dy=%.2f",
+            RCLCPP_WARN(node_->get_logger(), "✗ 未对准基站，停止行驶 - 夹角: %.2f° > 8°,dx=%.2f,dy=%.2f",
                         yaw_diff * 180.0 / M_PI, dx, dy);
             isstop = true;
             // RCLCPP_INFO(node_->get_logger(), "自动调整方向 - 旋转角速度: %.4f", adjust_angular);
@@ -1298,7 +1294,7 @@ void LidarScanController::publishFilteredScan(
     msg->intensities = original_msg->intensities;
 
     pub_filtered_->publish(*msg);
-    RCLCPP_WARN(node_->get_logger(), "已发布过滤后的激光数据");
+    // RCLCPP_WARN(node_->get_logger(), "已发布过滤后的激光数据");
 }
 
 LidarScanController::LaserScanSharedPtr LidarScanController::resetLaserData(

src/recharge/src/rotation_manager.cpp

@@ -1,6 +1,7 @@
 // rotation_manager.cpp
 #include "rotation_manager.hpp"
 #include <rclcpp/logging.hpp>
+#include "../../common/utils/topic.h"
 
 using namespace std::chrono_literals;
 
@@ -13,7 +14,7 @@ RotationManager::RotationManager(rclcpp::Node *node)
 {
     // 创建控制命令发布者
     cmd_vel_publisher_ = node_->create_publisher<geometry_msgs::msg::Twist>(
-        "/cmd_vel_raw", 10);
+        nav2ControlTopic, 10);
     rotation_timeout_pub1_ = node_->create_publisher<std_msgs::msg::Int32>(
         "/vlago/recharge/state", 10);
     rotation_timeout_pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);